def main(): logger.debug("initializing imu...") imu = MPU9250() # initialize mpu9250, the imu logger.debug("initializing esc...") esc_hdd = ESC(config['speed_pin_esc'], config['dir_pin1_esc'], config['dir_pin2_esc']) esc_hdd.init_sequence() # perform the esc initialization sequence # do we want to stream sensor data to the web interface if config['enable_mission_control']: logger.info("connecting to mission control server...") mc.connect() # connect to mission control server logger.info("starting main loop...") #acc_array = np.zeros(3,1) #mag_array = np.zeros(3,1) while True: # fetch values from the IMU imu.update_acc_reading() # update acc values imu.update_mag_reading() # update mag values acc_meas_raw = [imu.acc_y * -1, imu.acc_x, imu.acc_z * -1] # fix imu right hand rule mag_meas_raw = [imu.mag_x, imu.mag_y * -1, imu.mag_z] # fix imu right hand rule # generate the necessary vectors for TRIAD # TODO don't recreate numpy array every time, heavy memory computation acc_meas = utils.to_unit_vector(acc_meas_raw) # acc unit vector mag_meas = utils.to_unit_vector(mag_meas_raw) # mag unit vector #acc_array[:,0] = acc_meas_raw #mag_array[:,0] = mag_meas_raw #acc_meas = numpy.linalg.oth(acc_array) #mag_meas = numpu.linalg.oth(mag_array) acc_ref = np.array([0.0, 0.0, -1.0]) # acc ref vector mag_ref = np.array([1.0, 0.0, 0.0]) # mag ref vector # make sure none of the vectors are zero vectors if not (np.any(acc_meas) and np.any(mag_meas)): logger.warn("measurement zero vector computed") continue # compute the rotation matrix with the aforemened vectors rotation = triad.triad(acc_meas, mag_meas, acc_ref, mag_ref) logger.debug("acc: {} mag: {} rotation: {}".format( acc_meas, mag_meas, rotation)) # broadcast all data to mission control server # this is the real-time gui thing if config['enable_mission_control']: mc.broadcast(acc_meas_raw, mag_meas_raw, rotation) # wait a sec before proceeding, this is our # update frequency. time.sleep(0.05)
class QuadQav250: PID_ANGULAR = [[0.006,0.0,0.0],[0.006,0,0],[0,0,0]] def __init__(self): try: self.altimeter = TOF_VL53L1X() self.thrust = ESC() self.ahrs = AHRS() self.angular_pid_ahrs = PidController(QuadQav250.PID_ANGULAR, self.ahrs.get_angular_position, [0,0,0], PidController.t_angular) self.ahrs.start() except Exception: self.shutdown() print('Failed to initialize') def shutdown(self): try: self.thrust.disarm() finally: print('===DISARMED===') def test(self): for n in range(100): pass #print(self.ahrs.get_angular_position()) def kill(self): self.shutdown() def fly(self): try: print('============================') print('STARTING TEST FLIGHT') self.thrust.spin_test() base_throttle = [.20,.20,.20,.20] t0 = time.time() flying = True while flying: if time.time() > t0 + 0.20: base_throttle = np.add(base_throttle, 0.001) pid_update = self.angular_pid_ahrs.update() v_throttle = np.add(base_throttle, pid_update) self.thrust.set_throttle(v_throttle) xyz, xyz_dot, dt = self.altimeter.get_altitude() if xyz[2] > 0.200: flying = False print(v_throttle,self.thrust.v_pwm,self.ahrs.xyz) self.thrust.set_throttle([.2,.2,.2,.2]) time.sleep(0.5) self.thrust.set_throttle([0,0,0,0]) finally: self.shutdown()
def main(): logger.debug("Initializing the imu...") # Initialize the imu. imu = MPU9250() # Initialize the speed controller. logger.debug("initializing esc...") esc_hdd = ESC(config['speed_pin_esc'], config['dir_pin1_esc'], config['dir_pin2_esc']) esc_hdd.init_sequence() # If enabled, connect to the web interface server. if config['enable_mission_control']: logger.info("connecting to mission control server...") mc.connect() # connect to mission control server logger.info("starting main loop...") # acc_array = np.zeros(3,1) # mag_array = np.zeros(3,1) while True: # Fetch and calculate raw measure values from the IMU. raw_acc_measure, raw_mag_measure = get_updated_sensor_data(imu) # TODO don't recreate numpy array every time, heavy memory computation # Generate the necessary vectors for TRIAD. acc_measure_uvec = utils.to_unit_vector(raw_acc_measure) mag_measure_uvec = utils.to_unit_vector(raw_mag_measure) # acc_array[:,0] = acc_meas_raw # mag_array[:,0] = mag_meas_raw # acc_meas = numpy.linalg.oth(acc_array) # mag_meas = numpu.linalg.oth(mag_array) # Create reference vectors for the accelerometer and magnetometer. acc_refvec = np.array([0.0, 0.0, -1.0]) mag_refvec = np.array([1.0, 0.0, 0.0]) # Make sure none of the vectors are zero vectors. if not (np.any(acc_measure_uvec) and np.any(mag_measure_uvec)): logger.warn("measurement zero vector computed") continue # Compute the rotation matrix with the aforementioned vectors. rotation = triad.triad(acc_measure_uvec, mag_measure_uvec, acc_refvec, mag_refvec) logger.debug("acc: {} mag: {} rotation: {}".format( acc_measure_uvec, mag_measure_uvec, rotation)) # Broadcast the raw measures to the mission control server. if config['enable_mission_control']: mc.broadcast(raw_acc_measure, raw_mag_measure, rotation) # Simulate update frequency through sleeping. time.sleep(0.05)
def __init__(self): try: self.altimeter = TOF_VL53L1X() self.thrust = ESC() self.ahrs = AHRS() self.angular_pid_ahrs = PidController(QuadQav250.PID_ANGULAR, self.ahrs.get_angular_position, [0,0,0], PidController.t_angular) self.ahrs.start() except Exception: self.shutdown() print('Failed to initialize')
from car import Car from config import WEB_PORT from esc import ESC from led import LED import esc import threading import time import psutil #from printf import PRINTF import os app = Flask(__name__) led = LED() car = Car() esc = ESC() #printf = PRINTF() handle_map = { 'forward': car.forward, 'left': car.left, 'right': car.right, 'pause': car.stop, 'backward': car.backward, 'on': esc.on, 'off': esc.off, #'on': printf.add, #'off': printf.dec, 'work': led.work, }
# PID rr_pid = PID(p=0.7, i=1, imax=50) pr_pid = PID(p=0.7, i=1, imax=50) yr_pid = PID(p=2.7, i=1, imax=50) rs_pid = PID(p=4.5) ps_pid = PID(p=4.5) ys_pid = PID(p=10) # RC rc_thr = RC(0) # throttle rc_rol = RC(1) # roll rc_pit = RC(2) # pitch rc_yaw = RC(3) # yaw # ESC esc_0 = ESC(0) esc_1 = ESC(1) esc_2 = ESC(2) esc_3 = ESC(3) esc_4 = ESC(4) esc_5 = ESC(5) yaw_target = 0 while True: # MPU mpuIntStatus = mpu.getIntStatus() fifoCount = mpu.getFIFOCount() if mpuIntStatus < 2 or fifoCount == 1024: mpu.resetFIFO() print('FIFO overflow!') continue
import time import pwm #Used to enable PWM from esc import ESC #You can use your own method to enable PWM, but this just works pwm.enable(); esc = ESC() esc.attach("P9_16") # This example works with aeolian car (double direction) ESC which minimum throttle is 80. # Try changing values until you found the right value for your ESC esc.write(80) #esc.writeMicroseconds(1600) #Now the ESC es ready, in my case if I use values less than 80 the motor goes rearward # and if the values are greater than 80 the motor goes fordward