1  #
  2  # NOTE - Only for use on Raspberry Pi or other SBC.
  3  #
  4  
  5  # Simple two DC motor robot class.  Exposes a simple LOGO turtle-like API for
  6  # moving a robot forward, backward, and turning.  See RobotTest.py for an
  7  # example of using this class.
  8  # Author2: Tony DiCola, Chris Anderson
  9  # License: MIT License https://opensource.org/licenses/MIT
 10  
 11  
 12  # This assumes the Left motor is on Motor 1 and the Right motor is on Motor 2
 13  
 14  
 15  import time
 16  import atexit
 17  import board
 18  from adafruit_motorkit import MotorKit
 19  
 20  kit = MotorKit(i2c=board.I2C())
 21  
 22  
 23  class Robot:
 24      def __init__(self, left_trim=0, right_trim=0, stop_at_exit=True):
 25          """Create an instance of the robot.  Can specify the following optional
 26          parameter
 27           - left_trim: Amount to offset the speed of the left motor, can be positive
 28                        or negative and use useful for matching the speed of both
 29                        motors.  Default is 0.
 30           - right_trim: Amount to offset the speed of the right motor (see above).
 31           - stop_at_exit: Boolean to indicate if the motors should stop on program
 32                           exit.  Default is True (highly recommended to keep this
 33                           value to prevent damage to the bot on program crash!).
 34          """
 35  
 36          self._left_trim = left_trim
 37          self._right_trim = right_trim
 38          if stop_at_exit:
 39              atexit.register(self.stop)
 40  
 41      def _left_speed(self, speed):
 42          """Set the speed of the left motor, taking into account its trim offset.
 43          """
 44          assert -1 <= speed <= 1, "Speed must be a value between -1 to 1 inclusive!"
 45          speed += self._left_trim
 46          speed = max(-1, min(1, speed))  # Constrain speed to 0-255 after trimming.
 47          kit.motor1.throttle = speed
 48  
 49      def _right_speed(self, speed):
 50          """Set the speed of the right motor, taking into account its trim offset.
 51          """
 52          assert -1 <= speed <= 1, "Speed must be a value between -1 to 1 inclusive!"
 53          speed += self._right_trim
 54          speed = max(-1, min(1, speed))  # Constrain speed to 0-255 after trimming.
 55          kit.motor2.throttle = speed
 56  
 57      @staticmethod
 58      def stop():
 59          """Stop all movement."""
 60          kit.motor1.throttle = 0
 61          kit.motor2.throttle = 0
 62  
 63      def forward(self, speed, seconds=None):
 64          """Move forward at the specified speed (0-255).  Will start moving
 65          forward and return unless a seconds value is specified, in which
 66          case the robot will move forward for that amount of time and then stop.
 67          """
 68          # Set motor speed and move both forward.
 69          self._left_speed(speed)
 70          self._right_speed(speed)
 71          # If an amount of time is specified, move for that time and then stop.
 72          if seconds is not None:
 73              time.sleep(seconds)
 74              self.stop()
 75  
 76      def steer(self, speed, direction):
 77          # Move forward at the specified speed (0- 1).  Direction is +- 1.
 78          # Full left is -1, Full right is +1
 79          if (speed + direction / 2) > 1:
 80              speed = (
 81                  speed - direction / 2
 82              )  # calibrate so total motor output never goes above 1
 83          left = speed + direction / 2
 84          right = speed - direction / 2
 85          self._left_speed(left)
 86          self._right_speed(right)
 87  
 88      def backward(self, speed, seconds=None):
 89          """Move backward at the specified speed (0-255).  Will start moving
 90          backward and return unless a seconds value is specified, in which
 91          case the robot will move backward for that amount of time and then stop.
 92          """
 93          # Set motor speed and move both backward.
 94          self._left_speed(-1 * speed)
 95          self._right_speed(-1 * speed)
 96          # If an amount of time is specified, move for that time and then stop.
 97          if seconds is not None:
 98              time.sleep(seconds)
 99              self.stop()
100  
101      def right(self, speed, seconds=None):
102          """Spin to the right at the specified speed.  Will start spinning and
103          return unless a seconds value is specified, in which case the robot will
104          spin for that amount of time and then stop.
105          """
106          # Set motor speed and move both forward.
107          self._left_speed(speed)
108          self._right_speed(0)
109          # If an amount of time is specified, move for that time and then stop.
110          if seconds is not None:
111              time.sleep(seconds)
112              self.stop()
113  
114      def left(self, speed, seconds=None):
115          """Spin to the left at the specified speed.  Will start spinning and
116          return unless a seconds value is specified, in which case the robot will
117          spin for that amount of time and then stop.
118          """
119          # Set motor speed and move both forward.
120          self._left_speed(0)
121          self._right_speed(speed)
122          # If an amount of time is specified, move for that time and then stop.
123          if seconds is not None:
124              time.sleep(seconds)
125              self.stop()