Started auto, merged subsystems to auto

Author: prawny-boy

src/main/cpp/AlphaArm.cpp

@@ -0,0 +1,36 @@
+// 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 "AlphaArm.h"
+
+AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config) {}
+
+void AlphaArm::OnUpdate(units::second_t dt) {
+  switch (_state) {
+    case AlphaArmState::kIdle:
+      // transmission translate
+      _config.alphaArmGearbox.motorController->SetVoltage(0_V);
+      _config.wristGearbox.motorController->SetVoltage(0_V);
+
+      break;
+    case AlphaArmState::kRaw:
+      setAlphaArmVoltage = _armVoltage;
+      break;
+    default:
+      std::cout << "oops, wrong state" << std::endl;
+      break;
+  }
+  // transmission translate
+  // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
+  _config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
+  _config.wristGearbox.motorController->SetVoltage(setWristVoltage);
+}
+
+void AlphaArm::SetState(AlphaArmState state) {
+  _state = state;
+}
+
+void AlphaArm::SetRaw(units::volt_t voltR) {
+  _voltR = voltR;
+}

src/main/cpp/Auto.cpp

@@ -1,4 +1,4 @@
-// #include "Auto.h"
+#include "Auto.h"
 
 // std::shared_ptr<Behaviour> Taxi(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
 //   return 
@@ -248,4 +248,12 @@
 //     15. Shoot note
 //     16. Climb down
 //     */
-// }
+// }
+
+std::shared_ptr<Behaviour> AutoTest(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+  return
+    <<make<ArmToSetPoint>(_arm, 0, -90)
+    <<make<DriveToLocation>(_driveBase, raw, distance)
+    <<make<AutoShoot>(_shooter, 8_V)
+    <<make<AutoIntake>(_intake, 8_V)
+} // This auto is a test for auto to see if all things work, it does not build as the behaviours are not done.

src/main/cpp/Behaviours/AlphaArmBehaviour.cpp

@@ -0,0 +1,28 @@
+// 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 "Behaviours/AlphaArmBehaviour.h"
+
+#include <frc/XboxController.h>
+
+AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
+    : _alphaArm(alphaArm), _codriver(codriver) {
+  Controls(alphaArm);
+}
+
+void AlphaArmManualControl::OnTick(units::second_t dt) {
+  if (_codriver->GetXButtonPressed()) {
+    if (_rawControl == true) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+
+  if (_rawControl) {
+    _alphaArm->SetState(AlphaArmState::kRaw);
+    // mess around with _rightStick later
+    _alphaArm->SetRaw(_codriver->GetRightY() * 2_V);
+  }
+}

src/main/cpp/Robot.cpp

@@ -35,6 +35,7 @@ void Robot::RobotPeriodic() {
   mag->OnUpdate(dt);
   climber->OnUpdate(dt);
   intake->OnUpdate(dt);
+  alphaArm->OnUpdate(dt);
 }
 
 void Robot::TeleopInit() {
@@ -69,17 +70,22 @@ void Robot::TeleopInit() {
   climber = new Climber(robotmap.climberSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(climber);
   climber->SetDefaultBehaviour(
-      [this]() { return wom::make<ClimberManualControl>(climber, &robotmap.controllers.coDriver); });
+      [this]() { return wom::make<ClimberManualControl>(climber, &robotmap.controllers.codriver); });
 
   mag = new Mag(robotmap.magSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(mag);
   mag->SetDefaultBehaviour(
-      [this]() { return wom::make<MagManualControl>(mag, &robotmap.controllers.coDriver); });
+      [this]() { return wom::make<MagManualControl>(mag, &robotmap.controllers.codriver); });
 
   intake = new Intake(robotmap.intakeSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(intake);
   intake->SetDefaultBehaviour(
-      [this]() { return wom::make<IntakeManualControl>(intake, &robotmap.controllers.coDriver); });
+      [this]() { return wom::make<IntakeManualControl>(intake, &robotmap.controllers.codriver); });
+
+  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();

src/main/include/AlphaArm.h

@@ -0,0 +1,33 @@
+// 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
+
+#pragma once
+#include <frc/DigitalInput.h>
+
+#include "Wombat.h"
+
+struct AlphaArmConfig {
+  // wom::Gearbox armGearBox;
+  wom::Gearbox alphaArmGearbox;
+  wom::Gearbox wristGearbox;
+};
+
+enum class AlphaArmState { kIdle, kIntakeAngle, kAmpAngle, kSpeakerAngle, kRaw };
+
+class AlphaArm : public ::behaviour::HasBehaviour {
+ public:
+  explicit AlphaArm(AlphaArmConfig config);
+
+  void OnUpdate(units::second_t dt);
+  void SetRaw(units::volt_t volt);
+  void SetState(AlphaArmState state);
+
+ private:
+  AlphaArmConfig _config;
+  AlphaArmState _state = AlphaArmState::kIdle;
+  units::volt_t setAlphaArmVoltage = 0_V;
+  units::volt_t setWristVoltage = 0_V;
+  units::volt_t _armVoltage;
+  units::volt_t _voltR;
+};

src/main/include/Auto.h

@@ -1,17 +1,19 @@
-// #pragma once
+#pragma once
 
-// #include "Wombat.h"
+#include "Wombat.h"
 
-// namespace autos {
-//   std::shared_ptr<behaviour::Behaviour> Taxi(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+namespace autos {
+  std::shared_ptr<behaviour::Behaviour> AutoTest(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-//   std::shared_ptr<behaviour::Behaviour> QuadrupleClose(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  // std::shared_ptr<behaviour::Behaviour> Taxi(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-//   std::shared_ptr<behaviour::Behaviour> QuadrupleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  // std::shared_ptr<behaviour::Behaviour> QuadrupleClose(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-//   std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  // std::shared_ptr<behaviour::Behaviour> QuadrupleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-//   std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
-// }
+  // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+
+  // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+}
 
 

src/main/include/Behaviours/AlphaArmBehaviour.h

@@ -0,0 +1,24 @@
+// 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
+
+#pragma once
+
+#pragma once
+#include <frc/XboxController.h>
+
+#include "AlphaArm.h"
+#include "Wombat.h"
+
+class AlphaArmManualControl : public behaviour::Behaviour {
+ public:
+  explicit AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver);
+  void OnTick(units::second_t dt);
+
+ private:
+  AlphaArm* _alphaArm;
+  frc::XboxController* _codriver;
+  // units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05
+  // )?_codriver->GetRightY():0) * 2_V;
+  bool _rawControl;
+};

src/main/include/Robot.h

@@ -19,6 +19,11 @@
 
 #include <string>
 
+#include "AlphaArm.h"
+#include "Climber.h"
+#include "Intake.h"
+#include "Mag.h"
+#include "Behaviours/AlphaArmBehaviour.h"
 #include "Behaviours/ClimberBehaviour.h"
 #include "Behaviours/IntakeBehaviour.h"
 #include "Behaviours/MagBehaviour.h"
@@ -52,6 +57,8 @@ class Robot : public frc::TimedRobot {
   frc::SendableChooser<std::string> m_path_chooser;
 
   wom::SwerveDrive* _swerveDrive;
+
+  AlphaArm* alphaArm;
   Mag* mag;
   Climber* climber;
   Intake* intake;

src/main/include/RobotMap.h

@@ -21,17 +21,33 @@
 
 #include "Climber.h"
 #include "Mag.h"
-#include "Wombat.h"
+#include "AlphaArm.h"
 #include "Intake.h"
+#include "Behaviours/AlphaArmBehaviour.h"
+#include "Behaviours/ClimberBehaviour.h"
+#include "Behaviours/MagBehaviour.h"
+#include "Behaviours/IntakeBehaviour.h"
+#include "Wombat.h"
 
 struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
-    frc::XboxController coDriver = frc::XboxController(1);
+    frc::XboxController codriver = frc::XboxController(1);
     frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+  struct AlphaArmSystem {
+    rev::CANSparkMax alphaArmMotor{99, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax wristMotor{99, rev::CANSparkMax::MotorType::kBrushless};
+
+    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
+
+    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+  };
+  AlphaArmSystem alphaArmSystem;
+
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{17};