Saturday, March 4, 2017

Raspberry Pi Motor Board Python Class


Overview


We have made some additional changes to the Seeed Raspberry Pi Motor Board class. An obvious missing method is a way to change the speed of the motors. You can of course just change the duty attribute but this will only take effect the next time you change direction. So we have added a speed(duty) method. This will assign the new duty cycle and change the duty cycle of any motors which are already moving.

Note that in the Motor() class provided in the previous post:

def Stop():

should be:

def Stop(self):

Motor Control Class


Here is the updated Motor Control Class for the Seeed Motor Board. You many need to change the names of the direction methods as this will be determined by how you have wired your motors to the motor control board.

The MotorState enum class is used to record a history list of commands received. This may be useful when debugging the Robot in autonomous mode.

#!/usr/bin/python
# RS_MotorControl.py - Motor Control Class for the Seeed Raspberry Pi Motor Driver 
# Board v1.0 which uses the Freescale MC33932 dual H-Bridge Power IC.
#
# Based on Seeed Motor() Class 
# ref: http://wiki.seeed.cc/Raspberry_Pi_Motor_Driver_Board_v1.0/
#
# 1 March 2017 - 1.0 Original Issue
#
# Reefwing Software
# Simplified BSD Licence - see bottom of file.

import RPi.GPIO as GPIO
import os, signal

from time import sleep
from enum import Enum, unique
from PiSoftPwm import *

@unique
class MotorState(Enum):
    INIT         = 1
    STOPPED      = 2
    LEFT_FWD     = 3
    RIGHT_FWD    = 4
    BOTH_FWD     = 5
    LEFT_BACK    = 6
    RIGHT_BACK   = 7
    BOTH_BACK    = 8
    CHANGE_SPEED = 9

class MotorControl():
    def __init__(self, base_time=0.01, duty=50):
        # MC33932 pins connected to GPIO
        self.PWMA = 25  
        self.PWMB = 22
        self._IN1 = 23  
        self._IN2 = 24 
        self._IN3 = 17
        self._IN4 = 27

        self.base_time = base_time
        self.duty = duty
        self.history = [MotorState.INIT]

        # Initialize PWMA & PWMB 
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(self.PWMA, GPIO.OUT)
        GPIO.setup(self.PWMB, GPIO.OUT)
        GPIO.output(self.PWMA, True)
        GPIO.output(self.PWMB, True)

        # Initialize Software PWM outputs
        # Left Motor  = OUT_1 and OUT_2
        # Right Motor = OUT_3 and OUT_4
        self.OUT_1  = PiSoftPwm(self.base_time, 100, self._IN1, GPIO.BCM)
        self.OUT_2  = PiSoftPwm(self.base_time, 100, self._IN2, GPIO.BCM)
        self.OUT_3  = PiSoftPwm(self.base_time, 100, self._IN3, GPIO.BCM)
        self.OUT_4  = PiSoftPwm(self.base_time, 100, self._IN4, GPIO.BCM)

        # Start PWM for outputs - nbSlicesOn = 0, i.e. duty cycle = 0
        self.OUT_1.start(0)
        self.OUT_2.start(0)
        self.OUT_3.start(0)
        self.OUT_4.start(0)

    def __str__(self):
        # Return string representation of motor control
        return "Motor Control: base time - {0} seconds, duty - {1}%".format(self.base_time, self.duty)

    def left_back(self):
        self.OUT_1.changeBaseTime(self.base_time)
        self.OUT_2.changeBaseTime(self.base_time)
        self.OUT_1.changeNbSlicesOn(self.duty)
        self.OUT_2.changeNbSlicesOn(0)
        self.history.append(MotorState.LEFT_BACK)

    def left_forward(self):
        self.OUT_1.changeBaseTime(self.base_time)
        self.OUT_2.changeBaseTime(self.base_time)
        self.OUT_1.changeNbSlicesOn(0)
        self.OUT_2.changeNbSlicesOn(self.duty)
        self.history.append(MotorState.LEFT_FWD)

    def right_back(self):
        self.OUT_3.changeBaseTime(self.base_time)
        self.OUT_4.changeBaseTime(self.base_time)
        self.OUT_3.changeNbSlicesOn(0)
        self.OUT_4.changeNbSlicesOn(self.duty)
        self.history.append(MotorState.RIGHT_BACK)

    def right_forward(self):
        self.OUT_3.changeBaseTime(self.base_time)
        self.OUT_4.changeBaseTime(self.base_time)
        self.OUT_3.changeNbSlicesOn(self.duty)
        self.OUT_4.changeNbSlicesOn(0)
        self.history.append(MotorState.RIGHT_FWD)

    def speed(self, duty):
        # Change motor speed to duty (0-100) if not stopped (0)
        self.duty = duty
        self.OUT_1.nbSlicesOn = duty if self.OUT_1.nbSlicesOn else 0
        self.OUT_2.nbSlicesOn = duty if self.OUT_2.nbSlicesOn else 0
        self.OUT_3.nbSlicesOn = duty if self.OUT_3.nbSlicesOn else 0
        self.OUT_4.nbSlicesOn = duty if self.OUT_4.nbSlicesOn else 0
        self.history.append(MotorState.CHANGE_SPEED)

    def stop(self):
        self.OUT_1.changeNbSlicesOn(0)
        self.OUT_2.changeNbSlicesOn(0)
        self.OUT_3.changeNbSlicesOn(0)
        self.OUT_4.changeNbSlicesOn(0)
        self.history.append(MotorState.STOPPED)
        
    def cleanup(self):
        # Stop PWM on all outputs
        self.OUT_1.stop()
        self.OUT_2.stop()
        self.OUT_3.stop()
        self.OUT_4.stop()

def main():
    motor_control = MotorControl()    # create a new motor control instance
    print(motor_control)

    def endProcess(signum = None, frame = None):
        # Called on process termination. Stop motor control PWM
        if signum is not None:
            SIGNALS_NAMES_DICT = dict((getattr(signal, n), n) for n in dir(signal) if n.startswith('SIG') and '_' not in n )
            print("signal {} received by process with PID {}".format(SIGNALS_NAMES_DICT[signum], os.getpid()))
        print("\n-- Terminating program --")
        print("Cleaning up motor control PWM and GPIO...")
        motor_control.cleanup()
        GPIO.cleanup()
        print("Done.")
        exit(0)

    # Assign handler for process exit
    signal.signal(signal.SIGTERM, endProcess)
    signal.signal(signal.SIGINT, endProcess)
    signal.signal(signal.SIGHUP, endProcess)
    signal.signal(signal.SIGQUIT, endProcess)

    while True:
        print('Testing motors...')
        motor_control.left_forward()
        sleep(1)
        motor_control.left_back()
        sleep(1)
        motor_control.right_forward()
        sleep(1)
        motor_control.right_back()
        sleep(1)
        # speed = int(input("Enter Speed (0-100, CTRL c to quit): "))
        # motor_control.speed(speed)
        
if __name__ == "__main__":
    # execute only if run as a script
    main()

## Copyright (c) 2017, Reefwing Software
## All rights reserved.
##
## Redistribution and use in source and binary forms, with or without
## modification, are permitted provided that the following conditions are met:
##
## 1. Redistributions of source code must retain the above copyright notice, this
##   list of conditions and the following disclaimer.
## 2. Redistributions in binary form must reproduce the above copyright notice,
##   this list of conditions and the following disclaimer in the documentation
##   and/or other materials provided with the distribution.
##
## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
## ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
## WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
## DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
## ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
## (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
## LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
## ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
## (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
## SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.



No comments:

Post a Comment