Arduino Code

Program Overview

This page contains documentation for the arduino code used.

Program Overview
- Arduino Code - full code
- Loop Overview - high-level overview (pseudocode)
- Defines and Directives - list of predefines used
- Global and Structs - list of globals variables used and structs
String Valididation - checks to make sure data recieved is valid
Motor Control & Thresholding
- parseData() - converts string data (after validation) to usable string data
- updateMotors() - used by parseData() to update global motor values
- driveMotors() and Thresholding - sends pwm signal to the motors
Input watchdog - checks for disconnect and stops motors

#include <SoftwareSerial.h>

//PREPROCCESSOR DIRECTIVES

#define THRESHOLD        0.15
#define MAX_INPUT_LENGTH 16
//uncomment for debug statements
#define DEBUG

//pin defines
#define LEFT_MOTOR_PWM_PIN    6
#define LEFT_MOTOR_DIR_PIN    8
#define RIGHT_MOTOR_PWM_PIN   5
#define RIGHT_MOTOR_DIR_PIN   7
//STRUCT DEFINITIONS

struct MotorPower{
  float leftMotor;
  float rightMotor;
};

//global values
struct MotorPower motorPower;
unsigned long lastUpdateTime; //to keep track of time of last input

//function prototypes
bool isValidChar(char c);
bool isValidInput(String input);
void parseData(String data);
void updateMotors(String lpower, String rpower);
void driveMotors();
void updateTime();
void stopMotors();

SoftwareSerial mySerial(2,3);//RX, TX

void setup() {
  //Setting up serial
  Serial.begin(115200);
  mySerial.begin(57600);
  #ifdef DEBUG
  Serial.print("Started Serial\n");
  #endif  
  
  //Setting up motors
  pinMode(LEFT_MOTOR_PWM_PIN, OUTPUT);
  pinMode(LEFT_MOTOR_DIR_PIN, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM_PIN, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR_PIN, OUTPUT);  
  
  //setting last update time
  lastUpdateTime = millis();
}

void loop() {
  String str;
  if(mySerial.available()){
    str = mySerial.readStringUntil('\n');
    updateTime();
    if(isValidInput(str) == true){
      parseData(str); //updates teh global motorPower struct
      driveMotors();      
    }
  } else {
    //if last update sent was longer than 1 second, reset the motors to 0
    if((millis() - lastUpdateTime) >  500){
      stopMotors();      
    }
  }
}

void updateTime(){
  lastUpdateTime = millis();
}

String validCharacters = "LR:|1234567890.-;";

//returns false if char not found in string
bool isValidChar(char c){
  for(int i =0; i < validCharacters.length(); i++){
    if(c == validCharacters.charAt(i)){
      return true;
    }
  }
  return false;
}

bool isValidInput(String input){  
   //go through each character in the data string
  if(input.length() > MAX_INPUT_LENGTH){
    #ifdef DEBUG
    Serial.print("TOO long of input!\n");
    #endif
    return false;
  }
  for(int i = 0; i < input.length(); i++){
    if(isValidChar(input.charAt(i)) == false){
      //char not valid!
      #ifdef DEBUG
      Serial.print("char not valid!\n");
      #endif
      return false;
    }
  } //end of i for
  return true;
}

void parseData(String data){
  String leftMotorString = "";
  String rightMotorString = "";
  int state = 0;
  for(int i = 0; i < data.length(); i++){
    char c = data.charAt(i);
    if(c == 'L'){
      state = 0;
      continue;
    }
    if(c == 'R'){
      state = 1;
      continue;
    }
    if(c == ':' || c =='|' || c == ';'){
      continue;
    }
    
    switch(state) {
      case 0:
        // parsing leftMotorString data
        leftMotorString.concat(data.charAt(i));
        break;
      case 1:
        //parsing rightMotorString data
        rightMotorString.concat(data.charAt(i));        
        break;
      default:
        //don't do anything
        break;
    }
  } //end of for loop
  updateMotors(leftMotorString, rightMotorString);
}

void stopMotors(){
  motorPower.leftMotor = 0;
  motorPower.rightMotor = 0;
  
  //stop motors with pwm signal of 0
  analogWrite(LEFT_MOTOR_PWM_PIN, 0);  
  analogWrite(RIGHT_MOTOR_PWM_PIN, 0);     
}

void updateMotors(String lpower, String rpower){
  //creating char arrays since .toCharArray needs an array to write to
  int sizeOfArray = 8;
  char leftPowerCharArray[sizeOfArray];
  char rightPowerCharArray[sizeOfArray];
  lpower.toCharArray(leftPowerCharArray, sizeOfArray);
  rpower.toCharArray(rightPowerCharArray, sizeOfArray);
  
  motorPower.leftMotor = atof(leftPowerCharArray);  
  motorPower.rightMotor = atof(rightPowerCharArray);
}

void driveMotors(){
  #ifdef DEBUG
  Serial.print("Left Power: ");
  Serial.print(motorPower.leftMotor);
  Serial.print("\t\tRightPower: ");
  Serial.print(motorPower.rightMotor);
  Serial.print("\n");
  #endif
  
  //Left Drive
   if(abs(motorPower.leftMotor) > THRESHOLD){
     int LeftMotorPWM = map(abs(motorPower.leftMotor * 100), 0, 100, 0, 255);
     analogWrite(LEFT_MOTOR_PWM_PIN, LeftMotorPWM);
   
     if(motorPower.leftMotor < 0){ //move forward if stick pushed forward
       
       digitalWrite(LEFT_MOTOR_DIR_PIN, HIGH);
     } else {
       digitalWrite(LEFT_MOTOR_DIR_PIN, LOW);   
     }   
   } else {
     analogWrite(LEFT_MOTOR_PWM_PIN, 0);  
   }

   
  //Right Drive
  
   if(abs(motorPower.rightMotor) > THRESHOLD){
     int RightMotorPWM = map(abs(motorPower.rightMotor * 100), 0, 100, 0, 255);
     analogWrite(RIGHT_MOTOR_PWM_PIN, RightMotorPWM);    
   
     if(motorPower.rightMotor < 0){ //move forward if stick pushed forward
       digitalWrite(RIGHT_MOTOR_DIR_PIN, LOW);
     } else {
       digitalWrite(RIGHT_MOTOR_DIR_PIN, HIGH);   
     }      
   } else {
     analogWrite(RIGHT_MOTOR_PWM_PIN, 0);  
   }
}

Loop Overview

check (if data is being sent) :
  store ( data -> str , until fully sent)
  update (time since last input)
  check (str is valid):
    update motor values to set with str
    send motor values
else:
  if (time since last input) > 500 msec :
    stop motors
end

The pseudocode loop above describe the basic flow of the program. If the arudino detects data being sent over serial the arduino stores all the data until the command has been fully sent. The time since last <input is updated and the input is check for validity. If the string is valid, the motor values are stored and then sent to the motors over pwm. The rest of this section will explain the program and further improvements.

Defines and Directives

#include <SoftwareSerial.h>

#define THRESHOLD        0.15
#define MAX_INPUT_LENGTH 16
//uncomment for debug statements
#define DEBUG

//pin defines
#define LEFT_MOTOR_PWM_PIN    6
#define LEFT_MOTOR_DIR_PIN    8
#define RIGHT_MOTOR_PWM_PIN   5
#define RIGHT_MOTOR_DIR_PIN   7

The pin definitions for PWM and Direction to the MD10C motor controllers are placed as preprocesser directives for ease of readability in the program. THRESHOLD is used to determine if the joysticks are centered and MAX_INPUT_LENGTH is used to check if the string sent over serial is of the correct length.

Globals and Structs

struct MotorPower{
  float leftMotor;
  float rightMotor;
};

//global values
struct MotorPower motorPower;
unsigned long lastUpdateTime; //to keep track of time of last input

...

String validCharacters = "LR:|1234567890.-;";

motorPower is used as a global struct to be used in the helper functions and lastUpdateTime is used for our input watchdog. validCharacters is used in string validation.

String Validation

bool isValidChar(char c);
bool isValidInput(String input);

//returns false if char not found in string
bool isValidChar(char c){
  for(int i =0; i < validCharacters.length(); i++){
    if(c == validCharacters.charAt(i)){
      return true;
    }
  }
  return false;
}

bool isValidInput(String input){  
   //go through each character in the data string
  if(input.length() > MAX_INPUT_LENGTH){
    #ifdef DEBUG
    Serial.print("TOO long of input!\n");
    #endif
    return false;
  }
  for(int i = 0; i < input.length(); i++){
    if(isValidChar(input.charAt(i)) == false){
      //char not valid!
      #ifdef DEBUG
      Serial.print("char not valid!\n");
      #endif
      return false;
    }
  } //end of i for
  return true;
}

String validation is done in 2 parts. The string is checked using isValidInput() before passing off to parseData(). isValidInput first checks if the input string is of correct length and then checks each character against the global validCharacters string using the isValidChar() helper function. isValidChar() returns false if the character is not found in the global validCharacters string. If any of the character returns false from isValidChar() , isValidInput() returns false.

Motor Control & Thresholding

In loop() after recieving a valid string the parseData converts the string into usable data which updates the global motorPower struct using updateMotors() and sends the updated motor values to the MD10C motor controllers using driveMotors().

void parseData(String data){
  String leftMotorString = "";
  String rightMotorString = "";
  int state = 0;
  for(int i = 0; i < data.length(); i++){
    char c = data.charAt(i);
    if(c == 'L'){
      state = 0;
      continue;
    }
    if(c == 'R'){
      state = 1;
      continue;
    }
    if(c == ':' || c =='|' || c == ';'){
      continue;
    }
    
    switch(state) {
      case 0:
        // parsing leftMotorString data
        leftMotorString.concat(data.charAt(i));
        break;
      case 1:
        //parsing rightMotorString data
        rightMotorString.concat(data.charAt(i));        
        break;
      default:
        //don't do anything
        break;
    }
  } //end of for loop
  updateMotors(leftMotorString, rightMotorString);
}

void stopMotors(){
  motorPower.leftMotor = 0;
  motorPower.rightMotor = 0;
  
  //stop motors with pwm signal of 0
  analogWrite(LEFT_MOTOR_PWM_PIN, 0);  
  analogWrite(RIGHT_MOTOR_PWM_PIN, 0);     
}

void updateMotors(String lpower, String rpower){
  //creating char arrays since .toCharArray needs an array to write to
  int sizeOfArray = 8;
  char leftPowerCharArray[sizeOfArray];
  char rightPowerCharArray[sizeOfArray];
  lpower.toCharArray(leftPowerCharArray, sizeOfArray);
  rpower.toCharArray(rightPowerCharArray, sizeOfArray);
  
  motorPower.leftMotor = atof(leftPowerCharArray);  
  motorPower.rightMotor = atof(rightPowerCharArray);
}

void driveMotors(){
  #ifdef DEBUG
  Serial.print("Left Power: ");
  Serial.print(motorPower.leftMotor);
  Serial.print("\t\tRightPower: ");
  Serial.print(motorPower.rightMotor);
  Serial.print("\n");
  #endif
  
  //Left Drive
   if(abs(motorPower.leftMotor) > THRESHOLD){
     int LeftMotorPWM = map(abs(motorPower.leftMotor * 100), 0, 100, 0, 255);
     analogWrite(LEFT_MOTOR_PWM_PIN, LeftMotorPWM);
   
     if(motorPower.leftMotor < 0){ //move forward if stick pushed forward
       
       digitalWrite(LEFT_MOTOR_DIR_PIN, HIGH);
     } else {
       digitalWrite(LEFT_MOTOR_DIR_PIN, LOW);   
     }   
   } else {
     analogWrite(LEFT_MOTOR_PWM_PIN, 0);  
   }
  
  //Right Drive
  
   if(abs(motorPower.rightMotor) > THRESHOLD){
     int RightMotorPWM = map(abs(motorPower.rightMotor * 100), 0, 100, 0, 255);
     analogWrite(RIGHT_MOTOR_PWM_PIN, RightMotorPWM);    
   
     if(motorPower.rightMotor < 0){ //move forward if stick pushed forward
       digitalWrite(RIGHT_MOTOR_DIR_PIN, LOW);
     } else {
       digitalWrite(RIGHT_MOTOR_DIR_PIN, HIGH);   
     }      
   } else {
     analogWrite(RIGHT_MOTOR_PWM_PIN, 0);  
   }
}

parseData()

parseData() uses a finite state machine to keep track of which part of the string it is in. If a specifying character ('L' or 'R') is read, the state changes and the for loop continues and skips the switch-case below it. When the character read is not a delimiter ('|', ':', ';', '') or a specifying character the code appends to the appropriate motor string to be used in the updateMotors() function.

void parseData(String data){
  String leftMotorString = "";
  String rightMotorString = "";
  int state = 0;
  for(int i = 0; i < data.length(); i++){
    char c = data.charAt(i);
    if(c == 'L'){
      state = 0;
      continue;
    }
    if(c == 'R'){
      state = 1;
      continue;
    }
    if(c == ':' || c =='|' || c == ';'){
      continue;
    }
    
    switch(state) {
      case 0:
        // parsing leftMotorString data
        leftMotorString.concat(data.charAt(i));
        break;
      case 1:
        //parsing rightMotorString data
        rightMotorString.concat(data.charAt(i));        
        break;
      default:
        //don't do anything
        break;
    }
  } //end of for loop
  updateMotors(leftMotorString, rightMotorString);
}

updateMotors()

updateMotors() converts the string into a char array so the atof() function (converts char arrays to float values) can be used to set the global motorPower struct.

void updateMotors(String lpower, String rpower){
  //creating char arrays since .toCharArray needs an array to write to
  int sizeOfArray = 8;
  char leftPowerCharArray[sizeOfArray];
  char rightPowerCharArray[sizeOfArray];
  lpower.toCharArray(leftPowerCharArray, sizeOfArray);
  rpower.toCharArray(rightPowerCharArray, sizeOfArray);
  
  motorPower.leftMotor = atof(leftPowerCharArray);  
  motorPower.rightMotor = atof(rightPowerCharArray);
}

driveMotors() and Thresholding

void driveMotors(){
  #ifdef DEBUG
  Serial.print("Left Power: ");
  Serial.print(motorPower.leftMotor);
  Serial.print("\t\tRightPower: ");
  Serial.print(motorPower.rightMotor);
  Serial.print("\n");
  #endif
  
  //Left Drive
   if(abs(motorPower.leftMotor) > THRESHOLD){
     int LeftMotorPWM = map(abs(motorPower.leftMotor * 100), 0, 100, 0, 255);
     analogWrite(LEFT_MOTOR_PWM_PIN, LeftMotorPWM);
   
     if(motorPower.leftMotor < 0){ //move forward if stick pushed forward
       
       digitalWrite(LEFT_MOTOR_DIR_PIN, HIGH);
     } else {
       digitalWrite(LEFT_MOTOR_DIR_PIN, LOW);   
     }   
   } else {
     analogWrite(LEFT_MOTOR_PWM_PIN, 0);  
   }
  
  //Right Drive
  
   if(abs(motorPower.rightMotor) > THRESHOLD){
     int RightMotorPWM = map(abs(motorPower.rightMotor * 100), 0, 100, 0, 255);
     analogWrite(RIGHT_MOTOR_PWM_PIN, RightMotorPWM);    
   
     if(motorPower.rightMotor < 0){ //move forward if stick pushed forward
       digitalWrite(RIGHT_MOTOR_DIR_PIN, LOW);
     } else {
       digitalWrite(RIGHT_MOTOR_DIR_PIN, HIGH);   
     }      
   } else {
     analogWrite(RIGHT_MOTOR_PWM_PIN, 0);  
   }
}

driveMotors() first checks to see if the motor power levels are greater than THRESHOLD before send the pwm signal to the MD10C motor controllers. If, however, the motor power level is less than THRESHOLD, the motor power sent to the motor controllers is 0.

if(abs(motorPower.<motor>) > THRESHOLD){
  ...
  //set motor power code
  ...
} else {
 analogWrite(<motor pin>, 0);  
}

driveMotors() uses the global motorPower struct and scales the values to the appropriate PWM values using the map() function. map(<input>, <input low>, <input high>, <output low>, <output high>) scales the values of the input from 0-100 to 0-255. The motor power levels are multiplied by 100 since map() uses integers for scaling.

int LeftMotorPWM = map(abs(motorPower.leftMotor * 100), 0, 100, 0, 255);
int RightMotorPWM = map(abs(motorPower.rightMotor * 100), 0, 100, 0, 255);

The sign of the motor values is also checked in order to set the appropriate DIR signal (LOW or HIGH) to the motor controllers.

if(motorPower.leftMotor < 0){ //move forward if stick pushed forward
 digitalWrite(LEFT_MOTOR_DIR_PIN, HIGH);
} else {
 digitalWrite(LEFT_MOTOR_DIR_PIN, LOW);   
}   

if(motorPower.rightMotor < 0){ //move forward if stick pushed forward
 digitalWrite(RIGHT_MOTOR_DIR_PIN, LOW);
} else {
 digitalWrite(RIGHT_MOTOR_DIR_PIN, HIGH);   
}

Input Watchdog

void updateTime(){
  lastUpdateTime = millis();
}

void loop() {
  ...
  if(mySerial.available()){
    ...
    updateTime();
    ...
    //code to check input validation and send motor commands
  } else {
    if((millis() - lastUpdateTime) >  500){
      ...
      //code to stop motors
      ...
    }
  }
}

The input watchdog is simply the timer that gets updated with updateTime(). updateTime() is called every time a new input is sent. If no new input is sent (i.e. mySerial.available() returns false) then we check if we have not recieved new data for more than 500 milliseconds with if(millis() - lastUpdateTime) > 500). If no new inputs have been recieved for longer than 500 milliseconds then we send the command to stop the motors.

Arduino Code - UCSD-E4E/Wolf-Tracker-2014 GitHub Wiki

Program Overview

Contents

Arduino Code

Loop Overview

Defines and Directives

Globals and Structs

String Validation

Motor Control & Thresholding

parseData()

updateMotors()

driveMotors() and Thresholding

Input Watchdog

⚠️ GitHub.com Fallback ⚠️

Arduino Code - UCSD-E4E/Wolf-Tracker-2014 GitHub Wiki

Program Overview

Contents

Arduino Code

Loop Overview

Defines and Directives

Globals and Structs

String Validation

Motor Control & Thresholding

parseData()

updateMotors()

driveMotors() and Thresholding

Input Watchdog

⚠️ **GitHub.com Fallback** ⚠️

⚠️ GitHub.com Fallback ⚠️