Fixed up code after car voyage... Preparing to add ultrasonic code

MyRobot.cpp

@@ -1,4 +1,3 @@
-//TODO this 
 //Sonar code
 #include "WPILib.h"
 #include "SmartDashboard/SmartDashboard.h"
@@ -27,8 +26,12 @@ class RobotDemo : public SimpleRobot
   //Ultrasonic
   AnalogChannel BallSonicLeft, BallSonicRight, WallSonicLeft, WallSonicRight;
   DigitalOutput BallLeft, BallRight, WallLeft, WallRight;
+  //DigitalInput ballLimit;
 public:
   RobotDemo():
+    //Limit Switches
+    //TODO
+    //ballLimit(),
     //Joysticks
     Rstick(1),
     Lstick(2),
@@ -44,8 +47,8 @@ public:
     BallRight(2,4),
     WallRight(2,5),
     //Compressor
-    //compressor(2,3,1,1),
+    compressor(2, 5, 1, 1),
-    compressor(2, 5, 1, 1), //Solenoids
+    //Solenoids
     collectorSole1(1),
     collectorSole2(2),
     //Limit switches
@@ -105,7 +108,7 @@ public:
     SmartDashboard::PutNumber("Ball Left", voltToDistance(BallSonicLeft.GetAverageVoltage()));
     SmartDashboard::PutNumber("Ball Right", voltToDistance(BallSonicRight.GetAverageVoltage()));
     SmartDashboard::PutBoolean("Use Ultrasonic",false);
-    SmartDashboard::PutBoolean("Reverse Robot",false);
+    SmartDashboard::PutBoolean("Daniel Mode",false);
   }
   void updateDashboard() {
     SmartDashboard::PutNumber("Throttle", throttle);
@@ -269,8 +272,6 @@ public:
     float yoyo = atan((abs(voltToDistance(BallSonicLeft.GetAverageVoltage()) - voltToDistance(BallSonicRight.GetAverageVoltage())))/20.0f)*360.0f/(2.0f*3.141592653589793f);
     SmartDashboard::PutNumber("Distance", yoyo);
   }
-//  float getSonar(int sonar){
-//    switch(sonar){
   void Test() {
   }
   void Autonomous() {
@@ -321,20 +322,25 @@ public:
         }
       } else {
         if(i<=initalDriveTime*200) {
-          setMotorValue(6, 1, 1);
+          driveRobot(1.0f,0.0f);
+          //setMotorValue(6, 1, 1);
         } else if(i>shooterDelay&&i<shooterDelay+(shooterDuration*200)&&shooterMaxAngle>=potToDegrees(armPot.GetAverageVoltage())) {
           driveRobot(0, 0);
           shootRobot(1, true);
           setMotorValue(6, 1, 1);
-        } else if (i>1500&&i<1700) {
+        }
-          shootRobot(.1,true);
+        if(i>480&&i<430+initalDriveTime){
-          //driveRobot(-1,0);
+          driveRobot(-1,0);
-        } else {
+          //TODO
-          /*
+        } else if(i>430+initalDriveTime/*&&ballLimit.Get()==1*/) {
-          driveRobot(0, 0);
+          driveRobot(-0.6f,0.0f);
-          shootRobot(0, true);
+          setMotorValue(6, 1, 1);
-          setMotorValue(6, 1, 0);
+          //TODO
-          */
+        } else if(/*ballLimit.Get()==1&&*/i>430+initalDriveTime) {
+          driveRobot(1.0f,0.0f);
+        }
+        if(i>480&&i<480){
+          shootRobot(0.25f,true);
         }
       }
       updateDashboard();
@@ -354,80 +360,6 @@ public:
     }
     compressing = false;
     compressor.Stop();
-//    while (IsEnabled() && IsAutonomous()) {
-//      //Collect left average values from cur values 0 to 12
-//      if(cur<12){
-//        WallLeft.Set(1);
-//        BallLeft.Set(1);
-//        WallRight.Set(0);
-//        BallRight.Set(0);
-//        avgLeft+=voltToDistance(WallSonicLeft.GetAverageVoltage(),true);
-//      //After 12 cur iterations, find the average (data was recorded 12 times, divide by 12)
-//      }else if(cur==12){
-//        avgLeft/=12;
-//      //Collect right average values from cur values 38 to 50
-//      }else if(cur<100&&cur>88){
-//        WallLeft.Set(0);
-//        BallLeft.Set(0);
-//        WallRight.Set(1);
-//        BallRight.Set(1);
-//        avgRight+=voltToDistance(WallSonicRight.GetAverageVoltage(),true);
-//      //After 50 cur iterations, find the average (data was recorded 12 times, divide by 12)
-//      }else if(cur==100){
-//        cur=0;
-//        avgRight/=12;
-//        thisIsATest=atan((abs(avgLeft-avgRight))/20.0f)*360.0f/(2.0f*3.141592653589793f);
-//        SmartDashboard::PutNumber("Tanval", atan((abs(avgLeft-avgRight))/20.0f)*360.0f/(2.0f*3.141592653589793f));
-//      }
-//      //Calculate the inital distance and average it averageAmount times.
-//      if(i<averageAmount){
-//        avgDist+=(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)+voltToDistance(WallSonicRight.GetAverageVoltage(),true)/2);
-//      }else{
-//        avgDist/=averageAmount;
-//      }
-//      //Calculate the average distance from the wall
-//      curDist=thisIsATest;
-//      if(i%100==0){
-//        printf("Difference: %f\n",avgDist-curDist);
-//      }
-//      if (i>=5&&avgDist-curDist<=36.0f) {
-//        float xPower, yPower;
-//        xPower=1;
-//        yPower=(avgDist-curDist)/36.0f;
-//        if(yPower>1){
-//          yPower=1;
-//        }
-//        driveRobot(yPower, xPower);
-//        setMotorValue(6, 1, 1);
-//      } else if (i>1400&&i<1600&&125>=potToDegrees(armPot.GetAverageVoltage())) {
-//        driveRobot(0, 0);
-//        shootRobot(1, false);
-//        setMotorValue(6, 1, 1);
-//      } else if (i>1500&&i<1700) {
-//        shootRobot(.1,false);
-//        //driveRobot(-1,0);
-//      } else {
-//        /*
-//        driveRobot(0, 0);
-//        shootRobot(0, true);
-//        setMotorValue(6, 1, 0);
-//        */
-//      }
-//      updateDashboard();
-//      if (i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
-//        compressor.Stop();
-//        compressing = false;
-//        logMsg("Stopping the compressor",2);
-//      }
-//      if (i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) {
-//        compressor.Start();
-//        compressing = true;
-//        logMsg("Starting the compressor",2);
-//      }
-//      Wait(0.005f);
-//      i++;
-//      cur++;
-    //}
   }
   void OperatorControl() {
     myRobot.SetSafetyEnabled(false);
@@ -448,7 +380,7 @@ public:
         swap=!swap;
       }
       throttle = (-Lstick.GetRawAxis(4)+1)/2;
-      if(SmartDashboard::GetBoolean("Reverse Robot")){
+      if(SmartDashboard::GetBoolean("Daniel Mode")) {
         driveRobot(-Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
       } else {
         driveRobot(Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
@@ -497,7 +429,8 @@ public:
         if(collectorExtended == true) {
           shootRobot(-DownSpeed, false);
           logMsg("Collector is extended, going to fire",17);
-        } } else {
+        }
+      } else {
         shooting = false;
         //Stop all motors
         shootRobot(0, true);
@@ -525,4 +458,3 @@ public:
   }
 };
 START_ROBOT_CLASS(RobotDemo);
-