Exemplo n.º 1
0
import time




DELTA_T = 0.005
MIDDLE_POS = 550
P = 1.5
I = 0.02
D = 0
SET_ANGLE_ROLL = 0
SET_ANGLE_PITCH = 0
acc=Imu()
sroll=Servo(0,-1,MIDDLE_POS,MIDDLE_POS)
spitch=Servo(1,1,MIDDLE_POS,MIDDLE_POS)
pidroll = Pid(P,I,D)
pidpitch = Pid(P,I,D)

t = time.time()

while 1:
	
	
	acc.ReadIMU(1)
	

	if time.time() - t > DELTA_T: 
		pidroll.finderror(acc.roll,SET_ANGLE_ROLL)
		pidpitch.finderror(acc.pitch,SET_ANGLE_PITCH)

		sroll.move_pid(pidroll)
Exemplo n.º 2
0


DELTA_T = 0.02-.005  #50 Hz
MIDDLE_VIT = 300
P = 0.15
I = 0.02
D = 0 #Always 0 for now if not you have to check the init pid
SET_ANGLE_ROLL = 0
SET_ANGLE_PITCH = 0
OFFSET_ROLL = -.8
OFFSET_PITCH = 3.285
acc=Imu()
sroll=Servo(0,-1,MIDDLE_VIT,MIDDLE_VIT)
spitch=Servo(1,1,MIDDLE_VIT,MIDDLE_VIT)
pidroll = Pid(P,I,D)
pidpitch = Pid(P,I,D)
PRINTTIME = 0
WaitTime = 0
i=0
INIT_END=0
#Programme d'initialisation de la camera sans tout decalisser (pid vraiment lent)
acc.ReadIMU(1)
acc.ReadIMU(1)
pidroll.finderror(acc.roll,SET_ANGLE_ROLL)
pidpitch.finderror(acc.pitch,SET_ANGLE_PITCH)
OldTime = time.time()
#Initialisation Cycle
while i == 1: # Minimum of 10 cycle for init 
	
	acc.ReadIMU(1)