Cart's on-board Arduino Mega 2560 code

Note that the communication commands are supported by the Arduino Wi Fi shield (V.1) library.

Note that the motor control commands are supported by the Arduino motor shield (V.1) library.




//
//
//  ********* USE IDE VERSION 1.5.2 ********  ( V 1.6.3 gives pasword recognition failures )
//
//  This is WiFi control software for a soap box kartie-type RC vehicle (by IEC, May, 2014 - Apr. 2015).
//  This is the on-board server component, not the remote controlling client component.
//  This code programs an Arduino Mega ADK board fitted with WiFi and motor control shields.
//  The comms. core is a much extended form of Mellis and Igoe's simple chat server example.
//  This version incorporates the following added features:
//     improved readability over A06_SC by removing part of the control loop into a function;
//     supports proportional control (so most earlier press-button functions are removed;
//     supports kickstarting motors from the idle state;
//     continuously tests end-to-end connection and prevents runaway when connection is lost;
//     supports optional automatic restart after a lost WiFi connection;
//     supports a small LED array and a piezo buzzer for system state indications and warnings;
//     supports an LCD text display which reports connection status, failures & battery voltage;
//     provides a battery status check and voltage read out at start up and thereafter;
//     frequently checks the battery voltage, warns when a recharge is necessary and then cuts power;
//     incorporates a battery saving function which cuts power completely after 90 seconds of idle time.
//  Note 1. In the file name Kartibot_A07, the 'A' referes to 'no motor encoder feedback'.
//  Note 2. *** This version for SITECOM router ***
//  **** Works with Android client WiFi_Arduino_01P ***
//      (Corresponding VB client is KartwinP1)
//  Note 3. See note about WiFi status at foot of code
//
//  **** See IDE note at top ****

// Arduino library inclusions
#include 
#include 
#include 
#include 
#include 
#include  //for LCD
//#include 

//Function declarations
void setup();  // Arduino standard
void loop();   // Arduino standard
void blinkit(int led, int milli, int rept);
void blinkem(int led1, int led2, int milli, int rept);
void printWifiStatus();
void LCD_clear();
void ParseForCommands(String commandString);
void setMotors(int left,  int right);
void startLeftMotor(int speedLM);
void startRightMotor(int speedRM);
void Motors_AB_brake();
void Stop_loop(int source);
void Power_down();
void Shut_down();
void Reset_prog();
int stringToInt(String string);
String floatToString(float volt);
int BatteryCheck(int mode);
int setupTimer();

// Global variables
char ssid[] = "CartDemo";    // Router SSID
char pass[] = "medtracym";   // Router password
int keyIndex = 1;            // your network key Index number (needed only for WEP)
int serialOn;  // Controls serial output ( 1 is on and 0 is off - see setup())
SoftwareSerial LCD (47, 48); // RX, TX 
boolean enableRestart;  // Controls automatic restart after lost WiFi connection - see Setup()
int pinA = 31;  // green
int pinB = 33;  // yellow
int pinC = 35;  // red
int pinD = 37;  // beep
int PinE = 43;  // power control relay NEW
int pinV = A3;  // battery voltage sensor
int batteryState = 0;
float batteryFactor = 0.0136;  // 12.6 V charged batt. / 927 analogue pin measured @ full charge
float batteryVolt = 0.0;
float batteryLow = 11.6;  // Worst case: 2 cells full & one critical ie 4.3, 4.3, 3.0 total = 11.6
int lowSpeed = 96; //was 46
int highSpeed = 128;
int halfSpeed = lowSpeed; 
int fullSpeed = highSpeed;
char clientAlive = 127;  //  Not in  use
int currentSensorA = A0, currentSensorB = A1; // use with 'analogRead()'
unsigned long k = 0, c = 0;  // counters
long Then = 0, lapse = 0, maxlapse = 0, triglapse; // see setup
char thisChar = 0;
int Motor_A_state = 0;  // > 0 is forward, == 0 is stopped, < 0 is reversed
int Motor_B_state = 0;  // as above
int Motors_AB_spin = 0; // 0 if not bogie spinning, 1 if bogie is spinning
int lastLeftSpeed = 0;
int lastRightSpeed = 0;
boolean turboState = true;
boolean blinkAllowed = true;
int status = WL_IDLE_STATUS;
String instring;
String keepAlive = "&abcdefghijklmnopqrstuvwxyz"; // Char & retained as back compat.
IPAddress ip; 
WiFiServer server(23);
boolean alreadyConnected = false; // shows whether or not the client was connected previously
int timer5_counter = 0;
int interruptCounter = 0;
boolean idleAllowanceExpired = true;   //  Will be tested at first interrupt
  
//Prepare for control via WiFi and Internet protocol
void setup() {
  setupTimer5(); // set up the battery check interrupt timer (too messy to do it here!)
  serialOn = 0;  // 0 is OFF and 1 is ON
  enableRestart = false;
  triglapse = 2250; // connection time out test in milliseconds (1st iteration time is longer)
  pinMode(pinA, OUTPUT);
  pinMode(pinB, OUTPUT);
  pinMode(pinC, OUTPUT);
  pinMode(pinD, OUTPUT);
  pinMode(PinE, OUTPUT);  // NEW  
  digitalWrite(pinA, LOW);     // Green    off
  digitalWrite(pinB, LOW);     // Yellow   off
  digitalWrite(pinC, HIGH);    // Red      on
  digitalWrite(pinD, LOW);     // Beep     off
  digitalWrite(PinE, HIGH);    // Switch power relay on  NEW
  LCD.begin(9600); 
  delay(500); 
  LCD_clear(); 
  LCD.write("Initializing ..."); 
  //Initialize serial and wait for port to open:    
  if (serialOn == 1) {  
    Serial.begin(9600); 
    while (!Serial) {;} // wait for serial port to connect. Needed for Leonardo only
  }
  // battery has now been in use a little so check the voltage
  BatteryCheck(1);
  delay(5000);  //  allow time to read LCD
  // check for the presence of the shield: 
  char stries[2] = "";
  int maxtries = 5;
  int ntries = 0;
  while(ntries < 1) {
    ntries++;
    sprintf(stries, "%2d", ntries);
    LCD_clear(); 
    LCD.write("Finding on-board");
    LCD.write(254); LCD.write(192); 
    LCD.write("WiFi shield...");
    LCD.write(254); LCD.write(206); 
    LCD.write(stries);  // number of tries to find WiFi shield
    delay(5000); // To read display
    if(WiFi.status() != WL_NO_SHIELD) break;
    BatteryCheck(0); // this loop can hang therefore make frequent checks    
    if(ntries > maxtries) {
      LCD_clear(); 
      LCD.write("On-board WiFi");
      LCD.write(254); LCD.write(192); 
      LCD.write("not found...");
      LCD.write(254); LCD.write(206); 
      LCD.write(stries); 
      BatteryCheck(0);  // No delay here because only the low voltage danger will be displayed
      if (serialOn == 1) Serial.println("WiFi shield not present");
      while(true);  //  Failed, so loop for ever
      }
  }
  
  //blinkit(pinA, 100, 10);
  if(serialOn == 1) {Serial.println("Status: " + String(status));}
  // attempt to connect to Wifi network:
  // *** this code was changed to a loop because a single attempt often failed
  while ( status != WL_CONNECTED) { 
     LCD_clear();
     LCD.write("WiFi found.  Now"); 
     LCD.write(254); LCD.write(192); 
     LCD.write("seeking router.."); 
     digitalWrite(pinB, HIGH);     // Yellow   on
     digitalWrite(pinC, LOW);      // Red      offe    
     if (serialOn ==1) {
     Serial.print("Attempting to connect to SSID: ");
     Serial.println(ssid);
     }
     // Connect to WPA/WPA2 network. Change this line if using open or WEP network:
     status = WiFi.begin(ssid, pass);
     //status = WiFi.begin(ssid, keyIndex, key);
    // wait ? seconds for connection:
    delay(1000); //was 10000
  } 
  
  // start the server:
  server.begin();
  // you're connected now, so print out the status:
  LCD_clear(); //**** 
  LCD.write("Router found ..."); 
  LCD.write(254); LCD.write(192); 
  LCD.write("Awaiting address"); 
  delay(6000);  // To read display 
  if (serialOn ==1) {printWifiStatus();}
  LCD_clear(); //****
  ip = WiFi.localIP(); 
  LCD.write("This address is "); 
  LCD.write(254); LCD.write(192);  
  LCD.write("                ");
  LCD.write(254); LCD.write(192);   
  LCD.print(ip); 
  
  // Visual indication of readiness
  digitalWrite(pinB, LOW);
  blinkit(pinA, 250, 4);
  digitalWrite(pinA, HIGH);  //  Turn on green indicator
  
  // Setup motors
  
 //Setup Channel A
  pinMode(12, OUTPUT); //Initiates Motor Channel A pin
  pinMode(9, OUTPUT); //Initiates Brake Channel A pin

  //Setup Channel B
  pinMode(13, OUTPUT); //Initiates Motor Channel B pin
  pinMode(8, OUTPUT);  //Initiates Brake Channel B pin 
  
  //setupTimer5() was formerly executed at this point
  
  if (serialOn == 1) Serial.begin(9600);
  
 }

// Start of main control loop *******************************************
// The rather complex structure of this loop is based on the original Arduino WiFi example code
void loop() {
  k++;
  
  // wait for a new client:
  WiFiClient client = server.available();
  
  // when the client sends the first byte, say hello:
  if (client) {
    if (!alreadyConnected) {
      // clean out the input buffer:
      client.flush();    
      if(serialOn == 1) {Serial.println("We have a new client");}
      client.println("Hello driver");  // Originally 'Hello client'
      alreadyConnected = true;
      k = 0;
      c = 0;
      LCD_clear();
      LCD.write("Client connected"); 
      LCD.write(254); LCD.write(192);   
      LCD.write("                "); 
      LCD.write(254); LCD.write(192); 
      LCD.print(ip);
    }
  
  // Test response time of client and if too great assume that contact was lost
  if (c > 1) {
    lapse = millis() - Then;
    //if(lapse > maxlapse) maxlapse = lapse;  //  used for diagnosis
    //Serial.print("maxlapse = "); Serial.println(maxlapse);
    if(lapse > triglapse) {Stop_loop(1);}
    //triglapse = 1000; // (was2250;)  //  reduced to this value after first loop-through
    triglapse = 2000;
  }    
    if (client.available() > 0) {
      blinkHeart();  // Produce visible heart beat
      Then = millis();  //  Update the latest time that data was received
      // read the bytes incoming from the client:
      c++;
      thisChar = client.read();
      //echo the bytes back to the client:
      //server.write(thisChar);  produces strange ressults with vb client
      client.println(char(thisChar)); //  Unofficial but more reliable than server.write()
      //if(thisChar != '&'){  // Look for non-keepAlive char 
      if(keepAlive.indexOf(thisChar) == -1){  // Replacement to allow more flexibility   
      instring += thisChar;  // Build command string until terminator ^ is found
      if (thisChar == '^') ParseForCommands(instring); // String completed and sent to parse     
      }
      
    else // it's a keep alive character
      if(serialOn == 1) {
        Serial.print(thisChar); Serial.print("   ");Serial.println(millis());
        batteryState = analogRead(pinV);
        batteryVolt = batteryState * batteryFactor;
        int intVolt = batteryVolt * 100;
        String printVolt = String(intVolt);
        Serial.print("batteryState = ");  Serial.println(batteryState);
        Serial.print("batteryVolt = ");  Serial.println(batteryVolt);
        Serial.print("stringVolt = ");  Serial.println(printVolt);
      }
      
      //Serial.print("lapse    = "); Serial.println(lapse);
      //Serial.print("triglapse = "); Serial.println(triglapse);
      //Serial.print("BA   "); Serial.println(blinkAllowed);
      Serial.print("Motor A current = "); Serial.println(analogRead(A0));
      Serial.print("Motor B current = "); Serial.println(analogRead(A1));
    }
    
  } // End of top 'if'
  
  //  Stop motors if client was connected but becomes disonnected (fail safe strategy)
  // was if(!client.connected() && alreadyConnected) {
  //if(!client && alreadyConnected) {
  
  else {
    if (alreadyConnected) {  
      if (serialOn == 1) {Serial.println(String(k) + "   " + "Connection lost !");}
      LCD_clear();
      LCD.write("Connection lost!"); 
      LCD.write(254); LCD.write(192); 
      LCD.write(254); LCD.write(192);  
      LCD.write("                "); 
      LCD.print(ip);
      Stop_loop(2);    
    } 
  }
  
}  // End of main loop **********************************

// Parse command string for motor control actions
void ParseForCommands(String commandString) {
  
      int len = commandString.length();
      char a[len];
      commandString.toCharArray(a,len);
     
      String b = strtok(a,"`");
      String c = strtok(NULL, "`");
      String d = strtok(NULL,"`");
      b.trim();   // Found to be needed for Sitecom router
      
       if(b.indexOf('C') > -1) {
        idleAllowanceExpired = false;  // This was a valid command so renew idle allowance
        //  Added to avoid unwanted battery saving time-out shut-downs (currently set at 1 minute)
        blinkit(pinC,20,1);
      }
      
      if(b.indexOf('S') > -1) {
        idleAllowanceExpired = false;  // This was a valid command so renew idle allowance
        Motors_AB_brake();
        if(serialOn == 1) {Serial.println("Stop");}
        //blinkAllowed = true;
      }
      
      if(b == "P") {
        idleAllowanceExpired = false;  // This was a valid command so renew idle allowance
        //blinkAllowed = false;
        Serial.print("BA   "); Serial.println(blinkAllowed);
        int leftSpeed = stringToInt(c);
        int rightSpeed = stringToInt(d);
        if(serialOn == 1) {
          Serial.print("Speeds:   ");
          Serial.print(leftSpeed);
          Serial.print("  ");
          Serial.println(rightSpeed);
          }
        setMotors(leftSpeed, rightSpeed);
      } 
      
      if(b == "T") {
         idleAllowanceExpired = false;  // This was a valid command so renew idle allowance
         //Serial.print("BA   "); Serial.println(blinkAllowed);
         //if(blinkAllowed == true) blinkit(pinB, 150, 10); // upsets time out test when not allowed

         blinkit(pinB, 150, 10);
      } 
      
      instring = "";  
  
}  

// Communication parameter functions ***************************

void blinkHeart() {
  digitalWrite(pinB, HIGH);
  delay(20);
  digitalWrite(pinB, LOW);  
}

void blinkit(int led, int milli, int rept) {
  triglapse = 3250;
  for (int j = 0; j < rept; j++) {
    digitalWrite(led, HIGH);
    digitalWrite(pinD, HIGH); // beep on
    delay(milli);              
    digitalWrite(led, LOW);
    digitalWrite(pinD, LOW); // beep off   
    delay(milli);    
  }
}

void blinkem(int led1, int led2, int milli, int rept) {
  for (int j = 0; j < rept; j++) {
    digitalWrite(led1, HIGH);   
    delay(milli);    
    digitalWrite(led1, LOW);   
    delay(milli);
    digitalWrite(led2, HIGH);   
    delay(milli);               
    digitalWrite(led2, LOW);   
    delay(milli); 
  }
}

void printWifiStatus() {
  // print the SSID of the network you're attached to:
  if(serialOn == 1) {
    Serial.print("SSID: ");
    Serial.println(WiFi.SSID());
  }
  // print your WiFi shield's IP address:
  IPAddress ip = WiFi.localIP();
  if(serialOn == 1) {
    Serial.print("IP Address: ");
    Serial.println(ip);
  }
  // print the received signal strength:
  long rssi = WiFi.RSSI();
  if(serialOn == 1) {
    Serial.print("signal strength (RSSI):");
    Serial.print(rssi);
    Serial.println(" dBm");
  }
}

// Motor functions ************************************
 
 void Motors_AB_brake() {
  // Brake and cut both motors 
  digitalWrite(9, HIGH);  //Engage the Brake for Channel A
  digitalWrite(8, HIGH);  //Engage the Brake for Channel B
  //delay(1000);  //  Causes unwanted shut down
  digitalWrite(3, 0);  //Set speed to zero for Channel A
  digitalWrite(11, 0);  //Set speed to zero for Channel B
  Motor_A_state = 0;
  Motor_B_state = 0;
  Motors_AB_spin = 0;
  halfSpeed = lowSpeed;
  turboState = true;
  lastLeftSpeed = 0;  // Necessary to enable kickstart
  lastRightSpeed = 0;  
 }
 
 void Stop_loop(int source){
   
   if (source == 1) {
     // This is a high level time-out so cause is probably a lost client/router connection
     LCD_clear(); 
     LCD.write("Client to router"); 
     LCD.write(254); LCD.write(192); 
     LCD.print("connection lost"); 
     if(serialOn == 1) {
     Serial.println("Stop_loop source = 1");
     Serial.print("triglapse = "); Serial.println(triglapse);
     Serial.print("lapse    = "); Serial.println(lapse);
     } 
   }
   
   if(source == 2) {
     // This is detected by the WiFi shield so it is probably a lost router/server connection 
     LCD_clear(); 
     LCD.write("Server to router"); 
     LCD.write(254); LCD.write(192); 
     LCD.print("connection lost");
     delay(5000);  // to read display            
   }
   
   Shut_down(source); // Enter paralysis loop
   
 }
 
 void Shut_down(int source) { 
   Motors_AB_brake();
   digitalWrite(pinA, LOW);   // Turn off green indicator.
   WiFi.disconnect();  // Prevents client from hanging.
   if (enableRestart && source == 2) Reset_prog(); 
   // Loop until reset or power-down. Stops runaway when connection lost & reset not called.
   while(true) {
     BatteryCheck(0);  
     digitalWrite(pinC, HIGH);  // Flash the red LED.
     delay(750);
     digitalWrite(pinC, LOW);
     delay(750);
   }
   
 }
 
 void Power_down() {  //  Warning followed by relay switching off all power
   
   interrupts();  //  Blink function does not work with interrupts enabled
   digitalWrite(pinA, LOW); // Green
   blinkit(pinB, 350, 8);  // Yellow
   digitalWrite(pinC, HIGH); // Red
   delay(5000);
   digitalWrite(PinE, LOW);  // Set relay control pin LOW to cut power
   
 }  
 
  void Reset_prog() {
    
    asm volatile ("  jmp 0");  // reset function @ address 0
    
  }
 
 //void(* Reset_prog) (void) = 0; //  Alternative syntax for reset function
 
 // Action routine for proportional steering
 void setMotors(int left,  int right) {
    
    lastLeftSpeed = left;
    lastRightSpeed = right;
   
    right = right + 3;    // to balance motors especially at low speed
  
    boolean dirA, dirB;
    if(right < 0) dirA = LOW; else dirA = HIGH;
    if(left < 0) dirB = LOW; else dirB = HIGH;
    left = abs(left); right = abs(right);
  
    digitalWrite(12, dirA); //Establishes forward direction of Channel A
    digitalWrite(9, LOW);   //Disengage the Brake for Channel A
    //analogWrite(3, right);   //Spins the motor on Channel A at half speed
    startRightMotor(right);
  
    digitalWrite(13, dirB); //Establishes forward direction of Channel B
    digitalWrite(8, LOW);   //Disengage the Brake for Channel B
    //analogWrite(11, left);   //Spins the motor on Channel B at full speed
    startLeftMotor(left);
    
 }
 
 void startLeftMotor(int speedLM){
   int joltLeft = 127;
   if (speedLM > 127) joltLeft = 255;
   if (lastLeftSpeed == 0) {             // Kickstart (is a short pulse of full power)
     analogWrite(11, joltLeft);
     delay(50); // Length of kickstart pulse (milliseconds)  - was 75
   }
   analogWrite(11, speedLM);
 }
 
 void startRightMotor(int speedRM){
   int joltRight = 127;
   if (speedRM > 127) joltRight = 255;
   if (lastRightSpeed == 0) {            // Kickstart (is a short pulse of full power)
     analogWrite(3, joltRight);
     delay(75);
   }
   analogWrite(3, speedRM);
 }
  
 //  Utility functions ****************************************
   
 // convert arduino String to int
  int stringToInt(String string){
    char char_string[string.length()+1];
    string.toCharArray(char_string, string.length()+1);
    return atoi(char_string);
}

 // convert Arduino float to String (not tested for general use)
  String floatToString(float volt) {
    int intVolt = volt * 100;
    String strVolt = String(intVolt);
    String tailVolt = strVolt.substring(strVolt.length() - 2);
    String headVolt = String(int(volt));
    String myVolt = headVolt + "." + tailVolt;
    return(myVolt); 
 }

 // clear the LCD screen LCD
  void LCD_clear() {
    LCD.write(254); // move cursor to beginning of first line
    LCD.write(128);
    LCD.write("                "); // clear display
    LCD.write("                ");
    LCD.write(254); // move cursor to beginning of first line ...
    LCD.write(128); // ready for next writing of data
  }

 // Find the voltage of the battery (mode controls display)
 int BatteryCheck(int mode) {
    batteryState = analogRead(pinV);
    batteryVolt = batteryState * batteryFactor;
    // if the voltage is < 5 the battery is very probably turned off and ...
    // the system is running on USB power, therefore ...
    //if(batteryVolt < 5) {return(-1)};
    if (mode == 1) {
      String batteryString = floatToString(batteryVolt);      
      batteryString += " volts - OK";
      LCD_clear();
      LCD.write("Battery check..."); 
      LCD.write(254); LCD.write(192); 
      LCD.print(batteryString);
    }
    if(batteryVolt < batteryLow  && batteryVolt >= 5) {
      //  Stop motors and warn user by text, lights and sound
      Motors_AB_brake();
      String batteryString = floatToString(batteryVolt); 
      batteryString += " v. too LOW";
      LCD_clear();
      LCD.write("CHARGE BATTERY !"); 
      LCD.write(254); LCD.write(192); 
      LCD.print(batteryString);
      interrupts(); // Turn off any interrupts to allow delay function to work ok
      //  loop until manual power down
      //  Changed this to loop for 1 minute then cut power completely
      // while(true) { 
      for(int j=0; j<120; j++) {  
        digitalWrite(pinB, HIGH);  // yellow 
        digitalWrite(pinC, HIGH);  // red
        digitalWrite(pinD, LOW);   // beep
        delay(250); 
        digitalWrite(pinB, LOW);   // yellow 
        digitalWrite(pinC, LOW);   // red
        digitalWrite(pinD, HIGH);  // beep
        delay(250); 
      }
     
     Power_down();
      
    }
 } 
  
 // Timer interrupt settings & service routine *************************************
 
   void setupTimer5() {
  // initialize timer5 (for use with battery check)
  noInterrupts();           // disable all interrupts
  TCCR5A = 0;
  TCCR5B = 0;
  // Set timer5_counter to the correct value for our interrupt interval
  timer5_counter = 3036;     // preload timer 65536-16MHz/256/1Hz
  TCNT5 = timer5_counter;   // preload timer
  TCCR5B |= (1 << CS12);    // 256 prescaler;  triggers once per second 
  TIMSK5 |= (1 << TOIE5);   // enable timer overflow interrupt
  interrupts();             // enable all interrupts
}

ISR(TIMER5_OVF_vect) {
  // interrupt service routine
  TCNT5 = timer5_counter;   // preload timer
  interruptCounter ++;
  if(interruptCounter == 90) {   // trigger once every 90 approx. seconds
      interruptCounter = 0;
      if(idleAllowanceExpired) {Power_down();} 
      idleAllowanceExpired = true;   //  To prevent power down falsify this before next interrupt
      BatteryCheck(0); // check every time (in silent mode)
  }
}
   
// End of code  **********************************************


/*  NOTE about WiFi status
http://forum.arduino.cc/index.php?topic=162674.0 
This is from /arduino-1.0.5/libraries/WiFi/utility/wl_definitions.h

typedef enum {
      WL_NO_SHIELD = 255,
        WL_IDLE_STATUS = 0,
        WL_NO_SSID_AVAIL,
        WL_SCAN_COMPLETED,
        WL_CONNECTED,
        WL_CONNECT_FAILED,
        WL_CONNECTION_LOST,
        WL_DISCONNECTED
} wl_status_t;

That means
WL_NO_SHIELD = 255,
WL_IDLE_STATUS = 0,
WL_NO_SSID_AVAIL = 1
WL_SCAN_COMPLETED = 2
WL_CONNECTED = 3
WL_CONNECT_FAILED = 4
WL_CONNECTION_LOST = 5
WL_DISCONNECTED = 6
 
End of note */