Feat/drive straight

build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.4.2"
+    id "edu.wpi.first.GradleRIO" version "2023.4.3"
 }
 
 // Define my targets (RoboRIO) and artifacts (deployable files)

src/main/cpp/autos/ScorePreloadedAuto.cpp

@@ -21,15 +21,15 @@ ScorePreloadedAuto::ScorePreloadedAuto(std::shared_ptr<SwerveDrive> swerve, std:
     double score_position = RobotParams::GetParam("arm.score_high.intake_setpoint", 0.0);
 
     AddCommands(
-        IntakeCommand(intake, -0.1), // hold the cube/cone in
-        ArmSetStateCommand(arm, TeamOKC::ArmState(extend, degrees)),
-        frc2::WaitCommand(units::second_t(3.0)), // wait for the command to finish
-        frc2::WaitCommand(units::second_t(1.5)), // wait for the command to finish
-        IntakeCommand(intake, 0.3), // drop the cube
-        frc2::WaitCommand(units::second_t(0.5)), // wait for cube to be dropped
-        IntakeCommand(intake, 0), // stop the intake
-        ArmSetStateCommand(arm, TeamOKC::ArmState(1, 0)), // bring the arm back in the robot
-        frc2::WaitCommand(units::second_t(2)), // wait a second
-        AutoDriveCommand(swerve, 4.7, 1) // back slowly away
+       // IntakeCommand(intake, -0.1), // hold the cube/cone in
+        // ArmSetStateCommand(arm, TeamOKC::ArmState(extend, degrees)),
+        // frc2::WaitCommand(units::second_t(3.0)), // wait for the command to finish
+        // frc2::WaitCommand(units::second_t(1.5)), // wait for the command to finish
+        // IntakeCommand(intake, 0.3), // drop the cube
+        // frc2::WaitCommand(units::second_t(0.5)), // wait for cube to be dropped
+        // IntakeCommand(intake, 0), // stop the intake
+        // ArmSetStateCommand(arm, TeamOKC::ArmState(1, 0)), // bring the arm back in the robot
+        // frc2::WaitCommand(units::second_t(2)), // wait a second
+        AutoDriveCommand(swerve, 2.0, 1, 0) // back slowly away
     );
 }

src/main/cpp/autos/ScoreTwoAuto.cpp

@@ -26,13 +26,13 @@ ScoreTwoAuto::ScoreTwoAuto(std::shared_ptr<SwerveDrive> swerve, std::shared_ptr<
         IntakeCommand(intake, 0), // stop the intake
         ArmSetStateCommand(arm, TeamOKC::ArmState(1, 0)), // bring the arm back in the robot
         frc2::WaitCommand(units::second_t(2)), // wait a second
-        AutoDriveCommand(swerve, 5, 1), // back slowly away
+        AutoDriveCommand(swerve, 5, 1, 0), // back slowly away
         ArmSetStateCommand(arm, TeamOKC::ArmState(negative_pickup_extend, negative_pickup_degrees)), // get ready to pick another one up
         IntakeCommand(intake, -0.3), // suck the cube in
-        AutoDriveCommand(swerve, 0.5, 0.2), // drive into the cube
+        AutoDriveCommand(swerve, 0.5, 0.2, 0), // drive into the cube
         IntakeCommand(intake, -0.1), // just hold the cube in, not too much power
         ArmSetStateCommand(arm, TeamOKC::ArmState(extend, degrees)), // set the arm to score high
-        AutoDriveCommand(swerve, -5, 0.5), // drive back to the station
+        AutoDriveCommand(swerve, -5, 0.5, 0), // drive back to the station
         IntakeCommand(intake, 1.0) // spit the cube out really hard, hopefully it lands in the right spot
         // and we should be out of time, really.
     );

src/main/cpp/commands/swerve/AutoDriveCommand.cpp

@@ -1,10 +1,12 @@
 
 #include "commands/swerve/AutoDriveCommand.h"
 
-AutoDriveCommand::AutoDriveCommand(std::shared_ptr<SwerveDrive> swerve, double dist, double max_speed) {
+AutoDriveCommand::AutoDriveCommand(std::shared_ptr<SwerveDrive> swerve, double dist, double max_speed, double heading) {
     swerve_ = swerve;
     dist_ = dist;
     max_speed_ = max_speed;
+    heading_ = heading;
+
 
     if (swerve_ != nullptr) {
         this->AddRequirements(swerve_.get());
@@ -22,7 +24,7 @@ void AutoDriveCommand::Initialize() {
 }
 
 void AutoDriveCommand::Execute() {
-    VOKC_CALL(swerve_->DriveAuto(max_speed_));
+    VOKC_CALL(swerve_->DriveAuto(max_speed_, heading_));
 }
 
 void AutoDriveCommand::End(bool interrupted) {

src/main/cpp/subsystems/SwerveDrive.cpp

@@ -321,18 +321,23 @@ bool SwerveDrive::AtDistSetpoint(bool *at) {
     return true;
 }
 
-bool SwerveDrive::DriveAuto(double max_speed) {
+bool SwerveDrive::DriveAuto(double max_speed, double heading) {
     OKC_CHECK(this->interface_ != nullptr);
 
     double dist = 0;
     OKC_CALL(this->left_front_module_->GetDistance(&dist));
 
     double drive_power = this->dist_pid_->Calculate(dist);
 
-    this->interface_->left_front_drive_motor_output = drive_power;
-    this->interface_->left_back_drive_motor_output = drive_power;
-    this->interface_->right_front_drive_motor_output = drive_power;
-    this->interface_->right_back_drive_motor_output = drive_power;
+    double gyro_reading = 0.0;
+    OKC_CALL(this->GetHeading(&gyro_reading));
+
+    double turn_power = this->heading_pid_->Calculate(gyro_reading);
+
+    this->interface_->left_front_drive_motor_output = drive_power + turn_power;
+    this->interface_->left_back_drive_motor_output = drive_power + turn_power;
+    this->interface_->right_front_drive_motor_output = drive_power - turn_power;
+    this->interface_->right_back_drive_motor_output = drive_power - turn_power;
 
     // clamp the speed
     TeamOKC::Clamp(-max_speed, max_speed, &this->interface_->left_front_drive_motor_output);
@@ -342,17 +347,24 @@ bool SwerveDrive::DriveAuto(double max_speed) {
 
 
     // just drive straight
-    OKC_CALL(this->left_front_module_->SetAngle(0));
-    OKC_CALL(this->left_back_module_->SetAngle(0));
-    OKC_CALL(this->right_front_module_->SetAngle(0));
-    OKC_CALL(this->right_back_module_->SetAngle(0));
+    OKC_CALL(SetSteerOutput(heading));
+
+    return true;
+}
+
+bool SwerveDrive::SetSteerOutput(double angle) {
+    OKC_CHECK(interface_ != nullptr);
+
+    OKC_CALL(this->left_front_module_->SetAngle(angle));
+    OKC_CALL(this->left_back_module_->SetAngle(angle));
+    OKC_CALL(this->right_front_module_->SetAngle(angle));
+    OKC_CALL(this->right_back_module_->SetAngle(angle));
 
     OKC_CALL(this->left_front_module_->GetSteerOutput(&this->interface_->left_front_steer_motor_output));
     OKC_CALL(this->left_back_module_->GetSteerOutput(&this->interface_->left_back_steer_motor_output));
     OKC_CALL(this->right_front_module_->GetSteerOutput(&this->interface_->right_front_steer_motor_output));
     OKC_CALL(this->right_back_module_->GetSteerOutput(&this->interface_->right_back_steer_motor_output));
 
-
     return true;
 }
 
@@ -365,15 +377,7 @@ bool SwerveDrive::AutoBalance(double sign) {
         this->interface_->right_back_drive_motor_output = run_up_speed_ * sign;
 
         // and keep our wheels straight
-        OKC_CALL(this->left_front_module_->SetAngle(0.0));
-        OKC_CALL(this->left_back_module_->SetAngle(0.0));
-        OKC_CALL(this->right_front_module_->SetAngle(0.0));
-        OKC_CALL(this->right_back_module_->SetAngle(0.0));
-
-        OKC_CALL(this->left_front_module_->GetSteerOutput(&this->interface_->left_front_steer_motor_output));
-        OKC_CALL(this->left_back_module_->GetSteerOutput(&this->interface_->left_back_steer_motor_output));
-        OKC_CALL(this->right_front_module_->GetSteerOutput(&this->interface_->right_front_steer_motor_output));
-        OKC_CALL(this->right_back_module_->GetSteerOutput(&this->interface_->right_back_steer_motor_output));
+        OKC_CALL(SetSteerOutput(0.0));
 
         // once we've tilted enough, then we can move on to climbing slower to avoid rocketing over the thing
         if (this->interface_->imu_pitch > tilted_threshold_*sign) {
@@ -387,16 +391,7 @@ bool SwerveDrive::AutoBalance(double sign) {
         this->interface_->right_back_drive_motor_output = tilted_speed_ * sign;
 
         // keep the wheels straight
-        OKC_CALL(this->left_front_module_->SetAngle(0.0));
-        OKC_CALL(this->left_back_module_->SetAngle(0.0));
-        OKC_CALL(this->right_front_module_->SetAngle(0.0));
-        OKC_CALL(this->right_back_module_->SetAngle(0.0));
-
-        OKC_CALL(this->left_front_module_->GetSteerOutput(&this->interface_->left_front_steer_motor_output));
-        OKC_CALL(this->left_back_module_->GetSteerOutput(&this->interface_->left_back_steer_motor_output));
-        OKC_CALL(this->right_front_module_->GetSteerOutput(&this->interface_->right_front_steer_motor_output));
-        OKC_CALL(this->right_back_module_->GetSteerOutput(&this->interface_->right_back_steer_motor_output));
-
+        OKC_CALL(SetSteerOutput(0.0));
 
         // once we've tipped over the station, then we need to drive backwards a little bit
         if (interface_->imu_pitch < reverse_threshold_*sign) {
@@ -409,16 +404,7 @@ bool SwerveDrive::AutoBalance(double sign) {
         this->interface_->right_front_drive_motor_output = drive_backward_speed_ * sign;
         this->interface_->right_back_drive_motor_output = drive_backward_speed_ * sign;
 
-        OKC_CALL(this->left_front_module_->SetAngle(0.0));
-        OKC_CALL(this->left_back_module_->SetAngle(0.0));
-        OKC_CALL(this->right_front_module_->SetAngle(0.0));
-        OKC_CALL(this->right_back_module_->SetAngle(0.0));
-
-        OKC_CALL(this->left_front_module_->GetSteerOutput(&this->interface_->left_front_steer_motor_output));
-        OKC_CALL(this->left_back_module_->GetSteerOutput(&this->interface_->left_back_steer_motor_output));
-        OKC_CALL(this->right_front_module_->GetSteerOutput(&this->interface_->right_front_steer_motor_output));
-        OKC_CALL(this->right_back_module_->GetSteerOutput(&this->interface_->right_back_steer_motor_output));
-
+        OKC_CALL(SetSteerOutput(0.0));
 
         // and then if we're balanced, go ahead and stop us
         if (abs(interface_->imu_pitch) < pitch_threshold_*sign) {

src/main/include/commands/swerve/AutoDriveCommand.h

@@ -19,7 +19,7 @@ class AutoDriveCommand
      *
      * @param swerve The swerve drive used by this command.
      */
-    explicit AutoDriveCommand(std::shared_ptr<SwerveDrive> swerve, double dist, double max_speed);
+    explicit AutoDriveCommand(std::shared_ptr<SwerveDrive> swerve, double dist, double max_speed, double heading);
 
     void Initialize() override;
     void Execute() override;
@@ -30,4 +30,6 @@ class AutoDriveCommand
     std::shared_ptr<SwerveDrive> swerve_;
     double dist_;
     double max_speed_;
+    double heading_;
+
 };

src/main/include/subsystems/SwerveDrive.h

@@ -66,8 +66,9 @@ class SwerveDrive : public frc2::SubsystemBase {
 
     bool InitAuto(TeamOKC::Pose pos, bool keep_heading);
     bool SetDistance(double dist);
-    bool DriveAuto(double max_speed);
+    bool DriveAuto(double max_speed, double heading);
     bool AtDistSetpoint(bool *at);
+    bool SetSteerOutput(double angle);
 
     bool AutoBalance(double sign);
     bool AtBalanceSetpoint(bool *at);