Fixed most things 2

kill-shots · Feb 10, 2024 · a9905ba · a9905ba
1 parent ce91c7e
commit a9905ba
Show file tree

Hide file tree

Showing 3 changed files with 54 additions and 67 deletions.
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
@@ -15,7 +15,6 @@ void Intake::OnStart() {
 }
 
 void Intake::OnUpdate(units::second_t dt) {
-  //TODO you need to print the state to network tables
 
   switch (_state) {
     case IntakeState::kIdle: 
@@ -43,16 +42,20 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = 7_V;
       _pid.Reset();
       if (_config.intakeSensor->Get() == true) {
-        SetState(IntakeState::kIdle);
-        _ejecting = false;
+        SetState(IntakeState::kIdle); 
       }
     } 
     break;
 
     case IntakeState::kHold: 
     {
       _stringStateName = "Hold";
-      _setVoltage = 0_V;
+      //& add a check for if sensor is empty 
+
+      if (_config.intakeSensor->Get() == true) {
+        _setVoltage = 0_V;
+      }
+
     } 
     break;
 
@@ -62,7 +65,6 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = -7_V; 
       if (_config.intakeSensor->Get() == false) {
         SetState(IntakeState::kHold);
-        _intaking = false;
       }
     } 
     break;
@@ -73,7 +75,6 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = -7_V;
       if (_config.intakeSensor->Get() == true) {
         SetState(IntakeState::kIdle);
-        _passing = false;
       }
       _pid.SetSetpoint(_goal);
       units::volt_t pidCalculate =
@@ -88,7 +89,7 @@ void Intake::OnUpdate(units::second_t dt) {
       } else {
         _setVoltage = holdVoltage;
       }
-    } //TODO, logic is a bit ugly, make it nicer
+    } //&, logic is a bit ugly, make it nicer
     break;
 
     case IntakeState::kPID:
@@ -131,7 +132,7 @@ void Intake::SetState(IntakeState state) {
 void Intake::SetRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
 }
-IntakeState Intake::GetState() { //TODO Should be capital
+IntakeState Intake::GetState() {
   return _state;
 }
 void Intake::SetPidGoal(units::radians_per_second_t goal) {

diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
@@ -12,21 +12,27 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co
 }
 
 void IntakeManualControl::OnTick(units::second_t dt) {
+
+  /*
+  
+  if a button is held down:
+    set state to pass
+  else if right trigger is pressed:
+    set state to intaking
+  else if left trigger is pressed:
+    set state to eject
+  else
+    set state to idle
+  */
+
+
   if (_codriver.GetBButtonReleased()) {
-    if (_rawControl) {
-      _rawControl = false;
-      _intaking = false;
-      _ejecting = false;
+    if (_intake->GetState() == IntakeState::kRaw) {
       _intake->SetState(IntakeState::kIdle);
     } else {
-      _rawControl = true;
-      _intaking = false;
-      _ejecting = false;
-      _intake->SetState(IntakeState::kRaw);
+      _intake->SetState(IntakeState::kIdle); // this is not a toggle button
     }
-  }
-
-  if (_rawControl) {
+  } else if (_rawControl) {
     if (_codriver.GetRightTriggerAxis() > 0.1) {
       _intake->SetRaw(_codriver.GetRightTriggerAxis() * 10_V);
     } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
@@ -35,55 +41,35 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _intake->SetRaw(0_V);
     }
     _intake->SetState(IntakeState::kRaw);
-
+    
   } else {
     if (_codriver.GetRightTriggerAxis() > 0.1) {
-      if (_intaking) {
-        _intaking = false;
-        _intake->SetState(IntakeState::kIdle);
-      } else {
-        _intaking = true;
-        _ejecting = false;
-      }
-    }
-
-    if (_codriver.GetLeftTriggerAxis() > 0.1) {
-      if (_ejecting) {
-        _ejecting = false;
-        _intake->SetState(IntakeState::kIdle);
-      } else {
-        _ejecting = true;
-        _intaking = false;
-      }
-    }
-
-    if (_codriver.GetAButtonPressed()) {
-      if (_passing) {
-        _passing = false;
-        _intake->SetState(IntakeState::kIdle);
-      } else {
-        _passing = true;
-        _intaking = false;
-      }
-    }
-
-    if (_intaking) {
-      if (_intake->GetState() == IntakeState::kIdle) {
         _intake->SetState(IntakeState::kIntake);
-      }
-    }
+    } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
+        _intake->SetState(IntakeState::kIntake);
+    } else if (_codriver.GetAButtonPressed()) {
+      _intake->SetState(IntakeState::kPass);
+    } else {
+    _intake->SetState(IntakeState::kIdle);  
+  }
 
-    if (_passing) {
-      if (_intake->GetState() == IntakeState::kHold) {
-        _intake->SetState(IntakeState::kPass);
-      }
-    }
+      // if (_intaking) {
+    //   if (_intake->GetState() == IntakeState::kIdle) {
+    //     _intake->SetState(IntakeState::kIntake);
+    //   }
+    // }
 
-    if (_ejecting) {
-      if (_intake->GetState() == IntakeState::kIdle || _intake->GetState() == IntakeState::kHold) {
-        _intake->SetState(IntakeState::kEject);
-      }
-    }
+    // if (_passing) {
+    //   if (_intake->GetState() == IntakeState::kHold) {
+    //     _intake->SetState(IntakeState::kPass);
+    //   }
+    // }
+
+    // if (_ejecting) {
+    //   if (_intake->GetState() == IntakeState::kIdle || _intake->GetState() == IntakeState::kHold) {
+    //     _intake->SetState(IntakeState::kEject);
+    //   }
+    // }
   }
 }
 

diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
@@ -45,11 +45,11 @@ class Intake : public behaviour::HasBehaviour {
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
   units::volt_t holdVoltage = 0_V;
-  bool _intaking; //NEEDS DEFAULTS
-  bool _ejecting;
-  bool _passing;
+  // bool _intaking; //&NEEDS DEFAULTS
+  // bool _ejecting;
+  // bool _passing;
   units::radians_per_second_t _goal;
   wom::PIDController<units::radians_per_second, units::volt> _pid;
 
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
-};
+};