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
