Ri3d shooter (#61)

* oui oui baguette

* update googletest (bodge for now)

* shooter raw implemented for ri3d

---------

Co-authored-by: JoystickMaster-race <97269195+JoystickMaster-race@users.noreply.github.com>

src/main/cpp/Robot.cpp

@@ -9,8 +9,15 @@ static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
 
-    lastPeriodic = wom::now();
-    intake = new Intake(map.intakeSystem.config);
+ lastPeriodic = wom::now();
+ shooter = new Shooter(map.shooterSystem.config);
+ wom::BehaviourScheduler::GetInstance()->Register(shooter);
+ shooter->SetDefaultBehaviour([this] () {
+    return wom::make<ShooterManualControl>(shooter, map.codriver);
+
+ });
+}
+intake = new Intake(map.intakeSystem.config);
     wom::BehaviourScheduler::GetInstance()->Register(intake);
     intake->SetDefaultBehaviour([this]() {
         return wom::make<IntakeManualControl>(intake, map.codriver);
@@ -23,27 +30,28 @@ void Robot::RobotInit() {
     return wom::make<TankManualControl>(tank, map.driver);
   });
     });
-}
 void Robot::RobotPeriodic() {
-    units::second_t dt = wom::now() - lastPeriodic;
-    lastPeriodic = wom::now();
+ units::second_t dt = wom::now() - lastPeriodic;
+ lastPeriodic = wom::now();
 
-    loop.Poll();
-    wom::BehaviourScheduler::GetInstance()->Tick();
+ loop.Poll();
+ wom::BehaviourScheduler::GetInstance()->Tick();
 
-    intake->OnUpdate(dt);
-    tank->OnUpdate(dt);
+ shooter->OnUpdate(dt);
+ intake->OnUpdate(dt);
+ tank->OnUpdate(dt);
+
 }
 
 void Robot::AutonomousInit() {}
 void Robot::AutonomousPeriodic() {}
 
 void Robot::TeleopInit() {
-
     loop.Clear();
     wom::BehaviourScheduler *Scheduler = wom::BehaviourScheduler::GetInstance();
     Scheduler->InterruptAll();
 
+
 }
 void Robot::TeleopPeriodic() {}
 

src/main/cpp/Shooter.cpp

@@ -0,0 +1,82 @@
+#include "Shooter.h"
+
+Shooter::Shooter(ShooterConfig config) : _config(config) {}
+
+void Shooter::OnUpdate(units::second_t dt) {
+
+  switch (_launcherState) {
+    case LauncherState::kIdle: // no movement
+     _launcherVoltage = 0_V;
+     break;
+    // case LauncherState::kLaunching: // keep rpm constant at 300
+    //   _launcherPID.SetSetpoint(300_rpm);
+    //   _launcherVoltage = _launcherPID.Calculate(_launcherRpm, dt, 0.2_V);
+    //  break;
+    case LauncherState::kSpinUp: // get to 300 rpm
+      _launcherVoltage = 5_V;
+     break;
+    case LauncherState::kSpinDown: // go to 0 rpm (slowly)
+      _launcherVoltage = 0_V;
+     break;
+    // case LauncherState::kInBattery: // 300 rpm
+    //   ShooterConfig.launcherPID.SetSetpoint(300_rpm);
+    //   _launcherVoltage = ShooterConfig.launcherPID.Calculate(_launcherRpm);
+    //  break;
+    case LauncherState::kRaw: //set voltage manually
+     _launcherVoltage = _launcherRawVoltage;
+     break;
+    default:
+     std::cout << "Error launcher in invalid state" << std::endl;
+  }
+
+  switch (_loaderState) {
+    case LoaderState::kIdle: // no movement
+      _loaderVoltage = 0_V;
+     break;
+    case LoaderState::kLoading: // inserting (blind)
+     _loaderVoltage = 5_V;
+     break;
+    case LoaderState::kReversing: // go backwards
+      _loaderVoltage = -5_V;
+     break;
+    case LoaderState::kRaw: // set voltage manually
+      _loaderVoltage = _loaderRawVoltage;
+     break;
+    default:
+     std::cout << "Error loader in invalid state" << std::endl;
+  }
+
+  // Update motors 
+  _config.launcherGearBox.transmission->SetVoltage(_launcherVoltage);
+  _config.loaderGearBox.transmission->SetVoltage(_loaderVoltage);
+}
+
+
+// Launcher
+void Shooter::setLauncherState(LauncherState launcherState) {
+  _launcherState = launcherState;
+} 
+
+void Shooter::setLauncherRaw(units::volt_t voltage) {
+  _launcherRawVoltage = voltage;
+}
+
+void Shooter::setLauncherRPM(units::revolutions_per_minute_t rpm) {
+  _launcherRpm = rpm;
+}
+
+
+// Loader
+void Shooter::setLoaderState(LoaderState loaderState) {
+  _loaderState = loaderState;
+}
+
+void Shooter::setLoaderRaw(units::volt_t voltage) {
+  _loaderRawVoltage = voltage;
+}
+
+void Shooter::setLoaderRPM(units::revolutions_per_minute_t rpm) {
+  _loaderRpm = rpm;
+}
+
+

src/main/cpp/ShooterBehaviour.cpp

@@ -0,0 +1,54 @@
+#include "ShooterBehaviour.h"
+
+ShooterManualControl::ShooterManualControl(Shooter *shooter, frc::XboxController &codriver) : _shooter(shooter), _codriver(codriver) {
+ Controls(shooter);
+}
+
+void ShooterManualControl::OnTick(units::second_t dt) {
+
+  // if (_codriver.GetAButton()) {
+  //  _shooter->setLauncherState(LauncherState::kSpinUp);
+  // } else if (_codriver.GetBButton()) {
+  //  _shooter->setLauncherState(LauncherState::kSpinDown);
+  // } else if (_codriver.GetXButton()) {
+  //  _shooter->setLauncherState(LauncherState::kInBattery);
+  // } else if (_codriver.GetStartButton()) {
+  //  _shooter->setLauncherState(LauncherState::kIdle);
+  // } else if (_codriver.GetBackButton()) {
+  //  _shooter->setLauncherState(LauncherState::kIdle);
+  // }
+
+  // use raw states for now
+  if (_codriver.GetRightY() > 0.05) {
+    _shooter->setLauncherState(LauncherState::kRaw);
+    _shooter->setLauncherRaw(_codriver.GetRightY() * 8_V);
+  } else if (_codriver.GetRightY() < -0.05) {
+    _shooter->setLauncherState(LauncherState::kRaw);
+    _shooter->setLauncherRaw(_codriver.GetRightY() * 8_V);
+  } else {
+    _shooter->setLauncherState(LauncherState::kIdle);
+  }
+
+  if (_codriver.GetLeftY() > 0.05) {
+    _shooter->setLoaderState(LoaderState::kRaw);
+    _shooter->setLoaderRaw(_codriver.GetLeftY() * 4_V);
+  } else if (_codriver.GetLeftY() < -0.05) {
+    _shooter->setLoaderState(LoaderState::kRaw);
+    _shooter->setLoaderRaw(_codriver.GetLeftY() * 4_V);
+  } else {
+    _shooter->setLoaderState(LoaderState::kIdle);
+  }
+
+
+  if (_codriver.GetAButton()) {
+    _shooter->setLauncherState(LauncherState::kSpinUp);
+  } else if (_codriver.GetBButton()) {
+    _shooter->setLauncherState(LauncherState::kSpinDown);
+  // launch ball
+  } else if (_codriver.GetXButton()) {
+    _shooter->setLoaderState(LoaderState::kLoading);
+  }
+  else {
+    _shooter->setLoaderState(LoaderState::kIdle);
+  }
+}

src/main/cpp/practice-arm/arm.cpp

@@ -0,0 +1,29 @@
+#include "arm.h"
+
+Arm::Arm(ArmConfig config) : _config(config) {}
+
+void Arm::OnUpdate(units::second_t dt) {
+
+  switch (_state) {
+    case ArmState::kIdle:
+     break;
+    case ArmState::kRaw:
+     break;
+    case ArmState::kAngle:
+     break;
+    default:
+     std::cout << "Error arm in invalid state" << std::endl;
+  }
+}
+
+void Arm::setState(ArmState state) {
+  _state = state;
+}
+
+void Arm::setAngle(units::degree_t angle) {
+  _angle = angle;
+}
+
+void Arm::setRaw(units::volt_t voltage) {
+  _voltage = voltage;
+}

src/main/cpp/practice-arm/arm.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include "Wombat.h"
+
+struct ArmConfig {
+ wom::Gearbox armGearBox;
+};
+
+enum class ArmState {
+ kIdle,
+ kRaw,
+ kAngle,
+};
+
+class Arm : public behaviour::HasBehaviour {
+ public:
+  Arm(ArmConfig config);
+
+  void OnUpdate(units::second_t dt);
+
+  void setState(ArmState state);
+  void setAngle(units::degree_t angle);
+  void setRaw(units::volt_t voltage);
+
+ private:
+  ArmConfig _config;
+  ArmState _state = ArmState::kIdle;
+
+  units::degree_t _angle;
+  units::volt_t _voltage;
+};

src/main/cpp/practice-arm/armBehaviour.cpp

@@ -0,0 +1,17 @@
+#include "armBehaviour.h"
+
+ArmManualControl::ArmManualControl(Arm *arm, frc::XboxController &codriver) : _arm(arm), _codriver(codriver) {
+ Controls(arm);
+}
+
+void ArmManualControl::OnTick(units::second_t dt) {
+
+ if (_codriver.GetRightBumper()) {
+  _arm->setRaw(5_V);
+ } else if (_codriver.GetLeftBumper()) {
+  _arm->setRaw(-5_V);
+ } else {
+  _arm->setRaw(0_V);
+ }
+
+}

src/main/cpp/practice-arm/armBehaviour.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "Wombat.h"
+#include <frc/XboxController.h>
+#include "arm.h"
+
+class ArmManualControl : public behaviour::Behaviour {
+ public:
+  ArmManualControl(Arm *arm, frc::XboxController &codriver);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Arm *_arm;
+  frc::XboxController &_codriver;
+};

src/main/include/Robot.h

@@ -9,6 +9,10 @@
 #include "Wombat.h"
 #include "RobotMap.h"
 #include <frc/TimedRobot.h>
+#include "RobotMap.h"
+#include "Shooter.h"
+#include "ShooterBehaviour.h"
+#include <frc/event/EventLoop.h>
 
 #include "Wombat.h"
 #include "RobotMap.h"
@@ -31,10 +35,13 @@ class Robot : public frc::TimedRobot {
 
  private:
 
- frc::EventLoop loop;
- RobotMap map;
- Intake *intake;
- TankDrive *tank;
+  frc::EventLoop loop;
+
+
+  RobotMap map;
+  Shooter *shooter;
+  Intake *intake;
+  TankDrive *tank;
 
 };
 

src/main/include/Shooter.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "Wombat.h"
+
+struct ShooterConfig {
+  std::string path;
+
+  wom::Gearbox launcherGearBox;
+  wom::Gearbox loaderGearBox;
+};  
+
+enum class LauncherState {
+ kIdle,
+ kLaunching,
+ kSpinUp,
+ kSpinDown,
+ kInBattery,
+ kRaw
+};
+
+enum class LoaderState {
+  kIdle,
+  kLoading,
+  kReversing,
+  kRaw
+};
+
+class Shooter : public behaviour::HasBehaviour {
+ public:
+  Shooter(ShooterConfig config);
+
+  void OnUpdate(units::second_t dt);
+
+  void setLauncherState(LauncherState state);
+  void setLauncherRaw(units::volt_t voltage);
+  void setLauncherRPM(units::revolutions_per_minute_t rpm);
+  void setLoaderState(LoaderState state);
+  void setLoaderRaw(units::volt_t voltage);
+  void setLoaderRPM(units::revolutions_per_minute_t rpm);
+
+ private:
+  ShooterConfig _config;
+  LauncherState _launcherState = LauncherState::kIdle;
+
+  LoaderState _loaderState = LoaderState::kIdle;
+
+  units::volt_t _launcherVoltage;
+  units::volt_t _launcherRawVoltage;
+  units::revolutions_per_minute_t _launcherRpm = 0_rpm;
+
+  units::volt_t _loaderVoltage;
+  units::volt_t _loaderRawVoltage;
+  units::revolutions_per_minute_t _loaderRpm = 0_rpm;
+
+  // wom::PIDController<units::revolutions_per_minute_t, units::volt_t> _launcherPID();
+
+};

src/main/include/ShooterBehaviour.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "Wombat.h"
+#include "Shooter.h"
+#include <frc/XboxController.h>
+
+class ShooterManualControl : public behaviour::Behaviour {
+ public:
+  ShooterManualControl(Shooter *shooter, frc::XboxController &codriver);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Shooter *_shooter;
+  frc::XboxController &_codriver;
+};
+