Skip to content
Snippets Groups Projects
Select Git revision
  • ec23f19448ea70a9e6e20b0abad93895b0abf80c
  • without_tipselection default
  • develop protected
  • fix/grafana-local-dashboard
  • wasp
  • fix/dashboard-explorer-freeze
  • master
  • feat/timerqueue
  • test/sync_debug_and_650
  • feat/sync_revamp_inv
  • wip/sync
  • tool/db-recovery
  • portcheck/fix
  • fix/synchronization
  • feat/new-dashboard-analysis
  • feat/refactored-analysis-dashboard
  • feat/new-analysis-dashboard
  • test/demo-prometheus-fpc
  • prometheus_metrics
  • wip/analysis-server
  • merge/fpc-test-value-transfer
  • v0.2.2
  • v0.2.1
  • v0.2.0
  • v0.1.3
  • v0.1.2
  • v0.1.1
  • v0.1.0
28 results

docker-compose.yml

Blame
  • motor.py 4.65 KiB
    """
    File: motor.py
    Description: Provides an interface with the motor of the robot
    """
    
    import time
    from enum import IntEnum
    
    import RPi.GPIO as GPIO
    from pins import RaspiPin
    
    
    class MotorError(Exception):
        '''
        Deals with error related with the motor
        '''
    
    
    class MotorDirection(IntEnum):
        """
        Possible direction the motor can take
        """
        UNDEFINED = -1
        FORWARD = 0
        BACKWARD = 1
    
    
    class Motor:
        """
        Interfaces the right pins with the two motors
        """
        MOTOR_DELAY = 0.2
        FULL_SPEED = 1.
        STOP = 0.
    
        def __init__(self, battery_voltage=9.0, motor_voltage=6.0):
            if float(motor_voltage) / float(battery_voltage) > 1:
                raise MotorError("Motor voltage is higher than battery votage.")
    
            self.pwm_scale = float(motor_voltage) / float(battery_voltage)
    
            GPIO.setup(RaspiPin.LEFT_PWM_PIN, GPIO.OUT)
            self.left_pwm = GPIO.PWM(RaspiPin.LEFT_PWM_PIN, 500)
            self.left_pwm.start(0)
            GPIO.setup(RaspiPin.LEFT_1_PIN, GPIO.OUT)
            GPIO.setup(RaspiPin.LEFT_2_PIN, GPIO.OUT)
    
            GPIO.setup(RaspiPin.RIGHT_PWM_PIN, GPIO.OUT)
            self.right_pwm = GPIO.PWM(RaspiPin.RIGHT_PWM_PIN, 500)
            self.right_pwm.start(0)
            GPIO.setup(RaspiPin.RIGHT_1_PIN, GPIO.OUT)
            GPIO.setup(RaspiPin.RIGHT_2_PIN, GPIO.OUT)
    
            self.old_left_dir = MotorDirection.UNDEFINED
            self.old_right_dir = MotorDirection.UNDEFINED
    
        def _set_motors(self, left_pwm: float, left_dir: MotorDirection,
                        right_pwm: float, right_dir: MotorDirection):
            """
            Sets the modulation and direction of the motors.
            If any MotorDirection changes the current one, stops the motor beforehand.
    
            :param float left_pwd Modulation of the left motor
            :param MotorDirection left_dir MotorDirection of the left motor
            :param float right_pwd Modulation of the left motor
            :param MotorDirection right_pwd MotorDirection of the left motor
            """
            if self.old_left_dir != left_dir or self.old_right_dir != right_dir:
                # stop motors between sudden changes of direction
                self._set_driver_pins(self.STOP, MotorDirection.FORWARD, self.STOP,
                                      MotorDirection.FORWARD)
                time.sleep(self.MOTOR_DELAY)
            self._set_driver_pins(left_pwm, left_dir, right_pwm, right_dir)
            self.old_left_dir = left_dir
            self.old_right_dir = right_dir
    
        def _set_driver_pins(self, left_pwm: float, left_dir: MotorDirection,
                             right_pwm: float, right_dir: MotorDirection):
            """
            Sets the modulation and direction of the motors.
            Checks the modulation and raises a ValueError if they are not in the range [0, 1]
    
            :param float left_pwd Modulation of the left motor
            :param MotorDirection left_dir MotorDirection of the left motor
            :param float right_pwd Modulation of the left motor
            :param MotorDirection right_pwd MotorDirection of the left motor
            """
            if left_pwm < 0. or left_pwm > 1. or right_pwm < 0. or right_pwm > 1.:
                raise ValueError("Invalid motor speed")
            self.left_pwm.ChangeDutyCycle(left_pwm * 100 * self.pwm_scale)
            GPIO.output(RaspiPin.LEFT_1_PIN, left_dir)
            GPIO.output(RaspiPin.LEFT_2_PIN, not left_dir)
    
            self.right_pwm.ChangeDutyCycle(right_pwm * 100 * self.pwm_scale)
            GPIO.output(RaspiPin.RIGHT_1_PIN, right_dir)
            GPIO.output(RaspiPin.RIGHT_2_PIN, not right_dir)
    
        def forward(self, speed=0.5):
            """
            Set both motors forward
    
            :param float speed: The speed of both motors
            """
            self._set_motors(speed, MotorDirection.FORWARD, speed,
                             MotorDirection.FORWARD)
    
        def backward(self, speed=0.5):
            """
            Set both motors backwad
    
            :param float speed: The speed of both motors
            """
            self._set_motors(speed, MotorDirection.BACKWARD, speed,
                             MotorDirection.BACKWARD)
    
        def stop(self):
            """
            Stops both motors
            """
            self._set_motors(self.STOP, MotorDirection.FORWARD, self.STOP,
                             MotorDirection.FORWARD)
    
        def right(self, speed=0.5):
            """
            Spins the robot clockwise
    
            :param float speed: The speed of both motors
            """
            self._set_motors(speed, MotorDirection.FORWARD, speed,
                             MotorDirection.BACKWARD)
    
        def left(self, speed=0.5):
            """
            Spins the robot counter-clockwise
    
            :param float speed: The speed of both motors
            """
            self._set_motors(speed, MotorDirection.BACKWARD, speed,
                             MotorDirection.FORWARD)