From e961189ab39142690681cd29508ab180972873a2 Mon Sep 17 00:00:00 2001 From: Andrew McAlinden <52133386+droiddoes9@users.noreply.github.com> Date: Mon, 11 Apr 2022 19:39:17 -0500 Subject: [PATCH 1/2] Test auto poop --- Competition/.vscode/settings.json | 58 ++++++++- Competition/src/main/cpp/RobotContainer.cpp | 1 + .../src/main/cpp/subsystems/Feeder.cpp | 117 ++++++++++++++++-- .../src/main/cpp/subsystems/Shooter.cpp | 53 ++++++++ .../src/main/cpp/subsystems/TurretTracker.cpp | 9 +- Competition/src/main/include/Constants.h | 5 + .../src/main/include/subsystems/Feeder.h | 37 +++++- .../src/main/include/subsystems/Shooter.h | 16 +++ 8 files changed, 280 insertions(+), 16 deletions(-) diff --git a/Competition/.vscode/settings.json b/Competition/.vscode/settings.json index 569a9af..c9e8ee7 100644 --- a/Competition/.vscode/settings.json +++ b/Competition/.vscode/settings.json @@ -22,6 +22,62 @@ "memory_resource": "cpp", "memory": "cpp", "*.inc": "cpp", - "cmath": "cpp" + "cmath": "cpp", + "array": "cpp", + "atomic": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "codecvt": "cpp", + "condition_variable": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "deque": "cpp", + "list": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "filesystem": "cpp", + "functional": "cpp", + "iterator": "cpp", + "map": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "set": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeinfo": "cpp" } } diff --git a/Competition/src/main/cpp/RobotContainer.cpp b/Competition/src/main/cpp/RobotContainer.cpp index d0b55a0..c93d08e 100644 --- a/Competition/src/main/cpp/RobotContainer.cpp +++ b/Competition/src/main/cpp/RobotContainer.cpp @@ -12,6 +12,7 @@ RobotContainer::RobotContainer() : m_auto(&m_drivetrain, &m_shooter, &m_feeder, m_shooter.setDrivetrain(&m_drivetrain); m_turretTracker.setDrivetrain(&m_drivetrain); m_turretTracker.setShooter(&m_shooter); + m_feeder.setShooter(&m_shooter); } diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index b50c914..bb5eb55 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -17,16 +17,25 @@ Feeder::Feeder() : ValorSubsystem(), operatorController(NULL), motor_intake(FeederConstants::MOTOR_INTAKE_CAN_ID, "baseCAN"), motor_stage(FeederConstants::MOTOR_STAGE_CAN_ID, "baseCAN"), - banner(FeederConstants::BANNER_DIO_PORT) + banner(FeederConstants::BANNER_DIO_PORT), + revColorSensor(frc::I2C::kOnboard) { frc2::CommandScheduler::GetInstance().RegisterSubsystem(this); init(); } +void Feeder::setShooter(Shooter *s){ + shooter = s; +} + void Feeder::init() { initTable("Feeder"); + limeTable = nt::NetworkTableInstance::GetDefault().GetTable("limelight"); + + state.isRedAlliance = fmsTable->GetBoolean("IsRedAlliance", false); + motor_intake.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); motor_intake.SetNeutralMode(ctre::phoenix::motorcontrol::Coast); motor_intake.SetInverted(false); @@ -48,12 +57,18 @@ void Feeder::init() table->PutNumber("Feeder Forward Speed Shoot", FeederConstants::DEFAULT_FEEDER_SPEED_FORWARD_SHOOT); table->PutNumber("Intake Spike Current", FeederConstants::JAM_CURRENT); + table->PutNumber("Blue Threshold", FeederConstants::BLUE_THRESHOLD); + table->PutNumber("Red Threshold", FeederConstants::RED_THRESHOLD); + table->PutNumber("Average Amps", 0); table->PutBoolean("Spiked: ", 0); table->PutBoolean("Banner: ", 0); - + + table->PutBoolean("Auto Poop Enabled", false); + + state.timing = false; + fmsTable = nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo"); resetState(); - } void Feeder::setControllers(frc::XboxController *controllerO, frc::XboxController *controllerD) @@ -82,12 +97,12 @@ void Feeder::assessInputs() state.operator_leftBumperPressed = operatorController->GetLeftBumper(); - if (state.driver_rightTriggerPressed || state.operator_leftBumperPressed) { - state.feederState = FeederState::FEEDER_SHOOT; //intake and feeder run + if (state.driver_leftBumperPressed) { + state.feederState = FeederState::FEEDER_REVERSE; state.spiked = false; } - else if (state.driver_leftBumperPressed) { - state.feederState = FeederState::FEEDER_REVERSE; + else if (state.driver_rightTriggerPressed || state.operator_leftBumperPressed) { + state.feederState = FeederState::FEEDER_SHOOT; //intake and feeder run state.spiked = false; } else if (state.driver_rightBumperPressed) { @@ -114,8 +129,30 @@ void Feeder::analyzeDashboard() table->PutNumber("Average Amps", state.instCurrent); table->PutBoolean("Spiked: ", state.spiked); table->PutBoolean("Banner: ", state.bannerTripped); + table->PutBoolean("Queue top", state.ballHistory.front()); + + if (state.autoPoopEnabled){ + if (isRed()){ + state.curBall = 1; + } + else if (isBlue()){ + state.curBall = 2; + } + else{ + state.curBall = 0; + } + updateQueue(); + state.prevBall = state.curBall; + + state.blueThreshold = table->GetNumber("Blue Threshold", FeederConstants::BLUE_THRESHOLD); + state.redThreshold = table->GetNumber("Red Threshold", FeederConstants::RED_THRESHOLD); + + table->PutBoolean("isRed", isRed()); + table->PutBoolean("isBlue", isBlue()); + } table->PutNumber("current feeder state", state.feederState); + state.autoPoopEnabled = table->GetBoolean("Auto Poop Enabled", false); // Calculate instantaneous current calcCurrent(); } @@ -130,8 +167,27 @@ void Feeder::assignOutputs() motor_stage.Set(0); } else if (state.feederState == FeederState::FEEDER_SHOOT) { - motor_intake.Set(state.intakeForwardSpeed); - motor_stage.Set(state.feederForwardSpeedShoot); + if (state.autoPoopEnabled){ + bool currentBall = state.ballHistory.front(); + if (currentBall){ + shooter->state.offsetTurret = true; + state.counter = 16; + } + + if (state.counter <= 0){ + motor_intake.Set(state.intakeForwardSpeed); + motor_stage.Set(state.feederForwardSpeedShoot); + } + else{ + motor_intake.Set(0); + motor_stage.Set(0); + } + state.counter--; + } + else{ + motor_intake.Set(state.intakeForwardSpeed); + motor_stage.Set(state.feederForwardSpeedShoot); + } } else if (state.feederState == Feeder::FEEDER_REVERSE) { motor_intake.Set(state.intakeReverseSpeed); @@ -168,6 +224,10 @@ void Feeder::assignOutputs() motor_stage.Set(state.feederForwardSpeedDefault); } } + else if (state.feederState == FeederState::FEEDER_FEEDER_ONLY){ + motor_intake.Set(0); + motor_stage.Set(state.feederForwardSpeedShoot); + } else { motor_intake.Set(0); motor_stage.Set(0); @@ -187,6 +247,37 @@ void Feeder::calcCurrent() { state.instCurrent = sum / FeederConstants::CACHE_SIZE; } +void Feeder::updateQueue(){ + if (state.curBall != state.prevBall){ + if (state.curBall == 1){ //red + if (!state.isRedAlliance){ + state.ballHistory.push(true); + } + else{ + state.ballHistory.push(false); + } + } + else if (state.curBall == 2){ //blue + if (state.isRedAlliance){ + state.ballHistory.push(true); + } + else{ + state.ballHistory.push(false); + } + } + } + + if (state.feederState == FeederState::FEEDER_SHOOT){ + double current = shooter->flywheel_lead.GetSelectedSensorVelocity(); + double target = shooter->state.flywheelTarget; + + double diff = fabs((current - target) / target); + if (diff > .15){ + state.ballHistory.pop(); + } + } +} + void Feeder::resetDeque() { state.current_cache.clear(); for (int i = 0; i < FeederConstants::CACHE_SIZE; i++) { @@ -204,3 +295,11 @@ void Feeder::resetState() resetDeque(); } + +bool Feeder::isRed(){ + return revColorSensor.GetColor().red > state.redThreshold; +} + +bool Feeder::isBlue(){ + return revColorSensor.GetColor().blue > state.blueThreshold; +} diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index dd3e88a..850b264 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -29,6 +29,8 @@ void Shooter::init() { limeTable = nt::NetworkTableInstance::GetDefault().GetTable("limelight"); liftTable = nt::NetworkTableInstance::GetDefault().GetTable("Lift"); + fmsTable = nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo"); + feederTable = nt::NetworkTableInstance::GetDefault().GetTable("Feeder"); initTable("Shooter"); table->PutBoolean("Zero Turret", false); @@ -126,6 +128,30 @@ void Shooter::init() limelightTrack(true); setPIDProfile(0); + + setupCommands(); +} + +void Shooter::setupCommands(){ + frc2::FunctionalCommand poopShot( + [this]() { //onInit + state.hoodState = HoodState::HOOD_POOP; + state.offsetTurret = true; + //state.flywheelState = FlywheelState::FLYWHEEL_POOP; + }, + [this](){ + }, //onExecute + [this](bool){ //onEnd + state.hoodState = HoodState::HOOD_TRACK; + state.offsetTurret = false; + //state.flywheelState = FlywheelState::FLYWHEEL_TRACK; + }, + [this](){ //isFinished + return state.spiked; + } + ); + poopOneBall.AddCommands(frc2::WaitCommand((units::second_t)0.15)); + poopOneBall.AddCommands(poopShot); } void Shooter::resetState(){ @@ -145,6 +171,16 @@ void Shooter::resetState(){ state.currentBall = 0; } +bool Shooter::isOppositeColor(){ + if (feederTable->GetBoolean("isRed", false) && !fmsTable->GetBoolean("IsRedAlliance", false)){ + return true; + } + if (feederTable->GetBoolean("isBlue", false) && fmsTable->GetBoolean("IsRedAlliance", false)){ + return true; + } + return false; +} + void Shooter::resetEncoder(){ turretEncoder.SetPosition(0); hoodEncoder.SetPosition(0); @@ -213,6 +249,10 @@ void Shooter::assessInputs() state.flywheelState = FlywheelState::FLYWHEEL_TRACK; // Higher speed } + if (isOppositeColor() && state.autoPoopEnabled && state.tv){ + poopOneBall.Schedule(); + } + state.trackCorner = false;//state.rightBumper ? true : false; } @@ -238,6 +278,16 @@ void Shooter::analyzeDashboard() state.flywheelState = FlywheelState::FLYWHEEL_DISABLE; } + double rpm = state.flywheelTarget * ShooterConstants::falconMaxRPM; + double rp100ms = rpm / 600.0; + double ticsp100ms = rp100ms * ShooterConstants::falconGearRatio * ShooterConstants::ticsPerRev; + if (fabs((flywheel_lead.GetSelectedSensorVelocity() - ticsp100ms) / ticsp100ms) > 0.15){ + state.spiked = true; + } + else{ + state.spiked = false; + } + // Turret homing and zeroing if (table->GetBoolean("Zero Turret", false)) { turret.RestoreFactoryDefaults(); @@ -306,6 +356,8 @@ void Shooter::analyzeDashboard() state.powerC_2x = table->GetNumber("Power Y Int 2X", ShooterConstants::cPower_2x); state.pipeline = limeTable->GetNumber("pipeline", 0); + + state.autoPoopEnabled = feederTable->GetBoolean("Auto Poop Enabled", false); } //0 is close (1x zoom), 1 is far (2x zoom), 2 is auto (1x zoom) @@ -409,6 +461,7 @@ void Shooter::assignOutputs() double rpm = state.flywheelTarget * ShooterConstants::falconMaxRPM; double rp100ms = rpm / 600.0; double ticsp100ms = rp100ms * ShooterConstants::falconGearRatio * ShooterConstants::ticsPerRev; + state.flywheelTarget = ticsp100ms; table->PutNumber("FlyWheel State", state.flywheelState); table->PutNumber("FlyWheel Target", ticsp100ms); diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index acdcf67..f08d07b 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -57,8 +57,13 @@ void TurretTracker::assignOutputs() { // 0.75 = limeligh KP state.target = (-state.cachedTx * 0.75) + turretPos; - if(shooter-> state.driverBButton){ - state.target += 15; + if (shooter->state.offsetTurret){ + if (tx > 0){ + state.target += 15; + } + else{ + state.target -= 15; + } } // state.target = -1 * robotHeading + state.cachedTurretPos; diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 5f1d4ec..23ffe54 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -10,6 +10,7 @@ #include #include #include +#include /** * The Constants header provides a convenient place for teams to hold robot-wide @@ -232,6 +233,7 @@ namespace FeederConstants{ constexpr static int MOTOR_STAGE_CAN_ID = 10; constexpr static int BANNER_DIO_PORT = 5; + constexpr static int COLOR_SENSOR_DIO_PORT = 6; constexpr static double DEFAULT_INTAKE_SPEED_FORWARD = 0.7; constexpr static double DEFAULT_INTAKE_SPEED_REVERSE = -0.7; @@ -242,6 +244,9 @@ namespace FeederConstants{ constexpr static int CACHE_SIZE = 20; constexpr static double JAM_CURRENT = 20; + + constexpr static double BLUE_THRESHOLD = 0.26; + constexpr static double RED_THRESHOLD = 0.3; } namespace MathConstants{ diff --git a/Competition/src/main/include/subsystems/Feeder.h b/Competition/src/main/include/subsystems/Feeder.h index 416f974..e84ebab 100644 --- a/Competition/src/main/include/subsystems/Feeder.h +++ b/Competition/src/main/include/subsystems/Feeder.h @@ -14,6 +14,10 @@ #include #include #include +#include +#include "Shooter.h" + +#include #ifndef FEEDER_H #define FEEDER_H @@ -31,13 +35,15 @@ class Feeder : public ValorSubsystem void assignOutputs(); void resetState(); + void setShooter(Shooter *s); enum FeederState { FEEDER_DISABLE, FEEDER_REVERSE, FEEDER_SHOOT, FEEDER_CURRENT_INTAKE, - FEEDER_REGULAR_INTAKE + FEEDER_REGULAR_INTAKE, + FEEDER_FEEDER_ONLY }; struct x @@ -58,7 +64,6 @@ class Feeder : public ValorSubsystem bool currentBanner; bool reversed; - bool spiked; double intakeForwardSpeed; @@ -68,14 +73,30 @@ class Feeder : public ValorSubsystem double feederForwardSpeedDefault; double feederForwardSpeedShoot; double feederReverseSpeed; + + double blueThreshold; + double redThreshold; + + bool timing; + bool autoPoopEnabled; + + int curBall; //0 is nothing, 1 is red, 2 is blue + int prevBall; + + int counter; //int current_cache_index; //std::vector current_cache; std::deque current_cache; + bool isRedAlliance; + double instCurrent; FeederState feederState; + + std::queue ballHistory; + } state; void resetDeque(); @@ -88,11 +109,19 @@ void resetDeque(); WPI_TalonFX motor_stage; frc::DigitalInput banner; - - void calcCurrent(); + rev::ColorSensorV3 revColorSensor; + void calcCurrent(); + void updateQueue(); + bool isRed(); + bool isBlue(); + bool isOppositeColor(); + + std::shared_ptr fmsTable; + std::shared_ptr limeTable; + Shooter *shooter; }; #endif \ No newline at end of file diff --git a/Competition/src/main/include/subsystems/Shooter.h b/Competition/src/main/include/subsystems/Shooter.h index 17db95c..5bb0a09 100644 --- a/Competition/src/main/include/subsystems/Shooter.h +++ b/Competition/src/main/include/subsystems/Shooter.h @@ -26,6 +26,7 @@ #include #include "networktables/NetworkTable.h" #include +#include #ifndef SHOOTER_H #define SHOOTER_H @@ -56,6 +57,8 @@ class Shooter : public ValorSubsystem void assignTurret(double tg); + bool isOppositeColor(); + enum TurretState{ TURRET_DISABLE, // Not moving TURRET_MANUAL, // Manual control from operator @@ -137,9 +140,18 @@ class Shooter : public ValorSubsystem double tv; double tx; + bool spiked; + + bool offsetTurret; + + bool autoPoopEnabled; + + double flywheelRPM; + } state; void limelightTrack(bool track); + void setupCommands(); WPI_TalonFX flywheel_lead; @@ -156,11 +168,15 @@ class Shooter : public ValorSubsystem std::shared_ptr limeTable; std::shared_ptr liftTable; + std::shared_ptr fmsTable; + std::shared_ptr feederTable; Drivetrain *odom; frc::SendableChooser m_chooserLimelight; frc::SendableChooser m_chooserPID; + + frc2::SequentialCommandGroup poopOneBall; }; #endif \ No newline at end of file From e37a797938076a7e6ba92d24dc9887fe2ad74aaa Mon Sep 17 00:00:00 2001 From: Andrew McAlinden <52133386+droiddoes9@users.noreply.github.com> Date: Mon, 18 Apr 2022 17:49:10 -0500 Subject: [PATCH 2/2] empty queue protections --- Competition/src/main/cpp/subsystems/Feeder.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index bb5eb55..2054058 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -129,7 +129,12 @@ void Feeder::analyzeDashboard() table->PutNumber("Average Amps", state.instCurrent); table->PutBoolean("Spiked: ", state.spiked); table->PutBoolean("Banner: ", state.bannerTripped); - table->PutBoolean("Queue top", state.ballHistory.front()); + if (!(state.ballHistory.empty())){ + table->PutBoolean("Queue top", state.ballHistory.front()); + } + else{ + table->PutBoolean("Queue top", false); + } if (state.autoPoopEnabled){ if (isRed()){ @@ -168,7 +173,10 @@ void Feeder::assignOutputs() } else if (state.feederState == FeederState::FEEDER_SHOOT) { if (state.autoPoopEnabled){ - bool currentBall = state.ballHistory.front(); + bool currentBall = false; + if (!(state.ballHistory.empty())){ + currentBall = state.ballHistory.front(); + } if (currentBall){ shooter->state.offsetTurret = true; state.counter = 16; @@ -272,7 +280,7 @@ void Feeder::updateQueue(){ double target = shooter->state.flywheelTarget; double diff = fabs((current - target) / target); - if (diff > .15){ + if (diff > .15 && !(state.ballHistory.empty())){ state.ballHistory.pop(); } }