hurray robot map works!!

CurtinFRC · Nov 30, 2023 · 9622a93 · 9622a93
1 parent d707d65
commit 9622a93
Show file tree

Hide file tree

Showing 31 changed files with 345 additions and 381 deletions.
diff --git a/.clang-format b/.clang-format
@@ -1,7 +1,7 @@
 ---
 BasedOnStyle: Google
 AlignOperands: 'true'
-ColumnLimit: '80'
+ColumnLimit: '110'
 NamespaceIndentation: Inner
 TabWidth: '2'
 UseTab: Never

diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <ctre/Phoenix.h>
 #include <frc/XboxController.h>
 
 #include "Wombat.h"
@@ -12,8 +13,121 @@ struct RobotMap {
   };
   Controllers controllers;
 
-  struct Swerve {
+  WPI_TalonFX            frontLeftMovementMotor{99};
+  MotorVoltageController frontLeftMovementVoltageController{&frontLeftMovementMotor};
+  TalonFXEncoder         frontLeftMovementEncoder{&frontLeftMovementMotor, 100};
+  Gearbox                frontLeftMovement{&frontLeftMovementVoltageController, &frontLeftMovementEncoder,
+                            frc::DCMotor::Falcon500(1).WithReduction(6.75)};
 
+  WPI_TalonFX            frontLeftRotationMotor{99};
+  MotorVoltageController frontLeftRotationVoltageController{&frontLeftMovementMotor};
+  TalonFXEncoder         frontLeftRotationEncoder{&frontLeftMovementMotor, 100};
+  Gearbox                frontLeftRotation{&frontLeftMovementVoltageController, &frontLeftMovementEncoder,
+                            frc::DCMotor::Falcon500(1).WithReduction(6.75)};
+
+  SwerveModuleConfig frontLeftConfig{
+    frc::Translation2d{units::meter_t{1}, units::meter_t{1}},
+    frontLeftMovement,
+    frontLeftRotation,
+    PIDConfig<units::radians_per_second, units::volt>(""),
+    PIDConfig<units::meters_per_second, units::volt>(""),
+    PIDConfig<units::radian, units::radians_per_second>(""),
+    PIDConfig<units::meter, units::meters_per_second>(""),
+    units::meter_t{0.05},
+    SwerveModuleName::FrontLeft,
+    ""
+  };
+
+  SwerveModule frontLeft = SwerveModule(frontLeftConfig, SwerveModuleState::kIdle);
+
+  WPI_TalonFX            frontRightMovementMotor{99};
+  MotorVoltageController frontRightMovementVoltageController{&frontRightMovementMotor};
+  TalonFXEncoder         frontRightMovementEncoder{&frontRightMovementMotor, 100};
+  Gearbox                frontRightMovement{&frontRightMovementVoltageController, &frontRightMovementEncoder,
+                            frc::DCMotor::Falcon500(1).WithReduction(6.75)};
+
+  WPI_TalonFX            frontRightRotationMotor{99};
+  MotorVoltageController frontRightRotationVoltageController{&frontRightMovementMotor};
+  TalonFXEncoder         frontRightRotationEncoder{&frontRightMovementMotor, 100};
+  Gearbox                frontRightRotation{&frontRightMovementVoltageController, &frontRightMovementEncoder,
+                            frc::DCMotor::Falcon500(1).WithReduction(6.75)};
+
+  SwerveModuleConfig frontRightConfig{
+    frc::Translation2d{units::meter_t{1}, units::meter_t{1}},
+    frontLeftMovement,
+    frontLeftRotation,
+    PIDConfig<units::radians_per_second, units::volt>(""),
+    PIDConfig<units::meters_per_second, units::volt>(""),
+    PIDConfig<units::radian, units::radians_per_second>(""),
+    PIDConfig<units::meter, units::meters_per_second>(""),
+    units::meter_t{0.05},
+    SwerveModuleName::FrontRight,
+    ""
+  };
+
+  SwerveModule frontRight = SwerveModule(frontRightConfig, SwerveModuleState::kIdle);
+
+  WPI_TalonFX            backLeftMovementMotor{99};
+  MotorVoltageController backLeftMovementVoltageController{&backLeftMovementMotor};
+  TalonFXEncoder         backLeftMovementEncoder{&backLeftMovementMotor, 100};
+  Gearbox                backLeftMovement{&backLeftMovementVoltageController, &backLeftMovementEncoder,
+                            frc::DCMotor::Falcon500(1).WithReduction(6.75)};
+
+  WPI_TalonFX            backLeftRotationMotor{99};
+  MotorVoltageController backLeftRotationVoltageController{&backLeftMovementMotor};
+  TalonFXEncoder         backLeftRotationEncoder{&backLeftMovementMotor, 100};
+  Gearbox                backLeftRotation{&backLeftMovementVoltageController, &backLeftMovementEncoder,
+                            frc::DCMotor::Falcon500(1).WithReduction(6.75)};
+
+  SwerveModuleConfig backLeftConfig{
+    frc::Translation2d{units::meter_t{1}, units::meter_t{1}},
+    frontLeftMovement,
+    frontLeftRotation,
+    PIDConfig<units::radians_per_second, units::volt>(""),
+    PIDConfig<units::meters_per_second, units::volt>(""),
+    PIDConfig<units::radian, units::radians_per_second>(""),
+    PIDConfig<units::meter, units::meters_per_second>(""),
+    units::meter_t{0.05},
+    SwerveModuleName::BackLeft,
+    ""
+  };
+
+  SwerveModule backLeft = SwerveModule(backLeftConfig, SwerveModuleState::kIdle);
+
+  WPI_TalonFX            backRightMovementMotor{99};
+  MotorVoltageController backRightMovementVoltageController{&backRightMovementMotor};
+  TalonFXEncoder         backRightMovementEncoder{&backRightMovementMotor, 100};
+  Gearbox                backRightMovement{&backRightMovementVoltageController, &backRightMovementEncoder,
+                            frc::DCMotor::Falcon500(1).WithReduction(6.75)};
+
+  WPI_TalonFX            backRightRotationMotor{99};
+  MotorVoltageController backRightRotationVoltageController{&backRightMovementMotor};
+  TalonFXEncoder         backRightRotationEncoder{&backRightMovementMotor, 100};
+  Gearbox                backRightRotation{&backRightMovementVoltageController, &backRightMovementEncoder,
+                            frc::DCMotor::Falcon500(1).WithReduction(6.75)};
+
+  SwerveModuleConfig backRightConfig{
+    frc::Translation2d{units::meter_t{1}, units::meter_t{1}},
+    frontLeftMovement,
+    frontLeftRotation,
+    PIDConfig<units::radians_per_second, units::volt>(""),
+    PIDConfig<units::meters_per_second, units::volt>(""),
+    PIDConfig<units::radian, units::radians_per_second>(""),
+    PIDConfig<units::meter, units::meters_per_second>(""),
+    units::meter_t{0.05},
+    SwerveModuleName::BackRight,
+    ""
   };
-  Swerve swerve;
+
+  SwerveModule backRight = SwerveModule(backRightConfig, SwerveModuleState::kIdle);
+
+  SwerveConfig swerveConfig {
+    frontLeft,
+    frontRight,
+    backLeft,
+    backRight
+  };
+
+  Limelight limelight = Limelight("Limelight");
+  Swerve swerve = Swerve(swerveConfig, SwerveState::kIdle, limelight);
 };
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -4,9 +4,7 @@ using namespace behaviour;
 
 // Behaviour
 Behaviour::Behaviour(std::string name, units::time::second_t period)
-    : _bhvr_name(name),
-      _bhvr_period(period),
-      _bhvr_state(BehaviourState::INITIALISED) {}
+    : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {}
 Behaviour::~Behaviour() {
   if (!IsFinished()) Interrupt();
 }
@@ -73,8 +71,7 @@ bool Behaviour::Tick() {
 
     if (dt > 2 * _bhvr_period) {
       std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value()
-                << " Dt(deadline)=" << (2 * _bhvr_period).value()
-                << ". Bhvr: " << GetName() << std::endl;
+                << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl;
     }
 
     if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout)
@@ -91,8 +88,7 @@ bool Behaviour::IsRunning() const {
 }
 
 bool Behaviour::IsFinished() const {
-  return _bhvr_state != BehaviourState::INITIALISED &&
-         _bhvr_state != BehaviourState::RUNNING;
+  return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING;
 }
 
 void Behaviour::Stop(BehaviourState new_state) {
@@ -163,8 +159,7 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) {
 }
 
 std::string ConcurrentBehaviour::GetName() const {
-  std::string msg =
-      (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
+  std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
   for (auto b : _children) msg += b->GetName() + ", ";
   msg += "}";
   return msg;
@@ -179,8 +174,7 @@ void ConcurrentBehaviour::OnStart() {
         using namespace std::chrono_literals;
 
         b->Tick();
-        std::this_thread::sleep_for(std::chrono::milliseconds(
-            (int64_t)(b->GetPeriod().value() * 1000)));
+        std::this_thread::sleep_for(std::chrono::milliseconds((int64_t)(b->GetPeriod().value() * 1000)));
       }
 
       if (IsFinished() && !b->IsFinished()) b->Interrupt();
@@ -256,10 +250,8 @@ void WaitFor::OnTick(units::time::second_t dt) {
 }
 
 // WaitTime
-WaitTime::WaitTime(units::time::second_t time)
-    : WaitTime([time]() { return time; }) {}
-WaitTime::WaitTime(std::function<units::time::second_t()> time_fn)
-    : _time_fn(time_fn) {}
+WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {}
+WaitTime::WaitTime(std::function<units::time::second_t()> time_fn) : _time_fn(time_fn) {}
 
 void WaitTime::OnStart() {
   _time = _time_fn();

diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
@@ -17,8 +17,7 @@ BehaviourScheduler::~BehaviourScheduler() {
 BehaviourScheduler *_scheduler_instance;
 
 BehaviourScheduler *BehaviourScheduler::GetInstance() {
-  if (_scheduler_instance == nullptr)
-    _scheduler_instance = new BehaviourScheduler();
+  if (_scheduler_instance == nullptr) _scheduler_instance = new BehaviourScheduler();
   return _scheduler_instance;
 }
 
@@ -46,8 +45,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) {
         std::lock_guard<std::recursive_mutex> lk(_active_mtx);
         behaviour->Tick();
       }
-      std::this_thread::sleep_for(std::chrono::milliseconds(
-          (int64_t)(behaviour->GetPeriod().value() * 1000)));
+      std::this_thread::sleep_for(
+          std::chrono::milliseconds((int64_t)(behaviour->GetPeriod().value() * 1000)));
     }
   });
 }

diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
@@ -4,8 +4,7 @@
 
 using namespace behaviour;
 
-void HasBehaviour::SetDefaultBehaviour(
-    std::function<std::shared_ptr<Behaviour>(void)> fn) {
+void HasBehaviour::SetDefaultBehaviour(std::function<std::shared_ptr<Behaviour>(void)> fn) {
   _default_behaviour_producer = fn;
 }
 

diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
@@ -3,9 +3,7 @@
 using namespace frc;
 using namespace units;
 
-wom::drivetrain::Drivetrain::Drivetrain(
-    wom::drivetrain::DrivetrainConfig *config)
-    : _config(config) {}
+wom::drivetrain::Drivetrain::Drivetrain(wom::drivetrain::DrivetrainConfig *config) : _config(config) {}
 wom::drivetrain::Drivetrain::~Drivetrain() {}
 
 wom::drivetrain::DrivetrainConfig *wom::drivetrain::Drivetrain::GetConfig() {
@@ -15,8 +13,7 @@ wom::drivetrain::DrivetrainState wom::drivetrain::Drivetrain::GetState() {
   return _state;
 }
 
-void wom::drivetrain::Drivetrain::SetState(
-    wom::drivetrain::DrivetrainState state) {
+void wom::drivetrain::Drivetrain::SetState(wom::drivetrain::DrivetrainState state) {
   _state = state;
 }
 
@@ -28,8 +25,7 @@ void wom::drivetrain::Drivetrain::SetSpeed(wom::drivetrain::TankSpeed speed) {
   _speed = speed;
 }
 
-void wom::drivetrain::Drivetrain::TankControl(double rightSpeed,
-                                              double leftSpeed) {
+void wom::drivetrain::Drivetrain::TankControl(double rightSpeed, double leftSpeed) {
   _config->left1.transmission->SetVoltage(leftSpeed * maxVolts);
   _config->left2.transmission->SetVoltage(leftSpeed * maxVolts);
   _config->left3.transmission->SetVoltage(leftSpeed * maxVolts);

diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -1,12 +1,9 @@
 #include "drivetrain/SwerveDrive.h"
 
-wom::drivetrain::SwerveModule::SwerveModule(
-    wom::drivetrain::SwerveModuleConfig config,
-    wom::drivetrain::SwerveModuleState  state)
-    : _rotationalVelocityPID(config.path + "/pid/rotationalVelocity",
-                             config.rotationalVelocityPID),
-      _movementVelocityPID(config.path + "/pid/movementVelocity",
-                           config.movementVelocityPID),
+wom::drivetrain::SwerveModule::SwerveModule(wom::drivetrain::SwerveModuleConfig config,
+                                            wom::drivetrain::SwerveModuleState  state)
+    : _rotationalVelocityPID(config.path + "/pid/rotationalVelocity", config.rotationalVelocityPID),
+      _movementVelocityPID(config.path + "/pid/movementVelocity", config.movementVelocityPID),
       _rotationalPID(config.path + "/pid/rotationGearbox", config.rotationPID),
       _movementPID(config.path + "/pid/movementGearbox", config.movementPID),
       _config(config),
@@ -22,8 +19,7 @@ wom::drivetrain::SwerveModuleState wom::drivetrain::SwerveModule::GetState() {
   return _state;
 }
 
-void wom::drivetrain::SwerveModule::SetState(
-    wom::drivetrain::SwerveModuleState state) {
+void wom::drivetrain::SwerveModule::SetState(wom::drivetrain::SwerveModuleState state) {
   _state = state;
 }
 
@@ -53,34 +49,28 @@ void wom::drivetrain::SwerveModule::OnStart(units::radian_t offset) {
   _config.movementGearbox.encoder->SetEncoderPosition(offset);
 }
 
-void wom::drivetrain::SwerveModule::PIDControl(units::second_t dt,
-                                               units::radian_t rotation,
-                                               units::meter_t  movement) {
-  units::volt_t feedforwardRotationalVelocity =
-      _config.rotationGearbox.motor.Voltage(
-          0_Nm, _config.rotationGearbox.encoder->GetEncoderAngularVelocity());
-  voltageRotation = _rotationalVelocityPID.Calculate(
-      angularVelocity, dt, feedforwardRotationalVelocity);
+void wom::drivetrain::SwerveModule::PIDControl(units::second_t dt, units::radian_t rotation,
+                                               units::meter_t movement) {
+  units::volt_t feedforwardRotationalVelocity = _config.rotationGearbox.motor.Voltage(
+      0_Nm, _config.rotationGearbox.encoder->GetEncoderAngularVelocity());
+  voltageRotation = _rotationalVelocityPID.Calculate(angularVelocity, dt, feedforwardRotationalVelocity);
   if (voltageRotation > 12_V) {
     voltageRotation = 12_V;
   }
   _config.rotationGearbox.transmission->SetVoltage(voltageRotation);
 
-  units::volt_t feedforwardMovementVelocity =
-      _config.movementGearbox.motor.Voltage(
-          0_Nm, _config.movementGearbox.encoder->GetEncoderAngularVelocity());
-  voltageMovement =
-      _movementVelocityPID.Calculate(velocity, dt, feedforwardMovementVelocity);
+  units::volt_t feedforwardMovementVelocity = _config.movementGearbox.motor.Voltage(
+      0_Nm, _config.movementGearbox.encoder->GetEncoderAngularVelocity());
+  voltageMovement = _movementVelocityPID.Calculate(velocity, dt, feedforwardMovementVelocity);
   if (voltageMovement > 12_V) {
     voltageMovement = 12_V;
   }
   _config.movementGearbox.transmission->SetVoltage(voltageMovement);
 }
 
 units::meters_per_second_t wom::drivetrain::SwerveModule::GetSpeed() {
-  return units::meters_per_second_t{
-      _config.movementGearbox.encoder->GetEncoderAngularVelocity().value() *
-      _config.wheelRadius.value()};
+  return units::meters_per_second_t{_config.movementGearbox.encoder->GetEncoderAngularVelocity().value() *
+                                    _config.wheelRadius.value()};
 }
 
 void wom::drivetrain::SwerveModule::Log() {
@@ -90,10 +80,8 @@ void wom::drivetrain::SwerveModule::Log() {
   table->GetEntry("Angular Voltage").SetDouble(voltageRotation.value());
 }
 
-void wom::drivetrain::SwerveModule::OnUpdate(units::second_t dt,
-                                             units::radian_t rotation,
-                                             units::meter_t  movement,
-                                             units::volt_t   rotationVoltage) {
+void wom::drivetrain::SwerveModule::OnUpdate(units::second_t dt, units::radian_t rotation,
+                                             units::meter_t movement, units::volt_t rotationVoltage) {
   Log();
 
   switch (_state) {
@@ -114,9 +102,8 @@ void wom::drivetrain::SwerveModule::OnUpdate(units::second_t dt,
   }
 }
 
-wom::drivetrain::Swerve::Swerve(wom::drivetrain::SwerveConfig config,
-                                wom::drivetrain::SwerveState  state,
-                                wom::vision::Limelight        vision)
+wom::drivetrain::Swerve::Swerve(wom::drivetrain::SwerveConfig config, wom::drivetrain::SwerveState state,
+                                wom::vision::Limelight vision)
     : _config(config), _state(state), _vision(vision) {}
 
 wom::drivetrain::SwerveConfig wom::drivetrain::Swerve::GetConfig() {
@@ -131,10 +118,8 @@ void wom::drivetrain::Swerve::SetState(wom::drivetrain::SwerveState state) {
   _state = state;
 }
 
-void wom::drivetrain::Swerve::FieldRelativeControl(frc::Pose3d     desiredPose,
-                                                   units::second_t dt) {
-  units::meter_t movement =
-      units::math::hypot(desiredPose.X(), desiredPose.Y());
+void wom::drivetrain::Swerve::FieldRelativeControl(frc::Pose3d desiredPose, units::second_t dt) {
+  units::meter_t  movement = units::math::hypot(desiredPose.X(), desiredPose.Y());
   units::radian_t rotation = units::math::acos(movement / desiredPose.X());
 
   if (rotation > 0_rad) {
@@ -166,9 +151,8 @@ void wom::drivetrain::Swerve::OnStart() {
   std::cout << "Starting Swerve" << std::endl;
 }
 
-void wom::drivetrain::Swerve::OnUpdate(units::second_t        dt,
-                                       wom::vision::Limelight vision,
-                                       frc::Pose3d            desiredPose) {
+void wom::drivetrain::Swerve::OnUpdate(units::second_t dt, wom::vision::Limelight vision,
+                                       frc::Pose3d desiredPose) {
   vision.OnUpdate(dt);
 
   switch (_state) {

diff --git a/wombat/src/main/cpp/drivetrain/behaviours/DrivetrainBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/DrivetrainBehaviours.cpp
@@ -1,7 +1,7 @@
 #include "drivetrain/behaviours/DrivetrainBehaviours.h"
 
-wom::drivetrain::behaviours::TankDrive::TankDrive(
-    wom::drivetrain::Drivetrain *drivebase, frc::XboxController &driver)
+wom::drivetrain::behaviours::TankDrive::TankDrive(wom::drivetrain::Drivetrain *drivebase,
+                                                  frc::XboxController         &driver)
     : _drivebase(drivebase), _driver(driver) {
   Controls(drivebase);
 }