formatted (#133)

CurtinFRC · Feb 11, 2024 · 6e6b961 · 6e6b961
1 parent 5b51622
commit 6e6b961
Show file tree

Hide file tree

Showing 32 changed files with 449 additions and 1,347 deletions.
diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
@@ -3,107 +3,114 @@
 // of the MIT License at the root of this project
 
 #include "AlphaArm.h"
-//fiddle with these values
-AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config), _pidWom(_config.path + "/pid", config.pidConfigA) /*_table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)*/{}
+
+// fiddle with these values
+AlphaArm::AlphaArm(AlphaArmConfig config)
+    : _config(config),
+      _pidWom(_config.path + "/pid",
+              config.pidConfigA) /*_table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)*/ {}
 
 void AlphaArm::OnStart() {
   started = false;
   // _pidWom.Reset();
   // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition());
 }
 
-void AlphaArm::OnUpdate(units::second_t dt){
+void AlphaArm::OnUpdate(units::second_t dt) {
   _table->GetEntry("Error").SetDouble(_pidWom.GetError().value());
   _table->GetEntry("Current Pos").SetDouble(_config.alphaArmGearbox.encoder->GetEncoderPosition().value());
   _table->GetEntry("Setpoint").SetDouble(_pidWom.GetSetpoint().value());
   _table->GetEntry("State ").SetString(_stateName);
-        switch(_state){
-          case AlphaArmState::kIdle:
-            _stateName = "Idle";
-            // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition());
-
-
-            _setAlphaArmVoltage = 0_V;
+  switch (_state) {
+    case AlphaArmState::kIdle:
+      _stateName = "Idle";
+      // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition());
+
+      _setAlphaArmVoltage = 0_V;
 
-          break;
-          case AlphaArmState::kRaw:
-            _stateName = "Raw";
-
-            _setAlphaArmVoltage = _rawArmVoltage;
+      break;
+    case AlphaArmState::kRaw:
+      _stateName = "Raw";
 
-          break;
-          case AlphaArmState::kAmpAngle:
-          {
-            _stateName = "Amp Angle";
+      _setAlphaArmVoltage = _rawArmVoltage;
 
-            // _pidWom.SetSetpoint(_goal);
-            // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V);
+      break;
+    case AlphaArmState::kAmpAngle: {
+      _stateName = "Amp Angle";
 
-            if (started) {
-              if (_controlledRawVoltage.value() == 0) {
-                if (-_config.alphaArmGearbox.encoder->GetEncoderPosition() > (_startingPos + (3.1415_rad/2))) {
-                    // _pidWom.SetSetpoint(_encoderSetpoint);
-                    // _setAlphaArmVoltage = -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
-                //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
-                // } else if (_config.alphaArmGearbox.encoder->GetEncoderPosition() < 0_rad) {
-                //   _pidWom.SetSetpoint(_encoderSetpoint);
-                //   _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
-                //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
-                  _setAlphaArmVoltage = 0_V;
-                } else {
-                  _pidWom.SetSetpoint(_encoderSetpoint);
-                  _setAlphaArmVoltage = -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
-                //   _pidWom.Reset();
-                //   _encoderSetpoint = _config.alphaArmGearbox.encoder->GetEncoderPosition();
-                //   _setAlphaArmVoltage = _controlledRawVoltage;
-                }
-              } else {
-                _pidWom.Reset();
-                _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
-                _setAlphaArmVoltage = _controlledRawVoltage;
-              }
-            } else {
-              _pidWom.Reset();
-              _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
-              _setAlphaArmVoltage = _controlledRawVoltage;
+      // _pidWom.SetSetpoint(_goal);
+      // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt,
+      // 0_V);
 
-              if (std::abs(_controlledRawVoltage.value()) > 0) {
-                _startingPos = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
-                started = true;
-              }
-            }
-
+      if (started) {
+        if (_controlledRawVoltage.value() == 0) {
+          if (-_config.alphaArmGearbox.encoder->GetEncoderPosition() > (_startingPos + (3.1415_rad / 2))) {
+            // _pidWom.SetSetpoint(_encoderSetpoint);
+            // _setAlphaArmVoltage =
+            // -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
+            //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
+            // } else if (_config.alphaArmGearbox.encoder->GetEncoderPosition() < 0_rad) {
+            //   _pidWom.SetSetpoint(_encoderSetpoint);
+            //   _setAlphaArmVoltage =
+            //   _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
+            //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
+            _setAlphaArmVoltage = 0_V;
+          } else {
+            _pidWom.SetSetpoint(_encoderSetpoint);
+            _setAlphaArmVoltage =
+                -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
+            //   _pidWom.Reset();
+            //   _encoderSetpoint = _config.alphaArmGearbox.encoder->GetEncoderPosition();
+            //   _setAlphaArmVoltage = _controlledRawVoltage;
           }
-            break;
-          case AlphaArmState::kSpeakerAngle:
-            _stateName = "Speaker Angle";
-            //_pidWom.SetSetpoint(_goal);
-           // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V);
-              break;
-          case AlphaArmState::kStowed:
-            _stateName = "Stowed";
-            //_pidWom.SetSetpoint(_goal);
-            //_setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V);
-          break;
-          default:
-            _stateName = "Error";
-            std::cout << "oops, wrong state" << std::endl;
-          break;
+        } else {
+          _pidWom.Reset();
+          _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
+          _setAlphaArmVoltage = _controlledRawVoltage;
+        }
+      } else {
+        _pidWom.Reset();
+        _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
+        _setAlphaArmVoltage = _controlledRawVoltage;
+
+        if (std::abs(_controlledRawVoltage.value()) > 0) {
+          _startingPos = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
+          started = true;
         }
-        std::cout << " ARM POSITION: " << _config.alphaArmGearbox.encoder->GetEncoderPosition().value() << std::endl;
-        //std::cout << "OUTPUT VOLTAGE: " << _setAlphaArmVoltage.value() << std::endl;
-        _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
+      }
+    } break;
+    case AlphaArmState::kSpeakerAngle:
+      _stateName = "Speaker Angle";
+      // _pidWom.SetSetpoint(_goal);
+      // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt,
+      // 0_V);
+      break;
+    case AlphaArmState::kStowed:
+      _stateName = "Stowed";
+      // _pidWom.SetSetpoint(_goal);
+      // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt,
+      // 0_V);
+      break;
+    default:
+      _stateName = "Error";
+      std::cout << "oops, wrong state" << std::endl;
+      break;
+  }
+  std::cout << " ARM POSITION: " << _config.alphaArmGearbox.encoder->GetEncoderPosition().value()
+            << std::endl;
+  // std::cout << "OUTPUT VOLTAGE: " << _setAlphaArmVoltage.value() << std::endl;
+  _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
 }
 
 void AlphaArm::SetState(AlphaArmState state) {
   _state = state;
 }
 
-void AlphaArm::SetGoal(units::radian_t goal){
+void AlphaArm::SetGoal(units::radian_t goal) {
   _goal = goal;
 }
 
-void AlphaArm::SetArmRaw(units::volt_t voltage){
+void AlphaArm::SetArmRaw(units::volt_t voltage) {
   std::cout << "VOLTAGE: " << voltage.value() << std::endl;
   _rawArmVoltage = voltage;
 }

diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -12,7 +12,6 @@ AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxContro
 }
 
 void AlphaArmManualControl::OnTick(units::second_t dt) {
-
   if (_codriver->GetStartButtonPressed()) {
     if (_rawControl == true) {
       _rawControl = false;
@@ -23,7 +22,7 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
 
   if (_rawControl) {
     _alphaArm->SetState(AlphaArmState::kRaw);
-    _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V);  
+    _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V);
   } else {
     _alphaArm->SetState(AlphaArmState::kAmpAngle);
     _alphaArm->setControllerRaw(wom::deadzone(_codriver->GetRightY()) * 12_V);

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
@@ -78,4 +78,4 @@ void Intake::setRaw(units::volt_t voltage) {
 }
 IntakeState Intake::getState() {
   return _state;
-}
+}
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
@@ -60,4 +60,4 @@ IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
 
-void IntakeAutoControl::OnTick(units::second_t dt) {}
+void IntakeAutoControl::OnTick(units::second_t dt) {}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
@@ -49,7 +49,6 @@ void Robot::RobotInit() {
   // shooter->SetDefaultBehaviour(
   //     [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
 
-
   // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
   // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
 
@@ -59,20 +58,16 @@ void Robot::RobotInit() {
 
   // robotmap.swerveBase.gyro->Reset();
 
-  _swerveDrive =
-      new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  _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->SetDefaultBehaviour(
+      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
   // alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   // wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   // alphaArm->SetDefaultBehaviour(
   //     [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
-  
+
   // // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // // m_driveSim = wom::TempSimSwerveDrive();
 
@@ -97,38 +92,38 @@ void Robot::RobotInit() {
   // robotmap.swerveBase.moduleConfigs[0].driveMotor.motorController->SetInverted(true);
   // robotmap.swerveBase.moduleConfigs[2].driveMotor.motorController->SetInverted(true);
 
-  //robotmap.alphaArmSystem.armEncoder->Reset();
-
-
+  // robotmap.alphaArmSystem.armEncoder->Reset();
+
   // robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
   // robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
   // robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
   // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
 
   lastPeriodic = wom::now();
-
-
 }
 
 void Robot::RobotPeriodic() {
-  //double encoderDistance = robotmap.alphaArmSystem.armEncoder.GetDistance();
+  // double encoderDistance = robotmap.alphaArmSystem.armEncoder.GetDistance();
   auto dt = wom::now() - lastPeriodic;
   lastPeriodic = wom::now();
 
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
   sched->Tick();
 
-  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
   // shooter->OnUpdate(dt);
   // intake->OnUpdate(dt);
   // alphaArm->OnUpdate(dt);
   _swerveDrive->OnUpdate(dt);
-
 }
 
 void Robot::AutonomousInit() {

diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
@@ -1,22 +1,22 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
-#include "Shooter.h"
 
+#include "Shooter.h"
 
-Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {} 
+Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {}
 
 void Shooter::OnStart() {
   _pid.Reset();
 }
 
-
 void Shooter::OnUpdate(units::second_t dt) {
   // _pid.SetTolerance(0.5, 4);
   table->GetEntry("Error").SetDouble(_pid.GetError().value());
   // table->GetEntry("Acceleration Error").SetDouble(_pid.GetVelocityError());
   table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint().value());
-  // table->GetEntry("Current Pos").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
+  // table->GetEntry("Current
+  // Pos").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
   // table->GetEntry("EncoderValue").SetDouble(_config.ShooterGearbox.encoder->GetVelocityValue());
   table->GetEntry("Shooting").SetString(_statename);
   switch (_state) {
@@ -29,13 +29,13 @@ void Shooter::OnUpdate(units::second_t dt) {
       // if (_shooterSensor.Get()) {
       //   _state = ShooterState::kReverse;
       // }
-
     } break;
     case ShooterState::kSpinUp: {
       _statename = "SpinUp";
       std::cout << "KSpinUp" << std::endl;
       _pid.SetSetpoint(_goal);
-      units::volt_t pidCalculate = units::volt_t {_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      units::volt_t pidCalculate =
+          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
       std::cout << "KShooting" << std::endl;
 
       if (_pid.IsStable()) {
@@ -49,13 +49,13 @@ void Shooter::OnUpdate(units::second_t dt) {
       } else {
         _setVoltage = holdVoltage;
       }
-
     } break;
     case ShooterState::kShooting: {
       _statename = "Shooting";
-      
+
       _pid.SetSetpoint(_goal);
-      units::volt_t pidCalculate = units::volt_t {_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      units::volt_t pidCalculate =
+          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
       std::cout << "KShooting" << std::endl;
 
       if (_pid.IsStable()) {
@@ -68,7 +68,6 @@ void Shooter::OnUpdate(units::second_t dt) {
       } else {
         _setVoltage = holdVoltage;
       }
-
     } break;
 
     case ShooterState::kReverse: {
@@ -95,8 +94,8 @@ void Shooter::OnUpdate(units::second_t dt) {
     } break;
   }
   // table->GetEntry("Motor OutPut").SetDouble(_setVoltage.value());
-  table->GetEntry("Encoder Output").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
-
+  table->GetEntry("Encoder Output")
+      .SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
 
   _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
 }
@@ -116,6 +115,6 @@ void Shooter::SetRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
   _state = ShooterState::kRaw;
 }
-void Shooter::SetPidGoal(units::radians_per_second_t goal) { 
+void Shooter::SetPidGoal(units::radians_per_second_t goal) {
   _goal = goal;
 }
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
@@ -36,7 +36,7 @@ void ShooterManualControl::OnTick(units::second_t dt) {
     } else if (_codriver->GetYButton()) {
       _shooter->SetPidGoal(300_rad_per_s);
       _shooter->SetState(ShooterState::kSpinUp);
-    }  else if (_codriver->GetLeftTriggerAxis() > 0.1) {
+    } else if (_codriver->GetLeftTriggerAxis() > 0.1) {
       _shooter->SetState(ShooterState::kRaw);
       _shooter->SetRaw(12_V * _codriver->GetLeftTriggerAxis());
     } else if (_codriver->GetRightTriggerAxis() > 0.1) {
-Original file line number
+Diff line change
@@ Expand Up / @@ -78,4 +78,4 @@ void Intake::setRaw(units::volt_t voltage) { @@
     }
     IntakeState Intake::getState() {
       return _state;
-    }
+    }