""" The *motion_controller* module contains the `MotionController` class. CodeBot's lowest level motor control routines require you to work in terms of power settings and time durations. This makes it easy to get going, but hard to be precise. The `MotionController` class gives your code the ability to work in terms of distances and speeds. Both are measured in ticks, as reported by the wheel encoders. The `MotionController` class makes use of two other classes: * `MeteredMotors` - provides the ability to measure the motors movement. * `SpeedController` - handles some of the power level calculations for us. """ import botcore #import devbotcore as botcore from metered_motors import MeteredMotors from speed_controller import SpeedController from pyb import millis, elapsed_millis from botservices import BotServices, CancelableCallback class MotionController: """Builds upon MeteredMotors to provide a higher-level API. Even higher level APIs are possible (and planned). """ MOTOR_UPDATE_RATE = 10.0 # 10 Hz update rate seems to be working well... def __init__(self, left_speed_controller, right_speed_controller, svc = None): self.left_speed_controller = left_speed_controller self.right_speed_controller = right_speed_controller if svc is not None: self.svc = svc else: self.svc = BotServices() self.meteredMotors = MeteredMotors(botcore.motors, botcore.leds) self.requested_speed_left = 0 self.requested_ticks_left = 0 self.requested_speed_right = 0 self.requested_ticks_right = 0 self.requested_milliseconds = 0 self.move_complete_cb = None self.cancelable_motor_control_cb = None self.starting_milliseconds = 0 self.motor_power_left = 0 self.motor_power_right = 0 self.total_ticks_left = 0 self.total_ticks_right = 0 def stop_motion(self): """Stop any motion in-progress. For example, when your robot detects something interesting...""" self.meteredMotors.enable(False) self.meteredMotors.run(botcore.LEFT, 0) self.meteredMotors.run(botcore.RIGHT, 0) # Handle the case where stop_motion() was used to abort a move before it was completed... if self.cancelable_motor_control_cb is not None: self.cancelable_motor_control_cb.cancel() self.cancelable_motor_control_cb = None def request_motion(self, speed_left, ticks_left, speed_right, ticks_right, duration, complete_cb=None): """Request a move for a given distance, or an amount of time Args: speed_left (int): how fast to turn the left motor ticks_left (int): how far to turn the left motor speed_right (int): how fast to turn the right motor ticks_right (int): how far to turn the right motor duration (int): how long to move (specified in milliseconds) complete_cb (func): an optional function to call when the requested motion is completed. All speeds are specified in ticks/second. Positive speeds are forward, negative speeds are backwards. All distances are specified in ticks. A distance of 0 means there is no restriction on how far that motor is allowed to move. A duration of 0 means there is no time limit on the movement. The move will be ended when either condition is met, and the specified callback will be invoked. Call stop_motion() if you need to stop sooner. From this single function, all sorts of motions are possible: * If both requested speeds are the same, CodeBot will go straight. * If the speeds are different, CodeBot will move in an arc. * If one speed is 0 but the other is not, CodeBot will pivot on the stationary wheel. * If one speed is positive and the other speed is negative, CodeBot will spin in place. """ self.requested_speed_left = speed_left self.requested_ticks_left = ticks_left self.requested_speed_right = speed_right self.requested_ticks_right = ticks_right self.requested_milliseconds = duration self.move_complete_cb = complete_cb if self.requested_milliseconds != 0: self.starting_milliseconds = millis() # Scale requested speeds for our update rate self.requested_speed_left /= MotionController.MOTOR_UPDATE_RATE self.requested_speed_right /= MotionController.MOTOR_UPDATE_RATE # TODO Can we come up with a better pair of starting power levels? # Original #self.motor_power_left = 0 #self.motor_power_right = 0 # Experiment based on knowing that 5% will barely drive the motors if self.requested_speed_left < 0: self.motor_power_left = -5 elif self.requested_speed_left > 0: self.motor_power_left = 5 else: self.motor_power_left = 0 if self.requested_speed_right < 0: self.motor_power_right = -5 elif self.requested_speed_right > 0: self.motor_power_right = 5 else: self.motor_power_right = 0 self.meteredMotors.get_and_reset_ticks() # Clear out any previous ticks self.total_ticks_left = 0 self.total_ticks_right = 0 self.meteredMotors.run(botcore.LEFT, self.motor_power_left) self.meteredMotors.run(botcore.RIGHT, self.motor_power_right) self.meteredMotors.enable(True) self._schedule_motor_controller_cb() # Notice the extra step done in this routine to schedule a callback that # we might end up cancelling before it fires (see also stop_motion()) def _schedule_motor_controller_cb(self): self.cancelable_motor_control_cb = CancelableCallback(self._motor_controller_cb) self.svc.on_timeout(self.cancelable_motor_control_cb, int(1000 / MotionController.MOTOR_UPDATE_RATE)) # Periodically update the motor power settings based on the measured motor speeds # Check for "move complete" based on total measured ticks def _motor_controller_cb(self): ticks = self.meteredMotors.get_and_reset_ticks() delta_ticks_left = ticks[botcore.LEFT] delta_ticks_right = ticks[botcore.RIGHT] self.total_ticks_left += delta_ticks_left self.total_ticks_right += delta_ticks_right done = False # Will try to disprove this # See if any of our stopping conditions are met if self.requested_ticks_left != 0: if abs(self.total_ticks_left) >= abs(self.requested_ticks_left): done = True if self.requested_ticks_right != 0: if abs(self.total_ticks_right) >= abs(self.requested_ticks_right): done = True if self.requested_milliseconds != 0: if elapsed_millis(self.starting_milliseconds) >= self.requested_milliseconds: done = True if done: self.stop_motion() if self.move_complete_cb is not None: self.move_complete_cb() else: # We keep going, doing our best to maintain the requested speeds self.motor_power_left = self.left_speed_controller.compute_power(self.requested_speed_left, delta_ticks_left, self.motor_power_left, -100, 100) self.motor_power_right = self.right_speed_controller.compute_power(self.requested_speed_right, delta_ticks_right, self.motor_power_right, -100, 100) self.meteredMotors.run(botcore.LEFT, self.motor_power_left) self.meteredMotors.run(botcore.RIGHT, self.motor_power_right) self._schedule_motor_controller_cb()