Space coast showdown

simgui.json

@@ -13,7 +13,8 @@
       "/LiveWindow/Ungrouped/frc2::SubsystemBase": "Subsystem",
       "/SmartDashboard/chassis": "Subsystem",
       "/SmartDashboard/frc2::SubsystemBase": "Subsystem",
-      "/SmartDashboard/potato_cannon": "Subsystem"
+      "/SmartDashboard/potato_cannon": "Subsystem",
+      "/SmartDashboard/shooter": "Subsystem"
     },
     "windows": {
       "/SmartDashboard/chassis": {
@@ -64,7 +65,7 @@
               "autoFit": true
             }
           ],
-          "height": 320,
+          "height": 419,
           "series": [
             {
               "color": [

src/main/cpp/RobotContainer.cpp

@@ -4,6 +4,9 @@
 
 #include "Constants.h"
 #include "RobotContainer.h"
+#include "commands/Eject.h"
+#include "commands/Gather.h"
+#include "commands/Shoot.h"
 
 #include <frc/PowerDistribution.h>
 #include <frc/smartdashboard/SmartDashboard.h>
@@ -11,7 +14,7 @@
 #include <frc2/command/button/JoystickButton.h>
 
 RobotContainer::RobotContainer() :
-    m_autonomous_command(m_chassis, m_driver) {
+    m_autonomous_command(m_chassis, m_shooter, m_gatherer) {
   // Initialize all of your commands and subsystems here
   m_chassis.SetDefaultCommand(DriveWithJoysticks(m_chassis, m_driver));
 
@@ -24,11 +27,8 @@ RobotContainer::RobotContainer() :
 
 void RobotContainer::map_buttons() {
   // Configure your button bindings here
-  // frc2::JoystickButton(&m_manip, 4).WhenPressed(
-  //   SetShooterSpeed(&m_potatoCannon, SHOOTER_RPM));
-
   // Zero gyro
-  frc2::JoystickButton(&m_driver, 10).WhenPressed(
+  frc2::JoystickButton(&m_driver, 8).WhenPressed(
     frc2::InstantCommand{[this] {
       m_chassis.zero_gyro(); }, {&m_chassis}});
 
@@ -47,6 +47,11 @@ void RobotContainer::map_buttons() {
       m_shooter.set(0_rpm); }});
 
   // Gatherer
+  frc2::JoystickButton(&m_driver, 6).WhileHeld(Gather(m_gatherer));
+  frc2::JoystickButton(&m_driver, 4).WhileHeld(Eject(m_gatherer));
+
+  // Shoot
+  frc2::JoystickButton(&m_manip, 6).WhileHeld(Shoot(m_gatherer, m_shooter));
 }
 
 frc2::Command* RobotContainer::get_autonomous_command() {

src/main/cpp/commands/Auto.cpp

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/Auto.h"
+
+#include <frc/Timer.h>
+
+Auto::Auto(Chassis &chassis, Shooter &shooter, Gatherer &gatherer) :
+    m_chassis(chassis), m_shooter(shooter), m_gatherer(gatherer) {
+  // Use addRequirements() here to declare subsystem dependencies.
+  SetName("Auto");
+  AddRequirements({&m_chassis, &m_shooter, &m_gatherer});
+}
+
+// Called when the command is initially scheduled.
+void Auto::Initialize() {
+  m_start_time = frc::Timer::GetFPGATimestamp();
+}
+
+// Called repeatedly when this Command is scheduled to run
+void Auto::Execute() {
+  auto cur_time = frc::Timer::GetFPGATimestamp();
+  auto time = cur_time - m_start_time;
+  m_shooter.set(0.95_rpm);
+  if(time > 7_s) {
+    m_chassis.drive_field_relative(0_mps, 0_mps, 0_rad_per_s);
+    m_shooter.set(0_rpm);
+    m_gatherer.stop();
+  } else if(time > 4_s) {
+    auto speeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+      1.5_mps,
+      0_mps,
+      0_rad_per_s,
+      frc::Rotation2d(units::degree_t(0)));
+    m_chassis.drive(speeds);
+  } else if(time > 2_s) {
+    m_gatherer.shoot(false);
+  }
+}
+
+// Called once the command ends or is interrupted.
+void Auto::End(bool interrupted) {
+  m_gatherer.stop();
+}
+
+// Returns true when the command should end.
+bool Auto::IsFinished() {
+  return false;
+}

src/main/cpp/commands/DriveWithJoysticks.cpp

@@ -18,15 +18,15 @@ void DriveWithJoysticks::Initialize() {}
 void DriveWithJoysticks::Execute() {
     auto x = m_joystick.GetX();
     auto y = m_joystick.GetY();
-    auto z = m_joystick.GetZ();
+    auto z = m_joystick.GetRawAxis(4);
 
     x = utils::joy_expo(x, constants::joysticks::JOY_DRIVE_EXPO);
     y = utils::joy_expo(y, constants::joysticks::JOY_DRIVE_EXPO);
     z = utils::joy_expo(z, constants::joysticks::JOY_TURN_EXPO);
 
-    auto forward = -y * 2_fps;
-    auto left = -x * 2_fps;
-    auto theta = z * 1.5_rad_per_s;
+    auto forward = y * 21_fps;
+    auto left = x * 21_fps;
+    auto theta = z * 5.5_rad_per_s;
 
     m_chassis.drive_field_relative(forward, left, theta);
 }

src/main/cpp/subsystems/Chassis.cpp

@@ -27,7 +27,7 @@ void Chassis::zero_pod_encoders() {
 }
 
 void Chassis::zero_gyro() {
-  // m_gyro.ZeroYaw();
+  m_gyro.ZeroYaw();
 }
 
 void Chassis::drive(frc::ChassisSpeeds speeds) {
@@ -61,8 +61,8 @@ void Chassis::drive_field_relative(
     forward,
     left,
     theta,
-    // frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()))
-    frc::Rotation2d(units::degree_t(0))
+    frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()))
+    // frc::Rotation2d(units::degree_t(0))
   );
 
   drive(speeds);
@@ -85,28 +85,26 @@ void Chassis::set_pod(Pod &pod, frc::SwerveModuleState command) {
     units::ctre_count_t counts =
       command.angle.Degrees() * constants::chassis::ROTATE_RATIO;
 
-    std::string pod_name;
-    if(&pod == &m_lf_pod)
-      pod_name = "lf_pod";
-    if(&pod == &m_rf_pod)
-      pod_name = "rf_pod";
-    if(&pod == &m_lb_pod)
-      pod_name = "lb_pod";
-    if(&pod == &m_rb_pod)
-      pod_name = "rb_pod";
-    frc::SmartDashboard::PutNumber(pod_name + ".angle", command.angle.Degrees().value());
-    frc::SmartDashboard::PutNumber(pod_name + ".mag", drive_percent);
+    // std::string pod_name;
+    // if(&pod == &m_lf_pod)
+    //   pod_name = "lf_pod";
+    // if(&pod == &m_rf_pod)
+    //   pod_name = "rf_pod";
+    // if(&pod == &m_lb_pod)
+    //   pod_name = "lb_pod";
+    // if(&pod == &m_rb_pod)
+    //   pod_name = "rb_pod";
+    // frc::SmartDashboard::PutNumber(pod_name + ".angle", command.angle.Degrees().value());
+    // frc::SmartDashboard::PutNumber(pod_name + ".mag", drive_percent);
 
     if(abs(drive_percent) > constants::chassis::MIN_DRIVE_PERCENT)
       pod.turn.Set(ControlMode::Position, counts.value());
     pod.drive.Set(ControlMode::PercentOutput, drive_percent);
-    pod.tmp = command.angle.Degrees().value();
 }
 
 frc::Rotation2d Chassis::get_pod_angle(Pod &pod) {
   units::ctre_count_t counts{pod.turn.GetSelectedSensorPosition()};
   units::radian_t angle{counts / constants::chassis::ROTATE_RATIO};
-  return frc::Rotation2d{units::degree_t{pod.tmp}};
 
-  //return frc::Rotation2d{angle};
+  return frc::Rotation2d{angle};
 }

src/main/cpp/subsystems/Shooter.cpp

@@ -22,7 +22,7 @@ Shooter::Shooter() {
 void Shooter::set(units::revolutions_per_minute_t rpm) {
     rpm = rpm * constants::shooter::RATIO;
     m_shooter.Set(ControlMode::PercentOutput, rpm / constants::shooter::MAX_RPM);
-    frc::SmartDashboard::PutNumber("rpm", rpm / constants::shooter::MAX_RPM);
+    // frc::SmartDashboard::PutNumber("rpm", rpm / constants::shooter::MAX_RPM);
 }
 
 bool Shooter::at_speed() {

src/main/include/Constants.h

@@ -38,7 +38,7 @@ namespace constants {
 
   namespace chassis {
     // drive full speed meters
-    const auto MAX_SPEED = 30_fps;
+    const auto MAX_SPEED = 21_fps;
     // double MAX_SPEED = 1.0;
     
     // % of max speed that the turn motors will start to move

src/main/include/RobotContainer.h

@@ -9,6 +9,7 @@
 #include <frc2/command/Command.h>
 #include <frc2/command/InstantCommand.h>
 
+#include "commands/Auto.h"
 #include "commands/DriveWithJoysticks.h"
 #include "subsystems/Chassis.h"
 #include "subsystems/Shooter.h"
@@ -40,7 +41,7 @@ class RobotContainer {
   Gatherer m_gatherer;
   Shear m_shear;
 
-  DriveWithJoysticks m_autonomous_command;
+  Auto m_autonomous_command;
 
   void map_buttons();
 };

src/main/include/commands/Auto.h

@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/time.h>
+
+#include "subsystems/Chassis.h"
+#include "subsystems/Shooter.h"
+#include "subsystems/Gatherer.h"
+
+/**
+ * An example command.
+ *
+ * <p>Note that this extends CommandHelper, rather extending CommandBase
+ * directly; this is crucially important, or else the decorator functions in
+ * Command will *not* work!
+ */
+class Auto
+    : public frc2::CommandHelper<frc2::CommandBase, Auto> {
+ public:
+  Auto(Chassis &chassis, Shooter &shooter, Gatherer &gatherer);
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+ private:
+  Chassis &m_chassis;
+  Shooter &m_shooter;
+  Gatherer &m_gatherer;
+
+  units::second_t m_start_time;
+};

src/main/include/subsystems/Chassis.h

@@ -19,7 +19,6 @@ class Chassis : public frc2::SubsystemBase {
   struct Pod {
     TalonSRX turn;
     TalonFX drive;
-    double tmp;
   };
 
  public:
@@ -49,7 +48,7 @@ class Chassis : public frc2::SubsystemBase {
   Pod m_lb_pod{{constants::can_ids::LB_TURN}, {constants::can_ids::LB_DRIVE}};
   Pod m_rb_pod{{constants::can_ids::RB_TURN}, {constants::can_ids::RB_DRIVE}};
 
-  // AHRS m_gyro{frc::SPI::Port::kMXP};
+  AHRS m_gyro{frc::SPI::Port::kMXP};
 
   const frc::Translation2d LF_POD_LOCATION{ 
     constants::chassis::SWERVE_X_FROM_CENTER,