from Jay_class import Servo,Pid,Imu 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)
import time 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