Fixed compressor (not fully tested)

src/HHRobot.cpp

@@ -18,6 +18,8 @@ HHRobot::HHRobot():
     left2 = new Talon(DRIVE_LEFT_SIDECAR, DRIVE_LEFT_MOTOR_TWO);
     left3 = new Talon(DRIVE_LEFT_SIDECAR, DRIVE_LEFT_MOTOR_THREE);
     shooterTable->PutNumber("Target Shooter Angle",90);
+    timer = new Timer();
+    lastTime = 0;
   }
 void HHRobot::Init(){
   printf("Initing\n");
@@ -26,6 +28,8 @@ void HHRobot::Init(){
   //Put table values initally to avoid annoying refreshing
   shooterTable->PutNumber("Target Shooter Angle",90);
   shooterTable->PutNumber("Current Shooter Angle",-420);
+  timer->Start();
+  compressorSystem->StartCompressor();
 }
 bool HHRobot::CheckJoystickValues(){
   float x=controlSystem->GetJoystickAxis(1,"x");
@@ -79,6 +83,7 @@ void HHRobot::UpdateDashboard(){
   dashboard->PutFloatValue("Shooting Power",controlSystem->GetThrottle());
 }
 void HHRobot::RunAuto(){
+  timer->Reset();
   int step,time;
   compressorSystem->ExtendCollector();
   //TODO I have no idea what rate this loop runs at so we are going to have to fine tune the times
@@ -108,11 +113,14 @@ void HHRobot::RunAuto(){
 //Main function used to handle periodic tasks on the robot
 void HHRobot::Handler(){
   double targetAngle = shooterTable->GetNumber("Target Shooter Angle");
-  bool allowCompressing;
+  bool allowCompressing = true;
   //Periodic tasks that should be run by every loop
   shooter->UpdateShooterPosition(targetAngle);
-  //TODO Need to implement a timing system to not break the spike (this function doesn't run the compressor at the moment)
-  compressorSystem->CompressorSystemPeriodic(allowCompressing);
+  if(timer->Get()-lastTime>=0.5f){
+    // Update compressor when current time-last time is more than .5 seconds
+    lastTime=timer->Get();
+    compressorSystem->CompressorSystemPeriodic(allowCompressing);
+  }
   collector->UpdateCollector(shooter->isShooting,shooter->GetAngle());
   //TODO Fix whatever the heck is wrong with this
   DriveRobot(controlSystem->GetJoystickAxis(1,"z")+controlSystem->GetJoystickAxis(1,"x"),controlSystem->GetJoystickAxis(1,"y"));
@@ -152,13 +160,6 @@ void HHRobot::Handler(){
   }
   shooterTable->PutNumber("Target Shooter Angle",targetAngle);
   //TODO: Fix whatever this is supposed to do
-  //if(controlSystem->rightJoystickValues[DISABLE_COMPRESSOR]){}
-  if(false){
-    allowCompressing=false;
-  }else{
-    allowCompressing=true;
-  }
-  //TODO: Fix whatever this is supposed to do
   //if(controlSystem->rightJoystickValues[DRIVE_FOR_DISTANCE]){}
   if(false){
     targetAngle=100;

src/HHRobot.h

@@ -21,6 +21,8 @@ class HHRobot{
     HHCompressor *compressorSystem;
     HHDashboard *dashboard;
     HHSonar *sonar;
+    Timer *timer;
+    double lastTime;
   public:
     HHRobot();
     bool CheckJoystickValues();

src/Subsystems/Compressor.cpp

@@ -3,6 +3,7 @@ HHCompressor::HHCompressor(){
   compressor=new Compressor(COMPRESSOR_GAUGE_SIDECAR, COMPRESSOR_GAUGE, COMPRESSOR_RELAY_SIDECAR, COMPRESSOR_RELAY);
   solenoid1=new Solenoid(COMPRESSOR_SOLENOID_ONE);
   solenoid2=new Solenoid(COMPRESSOR_SOLENOID_TWO);
+  compressing=false;
 }
 void HHCompressor::CompressorSystemPeriodic(bool compressorEnabled){
   switch(e_CollectorSolenoidState){
@@ -20,13 +21,23 @@ void HHCompressor::CompressorSystemPeriodic(bool compressorEnabled){
       break;
   }
   if(compressorEnabled){
-    if(compressor->GetPressureSwitchValue()==1){
-      compressor->Start();
-    }else{
-      compressor->Stop();
+    printf("Compressor is enabled\n");
+    if(compressing&&compressor->GetPressureSwitchValue()==1){
+      // It is compressing, but the pressure is too high 
+      printf("Condition 1");
+      StopCompressor();
+    }else if(!compressing&&compressor->GetPressureSwitchValue()==0){
+      // It is not compressing and the pressure isn't too high
+      printf("Condition 2");
+      StartCompressor();
     }
   }else{
-    compressor->Stop();
+    printf("Condition 3");
+    if(compressing){
+      // If the compressor is not enabled, but it's still compressing
+      printf("Condition 4");
+      StopCompressor();
+    }
   }
   e_CollectorSolenoidState=IDLE;
 }
@@ -36,4 +47,14 @@ void HHCompressor::ExtendCollector(){
 void HHCompressor::RetractCollector(){
   e_CollectorSolenoidState=RETRACTED;
 }
+void HHCompressor::StopCompressor(){
+  printf("Stopping compressor\n");
+  compressor->Stop();
+  compressing=false;
+}
+void HHCompressor::StartCompressor(){
+  printf("Starting compressor\n");
+  compressor->Start();
+  compressing=true;
+}
 // vim: ts=2:sw=2:et

src/Subsystems/Compressor.h

@@ -5,6 +5,7 @@ class HHCompressor{
   private:
     Compressor *compressor;
     Solenoid *solenoid1, *solenoid2;
+    bool compressing;
   public:
     enum{
       EXTENDED,
@@ -15,5 +16,7 @@ class HHCompressor{
     void CompressorSystemPeriodic(bool compressorEnabled);
     void ExtendCollector();
     void RetractCollector();
+    void StopCompressor();
+    void StartCompressor();
 };
 // vim: ts=2:sw=2:et