def __init__( self, m1, m2, m3, m4, imu, s1=None, s2=None, s3=None, s4=None): self.motor_bl = m3 self.motor_br = m1 self.motor_fr = m2 self.motor_fl = m4 self.sensor_l = s1 self.sensor_f = s2 self.sensor_r = s3 self.sensor_b = s4 self.imu = imu self.thread = None self.running = False self.height = 10 self.kp_x = 1 self.kp_y = 1 self.kp_z = 0 self.ki_x = 0 self.ki_y = 0 self.ki_z = 0 self.kd_x = 0 self.kd_y = 0 self.kd_z = 0 self.kpr_x = 20 self.kpr_z = 50 self.kpr_y = 15 self.limit_px = 20 self.limit_py = 20 self.limit_ix = 10 self.limit_iy = 10 self.limit_pz = 10 self.limit_iz = 10 self.limit_rx = 20 self.limit_ry = 20 self.limit_rz = 20 self.offset_x = 0 self.offset_y = 0 self.offset_z = 0 self.zero_x = 0 self.zero_y = 0 self.zero_z = 0 self.trim_x = 0 self.trim_y = 0 self.trim_z = 0 #x^ top is front # y-> # z. dri_frequency = 50.0 samples_per_motion = 4 self.bfx = Butterworth(dri_frequency / samples_per_motion, 0.05, 8) self.bfy = Butterworth(dri_frequency / samples_per_motion, 0.05, 8) self.bfz = Butterworth(dri_frequency / samples_per_motion, 0.05, 8) self.imu.set_compass_offsets(9, -10, -140) self.motor_fl.start() self.motor_bl.start() self.motor_fr.start() self.motor_br.start() self.motor_fl.setW(0) self.motor_bl.setW(0) self.motor_fr.setW(0) self.motor_br.setW(0)
class Quad(object): def __init__( self, m1, m2, m3, m4, imu, s1=None, s2=None, s3=None, s4=None): self.motor_bl = m3 self.motor_br = m1 self.motor_fr = m2 self.motor_fl = m4 self.sensor_l = s1 self.sensor_f = s2 self.sensor_r = s3 self.sensor_b = s4 self.imu = imu self.thread = None self.running = False self.height = 10 self.kp_x = 1 self.kp_y = 1 self.kp_z = 0 self.ki_x = 0 self.ki_y = 0 self.ki_z = 0 self.kd_x = 0 self.kd_y = 0 self.kd_z = 0 self.kpr_x = 20 self.kpr_z = 50 self.kpr_y = 15 self.limit_px = 20 self.limit_py = 20 self.limit_ix = 10 self.limit_iy = 10 self.limit_pz = 10 self.limit_iz = 10 self.limit_rx = 20 self.limit_ry = 20 self.limit_rz = 20 self.offset_x = 0 self.offset_y = 0 self.offset_z = 0 self.zero_x = 0 self.zero_y = 0 self.zero_z = 0 self.trim_x = 0 self.trim_y = 0 self.trim_z = 0 #x^ top is front # y-> # z. dri_frequency = 50.0 samples_per_motion = 4 self.bfx = Butterworth(dri_frequency / samples_per_motion, 0.05, 8) self.bfy = Butterworth(dri_frequency / samples_per_motion, 0.05, 8) self.bfz = Butterworth(dri_frequency / samples_per_motion, 0.05, 8) self.imu.set_compass_offsets(9, -10, -140) self.motor_fl.start() self.motor_bl.start() self.motor_fr.start() self.motor_br.start() self.motor_fl.setW(0) self.motor_bl.setW(0) self.motor_fr.setW(0) self.motor_br.setW(0) def motor_init(self): self.motor_fl.setW(100) self.motor_bl.setW(100) self.motor_fr.setW(100) self.motor_br.setW(100) time.sleep(2) self.motor_fl.setW(0) self.motor_bl.setW(0) self.motor_fr.setW(0) self.motor_br.setW(0) time.sleep(1) def stop(self): self.running = False self.motor_fl.setW(0) self.motor_bl.setW(0) self.motor_fr.setW(0) self.motor_br.setW(0) self.motor_fl.stop() self.motor_bl.stop() self.motor_fr.stop() self.motor_br.stop() def set_PID(self, p, i, d, kpr=0): self.kp_x = p self.ki_x = i self.kd_x = d self.kp_y = p self.ki_y = i self.kd_y = d self.kpr_x = kpr self.kpr_y = kpr # self.kp_z = p # self.ki_z = i # self.kd_z = d def save_zero(self): self.set_zero_angle() f = open('zero.cfg', 'w') import json f.write(json.dumps({'x': self.zero_x, 'y': self.zero_y, 'z': self.zero_z})) f.close() def load_zero(self): f = open('zero.cfg', 'r') import json zero_angle = json.load(f) self.zero_x = zero_angle['x'] self.zero_y = zero_angle['y'] self.zero_z = zero_angle['z'] def set_zero_z(self): (pitch, roll, yaw, _) = self.imu.read_pitch_roll_yaw() self.zero_z = yaw def compute_PID_output( self, kp, ki, kd, angle, old_i, old_angle, limit_p=100, limit_i=100, log=False): p = kp * angle i = old_i + ki * angle d = kd * (angle - old_angle) if p > limit_p: p = limit_p if p < -limit_p: p = -limit_p if i > limit_i: i = limit_i if i < -limit_i: i = -limit_i if log: log.write('\t' + str(p) + '\t' + str(i) + '\t' + str(d) + '\t') return [p + i + d, i] def compute_rate_PID_output( self, kpr, kp, ki, kd, gyro, angle, old_i, old_angle, limit_p=100, limit_i=100, limit_pr=100, log=False): p = kp * angle i = old_i + ki * angle d = kd * (angle - old_angle) if p > limit_p: p = limit_p if p < -limit_p: p = -limit_p if i > limit_i: i = limit_i if i < -limit_i: i = -limit_i total = kpr*(p + i + gyro) print total,p,i,kpr,angle,gyro,kp if total > limit_pr: total = limit_pr if total < -limit_pr: total = -limit_pr if log: log.write('\t' + str(p) + '\t' + str(i) + '\t' + str(d) + '\t') return [total, i] def balancer(self): print("Started Balancing") self.thread = Thread(target=self.rate_balance) self.running = True self.thread.start() # self.balance() def set_zero_angle(self): (pitch, roll, yaw, _) = self.imu.read_pitch_roll_yaw() self.zero_x = pitch self.zero_y = roll self.zero_z = yaw def set_height(self, amt=0): self.height = amt def inc_height(self, amt=1): self.height = self.height + amt def dec_height(self, amt=1): self.height = self.height - amt def set_parameters(self, x, y, z, height): self.offset_x = x self.offset_y = y self.offset_z = z self.height = height def set_trims(self, x, y, z, relative=True): if relative: self.trim_x = self.trim_x + x self.trim_y = self.trim_y + y self.trim_z = self.trim_z + z else: self.trim_x = x self.trim_y = y self.trim_z = z def rate_balance(self): pitch = 0.0 roll = 0.0 yaw = 0.0 i_x = 0.0 i_y = 0.0 i_z = 0.0 log = open('logs/motor.log', 'w') old_pitch, old_roll, old_yaw = 0, 0, 0 while self.running: (roll, pitch, yaw, gy, gx, gz, imu_connected) = self.imu.read_pitch_roll_yaw_full() print gx, gy, gz, pitch, roll, yaw axis_output = {'x': 0, 'y': 0, 'z': 0} (axis_output['x'], i_x) = self.compute_rate_PID_output(self.kpr_x, self.kp_x, self.ki_x, self.kd_x, gx, pitch - self.offset_x - self.zero_x, i_x, old_pitch, self.limit_px ,self.limit_ix, self.limit_rx) (axis_output['y'], i_y) = self.compute_rate_PID_output(self.kpr_y, self.kp_y, self.ki_y, self.kd_y, gy, roll - self.offset_y - self.zero_y, i_y, old_roll, self.limit_py ,self.limit_iy, self.limit_ry) (axis_output['z'], i_z) = self.compute_rate_PID_output(self.kpr_z, self.kp_z, self.ki_z, self.kd_z, gz, yaw - self.offset_z - self.zero_z, i_z, old_yaw, self.limit_pz ,self.limit_iz, self.limit_rz) axis_output['x'] = axis_output['x'] - self.trim_x axis_output['y'] = axis_output['y'] - self.trim_y print axis_output self.motor_bl.setW( int(self.height + axis_output['x'] / 3 + axis_output['y'] / 3 + axis_output['z']/3)) self.motor_br.setW( int(self.height + axis_output['x'] / 2 - axis_output['y'] / 3 - axis_output['z']/3)) self.motor_fl.setW( int(self.height - axis_output['x'] / 3 + axis_output['y'] / 3 - axis_output['z']/3)) self.motor_fr.setW( int(self.height - axis_output['x'] / 3 - axis_output['y'] / 3 + axis_output['z']/3)) old_pitch, old_roll, old_yaw = pitch, roll, yaw def balance(self): pitch = 0.0 roll = 0.0 yaw = 0.0 i_x = 0.0 i_y = 0.0 i_z = 0.0 log = open('logs/motor.log', 'w') old_pitch, old_roll, old_yaw = 0, 0, 0 log.write( """iteration\theight\tx-ouput\ty-output\tpitch\troll\ttime-taken \tgx\tgy\tax\tay\n""") i = 0 ii = 0 ustart_time = time.time() samples = 0 while self.running: # print("Nothing") start_time = time.time() (pitch, roll, yaw, imu_connected) = self.imu.read_pitch_roll_yaw() ex = self.bfx.filter(pitch) ey = self.bfy.filter(roll) ez = self.bfz.filter(yaw) # (pitch, roll, yaw)=(ex, ey, ez) (_, _, gx, gy, _, ax, ay, _) = self.imu.read_all() axis_output = {'x': 0, 'y': 0, 'z': 0} if ii % 1 == 0: if imu_connected: log.write(str(i) + '\t') [axis_output['x'], i_x] = self.compute_PID_output(self.kp_x, self.ki_x, self.kd_x, pitch - self.offset_x - self.zero_x, i_x, old_pitch, self.limit_px, self.limit_ix, log) [axis_output['y'], i_y] = self.compute_PID_output(self.kp_y, self.ki_y, self.kd_x, roll - self.offset_y - self.zero_y, i_y, old_roll, self.limit_py, self.limit_iy, log) [axis_output['z'], i_z] = self.compute_PID_output( self.kp_z, self.ki_z, self.kd_z, yaw - self.offset_z - self.zero_z, i_z, old_yaw,self.limit_pz,self.limit_iz) axis_output['x'] = axis_output['x'] - self.trim_x axis_output['y'] = axis_output['y'] - self.trim_y self.motor_bl.setW( int(self.height + axis_output['x'] / 2 + axis_output['y'] / 2 + axis_output['z']/2)) self.motor_br.setW( int(self.height + axis_output['x'] / 2 - axis_output['y'] / 2) - axis_output['z']/2) self.motor_fl.setW( int(self.height - axis_output['x'] / 2 + axis_output['y'] / 2) - axis_output['z']/2) self.motor_fr.setW( int(self.height - axis_output['x'] / 2 - axis_output['y'] / 2) + axis_output['z']/2) log.write(str(self.height) + '\t' + str(axis_output['x'] / 2) + '\t' + str(axis_output['y'] / 2)) log.write('\t' + str(pitch) + '\t' + str(roll)) end_time = time.time() if (end_time - ustart_time) > 1: ustart_time = time.time() print(('samples ', samples, ustart_time)) samples = 0 print((pitch, roll, end_time, ustart_time, samples, i_x, i_y)) log.write('\t' + str(end_time - start_time)) log.write( '\t' + str(gx) + '\t' + str(gy) + '\t' + str(ax) + '\t' + 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