View difference between Paste ID: hZLWjT9e and cFscTs6d

 #include <Servo.h>
 #include "IOpins.h"
 #include "Constants.h"
 
 
 //-------------------------------------------------------------- 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 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 trigger = 2;                                        // Define Ping Trigger as D2
-const int echo = 4;                                           // aaa Define Ping Echo D4
+const int echo = 4;                                           // Define Ping Echo D4
 
 //-------------------------------------------------------------- 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 ------------------------------
 
   //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 -----------------
 
   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 ---------------------------
 
   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 -----------------
 
-  pinMode(trigger,OUTPUT);                                    // aaa Set digital pin 2 to trigger output
+  pinMode(trigger,OUTPUT);                                    // Set digital pin 2 to trigger output
-  pinMode(echo,INPUT);                                        // aaa Set digital pin 4 to echo input
+  pinMode(echo,INPUT);                                        // Set digital pin 4 to echo input
 }
 
 
 void loop()
 {
   //---------------------------------------- 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 ----
     //--------------------------- 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 -------------------------------------------------------
 
   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?
     {
       highVolts=Volts;                               // record the highest voltage. Used to detect peak charging.
       chargeTimer=millis();                          // when voltage increases record the time
     }
 
     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?
       {
         Charged=1;                                            // battery voltage has peaked
         digitalWrite (Charger,1);                             // turn off current regulator
       }
     }
   }
 
   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
         {
         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 ---------------------------------------------------------
 
   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
   {
     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);
   */
   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 --------------------------------------
+{// ------------------------------------------------------------ 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
 
 
   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
     }
   }
 }
 
 void Serialread()
 {//------------------ Read serial port until data has been received -----------------------------------
   do
   {
     data=Serial.read();
   } while (data<0);
 }
 
 
 
 
 
 
 
 void I2Cmode()
-{//----------------------- Your code goes here ------------------------------------------------------
+{//----------------------- Autonomous Code ------------------------------------------------------
     RightPWM = 128;                                         // sets the speed to half full speed
- void loop(){
+
     Rightmode = 2;                                          // sets the right motor to move forward (at half speed)
-    RightPWM = 128;                                           // sets the speed to half full speed
+    Leftmode = 2;                                           // see code to drive dual H bridges
 
     unsigned long duration, cm;
-    Leftmode = 2;                                               // see code to drive dual H bridges
+
     digitalWrite(trigger, LOW);                                 // Ensures a clean trigger pulse
     delayMicroseconds(5);                                       // 5 us long cleaning
     digitalWrite(trigger, HIGH);                                // Turn trigger pin on high (5V) to initiate pulse
     delayMicroseconds(10);                                      // delay 10 us (minimum time to start pulse)
-    delayMicroseconds(5);                                       // .1 ms long cleaning
+
 
     duration = pulseIn(echo, HIGH);                             // records the length of the echo pulse
     cm = duration/58;                                           // converts duration into centimeters
 
     if (cm < 40){                                              // is there an obstacle < 100cm away?
       RightPWM = 128;                                           // set right motor full speed
       LeftPWM = 2;                                              // set left motor to 0, therefore turning left
-    if (cm < 100);{                                             // is there an obstacle < 100cm away?
+
-      RightPWM = 256;                                           // set right motor full speed
+      RightPWM = 2;
-      LeftPWM = 0;                                              // set left motor to 0, therefore turning left
+      LeftPWM = 128;                                            // turn right (straighten back out)
       delay(3000);
-      RightPWM = 0;
+
-      LeftPWM = 256;                                            // turn right (straighten back out)
+
       }
 
 }