Figure 25 : Software Block Diagram

// creating our custom brake and boost commands

#define Boost 0x41
#define Brake 0x42
uint8_t boostset = 0x00;
static uint8_t brakeset = 0x00;
const uint8_t CMD_LIST_SIZE = 3; 

bool setbrake(uint8_t cmd, uint8_t param[], uint8_t n)
{
  if (param[0] == 0x01) brakeset = 0x01;
  else brakeset = 0x00;
  return false;
}

// boost handler
bool setBoost(uint8_t cmd, uint8_t param[], uint8_t n)
{
  if (param[0] == 0x01){
    digitalWrite(MISO, HIGH);
    boostset = 0x01;
  } else {
   digitalWrite(MISO, LOW);
   boostset = 0x00;
  }
  return false;
}

using our arxterra app we used our custom telemetry commands to implement out boost switch. This is used using boolean and is activated by a simple switch on the axterra app giving the ability to turn on and off the boost and auto brake features. After that we override the movehandler in order to get full control of the servo angle and motors controls.

// Set encoder as input with internal pullup
pinMode(SCL, INPUT_PULLUP);

// Attach interrupt
attachInterrupt(digitalPinToInterrupt(SCL), updateEncoderL, CHANGE);
pinMode(SDA, INPUT_PULLUP);

// Attach interrupt
attachInterrupt(digitalPinToInterrupt(SDA), updateEncoderR, CHANGE);

// Setup initial values for timer
previousMillis = millis();
previousMillis2 = millis();

we setup our encoders with a pullup in order to not get any noise and get more accurate readings. then we attach both of these to a interrupt so we can have constant count of the “ticks” sent by the encoder to be converted to rpm values.

// Update RPM value every second
currentMillis = millis();
if (currentMillis - previousMillis > interval) {
previousMillis = currentMillis;

// Calculate RPM
rpmL = (float)(encoderL * 126 / ENC_COUNT_REV);

// Only update display when there is a reading
if (rpmL > 0) {
 Serial.print(" PULSES: ");
 Serial.print(encoderL);
 Serial.print('\t');
 Serial.print(" SPEEDL: ");
 Serial.print(rpmL);
 Serial.println(" RPML");Serial.println(" w");
 Serial.println(W3);
 Serial.println(W4);
 Serial.println(rpm);
 Serial.println(xspeed1);
}
 encoderL = 0;
}

This code then takes the "ticks" counted and turns them into rpm values depending on the gear ratio.

// calculating rpm per motor differential speed

if(thetaset>FNB) answer = thetal * 3.14159 / 180;
// calculating rpm per motor differential speed
if(xspeed1==0) xspeed1=0;
else xspeed1=xspeed1-1;
   :
if (rpmR
Rpm of the motors coming from the encoder data is then compared with the calculated rpm values with a if statement and depending on if the motor is slower of fast then the desired rpm value then we will increase or decrease the speed of the motors.
Our final code include the parts for the differential drive and encoder reading but due to the inability to get both encoder to work simultaneously testing of the differential drive was difficult. But the code for differential drive calculation and  encoder reading work on their own. Encoder code issue one encoder is left without data when reading simultaneously.