from motor.motor_pigpio import Motor mymotor1 = Motor('m1', 17, debug=False, simulation=False) # RL mymotor2 = Motor('m2', 18, debug=False, simulation=False) # RR mymotor3 = Motor('m3', 27, debug=False, simulation=False) # FR mymotor4 = Motor('m4', 22, debug=False, simulation=False) # FL mymotor1.setW(0) mymotor2.setW(0) mymotor3.setW(0) mymotor4.setW(0)
from motor.motor_pigpio import Motor import sensors.imu2 as imu import time import smbus mymotor1 = Motor('m1', 17, debug=False, simulation=False) # RL mymotor2 = Motor('m2', 18, debug=False, simulation=False) # RR mymotor3 = Motor('m3', 27, debug=False, simulation=False) # FR mymotor4 = Motor('m4', 22, debug=False, simulation=False) # FL bus = smbus.SMBus(1) imu = imu.IMU(bus, 0x68, 0x53, 0x1e, "IMU") def measure(s): st = time.time() m=-9999 mean=0.0 i=0 while (time.time() - st) < s: g = imu.read_all() g = g[2:5] i=i+1 # print g total = abs(g[0]-0.0036424263999180213)+abs(g[1]+0.0036424263999180213)+abs(g[2]+0.013355563466366077) #- 0.00121414213331 # print total if i>1: mean = mean/(i-1) + total/i else: mean = total if total>m: m=total
str(ay)) log.write('\t' + str(ex) + '\t' + str(ey) + '\t' + str(ez)) log.write("\n") i = i + 1 else: print("Warning IMU disconnected") old_pitch, old_roll, old_yaw = pitch, roll, yaw samples = samples + 1 ii = ii + 1 if __name__ == "__main__": bus = smbus.SMBus(1) # i2c_raspberry_pi_bus_number()) imu_controller = imu.IMU(bus, 0x68, 0x53, 0x1e, "IMU") mymotor1 = Motor('m1', 17, debug=False, simulation=False) # RL mymotor2 = Motor('m2', 18, debug=False, simulation=False) # RR mymotor3 = Motor('m3', 27, debug=False, simulation=False) # FR mymotor4 = Motor('m4', 22, debug=False, simulation=False) # FL mymotor1.setMaxSpeed(30) mymotor2.setMaxSpeed(30) mymotor3.setMaxSpeed(30) mymotor4.setMaxSpeed(30) quadcopter = Quad(mymotor1, mymotor2, mymotor3, mymotor4, imu_controller) camera = PiCam('img',1) # p.take_pic() print ("""init > i | balance > b | stop > s | PID > p | set_zero > r