class DriveController:
"""
Tracks the robot's position via dead reckoning and handles the motor's
kinematics.
The robot uses skid steering which has simple forward and reverse
kinematics. DriveController is used to bridge the abstraction between a
point model of the robot and the robot's actual layout."""
def __init__(self):
self._left_motor_controller = MotorController(
0, left_pins, left_motor_pid_constants, 0)
self._right_motor_controller = MotorController(
1, right_pins, right_motor_pid_constants, 1)
self._speed_filter = IrregularDoubleExponentialFilter(
*robot_speed_smoothing_factors)
self.pos_heading = (ZERO_POS, 0.0)
self.robot_angle = 0.0
self.robot_speed = 0.0
def read_encoders(self, time_elapsed):
"""Reads the encoders and updates the robot's position, heading,
and wheel velocities"""
# get movement of left and right wheels
left_dist_traveled = self._left_motor_controller.read_encoder(
time_elapsed)
right_dist_traveled = self._right_motor_controller.read_encoder(
time_elapsed)
dist, angle = forward_kin(left_dist_traveled, right_dist_traveled)
# dead reckon pos and angle
self.robot_angle = correct_angle(self.robot_angle + angle)
# self.dead_reckon(dist, angle)
self.robot_speed = self._speed_filter.filter(dist / time_elapsed,
time_elapsed)
def dead_reckon(self, dist, angle):
"""This method was tested in a previous version of the robot's software
and it tracked the position of the robot very well when the tires didn't
slip. Regrettably there wasn't enough time to integrate position system
with the navigation system, so keeping track of the robot's position
wouldn't yield any advantages.
This method uses the point and shoot method. The point and shoot method
estimates movement as a turn followed by a movement forward. Obviously
the robot doesn't move like this but it is a good estimation when the
total turn is small. If the turn is too large (>small_angle), then the
movement is split into two consecutive point and shoots. This is done
recursively while the angle is too large."""
if abs(angle) <= small_angle:
pos = self.pos_heading[0].pos_from_dist_angle(dist, angle)
theta = correct_angle(self.pos_heading[1] + angle)
self.pos_heading = (pos, theta)
return
self.dead_reckon(dist / 2, angle / 2)
self.dead_reckon(dist / 2, angle / 2)
def update_motors(self, forward_vel, angular_vel, time_elapsed):
# Transform forward and angular to left and right and send to the
# motor controllers
left_target, right_target = reverse_kin(forward_vel, angular_vel)
self._left_motor_controller.adjust_motor_speed(left_target,
time_elapsed)
self._right_motor_controller.adjust_motor_speed(
right_target, time_elapsed)
def check_current(self):
# the current sensor don't work so this is never called in this version
self._left_motor_controller.current_sensor.check_current()
self._right_motor_controller.current_sensor.check_current()
def reset(self):
self._left_motor_controller.reset()
self._right_motor_controller.reset()
self.pos_heading = (ZERO_POS, 0.0)
self.robot_angle = 0.0
self.robot_speed = 0.0
def stop(self):
self._left_motor_controller.stop()
self._right_motor_controller.stop()
