fixed auto, added auto selection and drivebase

README.md

@@ -4,37 +4,24 @@ Our code for the 2024 FRC game, CRESCENDO, using GradleRIO, Wombat, and probably
 
 Setup
 ===
-
 First install WPILib and if running Windows the FRC game tools. Instructions can be found [here](https://docs.wpilib.org/en/stable/docs/zero-to-robot/step-2/index.html).
 
-Linux
----
 Fork this repository then open up a terminal and run :
 ```bash
 git clone https://github.com/*yourusernamehere*/2024-Crescendo.git
 cd 2024-Crescendo
 ```
 Now look in [CONTRIBUTING.md](./CONTRIBUTING.md) before continuing!
-
-Windows
----
-Fork this repository then open up a terminal and run :
-```powershell
-git clone https:\\github.com\*yourusernamehere*\2024-Crescendo.git
-cd 2024-Crescendo
-```
-Now look in [CONTRIBUTING.md](./CONTRIBUTING.md) before continuing!
-
 Quick Commands
 ===
 These commands can be used in a variety of combinations, feel free to experiment!
 
 Build
 ---
 `./gradlew build`
-Build will compile and get the code ready without deploying it. It will also run all automated tests, which is great for testing your code before it ever gets run on a robot (which also means you can build whenever).
+Build will compile your code without deploying it. It will also run all automated tests, which is great for testing code before it runs on a robot.
 
-`./gradlew :Wombat:build`
+`./gradlew :wombat:build`
 Will compile and build the Wombat library. Also runs all of Wombat's inbuilt tests.
 
 Deploy

build.gradle

@@ -111,6 +111,6 @@ subprojects {
 }
 
 wrapper {
-    gradleVersion = '8.5'
+    gradleVersion = '8.6'
     distributionType = Wrapper.DistributionType.BIN
 }

gradle/wrapper/gradle-wrapper.properties

@@ -1,6 +1,6 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=permwrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.6-bin.zip
 networkTimeout=10000
 validateDistributionUrl=true
 zipStoreBase=GRADLE_USER_HOME

gradlew.bat

@@ -43,11 +43,11 @@ set JAVA_EXE=java.exe
 %JAVA_EXE% -version >NUL 2>&1
 if %ERRORLEVEL% equ 0 goto execute
 
-echo.
-echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
-echo.
-echo Please set the JAVA_HOME variable in your environment to match the
-echo location of your Java installation.
+echo. 1>&2
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
 
 goto fail
 
@@ -57,11 +57,11 @@ set JAVA_EXE=%JAVA_HOME%/bin/java.exe
 
 if exist "%JAVA_EXE%" goto execute
 
-echo.
-echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
-echo.
-echo Please set the JAVA_HOME variable in your environment to match the
-echo location of your Java installation.
+echo. 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
 
 goto fail
 

src/main/cpp/AlphaArmBehaviour.cpp

@@ -33,7 +33,7 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
   }
 }
 
-ArmToSetPoint::ArmToSetPoint(AlphaArm* alphaArm, units::degree_t armAngle, float armSpeed) : _alphaArm(alphaArm) {
+ArmToSetPoint::ArmToSetPoint(AlphaArm* alphaArm, units::degree_t armAngle) : _alphaArm(alphaArm) {
   Controls(alphaArm);
 }
 // // ArmSpeed is a float from 0-1, 1 being instantly and 0 being don't move at all.

src/main/cpp/IntakeBehaviour.cpp

@@ -87,7 +87,7 @@ void IntakeManualControl::OnTick(units::second_t dt) {
   }
 }
 
-AutoIntake::AutoIntake(Intake* intake, units::volt_t intakeVolt) : _intake(intake) {
+AutoIntake::AutoIntake(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
 

src/main/cpp/Robot.cpp

@@ -57,8 +57,8 @@ void Robot::RobotInit() {
   _swerveDrive->SetDefaultBehaviour(
       [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
-  // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
-  // m_driveSim = wom::TempSimSwerveDrive();
+  // m_driveSim = new wom::TempSim_swerveDrive(&simulation_timer, &m_field);
+  // m_driveSim = wom::TempSim_swerveDrive();
 
   alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
@@ -118,17 +118,17 @@ void Robot::AutonomousInit() {
   m_autoSelected = m_chooser.GetSelected();
 
   if (m_autoSelected == "Taxi") {
-    sched->Schedule(autos::Taxi(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
+    sched->Schedule(autos::Taxi(_swerveDrive, shooter, intake,  alphaArm));
   } else if (m_autoSelected == "Auto Test") {
-    sched->Schedule(autos::AutoTest(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
+    sched->Schedule(autos::AutoTest(_swerveDrive, shooter, intake,  alphaArm));
   } else if (m_autoSelected == "Quadruple Close") {
-    sched->Schedule(autos::QuadrupleClose(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
+    sched->Schedule(autos::QuadrupleClose(_swerveDrive, shooter, intake,  alphaArm));
   } else if (m_autoSelected == "Quadruple Far") {
-    sched->Schedule(autos::QuadrupleFar(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
+    sched->Schedule(autos::QuadrupleFar(_swerveDrive, shooter, intake,  alphaArm));
   } else if (m_autoSelected == "Quadruple Close Double Far") {
-    sched->Schedule(autos::QuadrupleCloseDoubleFar(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
+    sched->Schedule(autos::QuadrupleCloseDoubleFar(_swerveDrive, shooter, intake,  alphaArm));
   } else if (m_autoSelected == "Quadruple Close Single Far") {
-    sched->Schedule(autos::QuadrupleCloseSingleFar(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
+    sched->Schedule(autos::QuadrupleCloseSingleFar(_swerveDrive, shooter, intake,  alphaArm));
   }
 }
 void Robot::AutonomousPeriodic() {

src/main/cpp/ShooterBehaviour.cpp

@@ -37,7 +37,7 @@ void ShooterManualControl::OnTick(units::second_t dt) {
   }
 }
 
-AutoShoot::AutoShoot(Shooter* shooter, units::volt_t shooterVolt) : _shooter(shooter) {
+AutoShoot::AutoShoot(Shooter* shooter) : _shooter(shooter) {
   Controls(shooter);
 }
 

src/main/include/AlphaArmBehaviour.h

@@ -25,7 +25,7 @@ class AlphaArmManualControl : public behaviour::Behaviour {
 
 class ArmToSetPoint : public behaviour::Behaviour {
  public:
-  explicit ArmToSetPoint(AlphaArm* alphaArm, units::degree_t armAngle, float armSpeed);
+  explicit ArmToSetPoint(AlphaArm* alphaArm, units::degree_t armAngle);
   void OnTick(units::second_t dt);
 
  private:

src/main/include/Auto.h

@@ -8,15 +8,15 @@
 
 
 namespace autos {
-  std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+  std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+  std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+  std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+  std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 }

src/main/include/IntakeBehaviour.h

@@ -29,7 +29,7 @@ class IntakeManualControl : public behaviour::Behaviour {
 
 class AutoIntake : public behaviour::Behaviour {
  public:
-  explicit AutoIntake(Intake* intake, units::volt_t intakeVolt);
+  explicit AutoIntake(Intake* intake);
   void OnTick(units::second_t dt) override;
 
  private:

src/main/include/ShooterBehaviour.h

@@ -28,7 +28,7 @@ class ShooterManualControl : public behaviour::Behaviour {
 
 class AutoShoot : public behaviour::Behaviour {
  public:
-  explicit AutoShoot(Shooter* shooter, units::volt_t shooterVolt);
+  explicit AutoShoot(Shooter* shooter);
   void OnTick(units::second_t dt) override;
 
  private:

wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp

@@ -223,3 +223,25 @@ void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() {
 void wom::drivetrain::behaviours::AutoSwerveDrive::SetPath(std::string path) {
   _simSwerveDrive->SetPath(path);
 }
+
+// Drivebase Pose Control behaviour
+wom::drivetrain::behaviours::DrivebasePoseBehaviour::DrivebasePoseBehaviour(SwerveDrive* swerveDrivebase,
+                                                                            frc::Pose2d pose,
+                                                                            units::volt_t voltageLimit,
+                                                                            bool hold)
+    : _swerveDrivebase(swerveDrivebase), _pose(pose), _hold(hold), _voltageLimit(voltageLimit) {
+  Controls(swerveDrivebase);
+}
+
+// used in autonomous for going to set drive poses
+void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t deltaTime) {
+  double currentAngle = _swerveDrivebase->GetPose().Rotation().Degrees().value();
+  units::degree_t adjustedAngle =
+      1_deg * (currentAngle - std::fmod(currentAngle, 360) + _pose.Rotation().Degrees().value());
+  _swerveDrivebase->SetVoltageLimit(_voltageLimit);
+  _swerveDrivebase->SetPose(frc::Pose2d{_pose.X(), _pose.Y(), adjustedAngle});
+
+  if (_swerveDrivebase->IsAtSetPose() && !_hold) {
+    SetDone();
+  }
+}

wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h

@@ -199,6 +199,39 @@ class AutoSwerveDrive {
 
   std::string m_path;
 };
+
+/**
+ * @brief Behaviour Class to hangle the swerve drivebase going to and potentially maintaining the position
+ */
+class DrivebasePoseBehaviour : public behaviour::Behaviour {
+ public:
+  /**
+   * @param swerveDrivebase
+   * A pointer to the swerve drivebase
+   * @param pose
+   * A variable containing an X coordinate, a Y coordinate, and a rotation, for the drivebase to go to
+   * @param hold
+   * An optional variable (defaulting false), to say whether this position should be maintained
+   */
+  DrivebasePoseBehaviour(SwerveDrive* swerveDrivebase, frc::Pose2d pose, units::volt_t voltageLimit = 10_V,
+                         bool hold = false);
+
+  /**
+   * @brief
+   *
+   * @param deltaTime change in time since the last iteration
+   */
+  void OnTick(units::second_t deltaTime) override;
+
+ private:
+  SwerveDrive* _swerveDrivebase;
+  frc::Pose2d _pose;
+  bool _hold;
+  units::volt_t _voltageLimit;
+
+  std::shared_ptr<nt::NetworkTable> _swerveDriveTable =
+      nt::NetworkTableInstance::GetDefault().GetTable("swerve");
+};
 }  // namespace behaviours
 }  // namespace drivetrain
 }  // namespace wom

wombat/src/main/include/utils/PID.h

@@ -104,7 +104,11 @@ class PIDController {
   void Reset() { _integralSum = sum_t{0}; }
 
   out_t Calculate(in_t pv, units::second_t dt, out_t feedforward = out_t{0}) {
-    pv = units::math::fabs(pv);
+    bool is_negative;
+    if (pv.value() < 0) {
+      is_negative = true;
+      pv = units::math::fabs(pv);
+    }
     auto error = do_wrap(_setpoint - pv);
     error = units::math::fabs(error);
     _integralSum += error * dt;
@@ -125,6 +129,9 @@ class PIDController {
     _last_pv = pv;
     _last_error = error;
     _iterations++;
+    if (is_negative) {
+      return out * -1;
+    }
     return out;
   }