Controlling motors using USB serial connection from Raspberry Pi to Arduino

Question

I am working on building an RC car/robot with Raspberry Pi and Arduino. I connected Arduino to Raspberry Pi using USB and send serial commands with python. I haven't done much programming with Arduino nor Python.

This just the beginning of this project and would like to get feedback on the code. Is it a good practice to use byte, bit masks and bitwise operators to send and decode commands? Is it better to send string commands? Also would I would like to get feedback on general structure and code logic for this type of program.

The car that I am using has two motors that control each side of the car. I think I have to slowdown the right side to go right and left side to go left.

I am using pygames to capture key presses. Some of the code was taken from GitHub.

Arduino

//Pin Functions
#define motorLeftSpeedPin 6 //pwm
#define motorLeftForwardPin A0
#define motorLeftBackwardPin A1
#define motorRightSpeedPin 5 //pwm
#define motorRightForwardPin A2
#define motorRightBackwardPin A3
#define speakerPin A5
#define ledPin 2
//define commands
#define STOP 0x00 //0000
#define FORWARD_DIRECTION 0x01 //0001
#define BACKWARD_DIRECTION 0x02 //0010 
#define LEFT_DIRECTION 0x04 //0100
#define RIGHT_DIRECTION 0x08 //1000
#define MOTORLEFT 0x10 //0001 0000
#define MOTORRIGHT 0x20 //0010 0000
#define SET_SPEED 0x40 //0100 0000
#define TURN_SPEED_OFFSET 20
#define MINIMUM_MOTOR_SPEED 100
#define MAXIMUM_MOTOR_SPEED 250
struct Motor
{
 byte mSide;
 byte mSpeed;
}motorLeft, motorRight;
struct Command
{
 byte cmdID;
 byte data1;
 byte data2;
 byte checkSum;
};
enum COMMAND_IDS
{
 INVALID_CMD = 0,
 DRIVE = 10
};
byte currentDirection = 0x00;
void dbg_print(const char * s)
{
//#if DEBUG
 Serial.print(s);
//#endif
}
void processCommand(struct Command &command)
{
 //prcess recieved command
 switch(command.cmdID)
 {
 case DRIVE:
 dbg_print("Drive ...");
 driveCar(command);
 break;
 default:
 //unknown command and do nothing
 dbg_print("Invalid cmd received...");
 break;
 }
}
void driveCar(struct Command &command)
{
 if(command.data1 & STOP){
 stopAllMotors();
 dbg_print("Stop ...");
 return;
 }
 if (!(command.data1 & LEFT_DIRECTION) && !(command.data1 & RIGHT_DIRECTION)){
 //if not turning sync the motor speeds
 setAllMotorsSpeed(command.data2);
 }
 if (command.data1 & FORWARD_DIRECTION){
 goForward();
 dbg_print("Drive Forward ..."); 
 }else if (command.data1 & BACKWARD_DIRECTION){
 goBackward();
 dbg_print("Drive Backward ...");
 }
 if (command.data1 & LEFT_DIRECTION){
 turnLeft();
 dbg_print("Turn Left ...");
 }else if (command.data1 & RIGHT_DIRECTION){
 turnRight();
 dbg_print("Turn Right ...");
 }else{
 //reset the direction bits
 currentDirection &= (RIGHT_DIRECTION | LEFT_DIRECTION);
 }
 if (command.data1 & SET_SPEED){
 setAllMotorsSpeed(command.data2);
 dbg_print("Set Speed ...");
 }
}
void turnLeft()
{
 //slow down the left motor to turn right
 if (!(currentDirection & LEFT_DIRECTION)){
 motorLeft.mSpeed-=TURN_SPEED_OFFSET; 
 setMotorSpeed(motorLeft);
 currentDirection |= LEFT_DIRECTION;
 }
}
void turnRight()
{
 //slow down the right motor to turn right
 if (!(currentDirection & RIGHT_DIRECTION)){
 motorRight.mSpeed-=TURN_SPEED_OFFSET; 
 setMotorSpeed(motorRight);
 currentDirection |= RIGHT_DIRECTION;
 }
}
void goForward()
{
 //if going backwards then stop motors and then go forward
 if(!(currentDirection & FORWARD_DIRECTION))
 {
 stopAllMotors();
 digitalWrite(motorLeftForwardPin,1);
 digitalWrite(motorRightForwardPin,1);
 setMotorSpeed(motorLeft);
 setMotorSpeed(motorRight);
 currentDirection |= FORWARD_DIRECTION; // set forward direction bit
 currentDirection &= BACKWARD_DIRECTION; // reset backward direction bit
 }
}
void goBackward()
{
 if(!(currentDirection & BACKWARD_DIRECTION))
 {
 //if not going backwards then stop motors and start going backward
 stopAllMotors();
 digitalWrite(motorLeftBackwardPin,1);
 digitalWrite(motorRightBackwardPin,1);
 setMotorSpeed(motorLeft);
 setMotorSpeed(motorRight);
 currentDirection |= BACKWARD_DIRECTION; // set backward direction bit
 currentDirection &= FORWARD_DIRECTION; // reset forward direction bit
 }
}
void stopAllMotors()
{
 setAllMotorsSpeed(0);
 digitalWrite(motorRightBackwardPin,0);
 digitalWrite(motorLeftBackwardPin,0);
 digitalWrite(motorRightForwardPin,0);
 digitalWrite(motorLeftForwardPin,0);
 delay(200);
}
void setAllMotorsSpeed(byte speedValue)
{
 if(speedValue < MAXIMUM_MOTOR_SPEED && speedValue > MINIMUM_MOTOR_SPEED){
 motorLeft.mSpeed = speedValue;
 motorRight.mSpeed = speedValue;
 setMotorSpeed(motorLeft);
 setMotorSpeed(motorRight);
 }else{
 dbg_print("Motor speed is two high:");
 }
}
void setMotorSpeed(struct Motor &motor)
{
 if(motor.mSide == MOTORLEFT){
 analogWrite(motorLeftSpeedPin, motor.mSpeed);
 }else if (motor.mSide == MOTORRIGHT){
 analogWrite(motorRightSpeedPin, motor.mSpeed);
 }else{
 dbg_print("Error Setting Motor Speed");
 }
}
void setup()
{
 Serial.begin(9600);
 pinMode(speakerPin, OUTPUT);
 pinMode(ledPin, OUTPUT);
 pinMode(motorLeftSpeedPin, OUTPUT);
 pinMode(motorLeftForwardPin, OUTPUT);
 pinMode(motorLeftBackwardPin, OUTPUT);
 pinMode(motorRightSpeedPin, OUTPUT);
 pinMode(motorRightForwardPin, OUTPUT);
 pinMode(motorRightBackwardPin, OUTPUT); 
 motorLeft.mSpeed = MAXIMUM_MOTOR_SPEED;
 motorRight.mSpeed = MAXIMUM_MOTOR_SPEED;
 motorLeft.mSide = MOTORLEFT;
 motorRight.mSide = MOTORRIGHT;
}
void loop()
{
 Command incomingCmd;
 if(Serial.available() >= sizeof(Command)){
 //read the incoming data
 Command *mem = &incomingCmd;
 unsigned char *p = (unsigned char *)mem;
 for(int i=0;i<sizeof(Command);i++)
 {
 unsigned int data = Serial.read();
 Serial.println(data);
 p[i] = data;
 }
 //verify checksum
 byte received_sum = incomingCmd.cmdID + incomingCmd.data1 + incomingCmd.data2;
 if (incomingCmd.cmdID != INVALID_CMD && received_sum == incomingCmd.checkSum) {
 driveCar(incomingCmd);
 dbg_print("Good Cmd - checksum matched");
 } else {
 //Checksum didn't match, don't process the command
 dbg_print("Bad Cmd - invalid cmd or checksum didn't match");
 }
 }
}

Python

import pygame
from pygame.locals import *
pygame.init()
#define commands
STOP = 0x00 
FORWARD_DIRECTION = 0x01
BACKWARD_DIRECTION = 0x02 
LEFT_DIRECTION = 0x04
RIGHT_DIRECTION = 0x08 
MOTORLEFT = 0x10
MOTORRIGHT = 0x20 
SET_SPEED = 0x40 
currentDirection = 0x00
carSpeed = 200
done = False
title = "Hello!"
width = 640
height = 400
screen = pygame.display.set_mode((width, height))
screen.fill((255, 255, 255))
clock = pygame.time.Clock()
pygame.display.set_caption(title)
#load images 
down_off = pygame.image.load('images/down_off.gif')
down_on = pygame.image.load('images/down_on.gif')
left_on = pygame.image.load('images/left_on.gif')
left_off = pygame.image.load('images/left_off.gif')
right_on = pygame.image.load('images/right_on.gif')
right_off = pygame.image.load('images/right_off.gif')
up_on = pygame.image.load('images/up_on.gif')
up_off = pygame.image.load('images/up_off.gif')
import serial
ser = serial.Serial('/dev/ttyACM0', 9600)
#show initally on the screen
def arrowsOff():
 screen.blit(down_off,(300,250))
 screen.blit(left_off,(230,180))
 screen.blit(right_off,(370,180))
 screen.blit(up_off,(300,110))
arrowsOff()
font=pygame.font.SysFont("monospace", 30)
def sendCommandToArduino():
 #send command drive with motor speed 0xA0
 checkSum = 0x01+currentDirection+0xA0 
 ser.write(chr(0x01)+chr(currentDirection)+chr(0xA0)+chr(checkSum))
def showSpeed():
 screen.fill(pygame.Color("white"), (0, 0, 110, 40))
 scoretext=font.render("Speed:"+str(carSpeed), 1,(0,0,0))
 screen.blit(scoretext, (0, 0))
while not done:
 for event in pygame.event.get():
 if (event.type == QUIT):
 done = True
 elif(event.type == KEYDOWN):
 if (event.key == K_ESCAPE):
 done = True
 keys = pygame.key.get_pressed()
 if keys[K_LEFT]:
 screen.blit(left_on,(230,180))
 print("Left");
 currentDirection|=LEFT_DIRECTION
 sendCommandToArduino()
 if keys[K_RIGHT]:
 screen.blit(right_on,(370,180))
 print("Right");
 currentDirection|=RIGHT_DIRECTION
 sendCommandToArduino()
 if keys[K_UP]:
 screen.blit(up_on,(300,110))
 print("Up");
 currentDirection|=FORWARD_DIRECTION
 sendCommandToArduino()
 if keys[K_DOWN]:
 screen.blit(down_on,(300,250))
 print("Down");
 currentDirection|=BACKWARD_DIRECTION
 sendCommandToArduino()
 if keys[K_LEFTBRACKET]:
 if(carSpeed>0):
 carSpeed-=5
 print("Slowdown");
 showSpeed()
 if keys[K_RIGHTBRACKET]:
 carSpeed+=5
 showSpeed()
 print("faster")
 elif(event.type == KEYUP):
 if keys[K_LEFT]:
 screen.blit(left_off,(230,180))
 print("Left");
 currentDirection&=LEFT_DIRECTION
 sendCommandToArduino()
 if keys[K_RIGHT]:
 screen.blit(right_off,(370,180))
 print("Right");
 currentDirection&=RIGHT_DIRECTION
 sendCommandToArduino()
 if keys[K_UP]:
 screen.blit(up_off,(300,110))
 print("Up");
 currentDirection&=FORWARD_DIRECTION
 sendCommandToArduino()
 if keys[K_DOWN]:
 screen.blit(down_off,(300,250))
 print("Down");
 currentDirection&=BACKWARD_DIRECTION
 sendCommandToArduino()
 pygame.display.update()
 clock.tick(60)

Answer 1

C++

Please use some form of consistent indentation, currently it's all over the place. For example:

 if (command.data1 & FORWARD_DIRECTION){
 goForward();
 dbg_print("Drive Forward ..."); 
 }else if (command.data1 & BACKWARD_DIRECTION){
 goBackward();
 dbg_print("Drive Backward ...");
 }

This is so much easier to read if the indentation is always the same amount.

Prefer const variables over #define, for example:

#define STOP 0x00 //0000

Could be better written as:

const uint8_t STOP = 0x00;

If you end up using a proper in circuit emulator you will probably find the additional information about the variable names useful.

If you aren't changing a parameter consider passing it by const reference. For example:

void driveCar(struct Command &command){

Can become

void driveCar(Command const& command){

Then if you attempt to modify command you will get a compiler error. This can help you write more correct code by avoiding a class of bugs.

Consider placing your incoming command handler in it's own ISR, this way you won't lose incoming information when other parts of your code use delays. In simpler code you might be fine because the USART on the atmega chips have a hardware buffer, but if your code spends too long somewhere else you will potentially lose data. This could be a problem especially if you use busy-wait delays in your code. If writing a proper ISR is a pain in the Adruino IDE you might also want to going with a proper C++ toolchain such as AVR-GCC.

You also have a lot of strings scattered throughout your code, these will use up the SRAM on the Arduino boards. You don't have a lot of space on these devices so moving these strings into progmem will save you precious memory.

Python

There is a bunch of trailing whitespace, this can cause issues when you import your code into a version control system. In general this is a bit of a code smell, you probably want to clean this up.

You have a disturbingly large number of global variables, you might want to consider grouping related variables into other structures.

For example:

STOP = 0x00 
FORWARD_DIRECTION = 0x01
BACKWARD_DIRECTION = 0x02 
LEFT_DIRECTION = 0x04
RIGHT_DIRECTION = 0x08 
MOTORLEFT = 0x10
MOTORRIGHT = 0x20 
SET_SPEED = 0x40 

Could perhaps be better stored in a dictionary

COMMANDS = {
 "STOP": 0x00,
 "FORWARD_DIRECTION": 0x01,
 "BACKWARD_DIRECTION": 0x02,
 "LEFT_DIRECTION": 0x04,
 "RIGHT_DIRECTION": 0x08,
 "MOTORLEFT": 0x10,
 "MOTORRIGHT": 0x20,
 "SET_SPEED": 0x40,
}

You might find doing some similar cleaning up with all the GUI variables to also be worthwhile.

It seems like your Python code doesn't handle the incoming prints from the Arduino, you might want to put some sort of input handling for the serial in place. This might require a separate thread if you wish to also be able to send commands at the same time as receiving data.

• Yan: Thank you very much for the response. I have a couple of questions. If I use instead of define const uint8_t should i then set all the variables to uint8_t instead of byte. I understand that uint8_t and byte is the same size but would i have to cast it to do bitwise operations? I didn't implement the serial read from python yet. The write is just to writing to the console for debuging. I actually had to disable debugging because it was really slowing down the communication and freezing python program. As you suggested i have too look into ISR to be able to serial read and write.
• shuttle87: I believe on most Arduino platforms byte is just a typedef for uint8_t. Bitwise operations will work properly if they are the same type and yes using these bitwise operations is totally fine in the context of embedded code.