 
 void RCmode()
 {
-  //------------------------------------------------------------ Code for RC inputs ---------------------------------------------------------
-  Speed=pulseIn(RCleft,HIGH,25000);                           // read throttle/left stick
-  Steer=pulseIn(RCright,HIGH,25000);                          // read steering/right stick
-
-  if (Speed==0) Speed=1500;                                   // if pulseIn times out (25mS) then set speed to stop
-  if (Steer==0) Steer=1500;                                   // if pulseIn times out (25mS) then set steer to centre
-
-  if (abs(Speed-1500)<RCdeadband) Speed=1500;                 // if Speed input is within deadband set to 1500 (1500uS=center position for most servos)
-  if (abs(Steer-1500)<RCdeadband) Steer=1500;                 // if Steer input is within deadband set to 1500 (1500uS=center position for most servos)
-
-  if (Mix==1)                                                 // Mixes speed and steering signals
-  {
+  //
+  // Code for RC inputs
+  //
+
+  // read throttle/left stick
+  Speed=pulseIn(RCleft,HIGH,25000);
+
+  // read steering/right stick
+  Steer=pulseIn(RCright,HIGH,25000);
+
+  // if pulseIn times out (25mS) then set speed to stop
+  if (Speed==0) Speed=1500;
+
+  // if pulseIn times out (25mS) then set steer to centre
+  if (Steer==0) Steer=1500;
+
+  // if Speed input is within deadband set to 1500 (1500uS=center position for most servos)
+  if (abs(Speed-1500)<RCdeadband) Speed=1500;
+
+  // if Steer input is within deadband set to 1500 (1500uS=center position for most servos)
+  if (abs(Steer-1500)<RCdeadband) Steer=1500;
+
+  if (Mix==1)                                                 
+  {
+    // Mixes speed and steering signals
     Steer=Steer-1500;
     Leftspeed=Speed-Steer;
     Rightspeed=Speed+Steer;
   }
-  else                                                        // Individual stick control
-  {
+  else                                                        
+  {
+    // Individual stick control
     Leftspeed=Speed;
     Rightspeed=Steer;
   }
-  
+
   /*
   Serial.print("Left:");
   Serial.println(Leftspeed);
   Serial.print("Right:");
   Serial.println(Rightspeed);
   */
- 
+
   Leftmode=2;
   Rightmode=2;
-  if (Leftspeed>(Leftcenter+RCdeadband)) Leftmode=0;          // if left input is forward then set left mode to forward
-  if (Rightspeed>(Rightcenter+RCdeadband)) Rightmode=0;       // if right input is forward then set right mode to forward
-
-  LeftPWM=abs(Leftspeed-Leftcenter)*10/scale;                 // scale 1000-2000uS to 0-255
-  LeftPWM=min(LeftPWM,255);                                   // set maximum limit 255
-
-  RightPWM=abs(Rightspeed-Rightcenter)*10/scale;              // scale 1000-2000uS to 0-255
-  RightPWM=min(RightPWM,255);                                 // set maximum limit 255
-}
-
-
-void SCmode()
-{// ------------------------------------------------------------ Code for Serial Communications --------------------------------------
-
-                                                              // FL = flush serial buffer
  
-                                                              // AN = report Analog inputs 1-5
-                                                              
-                                                              // SV = next 7 integers will be position information for servos 0-6
+  // if left input is forward then set left mode to forward
+  if (Leftspeed>(Leftcenter+RCdeadband)) Leftmode=0;
  
-                                                              // HB = "H" bridge data - next 4 bytes will be:
-                                                              //      left  motor mode 0-2
-                                                              //      left  motor PWM  0-255
-                                                              //      right motor mode 0-2
-                                                              //      right motor PWM  0-255
+  // if right input is forward then set right mode to forward
+  if (Rightspeed>(Rightcenter+RCdeadband)) Rightmode=0;
+
+  // scale 1000-2000uS to 0-255
+  LeftPWM=abs(Leftspeed-Leftcenter)*10/scale;
  
-  if (Serial.available()>1)                                   // command available
+  // set maximum limit 255
+  LeftPWM=min(LeftPWM,255);
+
+  // scale 1000-2000uS to 0-255
+  RightPWM=abs(Rightspeed-Rightcenter)*10/scale;
+ 
+  // set maximum limit 255
+  RightPWM=min(RightPWM,255);
+}
+
+//
+// Code for Serial Communications
+//
+// FL = flush serial buffer
+// AN = report Analog inputs 1-5
+// SV = next 7 integers will be position information for servos 0-6
+// HB = "H" bridge data - next 4 bytes will be:
+//      left  motor mode 0-2
+//      left  motor PWM  0-255
+//      right motor mode 0-2
+//      right motor PWM  0-255
+void SCmode() {
+  if (Serial.available()>1)
   {
     int A=Serial.read();
     int B=Serial.read();

    {
      if ((highVolts-Volts)>5 || (millis()-chargeTimer)>chargetimeout) // has voltage begun to drop or levelled out?
      {
        Charged=1;                                            // battery voltage has peaked
        digitalWrite(Charger,1);                             // turn off current regulator
        
        Serial.print("Charge complete after ");
        Serial.print((millis()-chargeStart)*1000 / 60);
        Serial.print(":");
        Serial.print((millis()-chargeStart)*1000 % 60);
        Serial.println("!");
        
        logBatteryVoltage();
      }
    } 
  }

  else

  {//----------------------------------------------------------- GOOD BATTERY speed controller opperates normally ----------------------

    switch(Cmode)
    {
    case 0:                                                   // RC mode via D0 and D1
      RCmode();
      break;

    case 1:                                                   // Serial mode via D0(RX) and D1(TX)
      SCmode();
      break;

    case 2:                                                   // I2C mode via A4(SDA) and A5(SCL)
      I2Cmode();
      break;
    }

    // --------------------------------------------------------- Code to drive dual "H" bridges --------------------------------------

    if (Charged==1)                                           // Only power motors if battery voltage is good
    {
      if ((millis()-leftoverload)>overloadtime)             
      {
        switch (Leftmode)                                     // if left motor has not overloaded recently
        {


  if (LeftAmps>Leftmaxamps)                                   // is motor current draw exceeding safe limit
  {
    // Turn off motors
    analogWrite (LmotorA,0);
    analogWrite (LmotorB,0);

    // Record time of overload
    leftoverload=millis();
  }

  if (RightAmps>Rightmaxamps)                                 // is motor current draw exceeding safe limit
  {
    // Turn off motors
    analogWrite (RmotorA,0);
    analogWrite (RmotorB,0);

    // Record time of overload
    rightoverload=millis();
  }

  if ((Volts<lowvolt) && (Charged==1))                        // check condition of the battery
  {                                                           // change battery status from charged to flat

    //
    // FLAT BATTERY speed controller shuts down until battery is recharged
    //
    //
    // This is a safety feature to prevent malfunction at low voltages!!
    //

    // Battery is flat
    Charged=0;

    // Record the voltage
    highVolts=Volts;
    startVolts=Volts;

    // Record the time
    chargeTimer=millis();

    // Record start time
    chargeStart=millis();

    Serial.print("Battery dead at ");
    Serial.print(Volts, DEC);
    Serial.println(", enabling charger...");

    // Enable current regulator to charge battery
    digitalWrite(Charger,0);
  }

  //
  // CHARGE BATTERY
  //

  if ((Charged==0) && (Volts-startVolts>67))                  // if battery is flat and charger has been connected (voltage has increased by at least 1V)
  {
    if (Volts>highVolts)                                      // has battery voltage increased?
    {
      // record the highest voltage. Used to detect peak charging.
      highVolts=Volts;

      // when voltage increases record the time
      chargeTimer=millis();
    }

    if ((millis()-reportTimer) > reportInterval) {
      logBatteryVoltage();
      reportTimer=millis();
