diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 17b4148c..4b4c13bb 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -21,7 +21,7 @@ #include "behaviour/HasBehaviour.h" #include "networktables/NetworkTableInstance.h" - +#include "drivetrain/behaviours/SwerveBehaviours.h" static units::second_t lastPeriodic; @@ -53,7 +53,7 @@ void Robot::RobotInit() { // simulation_timer = frc::Timer(); // robotmap.swerveBase.gyro->Reset(); - + _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d()); wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive); @@ -154,20 +154,8 @@ void Robot::TeleopInit() { wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance(); sched->InterruptAll(); - // frontLeft->SetVoltage(4_V); - // frontRight->SetVoltage(4_V); - // backLeft->SetVoltage(4_V); - // backRight->SetVoltage(4_V); - - - // FMAP("fmap.fmap"); - - // _swerveDrive->OnStart(); - // sched->InterruptAll(); - - //reimplement when vision is reimplemented - - // _swerveDrive->SetPose(_vision->GetAngleToObject(VisionTargetObjects::kNote).first); + robotmap.controllers.triggerDriver->XButton()->SetTrueBehaviour( + wom::make(_swerveDrive, frc::Pose2d(1_m, 0_m, frc::Rotation2d(0_rad)))); } void Robot::TeleopPeriodic() {} diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h index 842da856..68246986 100644 --- a/src/main/include/RobotMap.h +++ b/src/main/include/RobotMap.h @@ -30,15 +30,19 @@ #include "Wombat.h" #include "AlphaArm.h" #include "AlphaArmBehaviour.h" +#include "behaviour/TriggerXboxController.h" +#include "utils/Encoder.h" +#include "utils/PID.h" struct RobotMap { struct Controllers { frc::XboxController driver = frc::XboxController(0); frc::XboxController codriver = frc::XboxController(1); frc::XboxController testController = frc::XboxController(2); + wom::TriggerXboxController* triggerDriver = new wom::TriggerXboxController(0); }; Controllers controllers; - + struct AlphaArmSystem { rev::CANSparkMax alphaArmMotorUp{21, rev::CANSparkMax::MotorType::kBrushless}; rev::CANSparkMax alphaArmMotorDown{26, rev::CANSparkMax::MotorType::kBrushless}; @@ -58,7 +62,7 @@ struct RobotMap { AlphaArmSystem alphaArmSystem; - + struct SwerveBase { ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"}; ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};