UAV/test2.py at master · RoboticsatUCD/UAV · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
#Evan Racah
#9/18/2013
#Initialize IMU object to be used from this module
from imu import IMU
from robovero.extras import roboveroConfig
import time
from ComplementaryFilter import ComplementaryFilter
from motor import Motor
from PID import PIDControl


roboveroConfig()

# Initialize IMU
imu = IMU()
#imu.calibrate(0, 0, -1)
cfRoll = ComplementaryFilter(0.9, 0)
cfPitch = ComplementaryFilter(0.9, 0)

motor1 = Motor(1)
motor2 = Motor(6)
motor3 = Motor(5)
motor4 = Motor(4)
motors = [motor1, motor2, motor3, motor4]

#pidPitch = PIDControl(0, 1)
#pidRoll = PIDControl(0, 1)

#registers is list of three tuples for registers [(xlow,xhigh),(ylow,yhigh), etc]
string = ""

while(1):
	string = "Roll: " + str(imu.roll_angle)
	string += "\nPitch: " + str(imu.pitch_angle)
	string += "\nRoll rate: " + str(imu.roll_rate)
	string += "\nPitch rate: " + str(imu.pitch_rate)
	string += "\nfrom filter Roll: " + str(cfRoll.filter(imu.roll_angle, imu.roll_rate))
	string += "\nfrom filter Pitch: " + str(cfPitch.filter(imu.pitch_angle, imu.pitch_rate))

	speed = int(raw_input('Speed?'))
	for motore in motors:
		motore.setSpeed(speed)
		motore.go()

	print string