Motor Driver

The motor driver node is intended to drive the main two actuators of the AV1tenth platform. This is achieved by serially communicating with the Pololu Micro Maestro Servo controller.

Serial Commands

The Pololu has a set of sepcific serial data that needs to be sent over. These commands have been packaged in to a serial class called “serial_cmds”. That class is given below:

class serial_cmds:
    def __init__(self):
        # Change according to what serial port you connected the Maestro to
        self.serial_port = '/dev/ttyACM1'
        self.serial_usb = serial.Serial(
            self.serial_port)  # Opening the serial port
        # Setting the command variable to Pololu Protocol. First command is just an arbitrary starting byte.
        self.pol_prot_cmd = chr(0xaa) + chr(0x0c)
        # Second number is device number, default is 12

    def serial_close(self):
        self.serial_usb.close()  # Closing the serial port after completion

    def cmd_out(self, cmd):
        cmd_out_byte = self.pol_prot_cmd + cmd
        self.serial_usb.write(bytes(cmd_out_byte, 'latin-1'))

    def pos_read(self, chan):

        cmd = chr(0x10) + chr(chan)
        self.cmd_out(cmd)
        lsb = ord(self.serial_usb.read())
        msb = ord(self.serial_usb.read())

        return (msb << 8)+lsb

    def set_target(self, chan, target):
        # When setting the target/acceleration/speed the first seven bits will be considered the low bits and the last 7 bits will be considered the high bits.
        #  This is shown by the target of 1500 us X 4 = 6000 which in binary is 0101110 1110000
        # Converting from microseconds to quarter microseconds for data transmission
        # target = 1500*4 # Uncomment for debugging
        lsb = int(target) & 0x7f
        msb = (int(target) >> 7) & 0x7f
        cmd = chr(0x04) + chr(chan) + chr(lsb) + chr(msb)
        self.cmd_out(cmd)

    def set_speed(self, chan, speed):
        lsb = speed & 0x7f
        msb = (speed >> 7) & 0x7f
        cmd = chr(0x07) + chr(chan) + chr(lsb) + chr(msb)
        self.cmd_out(cmd)

    def set_acc(self, chan, acc):
        lsb = acc & 0x7f
        msb = (acc >> 7) & 0x7f
        # This is the cmd byte data for the acceleration state
        cmd = chr(0x09) + chr(chan) + chr(lsb) + chr(msb)
        self.cmd_out(cmd)

The serial port is opened with a 9600 (standard) baud rate and an arbitrary byte and the device number which is 12. You can daisy chain multiple of these servo controllers together, hence why the device number exists. That is the first part of the data that is to be sent. Next there are different functions that sends the command, closes the serial port (self explanatory), reads position, sets a target, sets a speed, and sets the acceleration. The cmd_out function takes all the necessary conditions from the other functions and sends it serially to the servo controller. The rest of the functions act similarly.

To send data in a compact form the data is bit shifted 7 spaces to generate a high and low bit, lsb being lower bits and msb being high bits. Each function has a specific cmd variable that needs to be sent to the cmd_out function. That variable defines whether you are setting the target, speed or acceleration with the first bit 0x04, 0x07, etc. and then adds in the lower and higher bits in order. This is how the program communicates with the controller.

The target sets the position (PWM period of the signal) which is generated in quarter-microseconds. Which is 4*microseconds. The speed is the speed to achieve that target, and acceleration is the quickness to achieve that speed. This will be true for the servo, for the drive motor the target will be speed, speed will be acceleration and the acceleration will be jerk, at least in a physical sense.

Note

This code will be standard, we recommend not changing it unless you are confident you understand what it does.

ROS2, Callbacks and Logic

The next part of the code is the ROS2 critical code inside a __init__ function. This is where you put in the subscriptions to a twist and a subscription for the warning LED’s. All the ROS2 code is packaged in a MotorController class. That code is shown below.

def __init__(self):
    super().__init__('motor_controller')
    self.serial_ = serial_cmds()
    ##
    self.serial_.set_target(4, 3000)
    self.subscription = self.create_subscription(
        Twist, 'vehicle_command', self.cmd_send, 20)
    self.subscription  # prevent unused variable warning
    self.sub2 = self.create_subscription(
        Int16, 'led_color', self.led_cllbk, 20)
    self.sub2
    self.cmd_send
    self.lastmsg = 0

Here the serial class is called, ROS2 is initialized, and the LED’s are set to off. Then the two subscriptions are called, one for vehicle_command topic and another for led_color topic. Both are self explanatory and each has a Twist and an Int16 message being subscribed to it. Next a callback for the LED subscription is defined, that is given below.

def led_cllbk(self, Int16):
msg = Int16.data
if msg is not self.lastmsg:
    if msg == 1:
        self.serial_.set_target(3, 6500)
        self.serial_.set_target(4, 3000)
    elif msg == 2:
        self.serial_.set_target(5, 6500)
        self.serial_.set_target(4, 3000)
self.lastmsg = msg

Here the channels 3 corresponds to yellow and 5 corresponds to red. Green is the safe indicator and that correspondsto channel 4. As in normal subscriptions, the message data is pulled into a variable and then a conditional is used to check whether a red or yellow LED is desired. If the number 0 is sent, all LED’s are turned off.

Next the main callback that handles the vehicle actuators is defined. This callback deals with taking your twist message and converting it into data that is appropriate for the Pololu servo controller. First we have to assign a variable to the message, which here we call msg. Thern to parse through the twist message, it is broken into two distinct functions inside the main class Twist. Those are linear and angular. Here we have the linear velocity and angular velocity of the vehicle respectively. Their units are m/s and rad/s.

msg = Twist
self.linear = float(msg.linear.x)  # m/s, +ve for fwd, -ve for rev
self.angular = float(msg.angular.z)  # rad/s, +ve for CCW rotation

Next to convert the linear velocity to a PWM signal we first conver the velocity into a throttle effort. This is based on the maximum speed the car can go. An estimation is done to determine the max speed based on the max RPM of the wheels and the wheel size. It is determined the car can travel at

MAX_LINEAR_SPEED = 585*(2*pi*120e-3)/60

Which is about 7.35 m/s. This is defined at the beginning as a global variable. Next a throttle ratio equation is used to figure out what the ratio of linear speed to change in throttle effort is used.

ratio_throttle = (self.linear/MAX_LINEAR_SPEED)*3000

The 3000 multiplication at the end is to ensure there is a value between 3000 and -3000. Then the throttle effort can be calculated.

thrtle_eff = round(6000 + ratio_throttle)

This ensures a minimum value of 3000 quarter-microseconds or 750 microseconds and a maximum value of 9000 quarter-microseconds or 2250 microseconds. The neutral point is 6000 or 1500 microseconds. The neutral point defines when the car stops from going backward to forward. 3000 being max speed backward and 9000 being the max speed forward.

To calculate the servo angle (basically a PWM value that corresponds to an steering angle) a conditional is used. This is to convert the angular rate to be between -45 and 45 degrees (The maximum steering value for the car). The conditional basically ensures there is not infinite values in the radius of curvature.

if self.angular < 1e-2 and self.angular > -1e-2:
    rad_curv = float(1e17)
else:
    rad_curv = float(self.linear/self.angular)
    if rad_curv < 1e-2 and rad_curv > -1e-2:
        rad_curv = 1e-10

steer_angle = atan(WHEELBASE/float(rad_curv))*R2D  # degrees

The last equation is converting the radius of curvature back to a steering angle. This is based on the bicycle kinematic model. The kinematic of our car is slightly off, so a supposed twist needs to be linearly multiplied to get an actual value of the radius of curvature. The simplest way to calculate radius of curvature is \(\dfrac {\dot{x}} {\dot{\phi}}\).

Then a ratio is defined to correlate a steering angle to a PWM signal. That is done by using:

ratio_steering = 3000/45

Then a PWM signal can be formed that will be between 3000 and 9000.

steering_target = round(6000 - steer_angle*ratio_steering)

The channels for the drive motor and the servo steering motor are set next. They are 1 for the drive motor and 0 for the steering servo. Once this has been completed you can serially send the appropriate commands to the servo controller.

self.serial_.set_speed(steering_chan, speed_steering)
self.serial_.set_speed(drive_chan, speed_drive)
self.serial_.set_acc(steering_chan, acc_steering)
self.serial_.set_acc(drive_chan, acc_drive)
self.serial_.set_target(drive_chan, thrtle_eff)
self.serial_.set_target(steering_chan, steering_target)

Shutting Down the Node

To appropriately shut down the node, a Try, Except and finally python function is used. This is to ensure that once the node is shutdown, the car will not continue to drive and to return the lights to a safe mode.

 try:
    rclpy.spin(motor_controller)

except KeyboardInterrupt:
    serial_cmds().set_target(1, 6000)
    serial_cmds().set_target(4, 6500)
    serial_cmds().set_target(3, 3000)
    serial_cmds().set_target(5, 3000)
finally:
    motor_controller.destroy_node()
    rclpy.shutdown()