"""
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
[docs]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
[docs] 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
[docs] 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()