From 633e8dc471b2c1bbaa410bf9f5eb0a6c464ab91a Mon Sep 17 00:00:00 2001 From: Thomas Date: Mon, 29 Aug 2022 18:13:47 -0400 Subject: [PATCH] Space coast showdown --- simgui.json | 5 +- src/main/cpp/RobotContainer.cpp | 15 ++++-- src/main/cpp/commands/Auto.cpp | 50 ++++++++++++++++++++ src/main/cpp/commands/DriveWithJoysticks.cpp | 8 ++-- src/main/cpp/subsystems/Chassis.cpp | 32 ++++++------- src/main/cpp/subsystems/Shooter.cpp | 2 +- src/main/include/Constants.h | 2 +- src/main/include/RobotContainer.h | 3 +- src/main/include/commands/Auto.h | 41 ++++++++++++++++ src/main/include/subsystems/Chassis.h | 3 +- 10 files changed, 128 insertions(+), 33 deletions(-) create mode 100644 src/main/cpp/commands/Auto.cpp create mode 100644 src/main/include/commands/Auto.h diff --git a/simgui.json b/simgui.json index 82546ad..674b338 100644 --- a/simgui.json +++ b/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": [ diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 391b014..01b4d27 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/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 #include @@ -11,7 +14,7 @@ #include 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() { diff --git a/src/main/cpp/commands/Auto.cpp b/src/main/cpp/commands/Auto.cpp new file mode 100644 index 0000000..46067bb --- /dev/null +++ b/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 + +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; +} diff --git a/src/main/cpp/commands/DriveWithJoysticks.cpp b/src/main/cpp/commands/DriveWithJoysticks.cpp index 40b0f82..45dd4da 100644 --- a/src/main/cpp/commands/DriveWithJoysticks.cpp +++ b/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); } diff --git a/src/main/cpp/subsystems/Chassis.cpp b/src/main/cpp/subsystems/Chassis.cpp index c81efd7..d0e2fea 100644 --- a/src/main/cpp/subsystems/Chassis.cpp +++ b/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}; } diff --git a/src/main/cpp/subsystems/Shooter.cpp b/src/main/cpp/subsystems/Shooter.cpp index c96e237..0a542ad 100644 --- a/src/main/cpp/subsystems/Shooter.cpp +++ b/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() { diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index e0fa8a0..191909c 100644 --- a/src/main/include/Constants.h +++ b/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 diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index e18cd93..0f341b0 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -9,6 +9,7 @@ #include #include +#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(); }; diff --git a/src/main/include/commands/Auto.h b/src/main/include/commands/Auto.h new file mode 100644 index 0000000..d14b648 --- /dev/null +++ b/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 +#include +#include + +#include "subsystems/Chassis.h" +#include "subsystems/Shooter.h" +#include "subsystems/Gatherer.h" + +/** + * An example command. + * + *

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 { + 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; +}; diff --git a/src/main/include/subsystems/Chassis.h b/src/main/include/subsystems/Chassis.h index f3340e0..9976401 100644 --- a/src/main/include/subsystems/Chassis.h +++ b/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,