What Do I Build Next? A RoboHat MM1 and EduCar based Project

from time import sleep, monotonic_ns
import board
import pulseio

class RoboHatEduCar(object):
    '''Class for controlling an educational car with stand alone RoboHat MM1 board.

    '''

    # configurations

    DRIVER_PWM_FREQ = 400
    '''pwm frequency of the motor driver'''

    DUTY_CYCLE_MOTOR_MIN = 15000
    '''the lowest duty cycle needed for the motor to keep turning the wheel '''

    DUTY_CYCLE_MOTOR_BRAKING = 7000
    '''the duty cycle needed against the direction of rotation for the motor to brake '''

    DUTY_CYCLE_MOTOR_START = 25000
    '''the duty_cycle to be used for the motor to turn the wheel from rest'''

    DEFAULT_PEDAL_VALUE = 30
    '''default speed in percent 0% - 100%'''

    CTRL_P_COEFF = 400
    ''' P coefficient '''

    ENC_DISTANCE_TO_360DEGREES = 77
    '''wheel distance in encoder ticks for a 360 degree turn'''

    ENC_DISTANCE_TO_1METER = 188
    ''' wheel distance in encoder ticks for the car to travel 1 meter'''

    ENC_DISTANCE_TO_TARGET_RAMPDOWN = 20
    '''when the enc_distance to target is lower than this setting we enter ST_RAMPDOWN state'''

    ENC_DISTANCE_TO_TARGET_BRAKING = 5  # 7
    '''when the enc_distance to target is lower than this setting we enter ST_BRAKING state'''

    CTR_EXIT_RAPDOWN = 200
    '''max amount of cycles we are allowed to stay in ST_RAMPDOWN state before giving up'''

    CTR_EXIT_BRAKING = 20
    '''max amount of cycles we are allowed to stay in ST_BRAKING state before giving up'''

    # class constants
    DIR_BACKWARD = -1
    DIR_FORWARD = 1

    CMD_TURN_RIGHT = -2
    '''Command to turn right'''

    CMD_DRV_BACKWARD = -1
    '''Command to drive backward'''

    CMD_STOP = 0
    '''Command to stop'''

    CMD_DRV_FORWARD = 1
    '''Command to drive forward'''

    CMD_TURN_LEFT = 2
    '''Command to turn left'''

    ST_STARTING = 11
    ST_CRUISING = 12
    ST_RAMPDOWN = 13
    ST_BRAKING = 14
    ST_TARGET_REACHED = 15

    WHEEL_LEFT = 0
    WHEEL_RIGHT = 1

    def __init__(self):
        # Left Ain1(backward) Ain2(Forward) Right Bin1(backward) Bin2(Forward)
        # Ain2
        self.driver_left_in2 = pulseio.PWMOut(board.SERVO3, frequency=RoboHatEduCar.DRIVER_PWM_FREQ)
        # Ain1
        self.driver_left_in1 = pulseio.PWMOut(board.SERVO4, frequency=RoboHatEduCar.DRIVER_PWM_FREQ)
        # Bin1
        self.driver_right_in1 = pulseio.PWMOut(board.SERVO5, frequency=RoboHatEduCar.DRIVER_PWM_FREQ)
        # Bin2
        self.driver_right_in2 = pulseio.PWMOut(board.SERVO6, frequency=RoboHatEduCar.DRIVER_PWM_FREQ)

        self.wheel_enc_left = pulseio.PulseIn(board.SERVO1, maxlen=30)
        self.wheel_enc_right = pulseio.PulseIn(board.SERVO2, maxlen=30)
        self.wheel_pos_left = 0
        self.wheel_pos_right = 0
        self.direction_left_wheel = RoboHatEduCar.DIR_FORWARD
        self.direction_right_wheel = RoboHatEduCar.DIR_FORWARD
        self.ctr_rampdown = [0, 0]
        self.ctr_braking = [0, 0]
        self.stop()

        """Drive the car forward or backward by the specified distance.

        Argument:
        command -- RoboHatEduCar.CMD_DRV_FORWARD or RoboHatEduCar.CMD_DRV_BACKWARD
        distance_in_m -- a number (integer or float) in meters
        """
    def drive(self, command, distance_in_m):
        enc_distance = int(distance_in_m * RoboHatEduCar.ENC_DISTANCE_TO_1METER)
        if enc_distance >= 5:
            self._drive(command, enc_distance)
        else:
            print("Distance too small '{}m'".format(distance_in_m))

        """Turn the car left or right by the specified angle in degrees.

        Argument:
        command -- RoboHatEduCar.CMD_TURN_LEFT or RoboHatEduCar.CMD_TURN_RIGHT
        angle -- a number (integer or float) in degrees
        """
    def turn(self, command, angle):
        enc_distance = int(angle * RoboHatEduCar.ENC_DISTANCE_TO_360DEGREES / 360)
        self._drive(command, enc_distance)

        """Stop the car.
        """
    def stop(self):
        self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_STOP)
        self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_STOP)

    def _get_wheel_positions(self):
        self.wheel_pos_left = self.wheel_pos_left + len(self.wheel_enc_left) * self.direction_left_wheel
        self.wheel_enc_left.clear()

        self.wheel_pos_right = self.wheel_pos_right + len(self.wheel_enc_right) * self.direction_right_wheel
        self.wheel_enc_right.clear()

        return (self.wheel_pos_left, self.wheel_pos_right)

    def _pedal_to_duty_cycle(self, pedal_value):
        if pedal_value <= 0 or pedal_value > 100:
            duty_cycle = 0
        else:
            duty_cycle = int(pedal_value * (65535 - RoboHatEduCar.DUTY_CYCLE_MOTOR_MIN) / 100) + RoboHatEduCar.DUTY_CYCLE_MOTOR_MIN
        return duty_cycle

    def _drive(self, command, enc_distance):
        if (command == RoboHatEduCar.CMD_DRV_FORWARD
            or command == RoboHatEduCar.CMD_DRV_BACKWARD
            or command == RoboHatEduCar.CMD_TURN_LEFT
            or command == RoboHatEduCar.CMD_TURN_RIGHT):
            pass
        else:
            raise ValueError("RoboHatEduCar error, command not defined `{}'.".format(command.__name__))

        distance_reached = False
        self.ctr_rampdown = [0, 0]
        self.ctr_braking = [0, 0]
        start_pos_left, start_pos_right = self._get_wheel_positions()
        duty_cycle_default = self._pedal_to_duty_cycle(RoboHatEduCar.DEFAULT_PEDAL_VALUE)

        print("{} command received with distance: {} start left pos: {}, start right pos: {}".format(
            command, enc_distance, start_pos_left, start_pos_right))

        while distance_reached is False:
            start_time = monotonic_ns()
            pos_left, pos_right = self._get_wheel_positions()
            abs_distance_left = abs(pos_left - start_pos_left)
            abs_distance_right = abs(pos_right - start_pos_right)
            error = abs_distance_right - abs_distance_left

            ctrl_state_left = self._next_state_logic(RoboHatEduCar.WHEEL_LEFT, abs_distance_left, enc_distance)
            ctrl_state_right = self._next_state_logic(RoboHatEduCar.WHEEL_RIGHT, abs_distance_right, enc_distance)

            error_left = error * RoboHatEduCar.CTRL_P_COEFF
            error_right = - error * RoboHatEduCar.CTRL_P_COEFF

            duty_cycle_left = self._output_logic(RoboHatEduCar.WHEEL_LEFT, ctrl_state_left, command, duty_cycle_default, error_left)
            duty_cycle_right = self._output_logic(RoboHatEduCar.WHEEL_RIGHT, ctrl_state_right, command, duty_cycle_default, error_right)

            if  (ctrl_state_left == RoboHatEduCar.ST_TARGET_REACHED and
                 ctrl_state_right == RoboHatEduCar.ST_TARGET_REACHED):
                distance_reached = True

            duration = monotonic_ns() - start_time
            # print("duration: {}".format(duration))
            # print("left pos: {} st: {} dc: {}, right pos: {} st:{} dc:{}".format(
            #    pos_left, ctrl_state_left, duty_cycle_left, pos_right, ctrl_state_right, duty_cycle_right))
            duration = monotonic_ns() - start_time
            if duration <= 0.009:
                sleep(0.01 - duration/1e9)

        # print("left pos: {}, right pos: {}".format(pos_left, pos_right))

    def _next_state_logic(self, wheel, curr_distance, target_distance):
        distance_to_target = target_distance - curr_distance
        if(distance_to_target <= 0):
            state = RoboHatEduCar.ST_TARGET_REACHED
        elif(curr_distance <= 0):
            state = RoboHatEduCar.ST_STARTING
        elif(distance_to_target > RoboHatEduCar.ENC_DISTANCE_TO_TARGET_RAMPDOWN):
            state = RoboHatEduCar.ST_CRUISING
        elif(distance_to_target > RoboHatEduCar.ENC_DISTANCE_TO_TARGET_BRAKING):
            if self.ctr_rampdown[wheel] < RoboHatEduCar.CTR_EXIT_RAPDOWN:
                state = RoboHatEduCar.ST_RAMPDOWN
                self.ctr_rampdown[wheel] += 1
            else:
                state = RoboHatEduCar.ST_TARGET_REACHED
        else:
            if self.ctr_braking[wheel] < RoboHatEduCar.CTR_EXIT_BRAKING:
                state = RoboHatEduCar.ST_BRAKING
                self.ctr_braking[wheel] += 1
            else:
                state = RoboHatEduCar.ST_TARGET_REACHED
        return state

    def _output_logic(self, wheel, ctrl_state, command, duty_cycle_cruising, error):
        braking = False
        if(ctrl_state == RoboHatEduCar.ST_STARTING):
            duty_cycle = RoboHatEduCar.DUTY_CYCLE_MOTOR_START
        elif(ctrl_state == RoboHatEduCar.ST_RAMPDOWN):
            duty_cycle = RoboHatEduCar.DUTY_CYCLE_MOTOR_MIN + error
        elif(ctrl_state == RoboHatEduCar.ST_BRAKING):
            command = -command  # reverse
            braking = True
            duty_cycle = RoboHatEduCar.DUTY_CYCLE_MOTOR_BRAKING
        elif(ctrl_state == RoboHatEduCar.ST_TARGET_REACHED):
            duty_cycle = 0
            command = RoboHatEduCar.CMD_STOP
        elif(ctrl_state == RoboHatEduCar.ST_CRUISING):
            duty_cycle = duty_cycle_cruising + error
        else:
            raise ValueError("RoboHatEduCar error, state not defined `{}'.".format(ctrl_state))
        self._drv_wheel(wheel, command, duty_cycle=duty_cycle, braking=braking)
        return duty_cycle

    def _drv_wheel(self, wheel, command, duty_cycle=None, pedal_value=DEFAULT_PEDAL_VALUE, braking=False):
        if duty_cycle is None:
            duty_cycle = self._pedal_to_duty_cycle(pedal_value)

        if(wheel == RoboHatEduCar.WHEEL_LEFT):
            driver_in1 = self.driver_left_in1
            driver_in2 = self.driver_left_in2
            direction_attr_name = 'direction_left_wheel'
        elif(wheel == RoboHatEduCar.WHEEL_RIGHT):
            driver_in1 = self.driver_right_in1
            driver_in2 = self.driver_right_in2
            direction_attr_name = 'direction_right_wheel'
        else:
            raise ValueError("RoboHatEduCar error, wheel not defined `{}'.".format(wheel))

        if  (command == RoboHatEduCar.CMD_DRV_FORWARD
             or (wheel == RoboHatEduCar.WHEEL_LEFT and command == RoboHatEduCar.CMD_TURN_RIGHT)
             or (wheel == RoboHatEduCar.WHEEL_RIGHT and command == RoboHatEduCar.CMD_TURN_LEFT)):
            driver_in1.duty_cycle = 0
            driver_in2.duty_cycle = duty_cycle
            if not braking:
                setattr(self, direction_attr_name, RoboHatEduCar.DIR_FORWARD)
        elif (command == RoboHatEduCar.CMD_DRV_BACKWARD
              or (wheel == RoboHatEduCar.WHEEL_LEFT and command == RoboHatEduCar.CMD_TURN_LEFT)
              or (wheel == RoboHatEduCar.WHEEL_RIGHT and command == RoboHatEduCar.CMD_TURN_RIGHT)):
            driver_in1.duty_cycle = duty_cycle
            driver_in2.duty_cycle = 0
            if not braking:
                setattr(self, direction_attr_name, RoboHatEduCar.DIR_BACKWARD)
        elif command == RoboHatEduCar.CMD_STOP:
            driver_in1.duty_cycle = 0
            driver_in2.duty_cycle = 0
        else:
            raise ValueError("RoboHatEduCar error, command not defined `{}'.".format(command))

    """Drive the car according to a test pattern.
    """
    def drv_testPattern(self):
        print("Test pattern")
        self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_DRV_FORWARD)
        self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_STOP)
        sleep(1)
        self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_DRV_FORWARD)
        self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_STOP)
        sleep(1)
        self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_DRV_BACKWARD)
        self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_STOP)
        sleep(1)
        self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_DRV_BACKWARD)
        self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_STOP)
        sleep(1)
        self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_DRV_BACKWARD)
        self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_DRV_FORWARD)
        sleep(1)
        self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_DRV_FORWARD)
        self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_DRV_BACKWARD)
        sleep(1)
        self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_DRV_FORWARD)
        self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_DRV_FORWARD)
        sleep(1)
        self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_DRV_BACKWARD)
        self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_DRV_BACKWARD)
        sleep(1)
        self._drv_wheel(RoboHatEduCar.WHEEL_LEFT, RoboHatEduCar.CMD_STOP)
        self._drv_wheel(RoboHatEduCar.WHEEL_RIGHT, RoboHatEduCar.CMD_STOP)


class EduCarTurtle(object):
    def __init__(self, car):
        self.car = car

    # def position():

    def forward(self, distance):
        """Move the turtle forward by the specified distance.

        Aliases: forward | fd

        Argument:
        distance -- a number (integer or float)

        Move the turtle forward by the specified distance, in the direction
        the turtle is headed.
        """
        self.car.drive(RoboHatEduCar.CMD_DRV_FORWARD, distance)

    def back(self, distance):
        """Move the turtle backward by distance.
        Aliases: back | backward | bk
        Argument:
        distance -- a number
        Move the turtle backward by distance ,opposite to the direction the
        turtle is headed. Do not change the turtle's heading.
        """
        self.car.drive(RoboHatEduCar.CMD_DRV_BACKWARD, distance)

    def left(self, angle):
        """Turn turtle left by angle units.

        Aliases: left | lt

        Argument:
        angle -- a number (integer or float)

        Turn turtle left by angle degrees.
        """
        self.car.turn(RoboHatEduCar.CMD_TURN_LEFT, angle=angle)

    def right(self, angle):
        """Turn turtle right by angle units.

        Aliases: right | rt

        Argument:
        angle -- a number (integer or float)

        Turn turtle right by angle degrees.
        """
        self.car.turn(RoboHatEduCar.CMD_TURN_RIGHT, angle=angle)

    # method aliases
    fd = forward
    bk = back
    backward = back
    rt = right
    lt = left