MotorController Python class and ROS node for the BeagleBone

Today I’m going to present the MotorController class for Python that handles – guess what – a motor controller using a BeagleBone along ROS wrapper node.

Usually motor controller hardwares are operating with 2 wires per channel: a PWM signal for speed and a  “boolean” signal for direction. It’s quite easy to handle such things using an Arduino but we’re trying to get rid of extra hardware since the BeagleBone is also capable of solving all this stuff.

There is the basic approach of hacking around in the terminal using the filesystem in Angstrom or in Debian. I’m not going to discuss that.

I’m still testing on our rover called Firebug. The BeagleBone is under that ugly perfboard which serves as my BeagleBone shield for now.

The pins used by the shield:

In this post I’m going to be using the pins marked with red.

The code is pretty simple. Since I have a dual channel motor controller I’m using 2 PWMs and  2 gpio pins to implement a tank-link 4wheel drive system – no steering. I also prepared a ROS node to make the necessary calls to the motor controller and yet again I’m using the PyBBIO library for Arduino-style calls wherever I can.

Let the code speak for itself:

from bbio import *
import pwm

PWM_PATH = "/sys/class/pwm/"
pwm_pins = { 
    "P8_13": { "name": "EHRPWM2B", "mux": "gpmc_ad9", "eeprom": 15, "pwm" : "ehrpwm.2:1"  },
    "P8_19": { "name": "EHRPWM2A", "mux": "gpmc_ad8", "eeprom": 14, "pwm" : "ehrpwm.2:0"  },
    "P9_14": { "name": "EHRPWM1A", "mux": "gpmc_a2", "eeprom": 34, "pwm" : "ehrpwm.1:0" },
    "P9_16": { "name": "EHRPWM1B", "mux": "gpmc_a3", "eeprom": 35, "pwm" : "ehrpwm.1:1" },
    "P9_31": { "name": "SPI1_SCLK", "mux": "mcasp0_aclkx", "eeprom": 65 , "pwm": "ehrpwm.0:0"},
    "P9_29": { "name": "SPI1_D0", "mux": "mcasp0_fsx", "eeprom": 61 , "pwm": "ehrpwm.0:1"},
    "P9_42": { "name": "GPIO0_7", "mux": "ecap0_in_pwm0_out", "eeprom": 4, "pwm": "ecap.0"},
    "P9_28": { "name": "SPI1_CS0", "mux": "mcasp0_ahclkr", "eeprom": 63, "pwm": "ecap.2" },
}

class MotorController:

    __actualspeed = 10
    MOTOR1 = GPIO1_16
    MOTOR2 = GPIO1_17
    PWM1 = 'P9_14'
    PWM2 = 'P9_16'
    MAX_SPEED = 98
    MIN_SPEED = 10
    PWM_FREQUENCY = 50 #hz
    __pwm1 = ''
    __pwm2 = ''

    def __init__(self):
        pwm.enable()
        self.__pwm1 = PWM_PATH+pwm_pins[self.PWM1]["pwm"]
        self.__pwm2 = PWM_PATH+pwm_pins[self.PWM2]["pwm"]
        # initialize pwm1
        open(self.__pwm1 + "/request", 'w').write("1")
        open(self.__pwm1 + "/run", 'w').write("0")
        open(self.__pwm1 + "/period_freq", 'w').write(str(self.PWM_FREQUENCY))
        open(self.__pwm1 + "/duty_percent", 'w').write(str(0)) #init to 0 degree
        open(self.__pwm1 + "/run", 'w').write("1")
        # initialize pwm2
        open(self.__pwm2 + "/request", 'w').write("1")
        open(self.__pwm2 + "/run", 'w').write("0")
        open(self.__pwm2 + "/period_freq", 'w').write(str(self.PWM_FREQUENCY))
        open(self.__pwm2 + "/duty_percent", 'w').write(str(0)) #init to 0 degree
        open(self.__pwm2 + "/run", 'w').write("1")
        pinMode(self.MOTOR1, OUTPUT)
        pinMode(self.MOTOR2, OUTPUT)

    def __del__(self):
        open(self.__pwm1 + "/run", 'w').write("0")
        open(self.__pwm1 + "/request", 'w').write("0")
        open(self.__pwm2 + "/run", 'w').write("0")
        open(self.__pwm2 + "/request", 'w').write("0")

    def forward(self):
        digitalWrite(self.MOTOR1, HIGH)
        digitalWrite(self.MOTOR2, LOW)
        if self.__actualspeed>self.MAX_SPEED: 
            return None
        self.__update_pwm__()

    def backward(self):
        digitalWrite(self.MOTOR1, LOW)
        digitalWrite(self.MOTOR2, HIGH)
        if self.__actualspeed>self.MAX_SPEED: 
            return None
        self.__update_pwm__()

    def increaseSpeed(self):
        if self.__actualspeed>self.MAX_SPEED: 
            return None
        self.__actualspeed+=1
        self.__update_pwm__()

    def decreaseSpeed(self):
        if self.__actualspeed<self.MIN_SPEED: 
            return None
        self.__actualspeed-=1
        self.__update_pwm__()

    def getSpeed(self):
        return self.__actualspeed

    def setSpeed(self, newspeed):
        self.__actualspeed=newspeed
        self.__update_pwm__()

    def brake(self):
        self.__actualspeed = 0
        self.__update_pwm__()

    def stop(self):
        self.brake()

    def turnLeft(self):
        digitalWrite(self.MOTOR1, LOW)
        digitalWrite(self.MOTOR2, LOW)
        self.__update_pwm__()

    def turnRight(self):
        digitalWrite(self.MOTOR1, HIGH)
        digitalWrite(self.MOTOR2, HIGH)
        self.__update_pwm__()

    def reverse(self):
        toggle(self.MOTOR1)
        toggle(self.MOTOR2)

    def __update_pwm__(self):
        open(self.__pwm1 +'/duty_percent','w').write(str(self.__actualspeed))
        open(self.__pwm2 +'/duty_percent','w').write(str(self.__actualspeed))

The repository containing the file + ROS node(s):

http://buggers.svn.sourceforge.net/viewvc/buggers/beaglebone/ros_pkgs/beaglebone/

References:

https://github.com/alexanderhiam/PyBBIO