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
