Arm (CurtinFRC#103)

* Alpha Arm vers 1

* Alpha Arm ver 1 w/ explicit

* Alpha arm v1 formatting

* Alpha arm w/ wrist v1

* formatted

* Alpha arm v1 with ports

* Working alpha arm no encoders

---------

Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>

src/main/cpp/AlphaArm.cpp

@@ -0,0 +1,67 @@
+// 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) {}
+
+AlphaArmConfig AlphaArm::GetConfig(){
+    return _config;
+}
+
+void AlphaArm::OnUpdate(units::second_t dt){
+        switch(_state){
+            case AlphaArmState::kIdle:
+
+            //_config.alphaArmGearbox.motorController->SetVoltage(0_V);
+            //_config.wristGearbox.motorController->SetVoltage(0_V);
+            _setAlphaArmVoltage = 0_V;
+            _setWristVoltage = 0_V;
+
+            break;
+            case AlphaArmState::kRaw:
+            _setAlphaArmVoltage = _rawArmVoltage;
+            _setWristVoltage = _rawWristVoltage;
+            _config.alphaArmGearbox.motorController->SetVoltage(_rawArmVoltage);
+            _config.wristGearbox.motorController->SetVoltage(_rawWristVoltage);
+
+            break;
+            case AlphaArmState::kForwardWrist:
+            _config.wristGearbox.motorController->SetVoltage(3_V);
+            _setWristVoltage = 3_V;
+
+            case AlphaArmState::kReverseWrist:
+            _config.wristGearbox.motorController->SetVoltage(-3_V);
+            _setWristVoltage = -3_V;
+            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);
+        _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
+        _config.wristGearbox.motorController->SetVoltage(_setWristVoltage);
+
+        //_config.wristGearbox.motorController->SetVoltage(_setVoltage);
+}
+
+void AlphaArm::SetState(AlphaArmState state) {
+  _state = state;
+}
+
+// void AlphaArm::SetRaw(units::volt_t voltage){
+//     _rawArmVoltage = voltage;
+//     _rawWristVoltage = voltage;
+// }
+
+void AlphaArm::SetArmRaw(units::volt_t voltage){
+    _rawArmVoltage = voltage;
+}
+
+void AlphaArm::setWristRaw(units::volt_t voltage){
+    _rawWristVoltage = voltage;
+}

src/main/cpp/AlphaArmBehaviour.cpp

@@ -0,0 +1,35 @@
+// 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 "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);
+    _alphaArm->SetArmRaw(_codriver->GetRightY() * 6_V);
+    _alphaArm->setWristRaw(_codriver->GetLeftY() * -6_V);
+  } else {
+    if (_codriver->GetRightBumperPressed()){
+        _alphaArm->SetState(AlphaArmState::kForwardWrist);
+    }
+    if (_codriver->GetLeftBumperPressed()){
+        _alphaArm->SetState(AlphaArmState::kReverseWrist);
+    }
+  }
+}

src/main/cpp/Robot.cpp

@@ -35,21 +35,25 @@ void Robot::RobotInit() {
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
-  frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
+  // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
 
-  m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
+  // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
 
-  m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
-  m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
+  // m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
+  // m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
 
-  frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
+  // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
 
-  frc::SmartDashboard::PutData("Field", &m_field);
+  // frc::SmartDashboard::PutData("Field", &m_field);
 
-  simulation_timer = frc::Timer();
+  // simulation_timer = frc::Timer();
 
-  robotmap.swerveBase.gyro->Reset();
+  // robotmap.swerveBase.gyro->Reset();
 
+  // _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 =
       new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
@@ -62,6 +66,12 @@ void Robot::RobotInit() {
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 
+  alphaArm = 
+      new AlphaArm(robotmap.alphaArmSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
+  alphaArm->SetDefaultBehaviour(
+      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
+
   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);
@@ -100,6 +110,8 @@ void Robot::RobotPeriodic() {
   robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
+  _swerveDrive->OnUpdate(dt);
+  alphaArm->OnUpdate(dt);
   intake->OnUpdate(dt);
 }
 

src/main/include/AlphaArm.h

@@ -0,0 +1,48 @@
+// 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 alphaArmGearbox;
+    wom::Gearbox wristGearbox;
+
+};
+
+enum class AlphaArmState {
+    kIdle,
+    kIntakeAngle,
+    kAmpAngle,
+    kSpeakerAngle,
+    kForwardWrist,
+    kReverseWrist,
+    kRaw
+};
+
+class AlphaArm : public::behaviour::HasBehaviour{
+    public:
+     explicit AlphaArm(AlphaArmConfig config);
+
+    void OnUpdate(units::second_t dt);
+    void SetArmRaw(units::volt_t voltage);
+    void setWristRaw(units::volt_t voltage);
+    void SetState(AlphaArmState state);
+    AlphaArmConfig GetConfig();
+    //void SetRaw(units::volt_t voltage);
+
+    private:
+    AlphaArmConfig _config;
+    AlphaArmState _state = AlphaArmState::kIdle;
+    units::volt_t _setAlphaArmVoltage = 0_V;
+    units::volt_t _setWristVoltage = 0_V;
+
+    units::volt_t _rawArmVoltage = 0_V;
+    units::volt_t _rawWristVoltage = 0_V;
+
+
+
+};

src/main/include/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

@@ -13,6 +13,8 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <string>
 
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "RobotMap.h"
@@ -49,6 +51,8 @@ class Robot : public frc::TimedRobot {
 
   wom::SwerveDrive* _swerveDrive;
 
+  AlphaArm *alphaArm;
+
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;

src/main/include/RobotMap.h

@@ -16,6 +16,8 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "Shooter.h"
@@ -25,9 +27,21 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
+    frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+  struct AlphaArmSystem {
+    rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax wristMotor{15, 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 IntakeSystem {
     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};