4
\$\begingroup\$

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)
\$\endgroup\$

1 Answer 1

Reset to default
3
\$\begingroup\$

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.

\$\endgroup\$
2
  • \$\begingroup\$ 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. \$\endgroup\$
    – Yan
    Aug 11, 2015 at 23:44
  • \$\begingroup\$ 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. \$\endgroup\$
    – shuttle87
    Aug 13, 2015 at 2:36

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.