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.