View difference between Paste ID: cFscTs6d and E4v3NSYR

 #include <Servo.h>
 #include "IOpins.h"
 #include "Constants.h"
 
 
-//-------------------------------------------------------------- define global variables --------------------------------------------
+//-------------------------------------------------------------- define global variables ---------------------
 
 unsigned int Volts;
 unsigned int LeftAmps;
 unsigned int RightAmps;
 unsigned long chargeTimer;
 unsigned long leftoverload;
 unsigned long rightoverload;
 int highVolts;
 int startVolts;
 int Leftspeed=0;
 int Rightspeed=0;
 int Speed;
 int Steer;
 byte Charged=1;                                               // 0=Flat battery  1=Charged battery
 int Leftmode=1;                                               // 0=reverse, 1=brake, 2=forward
 int Rightmode=1;                                              // 0=reverse, 1=brake, 2=forward
-byte Leftmodechange=0;                                        // Left input must be 1500 before brake or reverse can occur
+byte Leftmodechange=0;                                // Left input must be 1500 before brake or reverse can occur
-byte Rightmodechange=0;                                       // Right input must be 1500 before brake or reverse can occur
+byte Rightmodechange=0;                               // Right input must be 1500 before brake or reverse can occur
 int LeftPWM;                                                  // PWM value for left  motor speed / brake
 int RightPWM;                                                 // PWM value for right motor speed / brake
 int data;
 int servo[7];
 const int trigger = 2;                                        // aaa Define Ping Trigger as D2
 const int echo = 4;                                           // aaa Define Ping Echo D4
 
-//-------------------------------------------------------------- define servos ------------------------------------------------------
+//-------------------------------------------------------------- define servos --------------------------------
 
 
 Servo Servo0;                                                 // define servos
 Servo Servo1;                                                 // define servos
 Servo Servo2;                                                 // define servos
 Servo Servo3;                                                 // define servos
 Servo Servo4;                                                 // define servos
 Servo Servo5;                                                 // define servos
 Servo Servo6;                                                 // define servos
 
 void setup()
 {
-  //------------------------------------------------------------ Initialize Servos ----------------------------------------------------
+  //------------------------------------------------------------ Initialize Servos ------------------------------
 
   //Servo0.attach(S0);                                          // attach servo to I/O pin
   //Servo1.attach(S1);                                          // attach servo to I/O pin
   Servo2.attach(S2);                                          // attach servo to I/O pin
   Servo3.attach(S3);                                          // attach servo to I/O pin
   Servo4.attach(S4);                                          // attach servo to I/O pin
   Servo5.attach(S5);                                          // attach servo to I/O pin
   Servo6.attach(S6);                                          // attach servo to I/O pin
 
-  //------------------------------------------------------------ Set servos to default position ---------------------------------------
+  //------------------------------------------------------------ Set servos to default position -----------------
 
   Servo0.writeMicroseconds(DServo0);                          // set servo to default position
   Servo1.writeMicroseconds(DServo1);                          // set servo to default position
   Servo2.writeMicroseconds(DServo2);                          // set servo to default position
   Servo3.writeMicroseconds(DServo3);                          // set servo to default position
   Servo4.writeMicroseconds(DServo4);                          // set servo to default position
   Servo5.writeMicroseconds(DServo5);                          // set servo to default position
   Servo6.writeMicroseconds(DServo6);                          // set servo to default position
 
-  //------------------------------------------------------------ Initialize I/O pins --------------------------------------------------
+  //------------------------------------------------------------ Initialize I/O pins ---------------------------
 
   pinMode (Charger,OUTPUT);                                   // change Charger pin to output
   digitalWrite (Charger,1);                                   // disable current regulator to charge battery
 
   if (Cmode==1)
   {
     Serial.begin(Brate);                                      // enable serial communications if Cmode=1
     Serial.flush();                                           // flush buffer
   }
   //Serial.begin(57600);
 
-  //------------------------------------------------------------ Initilialize I/O Ping Sensor ----------------------------------------------
+  //------------------------------------------------------------ Initilialize I/O Ping Sensor -----------------
 
   pinMode(trigger,OUTPUT);                                    // aaa Set digital pin 2 to trigger output
   pinMode(echo,INPUT);                                        // aaa Set digital pin 4 to echo input
 }
 
 
 void loop()
 {
-  //------------------------------------------------------------ Check battery voltage and current draw of motors ---------------------
+  //---------------------------------------- Check battery voltage and current draw of motors ---------------
 
   Volts=analogRead(Battery);                                  // read the battery voltage
   LeftAmps=analogRead(LmotorC);                               // read left motor current draw
   RightAmps=analogRead(RmotorC);                              // read right motor current draw
 
   //Serial.print(LeftAmps);
   //Serial.print("    ");
   //Serial.println(RightAmps);
 
   if (LeftAmps>Leftmaxamps)                                   // is motor current draw exceeding safe limit
   {
     analogWrite (LmotorA,0);                                  // turn off motors
     analogWrite (LmotorB,0);                                  // turn off motors
     leftoverload=millis();                                    // record time of overload
   }
 
   if (RightAmps>Rightmaxamps)                                 // is motor current draw exceeding safe limit
   {
     analogWrite (RmotorA,0);                                  // turn off motors
     analogWrite (RmotorB,0);                                  // turn off motors
     rightoverload=millis();                                   // record time of overload
   }
 
   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 ----
+    //-------------------------- FLAT BATTERY speed controller shuts down until battery is recharged ----
-    //---------------------------------------------------------- This is a safety feature to prevent malfunction at low voltages!! ------
+    //--------------------------- This is a safety feature to prevent malfunction at low voltages!! ------
 
     Charged=0;                                                // battery is flat
     highVolts=Volts;                                          // record the voltage
     startVolts=Volts;
     chargeTimer=millis();                                     // record the time
 
     digitalWrite (Charger,0);                                 // enable current regulator to charge battery
   }
 
-  //------------------------------------------------------------ CHARGE BATTERY -------------------------------------------------------
+  //------------------------------- 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?
+    if (Volts>highVolts)                             // has battery voltage increased?
     {
-      highVolts=Volts;                                        // record the highest voltage. Used to detect peak charging.
+      highVolts=Volts;                               // record the highest voltage. Used to detect peak charging.
-      chargeTimer=millis();                                   // when voltage increases record the time
+      chargeTimer=millis();                          // when voltage increases record the time
     }
 
-    if (Volts>batvolt)                                        // battery voltage must be higher than this before peak charging can occur.
+    if (Volts>batvolt)                 // battery voltage must be higher than this before peak charging can occur.
     {
-      if ((highVolts-Volts)>5 || (millis()-chargeTimer)>chargetimeout) // has voltage begun to drop or levelled out?
+      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
       }
     }
   }
 
   else
 
-  {//----------------------------------------------------------- GOOD BATTERY speed controller opperates normally ----------------------
+  {//---------------------- 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 --------------------------------------
+    // ------------------------------ 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
         {
         case 2:                                               // left motor forward (2 = forward)
           analogWrite(LmotorA,0);
           analogWrite(LmotorB,LeftPWM);
           break;
 
         case 1:                                               // left motor brake (1 = brake)
           analogWrite(LmotorA,LeftPWM);
           analogWrite(LmotorB,LeftPWM);
           break;
 
         case 0:                                               // left motor reverse (0 = reverse)
           analogWrite(LmotorA,LeftPWM);
           analogWrite(LmotorB,0);
           break;
         }
       }
       if ((millis()-rightoverload)>overloadtime)
       {
         switch (Rightmode)                                    // if right motor has not overloaded recently
         {
         case 2:                                               // right motor forward
           analogWrite(RmotorA,0);
           analogWrite(RmotorB,RightPWM);
           break;
 
         case 1:                                               // right motor brake
           analogWrite(RmotorA,RightPWM);
           analogWrite(RmotorB,RightPWM);
           break;
 
         case 0:                                               // right motor reverse
           analogWrite(RmotorA,RightPWM);
           analogWrite(RmotorB,0);
           break;
         }
       }
     }
     else                                                      // Battery is flat
     {
       analogWrite (LmotorA,0);                                // turn off motors
       analogWrite (LmotorB,0);                                // turn off motors
       analogWrite (RmotorA,0);                                // turn off motors
       analogWrite (RmotorB,0);                                // turn off motors
     }
   }
 }
 
 
 
 
 
 
 void RCmode()
 {
-  //------------------------------------------------------------ Code for RC inputs ---------------------------------------------------------
+  //---------------------------- 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 (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 (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(Speed-1500)<RCdeadband) Speed=1500;      // if Speed input is within deadband set to 1500 (1500uS=center
-  if (abs(Steer-1500)<RCdeadband) Steer=1500;                 // if Steer input is within deadband set to 1500 (1500uS=center position for most servos)
+                                                   // position for most servos)
   if (abs(Steer-1500)<RCdeadband) Steer=1500;      // if Steer input is within deadband set to 1500
-  if (Mix==1)                                                 // Mixes speed and steering signals
+                                                   //(1500uS=center position for most servos)
 
   if (Mix==1)                                      // Mixes speed and steering signals
   {
     Steer=Steer-1500;
     Leftspeed=Speed-Steer;
     Rightspeed=Speed+Steer;
   }
   else                                                        // Individual stick control
   {
     Leftspeed=Speed;
     Rightspeed=Steer;
   }
   /*
   Serial.print("Left:");
   Serial.print(Leftspeed);
   Serial.print(" -- Right:");
   Serial.println(Rightspeed);
   */
-  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
+
   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
 
-                                                              // SV = next 7 integers will be position information for servos 0-6
+
 
                                                // 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
 
 
   if (Serial.available()>1)                                   // command available
   {
     int A=Serial.read();
     int B=Serial.read();
     int command=A*256+B;
     switch (command)
     {
       case 17996:                                             // FL
         Serial.flush();                                       // flush buffer
         break;
 
       case 16718:                                             // AN - return values of analog inputs 1-5
         for (int i=1;i<6;i++)                                 // index analog inputs 1-5
         {
           data=analogRead(i);                                 // read 10bit analog input
           Serial.write(highByte(data));                       // transmit high byte
           Serial.write(lowByte(data));                        // transmit low byte
         }
         break;
 
        case 21334:                                            // SV - receive postion information for servos 0-6
          for (int i=0;i<15;i++)                               // read 14 bytes of data
          {
            Serialread();
            servo[i]=data;
          }
          Servo0.writeMicroseconds(servo[0]*256+servo[1]);     // set servo position
          Servo1.writeMicroseconds(servo[2]*256+servo[3]);     // set servo position
          Servo2.writeMicroseconds(servo[4]*256+servo[5]);     // set servo position
          Servo3.writeMicroseconds(servo[6]*256+servo[7]);     // set servo position
          Servo4.writeMicroseconds(servo[8]*256+servo[9]);     // set servo position
          Servo5.writeMicroseconds(servo[10]*256+servo[11]);   // set servo position
          Servo6.writeMicroseconds(servo[12]*256+servo[13]);   // set servo position
          break;
 
        case 18498:                                            // HB - mode and PWM data for left and right motors
          Serialread();
          Leftmode=data;
          Serialread();
          LeftPWM=data;
          Serialread();
          Rightmode=data;
          Serialread();
          RightPWM=data;
          break;
 
        default:                                                // invalid command
          Serial.flush();                                       // flush buffer
     }
   }
 }
-{//---------------------------------------------------------- Read serial port until data has been received -----------------------------------
+
 void Serialread()
 {//------------------ Read serial port until data has been received -----------------------------------
   do
   {
     data=Serial.read();
   } while (data<0);
 }
 
 
 
 
 
 
-{//----------------------------------------------------------- Your code goes here ------------------------------------------------------------
+
 void I2Cmode()
 {//----------------------- Your code goes here ------------------------------------------------------
 
-    RightPWM = 128;                                             // sets the speed to half full speed
+
 
-    Rightmode = 2;                                              // sets the right motor to move forward (at half speed)
+    RightPWM = 128;                                           // sets the speed to half full speed
     LeftPWM = 128;
     Rightmode = 2;                                          // sets the right motor to move forward (at half speed)
     Leftmode = 2;                                               // see code to drive dual H bridges
 
     unsigned long duration, cm;
 
     digitalWrite(trigger, LOW);                                 // Ensures a clean trigger pulse
     delayMicroseconds(5);                                       // .1 ms long cleaning
     digitalWrite(trigger, HIGH);                                // Turn trigger pin on high (5V) to initiate pulse
     delayMicroseconds(10);                                      // delay 10 us (minimum time to start pulse)
     digitalWrite(trigger, LOW);                                 // stop tigger pulse to listen to echo
 
     duration = pulseIn(echo, HIGH);                             // records the length of the echo pulse
     cm = duration/58;                                           // converts duration into centimeters
 
     if (cm < 100);{                                             // is there an obstacle < 100cm away?
       RightPWM = 256;                                           // set right motor full speed
       LeftPWM = 0;                                              // set left motor to 0, therefore turning left
       delay(3000);                                              // allow 1s for turn to complete
       RightPWM = 0;
       LeftPWM = 256;                                            // turn right (straighten back out)
       delay(3000);
       RightPWM = 128;                                           // back to half speed forward
       LeftPWM = 128;
       }
    }
 }