def __init__(self):
# Velocity PID object setup
self.vel_pid_output_a = VelocityPidOutput()
self.vel_pid_output_b = VelocityPidOutput()
self.vel_pid_output_c = VelocityPidOutput()
self.vel_pid_a = Pid(self.vel_pid_output_a, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F)
self.vel_pid_b = Pid(self.vel_pid_output_b, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F)
self.vel_pid_c = Pid(self.vel_pid_output_c, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F)
# Qep encoder object setup
self.qep_a = Qep(Robot.MOTOR_A_QEP)
self.qep_b = Qep(Robot.MOTOR_B_QEP)
self.qep_c = Qep(Robot.MOTOR_C_QEP)
# Pwm object setup
self.pwm_a = Pwm(Robot.MOTOR_A_PWM)
self.pwm_b = Pwm(Robot.MOTOR_B_PWM)
self.pwm_c = Pwm(Robot.MOTOR_C_PWM)
self.pwms = [self.pwm_a, self.pwm_b, self.pwm_c]
# Create MotorController objects
self.motor_a = MotorController(self.pwm_a, self.vel_pid_a, self.vel_pid_output_a, self.qep_a, self.VEL_PID_ENABLED)
self.motor_b = MotorController(self.pwm_b, self.vel_pid_b, self.vel_pid_output_b, self.qep_b, self.VEL_PID_ENABLED)
self.motor_c = MotorController(self.pwm_c, self.vel_pid_c, self.vel_pid_output_c, self.qep_c, self.VEL_PID_ENABLED)
self.motors = [self.motor_a, self.motor_b, self.motor_c]
# set up pid
self.yaw_pid_output = YawPidOutput()
self.yaw_pid = Pid(self.yaw_pid_output, Robot.YAW_P, Robot.YAW_I, Robot.YAW_D, izone=1.0)
# initialise mpu/imu server module
self.mpu = Mpu()
self.yaw_pid_thread = YawPidThread(self.yaw_pid, self.mpu)
self.current_command = Robot.INIT_COMMAND
# pid *enabled* by default
self.yaw_pid_enabled = False
# pid not in *control* by default, this is automatically set by drive
self.pid_in_control = False
self.field_centered = True
self.enabled = False
self.interrupted = False
self.last_input_time = time.time()
def test_udp_receiver(self):
"""Test that the mpu submodule is able to pick up and interpret packets in the correct format"""
mpu = Mpu()
time.sleep(0.5)
s.sendto("0,1,2,3,4,5,6,7,8,9,10,11,12", ("127.0.0.1", MPU_PORT))
time.sleep(1)
self.assertEqual(mpu.get_euler(), [0, 1, 2])
self.assertEqual(mpu.get_gyro(), [3, 4, 5])
self.assertEqual(mpu.get_accel(), [6, 7, 8])
self.assertEqual(mpu.get_quat(), [9, 10, 11, 12])
mpu.stop_monitoring()
class Robot(object):
VEL_P = 0.0
VEL_I = 0.0
VEL_D = 0.0
VEL_F = 1.0
YAW_P = 0.0
YAW_I = 0
YAW_D = 0
# Pwm and Qep ids for motor controllers a, b and c
MOTOR_A_PWM = "PWM0B-29"
MOTOR_A_QEP = "QEP0"
MOTOR_B_PWM = "PWM1A-36"
MOTOR_B_QEP = "QEP1"
MOTOR_C_PWM = "PWM2A-45"
MOTOR_C_QEP = "QEP2"
INIT_COMMAND = "OmniDrive"
VEL_PID_ENABLED = True
ROBOT_MAX_X_SPEED = math.sin(math.radians(60)) # approx 0.87 (root 2 over 3)
ROBOT_MAX_Y_SPEED = math.cos(math.radians(60)) # 0.50
# the speed of rotation at which we shut down the pid so it does not oscillate
# as you set the set point while still rotating
YAW_MOMENTUM_THRESHOLD = math.radians(10.0) # degrees per second
TIME_TO_AUTO_DISABLE = 10 # seconds till we automatically disable
AUTO_DISABLE_THRESHOLD = 0.05
def __init__(self):
# Velocity PID object setup
self.vel_pid_output_a = VelocityPidOutput()
self.vel_pid_output_b = VelocityPidOutput()
self.vel_pid_output_c = VelocityPidOutput()
self.vel_pid_a = Pid(self.vel_pid_output_a, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F)
self.vel_pid_b = Pid(self.vel_pid_output_b, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F)
self.vel_pid_c = Pid(self.vel_pid_output_c, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F)
# Qep encoder object setup
self.qep_a = Qep(Robot.MOTOR_A_QEP)
self.qep_b = Qep(Robot.MOTOR_B_QEP)
self.qep_c = Qep(Robot.MOTOR_C_QEP)
# Pwm object setup
self.pwm_a = Pwm(Robot.MOTOR_A_PWM)
self.pwm_b = Pwm(Robot.MOTOR_B_PWM)
self.pwm_c = Pwm(Robot.MOTOR_C_PWM)
self.pwms = [self.pwm_a, self.pwm_b, self.pwm_c]
# Create MotorController objects
self.motor_a = MotorController(self.pwm_a, self.vel_pid_a, self.vel_pid_output_a, self.qep_a, self.VEL_PID_ENABLED)
self.motor_b = MotorController(self.pwm_b, self.vel_pid_b, self.vel_pid_output_b, self.qep_b, self.VEL_PID_ENABLED)
self.motor_c = MotorController(self.pwm_c, self.vel_pid_c, self.vel_pid_output_c, self.qep_c, self.VEL_PID_ENABLED)
self.motors = [self.motor_a, self.motor_b, self.motor_c]
# set up pid
self.yaw_pid_output = YawPidOutput()
self.yaw_pid = Pid(self.yaw_pid_output, Robot.YAW_P, Robot.YAW_I, Robot.YAW_D, izone=1.0)
# initialise mpu/imu server module
self.mpu = Mpu()
self.yaw_pid_thread = YawPidThread(self.yaw_pid, self.mpu)
self.current_command = Robot.INIT_COMMAND
# pid *enabled* by default
self.yaw_pid_enabled = False
# pid not in *control* by default, this is automatically set by drive
self.pid_in_control = False
self.field_centered = True
self.enabled = False
self.interrupted = False
self.last_input_time = time.time()
def drive(self, vX, vY, vZ, throttle):
if not self.enabled:
self.last_input_time = time.time()
for motor in self.motors:
motor.set_speed(0.0)
for p in self.pwms:
p.set_speed(0.0)
p.pwm_off()
return
else:
for p in self.pwms:
p.pwm_on()
if math.fabs(vX) <= self.AUTO_DISABLE_THRESHOLD and math.fabs(vY) <= self.AUTO_DISABLE_THRESHOLD and math.fabs(vZ) <= self.AUTO_DISABLE_THRESHOLD:
if time.time() - self.last_input_time > self.TIME_TO_AUTO_DISABLE:
self.enabled = False
else:
self.last_input_time = time.time()
vPID = 0.0
if self.field_centered:
vX, vY = self.field_orient(vX, vY, self.mpu.get_euler()[0])
# Drive equations that translate vX, vY and vZ into commands to be sent to the motors
# front motor
mA = -(((0.0*vX) + (vY * 1.0))/2.0 + vZ/3.0)
# bottom left motor
mB = (((-vX * math.sin(math.radians(60))) + (-vY / 2.0))/2.0 + vZ/3.0)
# bottom right motor
mC = (((vX * math.sin(math.radians(60))) + (-vY / 2.0))/2.0 + vZ/3.0)
motor_input = [mA, mB, mC]
if self.yaw_pid_enabled:
if not vZ == 0:
# spinning under command -> no PID
self.pid_in_control = False
elif math.fabs(self.mpu.get_gyro()[2]) < Robot.YAW_MOMENTUM_THRESHOLD:
# momentum is less than the threshold and we are not under command
# -> PID can now take control
self.pid_in_control = True
if self.pid_in_control:
vPID = self.yaw_pid_output.value
else:
heading = self.mpu.get_euler()[0] # yaw
self.yaw_pid.set_set_point(heading)
# zero the correction value so that the next time the code runs the existing
# correction value will not still be there
self.yaw_pid_output.value = 0.0
max = 1.0
# find the maximum motor speed
for i in range(3):
if math.fabs(motor_input[i]) > max:
max = abs(motor_input[i])
# scale between -1 and 1
for i in range(3):
motor_input[i] /= max
# multiply by throttle
for i in range(3):
motor_input[i] *= throttle
if self.yaw_pid_enabled:
max = 1
for i in range(3):
motor_input[i] -= vPID
if math.fabs(motor_input[i]) > max:
max = math.fabs(motor_input[i])
for i in range(3):
motor_input[i] /= max
# set the speeds of the motors
for i in range(3):
self.motors[i].set_speed(motor_input[i])
def field_orient(self, vX, vY, yaw_angle):
oriented_vx = vX*math.cos(yaw_angle)+vY*math.sin(yaw_angle)
oriented_vy = -vX*math.sin(yaw_angle)+vY*math.cos(yaw_angle)
return oriented_vx, oriented_vy
def _init_mpu(self):
self.mpu = Mpu()
self.mpu.add_callback(self.on_move)
class Remote():
MOVE_COUNTER = 5
ROTARY_COUNTER = 4
asrStart = "hermes/asr/startListening"
asrStop = "hermes/asr/stopListening"
def _init_rotary(self):
self.rotary = RotaryEncoder()
self.rotary.add_rotate_callback(self.on_rotary)
self.rotary.add_push_callback(self.on_press)
self.rotary.add_release_callback(self.on_release)
def _init_apds(self):
self.apds = Apds()
self.apds.add_dir_callback(self.on_swipe)
self.apds.add_light_up_callback(self.on_lup, 1000)
self.apds.add_light_down_callback(self.on_ldown, 300)
self.apds.add_near_callback(self.on_near, 40)
self.apds.add_far_callback(self.on_far, 17)
def _init_mpu(self):
self.mpu = Mpu()
self.mpu.add_callback(self.on_move)
def __init__(self, siteId, concierge):
self.siteId = siteId
self.haveBeenMoved = -1
self.rotarySet = -1
self.hotwordSended = False
self.bPressed = False
self.sessionId = None
self.c = concierge
self.run = True
self._init_mpu()
self._init_apds()
self._init_rotary()
self.c.subscribeAsrStart(self.on_startListening)
self.c.subscribeAsrStop(self.on_stopListening)
def start(self):
self.run = True
self.apds.start()
self.mpu.start()
self.t = threading.Thread(target=self.worker, args=())
self.t.start()
def worker(self):
while (self.run):
if (self.rotarySet > 0):
self.rotarySet -= 1
self.haveBeenMoved = -1
if (self.bPressed or self.hotwordSended):
self.haveBeenMoved = -1
if (self.haveBeenMoved == 0):
self.sendHotword()
self.haveBeenMoved = -1
if (self.haveBeenMoved > 0):
self.haveBeenMoved -= 1
time.sleep(0.2)
def stop(self):
self.run = False
self.mpu.stop()
Apds.run = False
def on_startListening(self, client, userdata, message):
if not self.hotwordSended:
return
msg = json.loads(message.payload.decode("utf-8", "ignore"))
if (msg['siteId'] != self.siteId):
return
self.sessionId = msg['sessionId']
def on_stopListening(self, client, userdata, message):
if not self.hotwordSended:
return
msg = json.loads(message.payload.decode("utf-8", "ignore"))
if (msg['siteId'] != self.siteId):
return
self.hotwordSended = False
self.sessionId = None
def sendHotword(self):
self.hotwordSended = True
self.c.startHotword()
def sendStopHotword(self):
self.hotwordSended = False
self.c.stopHotword()
def on_rotary(self, degree):
if (self.bPressed):
return
self.rotarySet = Remote.ROTARY_COUNTER
self.c.publishRotary(degree)
def on_press(self):
print("button press")
self.bPressed = True
self.sendHotword()
def on_release(self):
if (not self.bPressed or self.sessionId is None):
return
print("button release")
self.sendStopHotword()
self.sessionId = None
self.bPressed = False
def on_swipe(self, direction):
print("direction: {}".format(direction))
self.c.publishSwipe(direction)
def on_move(self):
if (not self.bPressed and self.rotarySet <= 0):
self.haveBeenMoved = Remote.MOVE_COUNTER
print("haveBeenMoved")
def on_lup(self):
print("lup")
def on_ldown(self):
print("ldown")
def on_near(self):
print("near")
def on_far(self):
print("far")
def run():
global plotItem, y, mpu, kalman, dt, yaw_int, exp
while True:
# gyro_xout, gyro_yout, gyro_zout, accel_xout_scaled, accel_yout_scaled, accel_zout_scaled, x_rotation, y_rotation = mpu.read()
gyro_xout, gyro_yout, gyro_zout = mpu.read()
# kalman.filterOnce(np.array([[gyro_zout]]))
# yaw, yaw_speed = kalman.x
exp.filterOnce(gyro_zout)
y[:-1] = y[1:]
y[-1] = yaw_int
yaw_int += (exp.state-1.5) * dt
plotItem.plot(t, y, clear=True)
pg.QtGui.QApplication.processEvents()
time.sleep(dt)
if __name__ == '__main__':
dt = 0.01
yaw_int = 0
kalman, exp = init(dt)
win = pg.GraphicsWindow(title='mpu6050 test')
plotItem = win.addPlot(title='yaw')
plotItem.setYRange(-90, 90)
t = np.arange(100)
y = np.zeros(100)
mpu = Mpu()
try:
run()
except KeyboardInterrupt:
exit(-1)
