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)