Intake - Manual/Auto fixes (CurtinFRC#114)

* First version Tank Drive * Intake draft * Intake draft fixed errors 1 * Intake draft fixed errors 2 * Intake draft fixed errors 3 * Intake draft fixed errors 4 * Intake draft fixed errors 5 * Network files 1 * Network files 2 * Network files 3 * Almost done i think * finished wpiformat errors * finished wpiformat errors 2 * finished wpiformat errors 3 * finished wpiformat errors 4 * ran wpiformat * finished wpiformat errors 5 * finished wpiformat errors 8 * finished wpiformat errors 9 * finished wpiformat errors 10 * Tested on robot * Fixed github build errors after modifying during robot tests * fixed more github build errors 2 * fixed more github build errors 3 * Tested on robot and works * added beam break * [docs] Update README.md (CurtinFRC#107) * [docs] Update README.md * Update README.md * Added auto to intake * [ci] add comment command action (CurtinFRC#111) * [ci] add comment command action * fix * Tested on robot * fixed intake manual auto, has not been tested on robot * fixed github build errors * Added intake better manual and verfied on robot! * Bump actions/setup-python from 4 to 5 (CurtinFRC#116) Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4 to 5. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](actions/setup-python@v4...v5) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] <support@github.com> Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> * Bump actions/github-script from 6 to 7 (CurtinFRC#115) Bumps [actions/github-script](https://github.com/actions/github-script) from 6 to 7. - [Release notes](https://github.com/actions/github-script/releases) - [Commits](actions/github-script@v6...v7) --- updated-dependencies: - dependency-name: actions/github-script dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] <support@github.com> Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> * Fixed readme (CurtinFRC#118) * ran wpiformat * fix * fix line endings * builds --------- Signed-off-by: dependabot[bot] <support@github.com> Co-authored-by: Kingsley Wong <kingsley.wong@student.education.wa.edu.au> Co-authored-by: Isaac Turner <spacey_sooty@outlook.com> Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Paul Hodges <111325206+Superbro525Alt@users.noreply.github.com>
Superbro525Alt · Feb 12, 2024 · c30477a · c30477a
1 parent 57d11c0
commit c30477a
Show file tree

Hide file tree

Showing 7 changed files with 123 additions and 68 deletions.
diff --git a/.gitattributes b/.gitattributes
@@ -1,5 +1,6 @@
 * text=auto
 *.sh text eol=lf
+*.bat text eol=crlf
 *.gradle text eol=lf
 *.java text eol=lf
 *.json text eol=lf

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
@@ -13,50 +13,60 @@ IntakeConfig Intake::GetConfig() {
 void Intake::OnUpdate(units::second_t dt) {
   switch (_state) {
     case IntakeState::kIdle: {
-      // _config.IntakeMotor.motorController->SetVoltage(0_V);
-      // if (_config.intakeSensor->Get()) {
-      //   setState(IntakeState::kHold);
-      // }
+      if (_config.intakeSensor->Get() == false) {
+        setState(IntakeState::kHold);
+      }
       _stringStateName = "Idle";
       _setVoltage = 0_V;
     } break;
+
     case IntakeState::kRaw: {
-      // _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
       _stringStateName = "Raw";
       _setVoltage = _rawVoltage;
     } break;
+
     case IntakeState::kEject: {
-      // _config.IntakeMotor.motorController->SetVoltage(-5_V);
-      // if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
-      //   setState(IntakeState::kIdle);
-      // }
       _stringStateName = "Eject";
-      _setVoltage = -5_V;
+      _setVoltage = 7_V;
+      if (_config.intakeSensor->Get() == true) {
+        setState(IntakeState::kIdle);
+        _ejecting = false;
+      }
     } break;
+
     case IntakeState::kHold: {
-      // _config.IntakeMotor.motorController->SetVoltage(0_V);
       _stringStateName = "Hold";
       _setVoltage = 0_V;
+      // if (_config.intakeSensor->Get() == false) {
+      //   setState(IntakeState::kHold);
+      // }
     } break;
+
     case IntakeState::kIntake: {
-      // _config.IntakeMotor.motorController->SetVoltage(5_V);
       _stringStateName = "Intake";
-      _setVoltage = 5_V;
+      _setVoltage = -7_V;
+      if (_config.intakeSensor->Get() == false) {
+        setState(IntakeState::kHold);
+        _intaking = false;
+      }
     } break;
+
     case IntakeState::kPass: {
-      // _config.IntakeMotor.motorController->SetVoltage(5_V);
-      // if (_config.shooterSensor->Get()) {
-      //   setState(IntakeState::kIdle);
-      //   _stringStateName = "Pass";
-      // }
-      _setVoltage = 5_V;
+      _stringStateName = "Pass";
+      _setVoltage = -7_V;
+      if (_config.intakeSensor->Get() == true) {
+        setState(IntakeState::kIdle);
+        _passing = false;
+      }
     } break;
     default:
       std::cout << "Error: Intake in INVALID STATE." << std::endl;
       break;
   }
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
+  _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
+  // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
   // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
   // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
   // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
@@ -72,3 +82,6 @@ void Intake::setState(IntakeState state) {
 void Intake::setRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
 }
+IntakeState Intake::getState() {
+  return _state;
+}
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
@@ -12,44 +12,83 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co
 }
 
 void IntakeManualControl::OnTick(units::second_t dt) {
-  if (_codriver.GetBButtonPressed()) {
-    if (_rawControl == true) {
+  if (_codriver.GetBButtonReleased()) {
+    if (_rawControl) {
       _rawControl = false;
+      _intaking = false;
+      _ejecting = false;
+      _intake->setState(IntakeState::kIdle);
     } else {
       _rawControl = true;
+      _intaking = false;
+      _ejecting = false;
+      _intake->setState(IntakeState::kRaw);
     }
   }
 
   if (_rawControl) {
-    _intake->setState(IntakeState::kRaw);
-    if (_codriver.GetLeftBumper()) {
-      _intake->setRaw(10_V);
-    } else if (_codriver.GetRightBumper()) {
-      _intake->setRaw(-10_V);
+    if (_codriver.GetRightTriggerAxis() > 0.1) {
+      _intake->setRaw(_codriver.GetRightTriggerAxis() * 10_V);
+    } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
+      _intake->setRaw(_codriver.GetLeftTriggerAxis() * -10_V);
     } else {
       _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;
+      }
+    }
 
-    // _intake->setRaw(_codriver.GetLeftBumper() * 10_V);
-    // _intake->setRaw(_codriver.GetRightBumper() * -10_V);
-    std::cout << "Raw" << std::endl;
+    if (_intaking) {
+      if (_intake->getState() == IntakeState::kIdle) {
+        _intake->setState(IntakeState::kIntake);
+      }
+    }
+
+    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);
+      }
+    }
   }
-  // } else {
-  //   if (_codriver.GetYButtonPressed()) {
-  //     _intake->setState(IntakeState::kIntake);
-  //   }
-  //   if (_codriver.GetAButtonPressed()) {
-  //     _intake->setState(IntakeState::kPass);
-  //   }
-  // }
 }
 
-// IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
+IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
+  Controls(intake);
+}
 
-// void IntakeAutoControl::OnTick(units::second_t dt) {
-//   if (_intake->GetConfig().intakeSensor->Get() == 1) {
-//     _intake->setState(IntakeState::kPass);
-//   } else if (_intake->GetConfig().magSensor->Get() == 0) {
-//     _intake->setState(IntakeState::kIdle);
-//   }
-// }
+void IntakeAutoControl::OnTick(units::second_t dt) {}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
@@ -46,10 +46,6 @@ void Robot::RobotInit() {
 
   // robotmap.swerveBase.gyro->Reset();
 
-  // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
-  // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  // _swerveDrive->SetDefaultBehaviour(
-  //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   _swerveDrive->SetDefaultBehaviour(
@@ -93,9 +89,7 @@ void Robot::RobotPeriodic() {
   shooter->OnUpdate(dt);
   sched->Tick();
 
-  robotmap.swerveTable.swerveDriveTable
-      ->GetEntry("frontLeftEncoder")
-
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
       .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
       .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
@@ -106,6 +100,7 @@ void Robot::RobotPeriodic() {
 
   _swerveDrive->OnUpdate(dt);
   alphaArm->OnUpdate(dt);
+  shooter->OnStart();
   intake->OnUpdate(dt);
 }
 
@@ -118,7 +113,6 @@ void Robot::AutonomousPeriodic() {}
 void Robot::TeleopInit() {
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
-  shooter->OnStart();
   sched->InterruptAll();
 
   // frontLeft->SetVoltage(4_V);

diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
@@ -28,6 +28,7 @@ class Intake : public behaviour::HasBehaviour {
 
   void setState(IntakeState state);
   void setRaw(units::volt_t voltage);
+  IntakeState getState();
   IntakeConfig GetConfig();
 
  private:
@@ -37,5 +38,9 @@ class Intake : public behaviour::HasBehaviour {
   units::volt_t _rawVoltage = 0_V;
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
+  bool _intaking;
+  bool _ejecting;
+  bool _passing;
+
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
 };
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
@@ -20,7 +20,11 @@ class IntakeManualControl : public behaviour::Behaviour {
   frc::XboxController& _codriver;
 
   units::volt_t _rawVoltage;
-  bool _rawControl;
+  units::volt_t _setVoltage;
+  bool _rawControl = true;
+  bool _intaking = false;
+  bool _ejecting = false;
+  bool _passing = false;
 };
 
 class IntakeAutoControl : public behaviour::Behaviour {

diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
@@ -27,10 +27,22 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
-    frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+  struct IntakeSystem {
+    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
+    frc::DigitalInput intakeSensor{4};
+    // frc::DigitalInput magSensor{0};
+    // frc::DigitalInput shooterSensor{0};
+
+    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
+
+    IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/};
+  };
+  IntakeSystem intakeSystem;
+
   struct Shooter {
     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
     // frc::DigitalInput shooterSensor{2};
@@ -52,19 +64,6 @@ struct RobotMap {
   };
   Shooter shooterSystem;
 
-  struct IntakeSystem {
-    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
-    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
-    // frc::DigitalInput intakeSensor{0};
-    // frc::DigitalInput magSensor{0};
-    // frc::DigitalInput shooterSensor{0};
-
-    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
-
-    IntakeConfig config{IntakeGearbox /*, &intakeSensor, &magSensor, &shooterSensor*/};
-  };
-  IntakeSystem intakeSystem;
-
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};