#QUAD_FORMATION_NORMAL first approach motorPowerM1 = limitThrust(thrust + pitch + yaw, 40); motorPowerM2 = limitThrust(thrust - roll - yaw, 40); motorPowerM3 = limitThrust(thrust - pitch + yaw, 40); motorPowerM4 = limitThrust(thrust + roll - yaw, 40); #Log the motor powers: print "------------------------" print "motorPowerM1:" + str(motorPowerM1) print "motorPowerM2:" + str(motorPowerM2) print "motorPowerM3:" + str(motorPowerM3) print "motorPowerM4:" + str(motorPowerM4) print "**************************" #Set motor speeds motor1.setSpeedBrushless(motorPowerM1) motor2.setSpeedBrushless(motorPowerM2) motor3.setSpeedBrushless(motorPowerM3) motor4.setSpeedBrushless(motorPowerM4) #Start Motors for mot in motors: mot.go() #Kalman Prediction #MyKalman.predict() #delay = 4e-3 #delay ms (250 Hz) # delay = 20e-3 #delay ms (50 Hz) # time.sleep(delay)
motorPowerM1 = limitThrust(u[0], limit_thrust) motorPowerM2 = limitThrust(u[1], limit_thrust) motorPowerM3 = limitThrust(u[2], limit_thrust) motorPowerM4 = limitThrust(u[3], limit_thrust) if logging: #Log the motor powers: print "------------------------" print "motorPowerM1 (limited):" + str(motorPowerM1) print "motorPowerM2 (limited):" + str(motorPowerM2) print "motorPowerM3 (limited):" + str(motorPowerM3) print "motorPowerM4 (limited):" + str(motorPowerM4) print "**************************" #Set motor speeds motor1.setSpeedBrushless(motorPowerM1) motor2.setSpeedBrushless(motorPowerM2) motor3.setSpeedBrushless(motorPowerM3) motor4.setSpeedBrushless(motorPowerM4) #Start Motors for mot in motors: mot.go() #Kalman Prediction #MyKalman.predict() # calculate the time each iteration takes time_u = (dt.datetime.now() - start).microseconds time_s = time_u / 1e6 frequency = 1e6 / time_u
python text_motor.py [speed = 10] [time = 5] ''' from motors import Motor import Adafruit_BBIO.GPIO as GPIO import time import os import sys speed = 10 if len(sys.argv) > 1: speed = int(sys.argv[1]) m1 = Motor(1); m1.setSpeedBrushless(speed) m1.go() m2 = Motor(2); m2.setSpeedBrushless(speed); m2.go() m3 = Motor(3); m3.setSpeedBrushless(speed); m3.go() m4 = Motor(4); m4.setSpeedBrushless(speed); m4.go() sleep_time = 5
def initMotors(): speed = 10 m1 = Motor(1); m1.setSpeedBrushless(speed) m1.go() m2 = Motor(2); m2.setSpeedBrushless(speed); m2.go() m3 = Motor(3); m3.setSpeedBrushless(speed); m3.go() m4 = Motor(4); m4.setSpeedBrushless(speed); m4.go() time.sleep(0.5) ##### # shutdown the motors ##### speed = 0 m1.setSpeedBrushless(speed); m1.go() m2.setSpeedBrushless(speed); m2.go() m3.setSpeedBrushless(speed); m3.go() m4.setSpeedBrushless(speed); m4.go()
def initMotors(): speed = 10 m1 = Motor(1) m1.setSpeedBrushless(speed) m1.go() m2 = Motor(2) m2.setSpeedBrushless(speed) m2.go() m3 = Motor(3) m3.setSpeedBrushless(speed) m3.go() m4 = Motor(4) m4.setSpeedBrushless(speed) m4.go() time.sleep(0.5) ##### # shutdown the motors ##### speed = 0 m1.setSpeedBrushless(speed) m1.go() m2.setSpeedBrushless(speed) m2.go() m3.setSpeedBrushless(speed) m3.go() m4.setSpeedBrushless(speed) m4.go()