ISAAC'S swerve updates

CurtinFRC · Dec 7, 2023 · 6a7cf20 · 6a7cf20
1 parent 440696f
commit 6a7cf20
Show file tree

Hide file tree

Showing 4 changed files with 27 additions and 14 deletions.
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
@@ -1,12 +1,21 @@
 #include "Robot.h"
 
-void Robot::RobotInit() {}
+void Robot::RobotInit() {
+  BehaviourScheduler::GetInstance()->Register(&robotmap.swerve);
+  robotmap.swerve.SetDefaultBehaviour([this]() {
+    return make<FieldRelativeSwerveDrive>(&robotmap.swerve, robotmap.controllers.driver);
+  });
+}
 void Robot::RobotPeriodic() {}
 
 void Robot::AutonomousInit() {}
 void Robot::AutonomousPeriodic() {}
 
-void Robot::TeleopInit() {}
+void Robot::TeleopInit() {
+  sched = BehaviourScheduler::GetInstance();
+  sched->InterruptAll();
+  sched->Schedule(make<FieldRelativeSwerveDrive>(&robotmap.swerve, robotmap.controllers.driver));
+}
 void Robot::TeleopPeriodic() {}
 
 void Robot::DisabledInit() {}

diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
@@ -3,6 +3,7 @@
 #include <frc/TimedRobot.h>
 
 #include "RobotMap.h"
+#include "Wombat.h"
 
 class Robot : public frc::TimedRobot {
  public:
@@ -21,4 +22,6 @@ class Robot : public frc::TimedRobot {
 
  protected:
  private:
-};
+  behaviour::BehaviourScheduler *sched;
+  RobotMap robotmap;
+};
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
@@ -13,15 +13,15 @@ struct RobotMap {
   };
   Controllers controllers;
 
-  WPI_TalonFX            frontLeftMovementMotor{8};
+  WPI_TalonFX            frontLeftMovementMotor{9};
   MotorVoltageController frontLeftMovementVoltageController{&frontLeftMovementMotor};
-  TalonFXEncoder         frontLeftMovementEncoder{&frontLeftMovementMotor, 18};
+  CanEncoder         frontLeftMovementEncoder{18};
   Gearbox                frontLeftMovement{&frontLeftMovementVoltageController, &frontLeftMovementEncoder,
                             frc::DCMotor::Falcon500(1).WithReduction(6.75)};
 
   WPI_TalonFX            frontLeftRotationMotor{7};
   MotorVoltageController frontLeftRotationVoltageController{&frontLeftMovementMotor};
-  TalonFXEncoder         frontLeftRotationEncoder{&frontLeftMovementMotor, 18};
+  CanEncoder         frontLeftRotationEncoder{18};
   Gearbox                frontLeftRotation{&frontLeftMovementVoltageController, &frontLeftMovementEncoder,
                             frc::DCMotor::Falcon500(1).WithReduction(6.75)};
 
@@ -40,13 +40,13 @@ struct RobotMap {
 
   WPI_TalonFX            frontRightMovementMotor{6};
   MotorVoltageController frontRightMovementVoltageController{&frontRightMovementMotor};
-  TalonFXEncoder         frontRightMovementEncoder{&frontRightMovementMotor, 16};
+  CanEncoder         frontRightMovementEncoder{16};
   Gearbox                frontRightMovement{&frontRightMovementVoltageController, &frontRightMovementEncoder,
                              frc::DCMotor::Falcon500(1).WithReduction(6.75)};
 
   WPI_TalonFX            frontRightRotationMotor{5};
   MotorVoltageController frontRightRotationVoltageController{&frontRightMovementMotor};
-  TalonFXEncoder         frontRightRotationEncoder{&frontRightMovementMotor, 16};
+  CanEncoder         frontRightRotationEncoder{16};
   Gearbox                frontRightRotation{&frontRightMovementVoltageController, &frontRightMovementEncoder,
                              frc::DCMotor::Falcon500(1).WithReduction(6.75)};
 
@@ -65,13 +65,13 @@ struct RobotMap {
 
   WPI_TalonFX            backLeftMovementMotor{2};
   MotorVoltageController backLeftMovementVoltageController{&backLeftMovementMotor};
-  TalonFXEncoder         backLeftMovementEncoder{&backLeftMovementMotor, 19};
+  CanEncoder         backLeftMovementEncoder{19};
   Gearbox                backLeftMovement{&backLeftMovementVoltageController, &backLeftMovementEncoder,
                            frc::DCMotor::Falcon500(1).WithReduction(6.75)};
 
   WPI_TalonFX            backLeftRotationMotor{1};
   MotorVoltageController backLeftRotationVoltageController{&backLeftMovementMotor};
-  TalonFXEncoder         backLeftRotationEncoder{&backLeftMovementMotor, 19};
+  CanEncoder         backLeftRotationEncoder{19};
   Gearbox                backLeftRotation{&backLeftMovementVoltageController, &backLeftMovementEncoder,
                            frc::DCMotor::Falcon500(1).WithReduction(6.75)};
 
@@ -90,14 +90,14 @@ struct RobotMap {
 
   WPI_TalonFX            backRightMovementMotor{4};
   MotorVoltageController backRightMovementVoltageController{&backRightMovementMotor};
-  TalonFXEncoder         backRightMovementEncoder{&backRightMovementMotor, 17};
+  CanEncoder         backRightMovementEncoder{17};
   Gearbox                backRightMovement{&backRightMovementVoltageController, &backRightMovementEncoder,
                             frc::DCMotor::Falcon500(1).WithReduction(6.75)};
 
   WPI_TalonFX            backRightRotationMotor{3};
   MotorVoltageController backRightRotationVoltageController{&backRightMovementMotor};
-  TalonFXEncoder         backRightRotationEncoder{&backRightMovementMotor, 17};
-  Gearbox                backRightRotation{&backRightMovementVoltageController, &backRightMovementEncoder,
+  CanEncoder         backRightRotationEncoder{17};
+  Gearbox                backRightRotation{&backRightMovementVoltageController, &backRightRotationEncoder,
                             frc::DCMotor::Falcon500(1).WithReduction(6.75)};
 
   SwerveModuleConfig backRightConfig{frc::Translation2d{units::meter_t{1}, units::meter_t{1}},
@@ -117,4 +117,4 @@ struct RobotMap {
 
   Limelight limelight = Limelight("Limelight");
   Swerve    swerve    = Swerve(swerveConfig, SwerveState::kIdle, &limelight);
-};
+};
diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h
@@ -6,6 +6,7 @@
 #include "drivetrain/Drivetrain.h"
 #include "drivetrain/SwerveDrive.h"
 #include "drivetrain/behaviours/DrivetrainBehaviours.h"
+#include "drivetrain/behaviours/SwerveBehaviours.h"
 #include "subsystems/Arm.h"
 #include "subsystems/Elevator.h"
 #include "subsystems/Shooter.h"