taibo.ca

Aum2: Raspberry Pi Zero W based robot

Tue, 03 May 2022 03:29:12 GMT

As soon as I got my Raspberry Pi Zero W, I started thinking how to test it. I chose to use it as brain of one of those standard robot bases available online.

I bought an Arduino Smart Car kit in Aliexpress, there is a lot of information online about this chassis, and it’s not expensive at all.

As I mentioned, the brain of the robot is a Raspberry Pi Zero W, as can be seen in the images, I also connected to it the camera module and 3 SR04 ultrasonic distance sensors.

Arduino smart car chassis

I’m not using them right now, but I thought it was a good idea to lay out all the hardware in place, so I can avoid surprises down the path, when I finally want to use them.

Aum Batteries view

Another choice I made was to use two different batteries, one for electronics, and another for the motors. This ensures I wont get weird stuff when the motors are draining current.

To control the motors I used a 16 servo driver, and I think that was the best decision I made on this robot. It simplified a lot the coding and made it look a lot more tidy, since many cables staid on the lower level instead of needing to connect them to the RPi.

Programming in python was so simple, that I am regretting not making the change long before. I love object oriented programming, and python gave me the perfect combination between OOP an access to hardware.

The first task I wanted to tackle on this robot was enabling it to be remote controled.

To accomplish this I am using my android’s phone accelerometer, with an app that sends to an IP address and port, using UDP protocol, the changes in the accelerometer (and other stuff, that I´m not currently using. You can download the app here.

On this video you can see how it worked out:

TODO Add video

So I’m pretty happy with it, it’s responsive enough and super intuitive to drive. I would make it more powerful though.. I might in another iteration.

So, lets go to the code..

I divided the code in these files, that I will be explaining one by one:

aum2.py
l298NDrive.py
remoteControl.py
utils.py

aum2.py

This is the main entry point of the application, and contains the robot class:

aum2.pyPY
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47import RPi.GPIO as GPIO
from twisted.internet import reactor, task
import l298NDrive
import remoteControl
import logging
import utils

logging.basicConfig(level=logging.WARN)
log = logging.getLogger('Aum2')

enableMotors = True # Motors can be disabled for testing with this
runFrequency = 1 # The frequency on wich the main Task will be ran

class Aum2(object):

    def __init__(self):
        log.debug("Initializing Aum2 robot...")
        # Configuration
        self.port = 5555 # Port to open so the remote can connect
        GPIO.setmode(GPIO.BOARD)
        # Configure min and max servo pulse lengths
        # Keep in mind these are not pins but servo outs
        self.motors = l298NDrive.L298NDrive(10, 11, 12, 13, 14, 15, enableMotors)
        self.rc = remoteControl.RemoteControl(self.port)

    def processRemoteCommand(self, cmd):
        (vr, vl) = utils.joystickToDiff(cmd.x, cmd.y, -9.8, 9.8, -4095, 4095)
        self.motors.setSpeeds(int(vr), int(-vl))

    def run(self):
        print("running...")

    def start(self):
        self.rc.start(self.processRemoteCommand)
        l = task.LoopingCall(self.run)
        l.start(runFrequency) # run will be called with this frequency

# Gather our code in a main() function
def main():
    robot = Aum2()
    robot.start()
    reactor.run()

# Standard boilerplate to call the main() function to begin
# the program.
if __name__ == '__main__':
    main()

Some points I’d like to comment about this file:

I am using the twisted library to be able to coordinate the Remote Control orders coming from an UDP server (I’ll show that in its corresponding source file) and the main Run method of the robot. As you can see, that method is doing nothing right now, but the idea is to add there the code to make the robot a little more autonomous.
The processRemoteCommand method will be called each time we get a message from the remote control, so we should act accordingly.
The start method is were I configure the two things that needs to be coordinated: listening for remote commands, and the robots run method.

remoteControl.py

This file contains two classes, the RemoteControl class, and the RCCommand class

remoteControl.pyPY
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47import logging
import traceback
from twisted.internet.protocol import DatagramProtocol
from twisted.internet import reactor
import utils

log = logging.getLogger('RemoteControl')


class RemoteControl(DatagramProtocol):

    def __init__(self, port):
        self.port = port

    def start(self, onCommandReceived):
        log.debug("Initializing RemoteControl...")
        self.onCommandReceived = onCommandReceived
        reactor.listenUDP(self.port, self)

    def datagramReceived(self, data, addr):
        try:
            log.debug("Received %r from %s" % (data, addr))
            command = RCCommand(data)
            self.onCommandReceived(command)
        except (KeyboardInterrupt, SystemExit):
            raise
        except:
            traceback.print_exc()


class RCCommand(object):
    # Timestamp [sec], sensorid, x, y, z, sensorid, x, y, z, sensorid, x, y, z
    # 890.71558, 3, 0.076, 9.809, 0.565, ...

    def __init__(self, message):
        self.x = 0
        self.y = 0
        self.z = 0
        self.values = message.split(",")

        if len(self.values) >= 5 and self.values[1].strip() == "3":
            self.x = -float(self.values[3].strip())
            self.y = -float(self.values[2].strip())
            self.z = float(self.values[4].strip())
            log.debug("x={} y={} z={}".format(self.x, self.y, self.z))
        else:
            log.warn("Invalid message: {}".format(message))

Again, the RemoteControl class, uses twisted framework to listen for UDP packages sent by the Android phone running the Wireless IMU app. This application sends messages with this structure:

890.71558, 3, 0.076, 9.809, 0.565, 4, -0.559, 0.032, -0.134, 5, -21.660,-36.960,-28.140

I’m only interested in the part of the message I highlighted above, this would be, sensor id 3, and the three measurements provided by the accelerometer (0.076, 9.809, 0.565). As you can see in the code, the class RCCommand translates the useful par of this message into the x, and y values we need for the robot, where x is the axis that goes from front to back, and y, goes from side to side. I also have code for the z axis, but I’m not using that for now.

l298NDrive.py

This is the file responsible to control the motors. For this it sends orders to the the 16 Servo driver, which is connected to the L298N Motor driver. I think I will probably reusing this class on every Raspberry Pi robot I build. It’s really simple and makes what it needs without complicating anything.

l298NDrive.pyPY
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66import Adafruit_PCA9685
import logging
import math
import utils

log = logging.getLogger('L298NDrive')


class L298NDrive(object):

    def __init__(self, enA, in1, in2, in3, in4, enB, enabled):
        log.debug("Initializing L298Drive...")
        self.minPwm = 0
        self.MaxPwm = 4095
        self.enA = enA
        self.in1 = in1
        self.in2 = in2
        self.in3 = in3
        self.in4 = in4
        self.enB = enB
        self.enabled = enabled
        if enabled:
            self.enable()

    def enable(self):
        self.enabled = True
        # Initialise the PCA9685 using the default address (0x40).
        self.pwm = Adafruit_PCA9685.PCA9685()
        self.setSpeeds(0, 0)

    def disable(self):
        self.enabled = False
        self.setSpeeds(0, 0)
        # Initialise the PCA9685 using the default address (0x40).
        self.pwm = None

    def setSpeeds(self, rSpeed, lSpeed):
        self.setSpeed(1, rSpeed)
        self.setSpeed(2, lSpeed)

    def setSpeed(self, motor, speed):
        pwm = int(math.fabs(speed))
        log.info("Motor: {} speed: {} pwm: {}".format(motor, speed, pwm))

        if motor == 1:
            if speed >= 0:
                if self.enabled:
                    self.pwm.set_pwm(self.in1, 0, 0)
                    self.pwm.set_pwm(self.in2, 0, self.MaxPwm)
            else:
                if self.enabled:
                    self.pwm.set_pwm(self.in1, 0, self.MaxPwm)
                    self.pwm.set_pwm(self.in2, 0, 0)
            if self.enabled:
                self.pwm.set_pwm(self.enA, 0, pwm)
       else: # motor == 2
            if speed >= 0:
                if self.enabled:
                    self.pwm.set_pwm(self.in3, 0, 0)
                    self.pwm.set_pwm(self.in4, 0, self.MaxPwm)
            else:
                if self.enabled:
                    self.pwm.set_pwm(self.in3, 0, self.MaxPwm)
                    self.pwm.set_pwm(self.in4, 0, 0)
            if self.enabled:
                self.pwm.set_pwm(self.enB, 0, pwm)

As can be seen, the main part of the code is in the method setSpeed(self, motor, speed) this method receives the motor number to configure (1=right, 2= left) and tells the Servo Driver what to do with the pins associated with that motor.

Maybe a little of explanation should be given to understand why I’m doing what I’m doing there..

The L298N motor driver

Uses 6 pins to drive two motors:

enA Is used to tell the speed (using PWM) we want the motor to turn
in1 When powered, Tells the driver that the motor 1 should move in one direction (in2 should be OFF)
in2 When powered, Tells the driver that the motor 1 should move in the opposite direction (in1 should be OFF)
in3 When powered, Tells the driver that the motor 2 should move in one direction (in4 should be OFF)
in4 When powered, Tells the driver that the motor 2 should move in the opposite direction (in4 should be OFF)
enB Is used to tell the speed (using PWM) we want the motor to turn

So in our case, as I’m using a servo driver for all of the pins (and only 2 of them need PWM):

when I do this:

PY
1self.pwm.set_pwm(self.in1, 0, 0)

I’m really telling the servo driver that that pin (in1) should be OFF during all the PWM cicle, (so I turn OFF that pin)

and when I do this:

PY
1self.pwm.set_pwm(self.in1, 0, self.MaxPwm) 

’m really telling the servo driver that that pin (in1) should be in ON during all the PWM cicle, (so I turn ON that pin)

And then, when I do this:

PY
1self.pwm.set_pwm(self.enA, 0, pwm)

I use the real PWM pin, and set the pulse with that I was requested with the speed parameter.

utils.py

Finally the last peace of this. This doesn’t contain a class, but all the useful functions that I think I’ll be reusing in other places.

utils.pyPY
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65import math
import logging

log = logging.getLogger('utils')


def map(v, in_min, in_max, out_min, out_max):
    # Check that the value is at least in_min
    if v < in_min:
    v = in_min

    # Check that the value is at most in_max
    if v > in_max:
       v = in_max
    return (v - in_min) * (out_max - out_min) // (in_max - in_min) + out_min


def joystickToDiff(x, y, minJoystick, maxJoystick, minSpeed, maxSpeed):

    # If x and y are 0, then there is not much to calculate...
    if x == 0 and y == 0:
        return (0, 0)

    # First Compute the angle in deg
    # First hypotenuse
    z = math.sqrt(x * x + y * y)

    # angle in radians
    rad = math.acos(math.fabs(x) / z)

    # and in degrees
    angle = rad * 180 / math.pi

    # Now angle indicates the measure of turn
    # Along a straight line, with an angle o, the turn co-efficient is same
    # this applies for angles between 0-90, with angle 0 the coeff is -1
    # with angle 45, the co-efficient is 0 and with angle 90, it is 1

    tcoeff = -1 + (angle / 90) * 2
    turn = tcoeff * math.fabs(math.fabs(y) - math.fabs(x))
    turn = round(turn * 100, 0) / 100

    # And max of y or x is the movement
    mov = max(math.fabs(y), math.fabs(x))

    # First and third quadrant
    if (x >= 0 and y >= 0) or (x < 0 and y < 0):
        rawLeft = mov
        rawRight = turn
    else:
        rawRight = mov
        rawLeft = turn

    # Reverse polarity
    if y < 0:
        rawLeft = 0 - rawLeft
        rawRight = 0 - rawRight

    # minJoystick, maxJoystick, minSpeed, maxSpeed
    # Map the values onto the defined rang
    rightOut = map(rawRight, minJoystick, maxJoystick, minSpeed, maxSpeed)
    leftOut = map(rawLeft, minJoystick, maxJoystick, minSpeed, maxSpeed)

    log.debug("x={} y={}, rOut={}, lOut={}".format(x, y, rightOut, leftOut))
    return (rightOut, leftOut)

There are just two methods here, the first is a port from Arduino of the map function. It converts a value (v) within the specified input limits (inmin, inmax) to the corresponding value within the output range (outmin, outmax).

I added limits for the input value, so I only accept a v, that is within the inmin and inmax.

The other function I have (joystickToDiff) is a little more complex, but its purpose is to convert from an joystick pair of input values, to the required speeds of the right and left motors of a Differential drive robot. This output is delivered already converted to the range of speeds specified as parameters. You can find more details on this part on this previous article

I hope this article makes sense for you, and if I can help you in any way to make it work in your own robot, please let me know. If you see something I could improve in the article, or the code, please also let me know, I will hear your recommendations.

Joystick to Differential Drive (Python)

Tue, 03 May 2022 03:08:42 GMT

As I’m now working on a Raspberry Pi Zero W based robot, doing my first test on Python programming, I needed to port the function created previously in this article for Arduino (c++).

This time though, I changed its name, and added the capability to provide the limits for the input and the output on any range you need it. I’ll explain why Let’s suppose you have an input that is like a joystick, so the x, and y values provided by the joystick will vary between -1 and 1, as can be seen in the diagram.

Cartesian - differentical drive

Joystick output requirements

We need to convert this pair of values, to something we can use to drive a Differential drive (aka tank drive) robot. This is what we need the bot to behave like:

Differential drive

differential drive

To achieve this, is that I made this function. Here you can check it:

PY
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57def joystickToDiff(x, y, minJoystick, maxJoystick, minSpeed, maxSpeed):
    # If x and y are 0, then there is not much to calculate...
    if x == 0 and y == 0:
        return (0, 0)

    # First Compute the angle in deg
    # First hypotenuse
    z = math.sqrt(x * x + y * y)

    # angle in radians
    rad = math.acos(math.fabs(x) / z)

    # and in degrees
    angle = rad * 180 / math.pi

    # Now angle indicates the measure of turn
    # Along a straight line, with an angle o, the turn co-efficient is same
    # this applies for angles between 0-90, with angle 0 the coeff is -1
    # with angle 45, the co-efficient is 0 and with angle 90, it is 1

    tcoeff = -1 + (angle / 90) * 2
    turn = tcoeff * math.fabs(math.fabs(y) - math.fabs(x))
    turn = round(turn * 100, 0) / 100

    # And max of y or x is the movement
    mov = max(math.fabs(y), math.fabs(x))

    # First and third quadrant
    if (x >= 0 and y >= 0) or (x < 0 and y < 0):
        rawLeft = mov
        rawRight = turn
    else:
        rawRight = mov
        rawLeft = turn

    # Reverse polarity
    if y < 0:
        rawLeft = 0 - rawLeft
        rawRight = 0 - rawRight

    # minJoystick, maxJoystick, minSpeed, maxSpeed
    # Map the values onto the defined rang
    rightOut = map(rawRight, minJoystick, maxJoystick, minSpeed, maxSpeed)
    leftOut = map(rawLeft, minJoystick, maxJoystick, minSpeed, maxSpeed)

    return (rightOut, leftOut)



def map(v, in_min, in_max, out_min, out_max):
    # Check that the value is at least in_min
    if v < in_min:
        v = in_min
    # Check that the value is at most in_max
    if v > in_max:
        v = in_max
    return (v - in_min) * (out_max - out_min) // (in_max - in_min) + out_min

I added as many comments as I could, trying to make everything clear.

So, what are the differences with the Arduino version?

The name: just because I think this is more accurate to the purpose of the function

It returns a tuple: So you just get the right and left speeds on it

The joystick and wheel speed limits

This change is the most important. In reality, I´m not using a joystick to control the robot, but my cellphone´s accelerometer, so, thanks to this parameters, I can reconfigure the limits of the input.

In case of a joystick might be -1 to 1, but in case of the accelerometer, it goes from -9.8 to 9.8 (the gravity)

Also, I wanted the output to be configurable with the minSpeed, maxSpeed parameters. This allows me to just get the range of values I need for my servo controller (in my case -4095 to 4095, but you can set whatever speed limits you want there, and the output will be scaled accordingly)

What about that map function?

If you are an Arduino programmer, you might know it already, since I port it from there. It is used to map an input value from the input range limits, to the output range limits.

As I´m not aware of it existing in python, I just added to my utils functions.

The only difference with the Arduino version, is that I added a constraint to the value, so it cannot be smaller or bigger than the input range specified.

I hope this makes sense, and if it doesn’t, please let me know. I promise I’ll do my best to make it more clear.

Joystick to Differential Drive (Arduino)

Mon, 02 May 2022 04:09:59 GMT

When you are building robots, one of the most common chassis you find are the ones that use Differential Drive. This means they have a motor per wheel, might be threaded, two wheels and a caster, 4 wheels, etc.

One way of driving this differential drive robots is sending PWM signals to each motor. But how do you convert for example the signals from a Joystick, (or any Cartesian pair of coordinates, like a direction vector) to the specific order for each wheel?

That is where this function is required. It receives a pair of cartesian x, and y coordinates, and calculates the differential drive output (right and left wheel commands) you will need to send to the motors.

This function will generate an output as can be seen in the following image:

LeftMotorOutput and RightMotorOutput will hold the values you should send to the right and left motors.

You can also configure the limits of x, y, and right and left motors by using the 4 constants defined on top of the code, this is handy to adapt the function to your specific scenario.

C++
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69#define DDRIVE_MIN -100 //The minimum value x or y can be.
#define DDRIVE_MAX 100  //The maximum value x or y can be.
#define MOTOR_MIN_PWM -255 //The minimum value the motor output can be.
#define MOTOR_MAX_PWM 255 //The maximum value the motor output can be.

int LeftMotorOutput; //will hold the calculated output for the left motor
int RightMotorOutput; //will hold the calculated output for the right motor



void DifferentialDriveService::CalculateTankDrive(float x, float y)
{
 float rawLeft;
 float rawRight;
 float RawLeft;
 float RawRight;


 // first Compute the angle in deg
 // First hypotenuse
 float z = sqrt(x * x + y * y);

// angle in radians
 float rad = acos(abs(x) / z);

// Cataer for NaN values
 if (isnan(rad) == true) {
 rad = 0;
 }

// and in degrees
 float angle = rad * 180 / PI;

// Now angle indicates the measure of turn
 // Along a straight line, with an angle o, the turn co-efficient is same
 // this applies for angles between 0-90, with angle 0 the co-eff is -1
 // with angle 45, the co-efficient is 0 and with angle 90, it is 1

float tcoeff = -1 + (angle / 90) * 2;
 float turn = tcoeff * abs(abs(y) - abs(x));
 turn = round(turn * 100) / 100;

// And max of y or x is the movement
 float mov = max(abs(y), abs(x));

// First and third quadrant
 if ((x >= 0 && y >= 0) || (x < 0 && y < 0))
 {
 rawLeft = mov; rawRight = turn;
 }
 else
 {
 rawRight = mov; rawLeft = turn;
 }

// Reverse polarity
 if (y < 0) {
 rawLeft = 0 - rawLeft;
 rawRight = 0 - rawRight;
 }

// Update the values
 RawLeft = rawLeft;
 RawRight = rawRight;

// Map the values onto the defined rang
 LeftMotorOutput = map(rawLeft, DDRIVE_MIN, DDRIVE_MAX, MOTOR_MIN_PWM, MOTOR_MAX_PWM);
 RightMotorOutput = map(rawRight, DDRIVE_MIN, DDRIVE_MAX, MOTOR_MIN_PWM, MOTOR_MAX_PWM);
}

If you find this code snippet useful, need help to make it work, or to understand it, please leave a comment and we can talk about it!

Radio Controlled Minnie Mouse Quad

Sun, 01 May 2022 17:43:04 GMT

Some time ago I found in the curb a perfectly working Minnie Mouse Quad, This project is about making that Minnie Mouse Quad radio controlled.

Why would I want to do that?

First: My daughter, the Quad and me frequently go to the park one block from home to ride it. We don’t take the stroller, as we have the quad for her to ride. After five minutes she get bored of riding it.. Now I have to carry home my daughter and the quad… Wouldn’t it be nice to just sit my daughter in the quad and drive it home? Or at least only carry her?

Second: Its cool to drive such a big car remotely!

So lets do it!

You can probably get based on this project to convert any kind of toddler vehicle to Radio Control. If that’s the case, please share some pics of your project, I’d love to see it!

The benefit of using an already electric car, is that I don’t need to worry much about power and adding a motor as those are already there and working.

The main issue here, is controlling the stearing remotely. For that, I designed a servo linear actuator, that produces the same movement the stearing wheel would.

You can find the 3D design of the servo linear actuator here at Printables.com

Once this is installed (as you can see it is done in the pics), the only remaining thing is to re work the connections, so we can control the motor from both the driver activated button, and the remote control receiver.

Starting a new blog..

Sun, 01 May 2022 05:19:55 GMT

So, I decided to give a try to this blogging thing, lets see how it goes!

...

Note to myself: Learn to write longer post.