From fdd79b65a18483777fc38c3f4b0a0063d6b97055 Mon Sep 17 00:00:00 2001 From: j a Date: Tue, 29 Mar 2022 20:38:19 -0500 Subject: [PATCH 01/62] intake and feeder motor -> falcon --- Competition/src/main/cpp/subsystems/Feeder.cpp | 12 ++++++------ Competition/src/main/include/subsystems/Feeder.h | 6 +++--- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index e542f06..1dd0cf5 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -14,8 +14,8 @@ Feeder::Feeder() : ValorSubsystem(), driverController(NULL), operatorController(NULL), - motor_intake(FeederConstants::MOTOR_INTAKE_CAN_ID, rev::CANSparkMax::MotorType::kBrushless), - motor_stage(FeederConstants::MOTOR_STAGE_CAN_ID, rev::CANSparkMax::MotorType::kBrushless), + motor_intake(FeederConstants::MOTOR_INTAKE_CAN_ID), + motor_stage(FeederConstants::MOTOR_STAGE_CAN_ID), banner(FeederConstants::BANNER_DIO_PORT) { @@ -26,12 +26,12 @@ Feeder::Feeder() : ValorSubsystem(), void Feeder::init() { initTable("Feeder"); - motor_intake.RestoreFactoryDefaults(); - motor_intake.SetIdleMode(rev::CANSparkMax::IdleMode::kCoast); + motor_intake.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); + motor_intake.SetNeutralMode(ctre::phoenix::motorcontrol::Coast); motor_intake.SetInverted(false); - motor_stage.RestoreFactoryDefaults(); - motor_stage.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake); + motor_stage.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); + motor_stage.SetNeutralMode(ctre::phoenix::motorcontrol::Coast); motor_stage.SetInverted(true); table->PutBoolean("Reverse Feeder?", false); diff --git a/Competition/src/main/include/subsystems/Feeder.h b/Competition/src/main/include/subsystems/Feeder.h index f519125..8264f63 100644 --- a/Competition/src/main/include/subsystems/Feeder.h +++ b/Competition/src/main/include/subsystems/Feeder.h @@ -12,7 +12,7 @@ #include #include -#include +#include #include #ifndef FEEDER_H @@ -84,8 +84,8 @@ class Feeder : public ValorSubsystem frc::XboxController *driverController; frc::XboxController *operatorController; - rev::CANSparkMax motor_intake; - rev::CANSparkMax motor_stage; + WPI_TalonFX motor_intake; + WPI_TalonFX motor_stage; frc::DigitalInput banner; From f2b7f2fa254c4f949373a3214065d456afadb97a Mon Sep 17 00:00:00 2001 From: j a Date: Wed, 30 Mar 2022 10:46:19 -0500 Subject: [PATCH 02/62] =?UTF-8?q?turret=20motor=20->=20falcon=C2=9E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/main/cpp/subsystems/Feeder.cpp | 4 - .../src/main/cpp/subsystems/Shooter.cpp | 74 +++++++++---------- .../src/main/cpp/subsystems/TurretTracker.cpp | 6 +- .../src/main/include/subsystems/Shooter.h | 5 +- 4 files changed, 41 insertions(+), 48 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 1dd0cf5..19d78a6 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -5,10 +5,6 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -//start button pulls swerve 0 positions from file -//back button pushes current swerve positions to file - - #include "subsystems/Feeder.h" Feeder::Feeder() : ValorSubsystem(), diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index d4cdca2..fe74d4c 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -12,7 +12,7 @@ Shooter::Shooter() : ValorSubsystem(), flywheel_lead{ShooterConstants::CAN_ID_FLYWHEEL_LEAD}, - turret{ShooterConstants::CAN_ID_TURRET, rev::CANSparkMax::MotorType::kBrushless}, + turret{ShooterConstants::CAN_ID_TURRET}, hood{ShooterConstants::CAN_ID_HOOD, rev::CANSparkMax::MotorType::kBrushless}, operatorController(NULL), driverController(NULL) @@ -66,29 +66,29 @@ void Shooter::init() flywheel_lead.SetInverted(false); flywheel_lead.SelectProfileSlot(0, 0); - turret.RestoreFactoryDefaults(); - turret.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake); + turret.ConfigFactoryDefault(); + turret.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); + turret.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); turret.SetInverted(true); - turret.EnableSoftLimit(rev::CANSparkMax::SoftLimitDirection::kForward, true); - turret.SetSoftLimit(rev::CANSparkMax::SoftLimitDirection::kForward, ShooterConstants::turretLimitLeft); + turret.ConfigForwardSoftLimitEnable(true); + turret.ConfigForwardSoftLimitThreshold(ShooterConstants::turretLimitLeft); - turret.EnableSoftLimit(rev::CANSparkMax::SoftLimitDirection::kReverse, true); - turret.SetSoftLimit(rev::CANSparkMax::SoftLimitDirection::kReverse, ShooterConstants::turretLimitRight); + turret.ConfigReverseSoftLimitEnable(true); + turret.ConfigReverseSoftLimitThreshold(ShooterConstants::turretLimitRight); - turretPidController.SetP(ShooterConstants::turretKP); - turretPidController.SetI(ShooterConstants::turretKI); - turretPidController.SetD(ShooterConstants::turretKD); - turretPidController.SetIZone(ShooterConstants::turretKIZ); - turretPidController.SetFF(ShooterConstants::turretKFF); - turretPidController.SetOutputRange(-1, 1); + turret.ConfigMotionAcceleration(ShooterConstants::turretMaxAccel); + turret.ConfigMotionCruiseVelocity(ShooterConstants::turretMaxV); + turret.ConfigAllowableClosedloopError(0, ShooterConstants::turretAllowedError); - turretPidController.SetSmartMotionMaxVelocity(ShooterConstants::turretMaxV); - turretPidController.SetSmartMotionMinOutputVelocity(ShooterConstants::turretMinV); - turretPidController.SetSmartMotionMaxAccel(ShooterConstants::turretMaxAccel); - turretPidController.SetSmartMotionAllowedClosedLoopError(ShooterConstants::turretAllowedError); + turret.ConfigAllowableClosedloopError(0, 0); + turret.Config_IntegralZone(0, 0); + turret.Config_kF(0, ShooterConstants::turretKFF); + turret.Config_kD(0, ShooterConstants::turretKD); + turret.Config_kI(0, ShooterConstants::turretKI); + turret.Config_kP(0, ShooterConstants::turretKP); - turretEncoder.SetPositionConversionFactor(360.0 * ShooterConstants::turretGearRatio); + turret.ConfigSelectedFeedbackCoefficient(360.0 * ShooterConstants::turretGearRatio); hood.RestoreFactoryDefaults(); hood.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake); @@ -140,7 +140,7 @@ void Shooter::resetState(){ } void Shooter::resetEncoder(){ - turretEncoder.SetPosition(0); + turret.SetSelectedSensorPosition(0); hoodEncoder.SetPosition(0); } @@ -237,19 +237,19 @@ void Shooter::analyzeDashboard() // Turret homing and zeroing if (table->GetBoolean("Zero Turret", false)) { - turret.RestoreFactoryDefaults(); - turret.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake); + turret.ConfigFactoryDefault(); + turret.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); turret.SetInverted(true); - turret.EnableSoftLimit(rev::CANSparkMax::SoftLimitDirection::kForward, true); - turret.SetSoftLimit(rev::CANSparkMax::SoftLimitDirection::kForward, ShooterConstants::turretLimitLeft); + turret.ConfigForwardSoftLimitEnable(true); + turret.ConfigForwardSoftLimitThreshold(ShooterConstants::turretLimitLeft); - turret.EnableSoftLimit(rev::CANSparkMax::SoftLimitDirection::kReverse, true); - turret.SetSoftLimit(rev::CANSparkMax::SoftLimitDirection::kReverse, ShooterConstants::turretLimitRight); + turret.ConfigReverseSoftLimitEnable(true); + turret.ConfigReverseSoftLimitThreshold(ShooterConstants::turretLimitRight); - turretEncoder = turret.GetEncoder(); - turretEncoder.SetPosition(0); - turretEncoder.SetPositionConversionFactor(360.0 * ShooterConstants::turretGearRatio); + turret.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); + turret.SetSelectedSensorPosition(0); + turret.ConfigSelectedFeedbackCoefficient(360.0 * ShooterConstants::turretGearRatio); } // Hood zeroing @@ -280,7 +280,7 @@ void Shooter::analyzeDashboard() state.lastTurretState = state.turretState; table->PutNumber("Hood degrees", hoodEncoder.GetPosition()); - table->PutNumber("Turret pos", turretEncoder.GetPosition()); + table->PutNumber("Turret pos", turret.GetSelectedSensorPosition()); table->PutNumber("Turret target", state.turretTarget); table->PutNumber("Turret Desired", state.turretDesired); @@ -328,43 +328,43 @@ void Shooter::assignOutputs() } //HOME else if(state.turretState == TurretState::TURRET_HOME_MID){ - if(fabs(turretEncoder.GetPosition() - ShooterConstants::homePositionMid) < 2){ + if(fabs(turret.GetSelectedSensorPosition() - ShooterConstants::homePositionMid) < 2){ state.turretState = TurretState::TURRET_DISABLE; } else { state.turretTarget = ShooterConstants::homePositionMid; - turretPidController.SetReference(state.turretTarget, rev::ControlType::kSmartMotion); + turret.Set(ControlMode::MotionMagic, state.turretTarget); } } else if(state.turretState == TurretState::TURRET_HOME_LEFT){ - if(fabs(turretEncoder.GetPosition() - ShooterConstants::homePositionLeft) < 2){ + if(fabs(turret.GetSelectedSensorPosition() - ShooterConstants::homePositionLeft) < 2){ state.turretState = TurretState::TURRET_DISABLE; } else { state.turretTarget = ShooterConstants::homePositionLeft; - turretPidController.SetReference(state.turretTarget, rev::ControlType::kSmartMotion); + turret.Set(ControlMode::MotionMagic, state.turretTarget); } } else if(state.turretState == TurretState::TURRET_HOME_RIGHT){ - if(fabs(turretEncoder.GetPosition() - ShooterConstants::homePositionRight) < 2){ + if(fabs(turret.GetSelectedSensorPosition() - ShooterConstants::homePositionRight) < 2){ state.turretState = TurretState::TURRET_DISABLE; } else { state.turretTarget = ShooterConstants::homePositionRight; - turretPidController.SetReference(state.turretTarget, rev::ControlType::kSmartMotion); + turret.Set(ControlMode::MotionMagic, state.turretTarget); } } //PRIMED else if (state.turretState == TurretState::TURRET_TRACK){ state.turretTarget = state.turretDesired; - turretPidController.SetReference(state.turretTarget, rev::ControlType::kSmartMotion); + turret.Set(ControlMode::MotionMagic, state.turretTarget); } //DISABLED else{ state.turretOutput = 0; turret.Set(state.turretOutput); } - table->PutNumber("Turret Error", state.turretTarget - turretEncoder.GetPosition()); + table->PutNumber("Turret Error", state.turretTarget - turret.GetSelectedSensorPosition()); /*////////////////////////////////////// // Flywheel // diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index 9f33a2a..0d1354b 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -44,18 +44,18 @@ void TurretTracker::assignOutputs() { if (tv == 1) { state.cachedTx = shooter->state.tx; // 0.75 = limeligh KP - state.target = (-state.cachedTx * 0.75) + shooter->turretEncoder.GetPosition(); + state.target = (-state.cachedTx * 0.75) + shooter->turret.GetSelectedSensorPosition(); state.cachedHeading = drivetrain->getPose_m().Rotation().Degrees().to(); state.cachedX = drivetrain->getPose_m().X().to(); state.cachedY = drivetrain->getPose_m().Y().to(); - state.cachedTurretPos = shooter->turretEncoder.GetPosition(); + state.cachedTurretPos = shooter->turret.GetSelectedSensorPosition(); } else { if (table->GetBoolean("Use Turret Shoot", false)) state.target = -1 * drivetrain->getPose_m().Rotation().Degrees().to() + state.cachedTurretPos - state.cachedTx; else - state.target = shooter->turretEncoder.GetPosition(); + state.target = shooter->turret.GetSelectedSensorPosition(); } if (state.target < -90) { diff --git a/Competition/src/main/include/subsystems/Shooter.h b/Competition/src/main/include/subsystems/Shooter.h index 0bedbe2..fb8ccf5 100644 --- a/Competition/src/main/include/subsystems/Shooter.h +++ b/Competition/src/main/include/subsystems/Shooter.h @@ -134,10 +134,7 @@ class Shooter : public ValorSubsystem void limelightTrack(bool track); WPI_TalonFX flywheel_lead; - - rev::CANSparkMax turret; - rev::SparkMaxRelativeEncoder turretEncoder = turret.GetEncoder(); - rev::SparkMaxPIDController turretPidController = turret.GetPIDController(); + WPI_TalonFX turret; rev::CANSparkMax hood; rev::SparkMaxRelativeEncoder hoodEncoder = hood.GetEncoder(); From 1761ccfee87c2a13d26fe5ea41392f401910c52c Mon Sep 17 00:00:00 2001 From: j a Date: Wed, 30 Mar 2022 17:15:20 -0500 Subject: [PATCH 03/62] added all falcons excpet flywheel to second can bus --- Competition/src/main/cpp/subsystems/Drivetrain.cpp | 6 +++--- Competition/src/main/cpp/subsystems/Feeder.cpp | 4 ++-- Competition/src/main/cpp/subsystems/Lift.cpp | 4 ++-- Competition/src/main/cpp/subsystems/Shooter.cpp | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Drivetrain.cpp b/Competition/src/main/cpp/subsystems/Drivetrain.cpp index fd2249c..41ca261 100644 --- a/Competition/src/main/cpp/subsystems/Drivetrain.cpp +++ b/Competition/src/main/cpp/subsystems/Drivetrain.cpp @@ -16,7 +16,7 @@ Drivetrain::Drivetrain() : ValorSubsystem(), driverController(NULL), - pigeon(DriveConstants::PIGEON_CAN), + pigeon(DriveConstants::PIGEON_CAN, "baseCAN"), kinematics(motorLocations[0], motorLocations[1], motorLocations[2], motorLocations[3]), odometry(kinematics, frc::Rotation2d{units::radian_t{0}}), config(units::velocity::meters_per_second_t{SwerveConstants::AUTO_MAX_SPEED_MPS}, units::acceleration::meters_per_second_squared_t{SwerveConstants::AUTO_MAX_ACCEL_MPSS}), @@ -45,7 +45,7 @@ Drivetrain::~Drivetrain() void Drivetrain::configSwerveModule(int i) { - azimuthMotors.push_back(new WPI_TalonFX(DriveConstants::AZIMUTH_CANS[i])); + azimuthMotors.push_back(new WPI_TalonFX(DriveConstants::AZIMUTH_CANS[i], "baseCAN")); azimuthMotors[i]->ConfigFactoryDefault(); azimuthMotors[i]->SetInverted(false); azimuthMotors[i]->ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); @@ -60,7 +60,7 @@ void Drivetrain::configSwerveModule(int i) azimuthMotors[i]->SetNeutralMode(NeutralMode::Brake); //azimuthMotors[i]->ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60, 80, .75)); - driveMotors.push_back(new WPI_TalonFX(DriveConstants::DRIVE_CANS[i])); + driveMotors.push_back(new WPI_TalonFX(DriveConstants::DRIVE_CANS[i], "baseCAN")); driveMotors[i]->ConfigFactoryDefault(); driveMotors[i]->SetInverted(false); driveMotors[i]->ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 19d78a6..cb837f5 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -10,8 +10,8 @@ Feeder::Feeder() : ValorSubsystem(), driverController(NULL), operatorController(NULL), - motor_intake(FeederConstants::MOTOR_INTAKE_CAN_ID), - motor_stage(FeederConstants::MOTOR_STAGE_CAN_ID), + motor_intake(FeederConstants::MOTOR_INTAKE_CAN_ID, "baseCAN"), + motor_stage(FeederConstants::MOTOR_STAGE_CAN_ID, "baseCAN"), banner(FeederConstants::BANNER_DIO_PORT) { diff --git a/Competition/src/main/cpp/subsystems/Lift.cpp b/Competition/src/main/cpp/subsystems/Lift.cpp index 32d92a5..3a80bb8 100644 --- a/Competition/src/main/cpp/subsystems/Lift.cpp +++ b/Competition/src/main/cpp/subsystems/Lift.cpp @@ -3,8 +3,8 @@ Lift::Lift() : ValorSubsystem(), operatorController(NULL), - leadMainMotor{LiftConstants::MAIN_CAN_ID}, - followMainMotor{LiftConstants::MAIN_FOLLOW_CAN_ID}, + leadMainMotor{LiftConstants::MAIN_CAN_ID, "baseCAN"}, + followMainMotor{LiftConstants::MAIN_FOLLOW_CAN_ID, "baseCAN"}, rotateMotor{LiftConstants::ROTATE_CAN_ID, rev::CANSparkMax::MotorType::kBrushless} { frc2::CommandScheduler::GetInstance().RegisterSubsystem(this); diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index fe74d4c..191ade8 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -12,7 +12,7 @@ Shooter::Shooter() : ValorSubsystem(), flywheel_lead{ShooterConstants::CAN_ID_FLYWHEEL_LEAD}, - turret{ShooterConstants::CAN_ID_TURRET}, + turret{ShooterConstants::CAN_ID_TURRET, "baseCAN"}, hood{ShooterConstants::CAN_ID_HOOD, rev::CANSparkMax::MotorType::kBrushless}, operatorController(NULL), driverController(NULL) From b0bced43e2c9c91cc6728f71fee4e7cfe8d8e230 Mon Sep 17 00:00:00 2001 From: Michael Ray Date: Fri, 1 Apr 2022 17:57:55 -0500 Subject: [PATCH 04/62] =?UTF-8?q?Revert=20"turret=20motor=20->=20falcon?= =?UTF-8?q?=C2=9E"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit 027ecb2392e4704a72bcbd5e54bf28c373f4fc5a. --- .../src/main/cpp/subsystems/Feeder.cpp | 4 + .../src/main/cpp/subsystems/Shooter.cpp | 74 +++++++++---------- .../src/main/cpp/subsystems/TurretTracker.cpp | 6 +- .../src/main/include/subsystems/Shooter.h | 5 +- 4 files changed, 48 insertions(+), 41 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index cb837f5..359767a 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -5,6 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +//start button pulls swerve 0 positions from file +//back button pushes current swerve positions to file + + #include "subsystems/Feeder.h" Feeder::Feeder() : ValorSubsystem(), diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index 191ade8..d4cdca2 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -12,7 +12,7 @@ Shooter::Shooter() : ValorSubsystem(), flywheel_lead{ShooterConstants::CAN_ID_FLYWHEEL_LEAD}, - turret{ShooterConstants::CAN_ID_TURRET, "baseCAN"}, + turret{ShooterConstants::CAN_ID_TURRET, rev::CANSparkMax::MotorType::kBrushless}, hood{ShooterConstants::CAN_ID_HOOD, rev::CANSparkMax::MotorType::kBrushless}, operatorController(NULL), driverController(NULL) @@ -66,29 +66,29 @@ void Shooter::init() flywheel_lead.SetInverted(false); flywheel_lead.SelectProfileSlot(0, 0); - turret.ConfigFactoryDefault(); - turret.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); - turret.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); + turret.RestoreFactoryDefaults(); + turret.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake); turret.SetInverted(true); - turret.ConfigForwardSoftLimitEnable(true); - turret.ConfigForwardSoftLimitThreshold(ShooterConstants::turretLimitLeft); + turret.EnableSoftLimit(rev::CANSparkMax::SoftLimitDirection::kForward, true); + turret.SetSoftLimit(rev::CANSparkMax::SoftLimitDirection::kForward, ShooterConstants::turretLimitLeft); - turret.ConfigReverseSoftLimitEnable(true); - turret.ConfigReverseSoftLimitThreshold(ShooterConstants::turretLimitRight); + turret.EnableSoftLimit(rev::CANSparkMax::SoftLimitDirection::kReverse, true); + turret.SetSoftLimit(rev::CANSparkMax::SoftLimitDirection::kReverse, ShooterConstants::turretLimitRight); - turret.ConfigMotionAcceleration(ShooterConstants::turretMaxAccel); - turret.ConfigMotionCruiseVelocity(ShooterConstants::turretMaxV); - turret.ConfigAllowableClosedloopError(0, ShooterConstants::turretAllowedError); + turretPidController.SetP(ShooterConstants::turretKP); + turretPidController.SetI(ShooterConstants::turretKI); + turretPidController.SetD(ShooterConstants::turretKD); + turretPidController.SetIZone(ShooterConstants::turretKIZ); + turretPidController.SetFF(ShooterConstants::turretKFF); + turretPidController.SetOutputRange(-1, 1); - turret.ConfigAllowableClosedloopError(0, 0); - turret.Config_IntegralZone(0, 0); - turret.Config_kF(0, ShooterConstants::turretKFF); - turret.Config_kD(0, ShooterConstants::turretKD); - turret.Config_kI(0, ShooterConstants::turretKI); - turret.Config_kP(0, ShooterConstants::turretKP); + turretPidController.SetSmartMotionMaxVelocity(ShooterConstants::turretMaxV); + turretPidController.SetSmartMotionMinOutputVelocity(ShooterConstants::turretMinV); + turretPidController.SetSmartMotionMaxAccel(ShooterConstants::turretMaxAccel); + turretPidController.SetSmartMotionAllowedClosedLoopError(ShooterConstants::turretAllowedError); - turret.ConfigSelectedFeedbackCoefficient(360.0 * ShooterConstants::turretGearRatio); + turretEncoder.SetPositionConversionFactor(360.0 * ShooterConstants::turretGearRatio); hood.RestoreFactoryDefaults(); hood.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake); @@ -140,7 +140,7 @@ void Shooter::resetState(){ } void Shooter::resetEncoder(){ - turret.SetSelectedSensorPosition(0); + turretEncoder.SetPosition(0); hoodEncoder.SetPosition(0); } @@ -237,19 +237,19 @@ void Shooter::analyzeDashboard() // Turret homing and zeroing if (table->GetBoolean("Zero Turret", false)) { - turret.ConfigFactoryDefault(); - turret.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); + turret.RestoreFactoryDefaults(); + turret.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake); turret.SetInverted(true); - turret.ConfigForwardSoftLimitEnable(true); - turret.ConfigForwardSoftLimitThreshold(ShooterConstants::turretLimitLeft); + turret.EnableSoftLimit(rev::CANSparkMax::SoftLimitDirection::kForward, true); + turret.SetSoftLimit(rev::CANSparkMax::SoftLimitDirection::kForward, ShooterConstants::turretLimitLeft); - turret.ConfigReverseSoftLimitEnable(true); - turret.ConfigReverseSoftLimitThreshold(ShooterConstants::turretLimitRight); + turret.EnableSoftLimit(rev::CANSparkMax::SoftLimitDirection::kReverse, true); + turret.SetSoftLimit(rev::CANSparkMax::SoftLimitDirection::kReverse, ShooterConstants::turretLimitRight); - turret.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); - turret.SetSelectedSensorPosition(0); - turret.ConfigSelectedFeedbackCoefficient(360.0 * ShooterConstants::turretGearRatio); + turretEncoder = turret.GetEncoder(); + turretEncoder.SetPosition(0); + turretEncoder.SetPositionConversionFactor(360.0 * ShooterConstants::turretGearRatio); } // Hood zeroing @@ -280,7 +280,7 @@ void Shooter::analyzeDashboard() state.lastTurretState = state.turretState; table->PutNumber("Hood degrees", hoodEncoder.GetPosition()); - table->PutNumber("Turret pos", turret.GetSelectedSensorPosition()); + table->PutNumber("Turret pos", turretEncoder.GetPosition()); table->PutNumber("Turret target", state.turretTarget); table->PutNumber("Turret Desired", state.turretDesired); @@ -328,43 +328,43 @@ void Shooter::assignOutputs() } //HOME else if(state.turretState == TurretState::TURRET_HOME_MID){ - if(fabs(turret.GetSelectedSensorPosition() - ShooterConstants::homePositionMid) < 2){ + if(fabs(turretEncoder.GetPosition() - ShooterConstants::homePositionMid) < 2){ state.turretState = TurretState::TURRET_DISABLE; } else { state.turretTarget = ShooterConstants::homePositionMid; - turret.Set(ControlMode::MotionMagic, state.turretTarget); + turretPidController.SetReference(state.turretTarget, rev::ControlType::kSmartMotion); } } else if(state.turretState == TurretState::TURRET_HOME_LEFT){ - if(fabs(turret.GetSelectedSensorPosition() - ShooterConstants::homePositionLeft) < 2){ + if(fabs(turretEncoder.GetPosition() - ShooterConstants::homePositionLeft) < 2){ state.turretState = TurretState::TURRET_DISABLE; } else { state.turretTarget = ShooterConstants::homePositionLeft; - turret.Set(ControlMode::MotionMagic, state.turretTarget); + turretPidController.SetReference(state.turretTarget, rev::ControlType::kSmartMotion); } } else if(state.turretState == TurretState::TURRET_HOME_RIGHT){ - if(fabs(turret.GetSelectedSensorPosition() - ShooterConstants::homePositionRight) < 2){ + if(fabs(turretEncoder.GetPosition() - ShooterConstants::homePositionRight) < 2){ state.turretState = TurretState::TURRET_DISABLE; } else { state.turretTarget = ShooterConstants::homePositionRight; - turret.Set(ControlMode::MotionMagic, state.turretTarget); + turretPidController.SetReference(state.turretTarget, rev::ControlType::kSmartMotion); } } //PRIMED else if (state.turretState == TurretState::TURRET_TRACK){ state.turretTarget = state.turretDesired; - turret.Set(ControlMode::MotionMagic, state.turretTarget); + turretPidController.SetReference(state.turretTarget, rev::ControlType::kSmartMotion); } //DISABLED else{ state.turretOutput = 0; turret.Set(state.turretOutput); } - table->PutNumber("Turret Error", state.turretTarget - turret.GetSelectedSensorPosition()); + table->PutNumber("Turret Error", state.turretTarget - turretEncoder.GetPosition()); /*////////////////////////////////////// // Flywheel // diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index 0d1354b..9f33a2a 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -44,18 +44,18 @@ void TurretTracker::assignOutputs() { if (tv == 1) { state.cachedTx = shooter->state.tx; // 0.75 = limeligh KP - state.target = (-state.cachedTx * 0.75) + shooter->turret.GetSelectedSensorPosition(); + state.target = (-state.cachedTx * 0.75) + shooter->turretEncoder.GetPosition(); state.cachedHeading = drivetrain->getPose_m().Rotation().Degrees().to(); state.cachedX = drivetrain->getPose_m().X().to(); state.cachedY = drivetrain->getPose_m().Y().to(); - state.cachedTurretPos = shooter->turret.GetSelectedSensorPosition(); + state.cachedTurretPos = shooter->turretEncoder.GetPosition(); } else { if (table->GetBoolean("Use Turret Shoot", false)) state.target = -1 * drivetrain->getPose_m().Rotation().Degrees().to() + state.cachedTurretPos - state.cachedTx; else - state.target = shooter->turret.GetSelectedSensorPosition(); + state.target = shooter->turretEncoder.GetPosition(); } if (state.target < -90) { diff --git a/Competition/src/main/include/subsystems/Shooter.h b/Competition/src/main/include/subsystems/Shooter.h index fb8ccf5..0bedbe2 100644 --- a/Competition/src/main/include/subsystems/Shooter.h +++ b/Competition/src/main/include/subsystems/Shooter.h @@ -134,7 +134,10 @@ class Shooter : public ValorSubsystem void limelightTrack(bool track); WPI_TalonFX flywheel_lead; - WPI_TalonFX turret; + + rev::CANSparkMax turret; + rev::SparkMaxRelativeEncoder turretEncoder = turret.GetEncoder(); + rev::SparkMaxPIDController turretPidController = turret.GetPIDController(); rev::CANSparkMax hood; rev::SparkMaxRelativeEncoder hoodEncoder = hood.GetEncoder(); From b9c86815c2aff85f1e54c7c9c51da602ce522d21 Mon Sep 17 00:00:00 2001 From: j a Date: Fri, 1 Apr 2022 18:51:40 -0500 Subject: [PATCH 05/62] enabled 10v saturation on volatge compensation on intake and feeder --- Competition/src/main/cpp/subsystems/Feeder.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 359767a..46ddd23 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -29,10 +29,14 @@ void Feeder::init() motor_intake.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); motor_intake.SetNeutralMode(ctre::phoenix::motorcontrol::Coast); motor_intake.SetInverted(false); + motor_intake.EnableVoltageCompensation(true); + motor_intake.ConfigVoltageCompSaturation(10); motor_stage.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); motor_stage.SetNeutralMode(ctre::phoenix::motorcontrol::Coast); motor_stage.SetInverted(true); + motor_stage.EnableVoltageCompensation(true); + motor_stage.ConfigVoltageCompSaturation(10); table->PutBoolean("Reverse Feeder?", false); table->PutNumber("Intake Reverse Speed", FeederConstants::DEFAULT_INTAKE_SPEED_REVERSE); From 32999ed89d33e2b2fd0b8d27ed7721671cc64fb0 Mon Sep 17 00:00:00 2001 From: Michael Ray Date: Sat, 2 Apr 2022 14:12:28 -0500 Subject: [PATCH 06/62] Moved lift wiring to old can bus --- Competition/src/main/cpp/subsystems/Lift.cpp | 4 ++-- Competition/src/main/cpp/subsystems/TurretTracker.cpp | 4 ++++ Competition/src/main/include/Constants.h | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Lift.cpp b/Competition/src/main/cpp/subsystems/Lift.cpp index 3a80bb8..32d92a5 100644 --- a/Competition/src/main/cpp/subsystems/Lift.cpp +++ b/Competition/src/main/cpp/subsystems/Lift.cpp @@ -3,8 +3,8 @@ Lift::Lift() : ValorSubsystem(), operatorController(NULL), - leadMainMotor{LiftConstants::MAIN_CAN_ID, "baseCAN"}, - followMainMotor{LiftConstants::MAIN_FOLLOW_CAN_ID, "baseCAN"}, + leadMainMotor{LiftConstants::MAIN_CAN_ID}, + followMainMotor{LiftConstants::MAIN_FOLLOW_CAN_ID}, rotateMotor{LiftConstants::ROTATE_CAN_ID, rev::CANSparkMax::MotorType::kBrushless} { frc2::CommandScheduler::GetInstance().RegisterSubsystem(this); diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index 9f33a2a..6663412 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -50,6 +50,10 @@ void TurretTracker::assignOutputs() { state.cachedX = drivetrain->getPose_m().X().to(); state.cachedY = drivetrain->getPose_m().Y().to(); state.cachedTurretPos = shooter->turretEncoder.GetPosition(); + + // state.target = -1 * drivetrain->getPose_m().Rotation().Degrees().to() + state.cachedTurretPos; + // atan2(drivetrain->getKinematics().ToChassisSpeeds().vx.to(()), drivetrain->getPose_m().X()); + } else { if (table->GetBoolean("Use Turret Shoot", false)) diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 6caf194..d43aaa5 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -166,7 +166,7 @@ namespace ShooterConstants{ constexpr static double turretMaxV = 10000; constexpr static double turretMinV = 0; constexpr static double turretMaxAccel = turretMaxV * 2; - constexpr static double turretAllowedError = 0.75; + constexpr static double turretAllowedError = 0.5; constexpr static double hoodKP = 5e-5; constexpr static double hoodKI = 0; From 059ed8b05f0ed3070850a2087b469e37786871d8 Mon Sep 17 00:00:00 2001 From: j a Date: Sat, 2 Apr 2022 16:52:11 -0500 Subject: [PATCH 07/62] adjust LOBF on 2x and 1x zoom. Only 1x implemented. intake and feeder speed adjustments. removed driver pipeline button --- .../src/main/cpp/subsystems/Feeder.cpp | 5 ++- .../src/main/cpp/subsystems/Shooter.cpp | 33 ++++++++++--------- Competition/src/main/include/Constants.h | 33 +++++++++++-------- .../src/main/include/subsystems/Shooter.h | 13 +++++--- 4 files changed, 48 insertions(+), 36 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 46ddd23..c24980b 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -29,7 +29,7 @@ void Feeder::init() motor_intake.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); motor_intake.SetNeutralMode(ctre::phoenix::motorcontrol::Coast); motor_intake.SetInverted(false); - motor_intake.EnableVoltageCompensation(true); + motor_intake.EnableVoltageCompensation(false); motor_intake.ConfigVoltageCompSaturation(10); motor_stage.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); @@ -124,8 +124,7 @@ void Feeder::assignOutputs() } else if (state.feederState == FeederState::FEEDER_SHOOT) { motor_intake.Set(state.intakeForwardSpeed); - //motor_stage.Set(state.feederForwardSpeedShoot); - motor_stage.SetVoltage(10_V * state.feederForwardSpeedShoot); + motor_stage.Set(state.feederForwardSpeedShoot); } else if (state.feederState == Feeder::FEEDER_REVERSE) { motor_intake.Set(state.intakeReverseSpeed); diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index d4cdca2..6ada789 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -40,8 +40,11 @@ void Shooter::init() table->PutNumber("Hood Top Position", ShooterConstants::hoodTop); table->PutNumber("Hood Bottom Position", ShooterConstants::hoodBottom); - table->PutNumber("Hood Y Int", ShooterConstants::cHood); - table->PutNumber("Power Y Int", ShooterConstants::cPower); + table->PutNumber("Hood Y Int 1X", ShooterConstants::cHood_1x); + table->PutNumber("Power Y Int 1X", ShooterConstants::cPower_1x); + + // table->PutNumber("Hood Y Int 2X", ShooterConstants::cHood_2x); + // table->PutNumber("Power Y Int 2X", ShooterConstants::cPower_2x); flywheel_lead.ConfigFactoryDefault(); flywheel_lead.ConfigAllowableClosedloopError(0, 0); @@ -178,10 +181,14 @@ void Shooter::assessInputs() else if (state.bButton){ state.turretState = TurretState::TURRET_TRACK; // Not moving } - else if(state.turretState == TurretState::TURRET_MANUAL){ + else if(state.turretState == TurretState::TURRET_MANUAL || (fabs(state.turretTarget - turretEncoder.GetPosition()) < ShooterConstants::turretAllowedError && state.turretState == TurretState::TURRET_HOME_MID)){ state.turretState = TurretState::TURRET_TRACK; } + if (state.yButton) { + state.turretState = TurretState::TURRET_HOME_MID; + } + //Hood if(state.aButton){ @@ -206,15 +213,6 @@ void Shooter::assessInputs() } state.trackCorner = false;//state.rightBumper ? true : false; - if (state.yButton) { - state.turretState = TurretState::TURRET_HOME_MID; - } - - //Limelight - else if (state.driverLeftTrigger && state.pipeline == 0 && state.driverLeftTrigger != state.driverLastLeftTrigger) { - setLimelight(1); - } - state.driverLastLeftTrigger = state.driverLeftTrigger; } void Shooter::analyzeDashboard() @@ -292,8 +290,11 @@ void Shooter::analyzeDashboard() table->PutNumber("Turret State", state.turretState); - state.hoodC = table->GetNumber("Hood Y Int", ShooterConstants::cHood); - state.powerC = table->GetNumber("Power Y Int", ShooterConstants::cPower); + state.hoodC_1x = table->GetNumber("Hood Y Int 1X", ShooterConstants::cHood_1x); + state.powerC_1x = table->GetNumber("Power Y Int 1X", ShooterConstants::cPower_1x); + + state.hoodC_2x = table->GetNumber("Hood Y Int 2X", ShooterConstants::cHood_2x); + state.powerC_2x = table->GetNumber("Power Y Int 2X", ShooterConstants::cPower_2x); } //0 is close, 1 is far, 2 is auto @@ -376,7 +377,7 @@ void Shooter::assignOutputs() state.flywheelTarget = state.flywheelLow; } else if(state.flywheelState == FlywheelState::FLYWHEEL_TRACK){ - state.flywheelTarget = ShooterConstants::aPower *(state.distanceToHub * state.distanceToHub) + ShooterConstants::bPower * state.distanceToHub + state.powerC ; //commented out for testing PID + state.flywheelTarget = ShooterConstants::aPower_1x *(state.distanceToHub * state.distanceToHub) + ShooterConstants::bPower_1x * state.distanceToHub + state.powerC_1x ; //commented out for testing PID } else if(state.flywheelState == FlywheelState::FLYWHEEL_POOP){ state.flywheelTarget = ShooterConstants::flywheelPoopValue; @@ -402,7 +403,7 @@ void Shooter::assignOutputs() state.hoodTarget = state.hoodLow; } else if(state.hoodState == HoodState::HOOD_TRACK){ - state.hoodTarget = ShooterConstants::aHood * (state.distanceToHub * state.distanceToHub) + ShooterConstants::bHood * state.distanceToHub + state.hoodC; //commented out for testing PID + state.hoodTarget = ShooterConstants::aHood_1x * (state.distanceToHub * state.distanceToHub) + ShooterConstants::bHood_1x * state.distanceToHub + state.hoodC_1x; //commented out for testing PID } else if(state.hoodState == HoodState::HOOD_POOP){ state.hoodTarget = ShooterConstants::hoodPoop; diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index d43aaa5..e88099f 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -117,25 +117,32 @@ namespace ShooterConstants{ constexpr static int CAN_ID_TURRET = 12; constexpr static int CAN_ID_HOOD = 15; - constexpr static double aPower = 0.0613; - constexpr static double bPower = -.139; //.215 - constexpr static double cPower = .496; - constexpr static double aHood = 8.95; - constexpr static double bHood = -12.9; - constexpr static double cHood = 1.8; + constexpr static double aPower_1x = 0.0452; + constexpr static double bPower_1x = -0.0879; + constexpr static double cPower_1x = 0.448; + constexpr static double aHood_1x = 5.33; + constexpr static double bHood_1x = -4.69; + constexpr static double cHood_1x = -1.51; + + constexpr static double aPower_2x = 0.123; + constexpr static double bPower_2x = -0.307; + constexpr static double cPower_2x = -0.618; + constexpr static double aHood_2x = 4.05; + constexpr static double bHood_2x = 6.72; + constexpr static double cHood_2x = -11.6; constexpr static double limelightTurnKP = (.3 / 25.445) * 1.25; constexpr static double limelightAngle = 50; constexpr static double hubHeight = 2.64; constexpr static double limelightHeight = .6075; - constexpr static double flywheelKP1 = 0.1; + constexpr static double flywheelKP1 = 0.088; //1 constexpr static double flywheelKI1 = 0; constexpr static double flywheelKD1 = 0; constexpr static double flywheelKIZ1 = 0; constexpr static double flywheelKFF1 = 0.04; - constexpr static double flywheelKP0 = 0.1 ; //.25 + constexpr static double flywheelKP0 = 0.088; //.1 constexpr static double flywheelKI0 = 0; constexpr static double flywheelKD0 = 0; constexpr static double flywheelKIZ0 = 0; @@ -151,7 +158,7 @@ namespace ShooterConstants{ constexpr static double flywheelPrimedValue = 0.46; constexpr static double flywheelAutoValue = 0.405; //can change to .4 constexpr static double flywheelDefaultValue = 0.42; //.375 - constexpr static double flywheelPoopValue = 0.25; + constexpr static double flywheelPoopValue = 0.3; constexpr static double flywheelLaunchpadValue = .455; constexpr static double flywheelSpeeds[] = {.372, .38125, .372}; //.387, .39125 @@ -221,19 +228,19 @@ namespace ShooterConstants{ } namespace FeederConstants{ - constexpr static int MOTOR_INTAKE_CAN_ID = 9; + constexpr static int MOTOR_INTAKE_CAN_ID = 9; //PDH slot 15 constexpr static int MOTOR_STAGE_CAN_ID = 10; constexpr static int BANNER_DIO_PORT = 5; - constexpr static double DEFAULT_INTAKE_SPEED_FORWARD = 0.9; + constexpr static double DEFAULT_INTAKE_SPEED_FORWARD = 0.7; constexpr static double DEFAULT_INTAKE_SPEED_REVERSE = -0.9; constexpr static double DEFAULT_FEEDER_SPEED_FORWARD_DEFAULT = 0.5; - constexpr static double DEFAULT_FEEDER_SPEED_FORWARD_SHOOT = 0.98; + constexpr static double DEFAULT_FEEDER_SPEED_FORWARD_SHOOT = 0.9; constexpr static double DEFAULT_FEEDER_SPEED_REVERSE = -1.0; - constexpr static int CACHE_SIZE = 25; + constexpr static int CACHE_SIZE = 15; constexpr static double JAM_CURRENT = 30; } diff --git a/Competition/src/main/include/subsystems/Shooter.h b/Competition/src/main/include/subsystems/Shooter.h index 0bedbe2..cf8c5d7 100644 --- a/Competition/src/main/include/subsystems/Shooter.h +++ b/Competition/src/main/include/subsystems/Shooter.h @@ -121,11 +121,16 @@ class Shooter : public ValorSubsystem int pipeline; - double hoodB; - double powerB; + double hoodB_1x; + double powerB_1x; + double hoodB_2x; + double powerB_2x; + + double powerC_1x; + double hoodC_1x; + double powerC_2x; + double hoodC_2x; - double powerC; - double hoodC; double tv; double tx; From 58a39a1ca89f5b80d1530291e8530c8c14763003 Mon Sep 17 00:00:00 2001 From: j a Date: Sat, 2 Apr 2022 19:23:20 -0500 Subject: [PATCH 08/62] new LoBF constants. swaps between 2x and 1x LoBFs --- .../src/main/cpp/subsystems/Feeder.cpp | 1 + .../src/main/cpp/subsystems/Shooter.cpp | 27 ++++++++++++++--- Competition/src/main/include/Constants.h | 30 +++++++++---------- .../src/main/include/subsystems/Shooter.h | 1 + 4 files changed, 40 insertions(+), 19 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index c24980b..2b77d3c 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -31,6 +31,7 @@ void Feeder::init() motor_intake.SetInverted(false); motor_intake.EnableVoltageCompensation(false); motor_intake.ConfigVoltageCompSaturation(10); + motor_intake.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60, 80, .75)); motor_stage.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); motor_stage.SetNeutralMode(ctre::phoenix::motorcontrol::Coast); diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index 6ada789..dc4ddc2 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -116,6 +116,9 @@ void Shooter::init() hoodPidController.SetSmartMotionMinOutputVelocity(ShooterConstants::hoodMinV); hoodPidController.SetSmartMotionMaxAccel(ShooterConstants::hoodMaxAccel); hoodPidController.SetSmartMotionAllowedClosedLoopError(ShooterConstants::hoodAllowedError); + + state.pipeline = 0; + state.LoBFZoom = 1; resetState(); resetEncoder(); @@ -181,7 +184,7 @@ void Shooter::assessInputs() else if (state.bButton){ state.turretState = TurretState::TURRET_TRACK; // Not moving } - else if(state.turretState == TurretState::TURRET_MANUAL || (fabs(state.turretTarget - turretEncoder.GetPosition()) < ShooterConstants::turretAllowedError && state.turretState == TurretState::TURRET_HOME_MID)){ + else if(state.turretState == TurretState::TURRET_MANUAL || (fabs(ShooterConstants::homePositionMid - turretEncoder.GetPosition()) < ShooterConstants::turretAllowedError * 10 && state.turretState == TurretState::TURRET_HOME_MID)){ state.turretState = TurretState::TURRET_TRACK; } @@ -290,14 +293,18 @@ void Shooter::analyzeDashboard() table->PutNumber("Turret State", state.turretState); + table->PutNumber("LoBF Zoom", state.LoBFZoom); + state.hoodC_1x = table->GetNumber("Hood Y Int 1X", ShooterConstants::cHood_1x); state.powerC_1x = table->GetNumber("Power Y Int 1X", ShooterConstants::cPower_1x); state.hoodC_2x = table->GetNumber("Hood Y Int 2X", ShooterConstants::cHood_2x); state.powerC_2x = table->GetNumber("Power Y Int 2X", ShooterConstants::cPower_2x); + + state.pipeline = limeTable->GetNumber("pipeline", 0); } -//0 is close, 1 is far, 2 is auto +//0 is close (1x zoom), 1 is far (2x zoom), 2 is auto (1x zoom) void Shooter::setLimelight(int pipeline){ limeTable->PutNumber("pipeline", pipeline); state.pipeline = pipeline; @@ -377,7 +384,12 @@ void Shooter::assignOutputs() state.flywheelTarget = state.flywheelLow; } else if(state.flywheelState == FlywheelState::FLYWHEEL_TRACK){ - state.flywheelTarget = ShooterConstants::aPower_1x *(state.distanceToHub * state.distanceToHub) + ShooterConstants::bPower_1x * state.distanceToHub + state.powerC_1x ; //commented out for testing PID + if (state.pipeline == 1) { + state.flywheelTarget = ShooterConstants::aPower_2x *(state.distanceToHub * state.distanceToHub) + ShooterConstants::bPower_2x * state.distanceToHub + state.powerC_2x ; + } + else { + state.flywheelTarget = ShooterConstants::aPower_1x *(state.distanceToHub * state.distanceToHub) + ShooterConstants::bPower_1x * state.distanceToHub + state.powerC_1x ; + } } else if(state.flywheelState == FlywheelState::FLYWHEEL_POOP){ state.flywheelTarget = ShooterConstants::flywheelPoopValue; @@ -403,7 +415,14 @@ void Shooter::assignOutputs() state.hoodTarget = state.hoodLow; } else if(state.hoodState == HoodState::HOOD_TRACK){ - state.hoodTarget = ShooterConstants::aHood_1x * (state.distanceToHub * state.distanceToHub) + ShooterConstants::bHood_1x * state.distanceToHub + state.hoodC_1x; //commented out for testing PID + if (state.pipeline == 1) { + state.hoodTarget = ShooterConstants::aHood_2x * (state.distanceToHub * state.distanceToHub) + ShooterConstants::bHood_2x * state.distanceToHub + state.hoodC_2x; + state.LoBFZoom = 2; + } + else { + state.hoodTarget = ShooterConstants::aHood_1x * (state.distanceToHub * state.distanceToHub) + ShooterConstants::bHood_1x * state.distanceToHub + state.hoodC_1x; + state.LoBFZoom = 1; + } } else if(state.hoodState == HoodState::HOOD_POOP){ state.hoodTarget = ShooterConstants::hoodPoop; diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index e88099f..68e6d31 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -117,19 +117,19 @@ namespace ShooterConstants{ constexpr static int CAN_ID_TURRET = 12; constexpr static int CAN_ID_HOOD = 15; - constexpr static double aPower_1x = 0.0452; - constexpr static double bPower_1x = -0.0879; - constexpr static double cPower_1x = 0.448; - constexpr static double aHood_1x = 5.33; - constexpr static double bHood_1x = -4.69; - constexpr static double cHood_1x = -1.51; - - constexpr static double aPower_2x = 0.123; - constexpr static double bPower_2x = -0.307; - constexpr static double cPower_2x = -0.618; - constexpr static double aHood_2x = 4.05; - constexpr static double bHood_2x = 6.72; - constexpr static double cHood_2x = -11.6; + constexpr static double aPower_1x = 0.114; + constexpr static double bPower_1x = -0.36; + constexpr static double cPower_1x = 0.705; + constexpr static double aHood_1x = 12.5; + constexpr static double bHood_1x = -32.7; + constexpr static double cHood_1x = 23.1; + + constexpr static double aPower_2x = 0.165; + constexpr static double bPower_2x = -0.432; + constexpr static double cPower_2x = 0.704; + constexpr static double aHood_2x = 18.7; + constexpr static double bHood_2x = -41.2; + constexpr static double cHood_2x = 24.6; constexpr static double limelightTurnKP = (.3 / 25.445) * 1.25; constexpr static double limelightAngle = 50; @@ -173,7 +173,7 @@ namespace ShooterConstants{ constexpr static double turretMaxV = 10000; constexpr static double turretMinV = 0; constexpr static double turretMaxAccel = turretMaxV * 2; - constexpr static double turretAllowedError = 0.5; + constexpr static double turretAllowedError = 0.75; constexpr static double hoodKP = 5e-5; constexpr static double hoodKI = 0; @@ -240,7 +240,7 @@ namespace FeederConstants{ constexpr static double DEFAULT_FEEDER_SPEED_FORWARD_SHOOT = 0.9; constexpr static double DEFAULT_FEEDER_SPEED_REVERSE = -1.0; - constexpr static int CACHE_SIZE = 15; + constexpr static int CACHE_SIZE = 20; constexpr static double JAM_CURRENT = 30; } diff --git a/Competition/src/main/include/subsystems/Shooter.h b/Competition/src/main/include/subsystems/Shooter.h index cf8c5d7..4b446b6 100644 --- a/Competition/src/main/include/subsystems/Shooter.h +++ b/Competition/src/main/include/subsystems/Shooter.h @@ -120,6 +120,7 @@ class Shooter : public ValorSubsystem int currentBall; int pipeline; + int LoBFZoom; double hoodB_1x; double powerB_1x; From fa9a7208347576f26f8bb1e9dc83dfe0c9c5abd6 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Mon, 4 Apr 2022 20:00:40 -0500 Subject: [PATCH 09/62] tuned autos and lift, fixed dumbassery of Rohit and Andrew --- Competition/src/main/cpp/ValorAuto.cpp | 46 +++++++++++++------ Competition/src/main/cpp/ValorSubsystem.cpp | 2 +- .../src/main/cpp/subsystems/Drivetrain.cpp | 4 +- .../src/main/cpp/subsystems/Feeder.cpp | 13 ++++-- Competition/src/main/cpp/subsystems/Lift.cpp | 6 ++- .../src/main/cpp/subsystems/ValorSwerve.cpp | 4 +- Competition/src/main/include/Constants.h | 2 +- .../src/main/include/subsystems/Feeder.h | 1 + 8 files changed, 53 insertions(+), 25 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 405640a..fb95676 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -46,10 +46,10 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Translation2d porkyEntryRed = frc::Translation2d(1.3_m, 3.25_m); //3.5 frc::Pose2d porkyRed = frc::Pose2d(-0.35_m, 2.1_m, frc::Rotation2d(212_deg)); - frc::Pose2d porkyStepBackRed = frc::Pose2d(.3_m, 2.8_m, frc::Rotation2d(212_deg)); + frc::Pose2d porkyStepBackRed = frc::Pose2d(.7_m, 3.2_m, frc::Rotation2d(212_deg)); frc::Translation2d porkyEntryBlue = frc::Translation2d(1.3_m, 3.25_m); //3.5 frc::Pose2d porkyBlue = frc::Pose2d(-0.35_m, 2.1_m, frc::Rotation2d(212_deg)); - frc::Pose2d porkyStepBackBlue = frc::Pose2d(.3_m, 2.8_m, frc::Rotation2d(212_deg)); + frc::Pose2d porkyStepBackBlue = frc::Pose2d(.7_m, 3.2_m, frc::Rotation2d(212_deg)); frc::Translation2d shootConstrainRed = frc::Translation2d(3.15_m, 2_m); //1.2_m in case we need to push it more towards wall frc::Pose2d shootRed = frc::Pose2d(6_m, 1.2_m, frc::Rotation2d(53_deg)); // lower angle to 50 in case of time @@ -65,17 +65,17 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d marvinShootRed = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); frc::Pose2d marvinShootBlue = frc::Pose2d(units::meter_t(5.2), units::meter_t(6.8), frc::Rotation2d(27_deg)); - frc::Pose2d oppRemoveRed = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.8), frc::Rotation2d(30_deg)); - frc::Pose2d oppRemoveBlue = frc::Pose2d(units::meter_t(5.969), units::meter_t(7.26), frc::Rotation2d(30_deg)); + frc::Pose2d tasRed = frc::Pose2d(units::meter_t(5.869), units::meter_t(7.6), frc::Rotation2d(30_deg)); + frc::Pose2d tasBlue = frc::Pose2d(units::meter_t(5.869), units::meter_t(7.6), frc::Rotation2d(30_deg)); - frc::Pose2d hangarSpotRed = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(180_deg)); - frc::Pose2d hangarSpotBlue = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(180_deg)); + frc::Pose2d hangarSpotRed = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(190_deg)); + frc::Pose2d hangarSpotBlue = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(190_deg)); frc::Pose2d endPose2ballRed = frc::Pose2d(10_m, 10_m, frc::Rotation2d(0_deg)); frc::Pose2d endPose2ballBlue = frc::Pose2d(10_m, 10_m, frc::Rotation2d(0_deg)); frc2::InstantCommand cmd_printHeading = frc2::InstantCommand( [&] { - std::cout << drivetrain->getPose_m().Rotation().Degrees().to() << std::endl; + // std::cout << drivetrain->getPose_m().Rotation().Degrees().to() << std::endl; } ); frc2::InstantCommand cmd_nextBall = frc2::InstantCommand( [&] { @@ -215,13 +215,18 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder auto moveOppBallRed = frc::TrajectoryGenerator::GenerateTrajectory( marvinShootRed, {}, - oppRemoveRed, + tasRed, config); auto moveHangarRed = frc::TrajectoryGenerator::GenerateTrajectory( - oppRemoveRed, + tasRed, {}, hangarSpotRed, reverseConfig); + auto moveEndRed = frc::TrajectoryGenerator::GenerateTrajectory( + hangarSpotRed, + {}, + tasRed, + reverseConfig); frc2::SwerveControllerCommand<4> cmd_move_moveBugsRed( moveBugsRed, @@ -412,6 +417,16 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); + frc2::SwerveControllerCommand<4> cmd_move_moveEndRed( + moveEndRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); frc2::SequentialCommandGroup *shoot3Red = new frc2::SequentialCommandGroup(); @@ -458,8 +473,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder (cmd_setOdometryRed, cmd_turretDisable, cmd_shooterAuto, - cmd_intakeAuto, + cmd_intakeOne, cmd_move_moveBugsRed, + cmd_intakeDisable, cmd_move_moveBackBugsRed, cmd_intakeDisable, cmd_move_movePreDaffyRed, @@ -467,18 +483,16 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).25), cmd_intakeShoot, frc2::WaitCommand((units::second_t).6), - cmd_nextBall, cmd_intakeAuto, cmd_move_moveDaffyFromPredaffyRed, frc2::WaitCommand((units::second_t).25), cmd_intakeShoot, frc2::WaitCommand((units::second_t).2), - cmd_nextBall, cmd_turretDisable, cmd_intakeAuto, cmd_move_movePorkyFromDaffyRed, cmd_move_moveStepBackRed, - cmd_intakeAuto, + //cmd_intakeAuto, frc2::WaitCommand((units::second_t).5), cmd_turretHomeMid, cmd_move_moveShootRed, @@ -519,6 +533,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).225), cmd_intakeShoot ); + frc2::SequentialCommandGroup *shoot2RedAlt = new frc2::SequentialCommandGroup(); shoot2RedAlt->AddCommands (cmd_set2ballOdometryRed, @@ -534,7 +549,10 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeAuto, cmd_move_moveOppRemoveRed, cmd_move_moveHangarRed, - cmd_intakeReverse + cmd_intakeReverse, + frc2::WaitCommand((units::second_t)1), + cmd_intakeDisable, + cmd_move_moveEndRed ); m_chooser.AddOption("RED 2 ball auto alt", shoot2RedAlt); diff --git a/Competition/src/main/cpp/ValorSubsystem.cpp b/Competition/src/main/cpp/ValorSubsystem.cpp index bd9ea55..1d9762b 100644 --- a/Competition/src/main/cpp/ValorSubsystem.cpp +++ b/Competition/src/main/cpp/ValorSubsystem.cpp @@ -32,7 +32,7 @@ ValorSubsystem& ValorSubsystem::GetInstance() { void ValorSubsystem::init() { // init subsystem - std::cout << "init valor subsytem" << std::endl; + //std::cout << "init valor subsytem" << std::endl; } void ValorSubsystem::analyzeDashboard() { diff --git a/Competition/src/main/cpp/subsystems/Drivetrain.cpp b/Competition/src/main/cpp/subsystems/Drivetrain.cpp index 41ca261..35770bf 100644 --- a/Competition/src/main/cpp/subsystems/Drivetrain.cpp +++ b/Competition/src/main/cpp/subsystems/Drivetrain.cpp @@ -29,7 +29,7 @@ Drivetrain::Drivetrain() : ValorSubsystem(), void Drivetrain::setKF(){ azimuthMotors[0]->Config_kF(0, SwerveConstants::KF); - std::cout << "set kf" << std::endl; + //std::cout << "set kf" << std::endl; } Drivetrain::~Drivetrain() @@ -98,7 +98,7 @@ void Drivetrain::init() resetState(); //pullSwerveModuleZeroReference(); - std::cout <<"init drivetrain" << std::endl; + //std::cout <<"init drivetrain" << std::endl; } void Drivetrain::setController(frc::XboxController *controller) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 2b77d3c..28d855a 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -10,6 +10,7 @@ #include "subsystems/Feeder.h" +#include Feeder::Feeder() : ValorSubsystem(), driverController(NULL), @@ -46,6 +47,10 @@ void Feeder::init() table->PutNumber("Feeder Forward Speed Default", FeederConstants::DEFAULT_FEEDER_SPEED_FORWARD_DEFAULT); table->PutNumber("Feeder Forward Speed Shoot", FeederConstants::DEFAULT_FEEDER_SPEED_FORWARD_SHOOT); table->PutNumber("Intake Spike Current", FeederConstants::JAM_CURRENT); + + table->PutNumber("Average Amps", 0); + table->PutBoolean("Spiked: ", 0); + table->PutBoolean("Banner: ", 0); resetState(); @@ -94,9 +99,6 @@ void Feeder::assessInputs() else { state.feederState = FeederState::FEEDER_DISABLE; } - - // Calculate instantaneous current - calcCurrent(); } void Feeder::analyzeDashboard() @@ -112,6 +114,10 @@ void Feeder::analyzeDashboard() table->PutNumber("Average Amps", state.instCurrent); table->PutBoolean("Spiked: ", state.spiked); table->PutBoolean("Banner: ", state.bannerTripped); + + table->PutNumber("current feeder state", state.feederState); + // Calculate instantaneous current + calcCurrent(); } void Feeder::assignOutputs() @@ -188,7 +194,6 @@ void Feeder::resetDeque() { } } - void Feeder::resetState() { state.feederState = FeederState::FEEDER_DISABLE; diff --git a/Competition/src/main/cpp/subsystems/Lift.cpp b/Competition/src/main/cpp/subsystems/Lift.cpp index 32d92a5..d339244 100644 --- a/Competition/src/main/cpp/subsystems/Lift.cpp +++ b/Competition/src/main/cpp/subsystems/Lift.cpp @@ -27,6 +27,8 @@ void Lift::init() rotateMotor.EnableSoftLimit(rev::CANSparkMax::SoftLimitDirection::kReverse, true); rotateMotor.SetSoftLimit(rev::CANSparkMax::SoftLimitDirection::kReverse, LiftConstants::rotateReverseLimit); + //rotateMotor.SetSecondaryCurrentLimit(40, 5); + leadMainMotor.ConfigForwardSoftLimitThreshold(LiftConstants::extendForwardLimit); leadMainMotor.ConfigReverseSoftLimitThreshold(LiftConstants::extendReverseLimit); @@ -48,6 +50,7 @@ void Lift::init() rotateMotorPidController.SetIZone(LiftConstants::rotate_kIz); rotateMotorPidController.SetFF(LiftConstants::rotate_kFF); rotateMotorPidController.SetOutputRange(LiftConstants::rotate_kMinOutput, LiftConstants::rotate_kMaxOutput); + rotateMotorPidController.SetSmartMotionMaxVelocity(LiftConstants::rotate_kMaxVel); rotateMotorPidController.SetSmartMotionMinOutputVelocity(LiftConstants::rotate_kMinVel); @@ -172,7 +175,7 @@ void Lift::setupCommands() //frc2::ParallelCommandGroup grabBar(liftRotateIn, liftPullUp); liftSequenceUp.AddCommands(liftExtend, liftRotateOut, liftExtend2, liftRotateIn); - liftSequenceDown.AddCommands(liftPullUpStop, liftMainSlowUp); + liftSequenceDown.AddCommands(liftPullUpStop);//, liftMainSlowUp); } void Lift::assessInputs() @@ -261,6 +264,7 @@ void Lift::analyzeDashboard() table->PutNumber("Lift Main Encoder Value", getExtensionEncoderValue()); table->PutNumber("Lift Rotate Encoder Value", getRotationEncoderValue()); + table->PutNumber("Lift rotate current draw", rotateMotor.GetOutputCurrent()); table->PutNumber("Lift Automation State", state.liftStateAutomation); diff --git a/Competition/src/main/cpp/subsystems/ValorSwerve.cpp b/Competition/src/main/cpp/subsystems/ValorSwerve.cpp index d586cca..a5e04a9 100644 --- a/Competition/src/main/cpp/subsystems/ValorSwerve.cpp +++ b/Competition/src/main/cpp/subsystems/ValorSwerve.cpp @@ -87,7 +87,7 @@ void ValorSwerve::storeAzimuthZeroReference() ofs.open(stream.str(), std::ofstream::out); ofs << std::to_string(position); ofs.close(); - std::cout << "stored position in file" << std::endl; + //std::cout << "stored position in file" << std::endl; } void ValorSwerve::loadAndSetAzimuthZeroReference() @@ -117,7 +117,7 @@ void ValorSwerve::loadAndSetAzimuthZeroReference() // Set the azimuth offset to the calculated setpoint (which will take over in teleop) azimuthFalcon->SetSelectedSensorPosition(azimuthSetpoint, 0, 10); - std::cout << "pulled pospition from file" << std::endl; + //std::cout << "pulled pospition from file" << std::endl; } WPI_TalonFX* ValorSwerve::getAzimuthFalcon() diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 68e6d31..1312ebf 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -241,7 +241,7 @@ namespace FeederConstants{ constexpr static double DEFAULT_FEEDER_SPEED_REVERSE = -1.0; constexpr static int CACHE_SIZE = 20; - constexpr static double JAM_CURRENT = 30; + constexpr static double JAM_CURRENT = 20; } namespace MathConstants{ diff --git a/Competition/src/main/include/subsystems/Feeder.h b/Competition/src/main/include/subsystems/Feeder.h index 8264f63..d88fc55 100644 --- a/Competition/src/main/include/subsystems/Feeder.h +++ b/Competition/src/main/include/subsystems/Feeder.h @@ -92,6 +92,7 @@ class Feeder : public ValorSubsystem void calcCurrent(); void resetDeque(); + }; #endif \ No newline at end of file From 2d10448add96d26534fb1a1d107344b067a49ae4 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Mon, 4 Apr 2022 23:29:11 -0500 Subject: [PATCH 10/62] made 2 ball auto for blue, matched blue to be mirror of red --- Competition/src/main/cpp/ValorAuto.cpp | 130 +++++++++++++++++++++++-- 1 file changed, 120 insertions(+), 10 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index fb95676..28a8249 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -106,6 +106,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::InstantCommand cmd_intakeAuto = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_CURRENT_INTAKE; } ); frc2::InstantCommand cmd_intakeShoot = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_SHOOT; } ); frc2::InstantCommand cmd_intakeReverse = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_REVERSE; } ); + frc2::InstantCommand cmd_setOdometryRed = frc2::InstantCommand( [&] { drivetrain->resetOdometry(startPoseRed); }); @@ -115,6 +116,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::InstantCommand cmd_set2ballOdometryRed = frc2::InstantCommand( [&] { drivetrain->resetOdometry(startPose2ballRed); }); + frc2::InstantCommand cmd_set2ballOdometryBlue = frc2::InstantCommand( [&] { + drivetrain->resetOdometry(startPose2ballBlue); + }); frc2::InstantCommand cmd_setEnd2ballRed = frc2::InstantCommand( [&] { drivetrain->resetOdometry(endPose2ballRed); }); @@ -204,29 +208,57 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder {}, marvinRed, config); + auto moveMarvinBlue = frc::TrajectoryGenerator::GenerateTrajectory( + startPose2ballBlue, + {}, + marvinBlue, + config); - auto moveMarvinRedShoot = frc::TrajectoryGenerator::GenerateTrajectory( + auto moveMarvinShootRed = frc::TrajectoryGenerator::GenerateTrajectory( marvinRed, {}, marvinShootRed, config ); + auto moveMarvinShootBlue = frc::TrajectoryGenerator::GenerateTrajectory( + marvinBlue, + {}, + marvinShootBlue, + config + ); - auto moveOppBallRed = frc::TrajectoryGenerator::GenerateTrajectory( + auto moveTasRed = frc::TrajectoryGenerator::GenerateTrajectory( marvinShootRed, {}, tasRed, config); + auto moveTasBlue = frc::TrajectoryGenerator::GenerateTrajectory( + marvinShootBlue, + {}, + tasBlue, + config); + auto moveHangarRed = frc::TrajectoryGenerator::GenerateTrajectory( tasRed, {}, hangarSpotRed, reverseConfig); + auto moveHangarBlue = frc::TrajectoryGenerator::GenerateTrajectory( + tasBlue, + {}, + hangarSpotBlue, + reverseConfig); + auto moveEndRed = frc::TrajectoryGenerator::GenerateTrajectory( hangarSpotRed, {}, tasRed, reverseConfig); + auto moveEndBlue = frc::TrajectoryGenerator::GenerateTrajectory( + hangarSpotBlue, + {}, + tasBlue, + reverseConfig); frc2::SwerveControllerCommand<4> cmd_move_moveBugsRed( moveBugsRed, @@ -385,9 +417,29 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); + frc2::SwerveControllerCommand<4> cmd_move_moveMarvinBlue( + moveMarvinBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); frc2::SwerveControllerCommand<4> cmd_move_moveMarvinShootRed( - moveMarvinRedShoot, + moveMarvinShootRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveMarvinShootBlue( + moveMarvinShootBlue, [&] () { return drivetrain->getPose_m(); }, drivetrain->getKinematics(), frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), @@ -397,8 +449,19 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder {drivetrain} ); - frc2::SwerveControllerCommand<4> cmd_move_moveOppRemoveRed( - moveOppBallRed, + frc2::SwerveControllerCommand<4> cmd_move_moveTasRed( + moveTasRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + + frc2::SwerveControllerCommand<4> cmd_move_moveTasBlue( + moveTasBlue, [&] () { return drivetrain->getPose_m(); }, drivetrain->getKinematics(), frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), @@ -407,6 +470,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); + frc2::SwerveControllerCommand<4> cmd_move_moveHangarRed( moveHangarRed, [&] () { return drivetrain->getPose_m(); }, @@ -417,7 +481,18 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); - frc2::SwerveControllerCommand<4> cmd_move_moveEndRed( + frc2::SwerveControllerCommand<4> cmd_move_moveHangarBlue( + moveHangarBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + + frc2::SwerveControllerCommand<4> cmd_move_moveEndRed( moveEndRed, [&] () { return drivetrain->getPose_m(); }, drivetrain->getKinematics(), @@ -427,14 +502,25 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); + frc2::SwerveControllerCommand<4> cmd_move_moveEndBlue( + moveEndBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); frc2::SequentialCommandGroup *shoot3Red = new frc2::SequentialCommandGroup(); shoot3Red->AddCommands (cmd_setOdometryRed, cmd_shooterAuto, - cmd_intakeAuto, + cmd_intakeOne, cmd_move_moveBugsRed, + cmd_intakeDisable, cmd_move_movePreDaffyRed, cmd_turretTrack, frc2::WaitCommand((units::second_t).5), @@ -452,8 +538,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shoot3Blue->AddCommands (cmd_setOdometryBlue, cmd_shooterAuto, - cmd_intakeAuto, + cmd_intakeOne, cmd_move_moveBugsBlue, + cmd_intakeDisable, cmd_move_movePreDaffyBlue, cmd_turretTrack, frc2::WaitCommand((units::second_t).5), @@ -505,8 +592,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder (cmd_setOdometryBlue, cmd_turretDisable, cmd_shooterAuto, - cmd_intakeAuto, + cmd_intakeOne, cmd_move_moveBugsBlue, + cmd_intakeDisable, cmd_move_moveBackBugsBlue, cmd_intakeDisable, cmd_move_movePreDaffyBlue, @@ -547,7 +635,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeAuto, - cmd_move_moveOppRemoveRed, + cmd_move_moveTasRed, cmd_move_moveHangarRed, cmd_intakeReverse, frc2::WaitCommand((units::second_t)1), @@ -555,7 +643,29 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_move_moveEndRed ); + frc2::SequentialCommandGroup *shoot2BlueAlt = new frc2::SequentialCommandGroup(); + shoot2BlueAlt->AddCommands + (cmd_set2ballOdometryBlue, + cmd_intakeAuto, + cmd_shooterAuto, + cmd_move_moveMarvinBlue, + cmd_intakeDisable, + cmd_move_moveMarvinShootBlue, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t).5), + cmd_intakeAuto, + cmd_move_moveTasBlue, + cmd_move_moveHangarBlue, + cmd_intakeReverse, + frc2::WaitCommand((units::second_t)1), + cmd_intakeDisable, + cmd_move_moveEndBlue + ); + m_chooser.AddOption("RED 2 ball auto alt", shoot2RedAlt); + m_chooser.AddOption("Blue 2 ball auto alt", shoot2BlueAlt); m_chooser.AddOption("RED 3 ball auto", shoot3Red); From c79fa64f748671c78f7bcf180f7331503e81afa1 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Tue, 5 Apr 2022 17:15:58 -0500 Subject: [PATCH 11/62] current sensing in auto fixes --- Competition/src/main/cpp/ValorAuto.cpp | 142 +++++++++++++++++++++++-- 1 file changed, 135 insertions(+), 7 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 28a8249..d679f83 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -68,12 +68,22 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d tasRed = frc::Pose2d(units::meter_t(5.869), units::meter_t(7.6), frc::Rotation2d(30_deg)); frc::Pose2d tasBlue = frc::Pose2d(units::meter_t(5.869), units::meter_t(7.6), frc::Rotation2d(30_deg)); + frc::Translation2d tasToSpeedyConstrainRed = frc::Translation2d(.75_m, 2.5_m); //1.2_m in case we need to push it more towards wall + frc::Translation2d tasToSpeedyConstrainBlue = frc::Translation2d(.75_m, 2.5_m); //1.2_m in case we need to push it more towards wall + frc::Pose2d hangarSpotRed = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(190_deg)); frc::Pose2d hangarSpotBlue = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(190_deg)); + frc::Pose2d speedyRed = frc::Pose2d(3.45_m, 2.15_m, frc::Rotation2d(-60_deg)); //originally 0 + frc::Pose2d speedyBlue = frc::Pose2d(3.45_m, 2.15_m, frc::Rotation2d(-60_deg)); //originally 0 + + frc::Pose2d trenchEndRed = frc::Pose2d(3.75_m, 2.15_m, frc::Rotation2d(0_deg)); + frc::Pose2d trenchEndBlue = frc::Pose2d(3.75_m, 2.15_m, frc::Rotation2d(0_deg)); + frc::Pose2d endPose2ballRed = frc::Pose2d(10_m, 10_m, frc::Rotation2d(0_deg)); frc::Pose2d endPose2ballBlue = frc::Pose2d(10_m, 10_m, frc::Rotation2d(0_deg)); + frc2::InstantCommand cmd_printHeading = frc2::InstantCommand( [&] { // std::cout << drivetrain->getPose_m().Rotation().Degrees().to() << std::endl; } ); @@ -88,6 +98,10 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shooter->state.flywheelState = Shooter::FlywheelState::FLYWHEEL_TRACK; shooter->state.hoodState = Shooter::HoodState::HOOD_TRACK; } ); + frc2::InstantCommand cmd_shooterPoop = frc2::InstantCommand( [&] { + shooter->state.flywheelState = Shooter::FlywheelState::FLYWHEEL_POOP; + shooter->state.hoodState = Shooter::HoodState::HOOD_POOP; + } ); frc2::InstantCommand cmd_turretTrack = frc2::InstantCommand( [&] { shooter->state.turretState = Shooter::TurretState::TURRET_TRACK; @@ -96,6 +110,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::InstantCommand cmd_turretHomeMid = frc2::InstantCommand( [&] { shooter->state.turretState = Shooter::TurretState::TURRET_HOME_MID; } ); + frc2::InstantCommand cmd_turretHomeLeft = frc2::InstantCommand( [&] { + shooter->state.turretState = Shooter::TurretState::TURRET_HOME_LEFT; + } ); frc2::InstantCommand cmd_turretDisable = frc2::InstantCommand( [&] { shooter->state.turretState = Shooter::TurretState::TURRET_DISABLE; @@ -259,6 +276,28 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder {}, tasBlue, reverseConfig); + + auto moveSpeedyFromTasRed = frc::TrajectoryGenerator::GenerateTrajectory( + tasRed, + {tasToSpeedyConstrainRed}, + speedyRed, + reverseConfig); + auto moveSpeedyFromTasBlue = frc::TrajectoryGenerator::GenerateTrajectory( + tasBlue, + {tasToSpeedyConstrainBlue}, + speedyBlue, + reverseConfig); + + auto moveEndFromSpeedyRed = frc::TrajectoryGenerator::GenerateTrajectory( + speedyRed, + {}, + trenchEndRed, + config); + auto moveEndFromSpeedyBlue = frc::TrajectoryGenerator::GenerateTrajectory( + speedyBlue, + {}, + trenchEndBlue, + config); frc2::SwerveControllerCommand<4> cmd_move_moveBugsRed( moveBugsRed, @@ -491,6 +530,48 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); + frc2::SwerveControllerCommand<4> cmd_move_moveSpeedyFromTasRed( + moveSpeedyFromTasRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + + frc2::SwerveControllerCommand<4> cmd_move_moveEndFromSpeedyRed( + moveEndFromSpeedyRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveSpeedyFromTasRed( + moveSpeedyFromTasBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + + frc2::SwerveControllerCommand<4> cmd_move_moveEndFromSpeedyBlue( + moveEndFromSpeedyBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); frc2::SwerveControllerCommand<4> cmd_move_moveEndRed( moveEndRed, @@ -576,10 +657,10 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeShoot, frc2::WaitCommand((units::second_t).2), cmd_turretDisable, - cmd_intakeAuto, + cmd_intakeOne, cmd_move_movePorkyFromDaffyRed, cmd_move_moveStepBackRed, - //cmd_intakeAuto, + cmd_intakeAuto, frc2::WaitCommand((units::second_t).5), cmd_turretHomeMid, cmd_move_moveShootRed, @@ -610,7 +691,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).2), cmd_nextBall, cmd_turretDisable, - cmd_intakeAuto, + cmd_intakeOne, cmd_move_movePorkyFromDaffyBlue, cmd_move_moveStepBackBlue, cmd_intakeAuto, @@ -625,7 +706,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::SequentialCommandGroup *shoot2RedAlt = new frc2::SequentialCommandGroup(); shoot2RedAlt->AddCommands (cmd_set2ballOdometryRed, - cmd_intakeAuto, + cmd_intakeOne, cmd_shooterAuto, cmd_move_moveMarvinRed, cmd_intakeDisable, @@ -634,7 +715,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).5), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), - cmd_intakeAuto, + cmd_intakeOne, cmd_move_moveTasRed, cmd_move_moveHangarRed, cmd_intakeReverse, @@ -646,7 +727,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::SequentialCommandGroup *shoot2BlueAlt = new frc2::SequentialCommandGroup(); shoot2BlueAlt->AddCommands (cmd_set2ballOdometryBlue, - cmd_intakeAuto, + cmd_intakeOne, cmd_shooterAuto, cmd_move_moveMarvinBlue, cmd_intakeDisable, @@ -655,7 +736,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).5), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), - cmd_intakeAuto, + cmd_intakeOne, cmd_move_moveTasBlue, cmd_move_moveHangarBlue, cmd_intakeReverse, @@ -664,6 +745,53 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_move_moveEndBlue ); + frc2::SequentialCommandGroup *shoot2Def2Red = new frc2::SequentialCommandGroup(); + shoot2Def2Red->AddCommands + (cmd_set2ballOdometryRed, + cmd_intakeOne, + cmd_shooterAuto, + cmd_move_moveMarvinRed, + cmd_intakeDisable, + cmd_move_moveMarvinShootRed, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t).5), + cmd_intakeAuto, + cmd_move_moveTasRed, + cmd_move_moveSpeedyFromTasRed, + cmd_turretHomeLeft, + cmd_shooterPoop, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)1), + cmd_intakeDisable, + cmd_move_moveEndFromSpeedyRed + ); + frc2::SequentialCommandGroup *shoot2Def2Blue = new frc2::SequentialCommandGroup(); + shoot2Def2Blue->AddCommands + (cmd_set2ballOdometryBlue, + cmd_intakeOne, + cmd_shooterAuto, + cmd_move_moveMarvinBlue, + cmd_intakeDisable, + cmd_move_moveMarvinShootBlue, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t).5), + cmd_intakeAuto, + cmd_move_moveTasBlue, + //cmd_move_moveSpeedyFromTasBlue, + cmd_turretHomeLeft, + cmd_shooterPoop, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)1), + cmd_intakeDisable, + cmd_move_moveEndFromSpeedyBlue + ); + m_chooser.AddOption("RED 2 ball auto alt", shoot2RedAlt); m_chooser.AddOption("Blue 2 ball auto alt", shoot2BlueAlt); From 8fe0270d27bf53b0e720b7b26ecac5197babd45e Mon Sep 17 00:00:00 2001 From: Andrew McAlinden <52133386+droiddoes9@users.noreply.github.com> Date: Tue, 5 Apr 2022 19:37:24 -0500 Subject: [PATCH 12/62] working code from Tuesday Night, all 3 autos look good --- Competition/src/main/cpp/ValorAuto.cpp | 211 ++++++++++++++---- .../src/main/cpp/subsystems/Drivetrain.cpp | 2 +- .../src/main/cpp/subsystems/Feeder.cpp | 1 + Competition/src/main/include/Constants.h | 2 +- .../src/main/include/subsystems/Feeder.h | 4 +- 5 files changed, 169 insertions(+), 51 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index d679f83..ad28a27 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -45,10 +45,10 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d daffyBlue = frc::Pose2d(3.55_m, 2.15_m, frc::Rotation2d(115_deg)); frc::Translation2d porkyEntryRed = frc::Translation2d(1.3_m, 3.25_m); //3.5 - frc::Pose2d porkyRed = frc::Pose2d(-0.35_m, 2.1_m, frc::Rotation2d(212_deg)); + frc::Pose2d porkyRed = frc::Pose2d(-0.45_m, 2.35_m, frc::Rotation2d(212_deg)); frc::Pose2d porkyStepBackRed = frc::Pose2d(.7_m, 3.2_m, frc::Rotation2d(212_deg)); frc::Translation2d porkyEntryBlue = frc::Translation2d(1.3_m, 3.25_m); //3.5 - frc::Pose2d porkyBlue = frc::Pose2d(-0.35_m, 2.1_m, frc::Rotation2d(212_deg)); + frc::Pose2d porkyBlue = frc::Pose2d(-0.45_m, 2.35_m, frc::Rotation2d(212_deg)); frc::Pose2d porkyStepBackBlue = frc::Pose2d(.7_m, 3.2_m, frc::Rotation2d(212_deg)); frc::Translation2d shootConstrainRed = frc::Translation2d(3.15_m, 2_m); //1.2_m in case we need to push it more towards wall @@ -59,14 +59,17 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d startPose2ballRed = frc::Pose2d(units::meter_t(5.75), units::meter_t(5.42), frc::Rotation2d(137_deg)); frc::Pose2d startPose2ballBlue = frc::Pose2d(units::meter_t(5.75), units::meter_t(5.42), frc::Rotation2d(137_deg)); - frc::Pose2d marvinRed = frc::Pose2d(units::meter_t(3.7), units::meter_t(7.2), frc::Rotation2d(137_deg)); - frc::Pose2d marvinBlue = frc::Pose2d(units::meter_t(4.5), units::meter_t(6.4), frc::Rotation2d(137_deg)); + frc::Pose2d marvinRed = frc::Pose2d(units::meter_t(4), units::meter_t(6.9), frc::Rotation2d(137_deg)); + frc::Pose2d marvinBlue = frc::Pose2d(units::meter_t(4), units::meter_t(6.9), frc::Rotation2d(137_deg)); frc::Pose2d marvinShootRed = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); - frc::Pose2d marvinShootBlue = frc::Pose2d(units::meter_t(5.2), units::meter_t(6.8), frc::Rotation2d(27_deg)); + frc::Pose2d marvinShootBlue = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); - frc::Pose2d tasRed = frc::Pose2d(units::meter_t(5.869), units::meter_t(7.6), frc::Rotation2d(30_deg)); - frc::Pose2d tasBlue = frc::Pose2d(units::meter_t(5.869), units::meter_t(7.6), frc::Rotation2d(30_deg)); + frc::Pose2d marvinShootAltRed = frc::Pose2d(units::meter_t(3.35), units::meter_t(4), frc::Rotation2d(-90_deg)); + frc::Pose2d marvinShootAltBlue = frc::Pose2d(units::meter_t(3.35), units::meter_t(4), frc::Rotation2d(-90_deg)); + + frc::Pose2d tasRed = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.8), frc::Rotation2d(60_deg)); + frc::Pose2d tasBlue = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.8), frc::Rotation2d(60_deg)); frc::Translation2d tasToSpeedyConstrainRed = frc::Translation2d(.75_m, 2.5_m); //1.2_m in case we need to push it more towards wall frc::Translation2d tasToSpeedyConstrainBlue = frc::Translation2d(.75_m, 2.5_m); //1.2_m in case we need to push it more towards wall @@ -74,11 +77,11 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d hangarSpotRed = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(190_deg)); frc::Pose2d hangarSpotBlue = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(190_deg)); - frc::Pose2d speedyRed = frc::Pose2d(3.45_m, 2.15_m, frc::Rotation2d(-60_deg)); //originally 0 - frc::Pose2d speedyBlue = frc::Pose2d(3.45_m, 2.15_m, frc::Rotation2d(-60_deg)); //originally 0 + frc::Pose2d speedyRed = frc::Pose2d(3.35_m, 2.8_m, frc::Rotation2d(-60_deg)); //originally 0 + frc::Pose2d speedyBlue = frc::Pose2d(3.35_m, 2.8_m, frc::Rotation2d(-60_deg)); //originally 0 - frc::Pose2d trenchEndRed = frc::Pose2d(3.75_m, 2.15_m, frc::Rotation2d(0_deg)); - frc::Pose2d trenchEndBlue = frc::Pose2d(3.75_m, 2.15_m, frc::Rotation2d(0_deg)); + frc::Pose2d trenchEndRed = frc::Pose2d(3.75_m, 3_m, frc::Rotation2d(135_deg)); + frc::Pose2d trenchEndBlue = frc::Pose2d(3.75_m, 3_m, frc::Rotation2d(135_deg)); frc::Pose2d endPose2ballRed = frc::Pose2d(10_m, 10_m, frc::Rotation2d(0_deg)); frc::Pose2d endPose2ballBlue = frc::Pose2d(10_m, 10_m, frc::Rotation2d(0_deg)); @@ -113,6 +116,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::InstantCommand cmd_turretHomeLeft = frc2::InstantCommand( [&] { shooter->state.turretState = Shooter::TurretState::TURRET_HOME_LEFT; } ); + frc2::InstantCommand cmd_turretHomeRight = frc2::InstantCommand( [&] { + shooter->state.turretState = Shooter::TurretState::TURRET_HOME_RIGHT; + } ); frc2::InstantCommand cmd_turretDisable = frc2::InstantCommand( [&] { shooter->state.turretState = Shooter::TurretState::TURRET_DISABLE; @@ -121,6 +127,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::InstantCommand cmd_intakeOne = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_REGULAR_INTAKE; } ); frc2::InstantCommand cmd_intakeDisable = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_DISABLE; } ); frc2::InstantCommand cmd_intakeAuto = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_CURRENT_INTAKE; } ); + frc2::InstantCommand cmd_intakeClearDeque = frc2::InstantCommand( [&] { feeder->resetDeque();} ); frc2::InstantCommand cmd_intakeShoot = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_SHOOT; } ); frc2::InstantCommand cmd_intakeReverse = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_REVERSE; } ); @@ -244,6 +251,19 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder config ); + auto moveMarvinShootAltRed = frc::TrajectoryGenerator::GenerateTrajectory( + marvinRed, + {}, + marvinShootAltRed, + config + ); + auto moveMarvinShootAltBlue = frc::TrajectoryGenerator::GenerateTrajectory( + marvinBlue, + {}, + marvinShootAltBlue, + config + ); + auto moveTasRed = frc::TrajectoryGenerator::GenerateTrajectory( marvinShootRed, {}, @@ -279,14 +299,36 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder auto moveSpeedyFromTasRed = frc::TrajectoryGenerator::GenerateTrajectory( tasRed, - {tasToSpeedyConstrainRed}, + {}, speedyRed, reverseConfig); auto moveSpeedyFromTasBlue = frc::TrajectoryGenerator::GenerateTrajectory( tasBlue, - {tasToSpeedyConstrainBlue}, + {}, speedyBlue, reverseConfig); + + auto moveSpeedyFromAltRed = frc::TrajectoryGenerator::GenerateTrajectory( + marvinShootAltRed, + {}, + speedyRed, + config); + auto moveSpeedyFromAltBlue = frc::TrajectoryGenerator::GenerateTrajectory( + marvinShootAltBlue, + {}, + speedyBlue, + config); + + auto moveTasFromSpeedyRed = frc::TrajectoryGenerator::GenerateTrajectory( + speedyRed, + {}, + tasRed, + reverseConfig); + auto moveTasFromSpeedyBlue = frc::TrajectoryGenerator::GenerateTrajectory( + speedyBlue, + {}, + tasBlue, + reverseConfig); auto moveEndFromSpeedyRed = frc::TrajectoryGenerator::GenerateTrajectory( speedyRed, @@ -487,6 +529,27 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); + + frc2::SwerveControllerCommand<4> cmd_move_moveMarvinShootAltRed( + moveMarvinShootAltRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveMarvinShootAltBlue( + moveMarvinShootAltBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); frc2::SwerveControllerCommand<4> cmd_move_moveTasRed( moveTasRed, @@ -540,9 +603,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); - - frc2::SwerveControllerCommand<4> cmd_move_moveEndFromSpeedyRed( - moveEndFromSpeedyRed, + frc2::SwerveControllerCommand<4> cmd_move_moveSpeedyFromTasBlue( + moveSpeedyFromTasBlue, [&] () { return drivetrain->getPose_m(); }, drivetrain->getKinematics(), frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), @@ -551,8 +613,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); - frc2::SwerveControllerCommand<4> cmd_move_moveSpeedyFromTasRed( - moveSpeedyFromTasBlue, + frc2::SwerveControllerCommand<4> cmd_move_moveSpeedyFromAltRed( + moveSpeedyFromAltRed, [&] () { return drivetrain->getPose_m(); }, drivetrain->getKinematics(), frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), @@ -561,7 +623,49 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); - + frc2::SwerveControllerCommand<4> cmd_move_moveSpeedyFromAltBlue( + moveSpeedyFromAltBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + + frc2::SwerveControllerCommand<4> cmd_move_moveTasFromSpeedyRed( + moveTasFromSpeedyRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveTasFromSpeedyBlue( + moveTasFromSpeedyBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + + frc2::SwerveControllerCommand<4> cmd_move_moveEndFromSpeedyRed( + moveEndFromSpeedyRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveEndFromSpeedyBlue( moveEndFromSpeedyBlue, [&] () { return drivetrain->getPose_m(); }, @@ -659,13 +763,14 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretDisable, cmd_intakeOne, cmd_move_movePorkyFromDaffyRed, + cmd_turretHomeMid, cmd_move_moveStepBackRed, + cmd_intakeClearDeque, cmd_intakeAuto, - frc2::WaitCommand((units::second_t).5), - cmd_turretHomeMid, + frc2::WaitCommand((units::second_t).25), + cmd_turretTrack, cmd_move_moveShootRed, - cmd_turretTrack, // if we lower the angle to 50 we could potentially cut tracking / lower time - frc2::WaitCommand((units::second_t).225), + frc2::WaitCommand((units::second_t).375), cmd_intakeShoot ); frc2::SequentialCommandGroup *shoot5Blue = new frc2::SequentialCommandGroup(); @@ -693,13 +798,14 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretDisable, cmd_intakeOne, cmd_move_movePorkyFromDaffyBlue, + cmd_turretHomeMid, cmd_move_moveStepBackBlue, + cmd_intakeClearDeque, cmd_intakeAuto, - frc2::WaitCommand((units::second_t).5), - cmd_turretHomeMid, + frc2::WaitCommand((units::second_t).25), + cmd_turretTrack, cmd_move_moveShootBlue, - cmd_turretTrack, //if we lower the angle to 50 we could potentially cut the tracking / wait - frc2::WaitCommand((units::second_t).225), + frc2::WaitCommand((units::second_t).375), cmd_intakeShoot ); @@ -749,51 +855,62 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shoot2Def2Red->AddCommands (cmd_set2ballOdometryRed, cmd_intakeOne, + cmd_turretHomeLeft, cmd_shooterAuto, + frc2::WaitCommand((units::second_t).125), + cmd_intakeClearDeque, + cmd_intakeAuto, cmd_move_moveMarvinRed, cmd_intakeDisable, - cmd_move_moveMarvinShootRed, + cmd_move_moveMarvinShootAltRed, cmd_turretTrack, frc2::WaitCommand((units::second_t).5), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), - cmd_intakeAuto, - cmd_move_moveTasRed, - cmd_move_moveSpeedyFromTasRed, - cmd_turretHomeLeft, - cmd_shooterPoop, - frc2::WaitCommand((units::second_t).5), - cmd_intakeShoot, + cmd_intakeOne, + cmd_move_moveSpeedyFromAltRed, + cmd_move_moveTasFromSpeedyRed, + cmd_move_moveHangarRed, + cmd_intakeReverse, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, - cmd_move_moveEndFromSpeedyRed + cmd_move_moveEndRed, + cmd_turretHomeRight ); + frc2::SequentialCommandGroup *shoot2Def2Blue = new frc2::SequentialCommandGroup(); shoot2Def2Blue->AddCommands (cmd_set2ballOdometryBlue, cmd_intakeOne, + cmd_turretHomeLeft, cmd_shooterAuto, + frc2::WaitCommand((units::second_t).125), + cmd_intakeClearDeque, + cmd_intakeAuto, cmd_move_moveMarvinBlue, cmd_intakeDisable, - cmd_move_moveMarvinShootBlue, + cmd_move_moveMarvinShootAltBlue, cmd_turretTrack, frc2::WaitCommand((units::second_t).5), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), - cmd_intakeAuto, - cmd_move_moveTasBlue, - //cmd_move_moveSpeedyFromTasBlue, - cmd_turretHomeLeft, - cmd_shooterPoop, - frc2::WaitCommand((units::second_t).5), - cmd_intakeShoot, + cmd_intakeOne, + cmd_move_moveSpeedyFromAltBlue, + cmd_move_moveTasFromSpeedyBlue, + cmd_move_moveHangarBlue, + cmd_intakeReverse, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, - cmd_move_moveEndFromSpeedyBlue + cmd_move_moveEndBlue, + cmd_turretHomeRight ); - m_chooser.AddOption("RED 2 ball auto alt", shoot2RedAlt); - m_chooser.AddOption("Blue 2 ball auto alt", shoot2BlueAlt); + m_chooser.AddOption("RED 2 ball + 1 Defensive", shoot2RedAlt); + m_chooser.AddOption("Blue 2 ball + 1 Defensive", shoot2BlueAlt); + + m_chooser.AddOption("RED 2 ball + 2 Defensive", shoot2Def2Red); + m_chooser.AddOption("Blue 2 ball + 2 Defensive", shoot2Def2Blue); + m_chooser.AddOption("RED 3 ball auto", shoot3Red); diff --git a/Competition/src/main/cpp/subsystems/Drivetrain.cpp b/Competition/src/main/cpp/subsystems/Drivetrain.cpp index 35770bf..5e76539 100644 --- a/Competition/src/main/cpp/subsystems/Drivetrain.cpp +++ b/Competition/src/main/cpp/subsystems/Drivetrain.cpp @@ -252,7 +252,7 @@ void Drivetrain::assignOutputs() ySpeedMPS = units::meters_per_second_t{ySpeed}; if(rot != 0){ int sign = std::signbit(rot) == 0 ? 1 : -1; - rotRPS = units::radians_per_second_t{SwerveConstants::ROTATION_SLOW_SPEED_RPS}; + rotRPS = units::radians_per_second_t{rot * SwerveConstants::ROTATION_SLOW_SPEED_RPS}; } } // double heading = getPose_m().Rotation().Degrees().to(); diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 28d855a..7ce18c7 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -192,6 +192,7 @@ void Feeder::resetDeque() { for (int i = 0; i < FeederConstants::CACHE_SIZE; i++) { state.current_cache.push_back(0); } + state.spiked = false; } void Feeder::resetState() diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 1312ebf..7160a61 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -234,7 +234,7 @@ namespace FeederConstants{ constexpr static int BANNER_DIO_PORT = 5; constexpr static double DEFAULT_INTAKE_SPEED_FORWARD = 0.7; - constexpr static double DEFAULT_INTAKE_SPEED_REVERSE = -0.9; + constexpr static double DEFAULT_INTAKE_SPEED_REVERSE = -1; constexpr static double DEFAULT_FEEDER_SPEED_FORWARD_DEFAULT = 0.5; constexpr static double DEFAULT_FEEDER_SPEED_FORWARD_SHOOT = 0.9; diff --git a/Competition/src/main/include/subsystems/Feeder.h b/Competition/src/main/include/subsystems/Feeder.h index d88fc55..416f974 100644 --- a/Competition/src/main/include/subsystems/Feeder.h +++ b/Competition/src/main/include/subsystems/Feeder.h @@ -78,7 +78,7 @@ class Feeder : public ValorSubsystem FeederState feederState; } state; - +void resetDeque(); private: frc::XboxController *driverController; @@ -91,7 +91,7 @@ class Feeder : public ValorSubsystem void calcCurrent(); - void resetDeque(); + }; From 700bce6d8a8d2b4862ebb3e2033e2ac2d9a483e2 Mon Sep 17 00:00:00 2001 From: Michael Ray Date: Thu, 7 Apr 2022 06:53:40 -0500 Subject: [PATCH 13/62] Make lower limit 1.46m from the limelight (#60) --- Competition/src/main/cpp/subsystems/Shooter.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index dc4ddc2..7d84d6f 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -227,6 +227,10 @@ void Shooter::analyzeDashboard() double deltaH = ShooterConstants::hubHeight - ShooterConstants::limelightHeight; double xDist = deltaH / tan(angle * MathConstants::toRadians); state.distanceToHub = xDist; + + if (state.distanceToHub < 1.46) + state.distanceToHub = 1.46; + table->PutNumber("x distance to hub", xDist); } From 699870f421f29902edbee5ad71a2f6775fe55536 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Wed, 6 Apr 2022 14:12:17 -0500 Subject: [PATCH 14/62] changed start up procedure of intake on all autos --- Competition/src/main/cpp/ValorAuto.cpp | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index ad28a27..9466e99 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -109,6 +109,10 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::InstantCommand cmd_turretTrack = frc2::InstantCommand( [&] { shooter->state.turretState = Shooter::TurretState::TURRET_TRACK; } ); + frc2::InstantCommand cmd_shooterTarmac = frc2::InstantCommand( [&] { + shooter->state.flywheelState = Shooter::FlywheelState::FLYWHEEL_DEFAULT; + shooter->state.hoodState = Shooter::HoodState::HOOD_DOWN; + } ); frc2::InstantCommand cmd_turretHomeMid = frc2::InstantCommand( [&] { shooter->state.turretState = Shooter::TurretState::TURRET_HOME_MID; @@ -704,6 +708,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder (cmd_setOdometryRed, cmd_shooterAuto, cmd_intakeOne, + frc2::WaitCommand((units::second_t).125), + cmd_intakeClearDeque, + cmd_intakeAuto, cmd_move_moveBugsRed, cmd_intakeDisable, cmd_move_movePreDaffyRed, @@ -724,6 +731,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder (cmd_setOdometryBlue, cmd_shooterAuto, cmd_intakeOne, + frc2::WaitCommand((units::second_t).125), + cmd_intakeClearDeque, + cmd_intakeAuto, cmd_move_moveBugsBlue, cmd_intakeDisable, cmd_move_movePreDaffyBlue, @@ -746,6 +756,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretDisable, cmd_shooterAuto, cmd_intakeOne, + frc2::WaitCommand((units::second_t).125), + cmd_intakeClearDeque, + cmd_intakeAuto, cmd_move_moveBugsRed, cmd_intakeDisable, cmd_move_moveBackBugsRed, @@ -769,6 +782,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeAuto, frc2::WaitCommand((units::second_t).25), cmd_turretTrack, + cmd_shooterTarmac, cmd_move_moveShootRed, frc2::WaitCommand((units::second_t).375), cmd_intakeShoot @@ -779,6 +793,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretDisable, cmd_shooterAuto, cmd_intakeOne, + frc2::WaitCommand((units::second_t).125), + cmd_intakeClearDeque, + cmd_intakeAuto, cmd_move_moveBugsBlue, cmd_intakeDisable, cmd_move_moveBackBugsBlue, @@ -804,6 +821,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeAuto, frc2::WaitCommand((units::second_t).25), cmd_turretTrack, + cmd_shooterTarmac, cmd_move_moveShootBlue, frc2::WaitCommand((units::second_t).375), cmd_intakeShoot @@ -813,6 +831,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shoot2RedAlt->AddCommands (cmd_set2ballOdometryRed, cmd_intakeOne, + frc2::WaitCommand((units::second_t).125), + cmd_intakeClearDeque, + cmd_intakeAuto, cmd_shooterAuto, cmd_move_moveMarvinRed, cmd_intakeDisable, @@ -834,6 +855,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shoot2BlueAlt->AddCommands (cmd_set2ballOdometryBlue, cmd_intakeOne, + frc2::WaitCommand((units::second_t).125), + cmd_intakeClearDeque, + cmd_intakeAuto, cmd_shooterAuto, cmd_move_moveMarvinBlue, cmd_intakeDisable, From 4664543730a8887f56399453328ab04c2b6eb5b6 Mon Sep 17 00:00:00 2001 From: j a Date: Thu, 7 Apr 2022 21:06:43 -0500 Subject: [PATCH 15/62] day1 --- Competition/src/main/cpp/ValorAuto.cpp | 2 ++ Competition/src/main/cpp/subsystems/Shooter.cpp | 4 ++-- Competition/src/main/include/Constants.h | 4 ++-- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 9466e99..50cdab4 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -784,6 +784,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretTrack, cmd_shooterTarmac, cmd_move_moveShootRed, + cmd_shooterAuto, frc2::WaitCommand((units::second_t).375), cmd_intakeShoot ); @@ -823,6 +824,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretTrack, cmd_shooterTarmac, cmd_move_moveShootBlue, + cmd_shooterAuto, frc2::WaitCommand((units::second_t).375), cmd_intakeShoot ); diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index 7d84d6f..9cf32bc 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -228,8 +228,8 @@ void Shooter::analyzeDashboard() double xDist = deltaH / tan(angle * MathConstants::toRadians); state.distanceToHub = xDist; - if (state.distanceToHub < 1.46) - state.distanceToHub = 1.46; + if (state.distanceToHub < 1.6) + state.distanceToHub = 1.6; table->PutNumber("x distance to hub", xDist); } diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 7160a61..38a3c85 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -157,7 +157,7 @@ namespace ShooterConstants{ constexpr static double flywheelPrimedValue = 0.46; constexpr static double flywheelAutoValue = 0.405; //can change to .4 - constexpr static double flywheelDefaultValue = 0.42; //.375 + constexpr static double flywheelDefaultValue = 0.45; //.375 constexpr static double flywheelPoopValue = 0.3; constexpr static double flywheelLaunchpadValue = .455; @@ -234,7 +234,7 @@ namespace FeederConstants{ constexpr static int BANNER_DIO_PORT = 5; constexpr static double DEFAULT_INTAKE_SPEED_FORWARD = 0.7; - constexpr static double DEFAULT_INTAKE_SPEED_REVERSE = -1; + constexpr static double DEFAULT_INTAKE_SPEED_REVERSE = -0.7; constexpr static double DEFAULT_FEEDER_SPEED_FORWARD_DEFAULT = 0.5; constexpr static double DEFAULT_FEEDER_SPEED_FORWARD_SHOOT = 0.9; From a7427ddc86c4dffe6d3ae8bc4b79d026cb12573e Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Thu, 7 Apr 2022 21:13:05 -0500 Subject: [PATCH 16/62] changed to shooterPoop instead of intakeReverse on defensive autos --- Competition/src/main/cpp/ValorAuto.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 50cdab4..749017d 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -845,9 +845,11 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeOne, + cmd_turretHomeMid, + cmd_shooterPoop, cmd_move_moveTasRed, cmd_move_moveHangarRed, - cmd_intakeReverse, + cmd_intakeShoot, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, cmd_move_moveEndRed @@ -869,9 +871,11 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeOne, + cmd_turretHomeMid, + cmd_shooterPoop, cmd_move_moveTasBlue, cmd_move_moveHangarBlue, - cmd_intakeReverse, + cmd_intakeShoot, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, cmd_move_moveEndBlue @@ -894,10 +898,12 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeOne, + cmd_turretHomeMid, + cmd_shooterPoop, cmd_move_moveSpeedyFromAltRed, cmd_move_moveTasFromSpeedyRed, cmd_move_moveHangarRed, - cmd_intakeReverse, + cmd_intakeShoot, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, cmd_move_moveEndRed, @@ -921,10 +927,12 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeOne, + cmd_turretHomeMid, + cmd_shooterPoop, cmd_move_moveSpeedyFromAltBlue, cmd_move_moveTasFromSpeedyBlue, cmd_move_moveHangarBlue, - cmd_intakeReverse, + cmd_intakeShoot, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, cmd_move_moveEndBlue, From 6cd0cf1f9a7437caed36b3ddaa2db5e19ed33315 Mon Sep 17 00:00:00 2001 From: j a Date: Sun, 10 Apr 2022 14:09:38 -0500 Subject: [PATCH 17/62] final at tournement code --- Competition/src/main/cpp/ValorAuto.cpp | 12 ++++++++---- Competition/src/main/include/Constants.h | 6 +++--- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 749017d..0639c52 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -113,6 +113,10 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shooter->state.flywheelState = Shooter::FlywheelState::FLYWHEEL_DEFAULT; shooter->state.hoodState = Shooter::HoodState::HOOD_DOWN; } ); + frc2::InstantCommand cmd_shooterDisable = frc2::InstantCommand( [&] { + shooter->state.flywheelState = Shooter::FlywheelState::FLYWHEEL_DISABLE; + shooter->state.hoodState = Shooter::HoodState::HOOD_DOWN; + } ); frc2::InstantCommand cmd_turretHomeMid = frc2::InstantCommand( [&] { shooter->state.turretState = Shooter::TurretState::TURRET_HOME_MID; @@ -849,7 +853,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_shooterPoop, cmd_move_moveTasRed, cmd_move_moveHangarRed, - cmd_intakeShoot, + cmd_intakeReverse, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, cmd_move_moveEndRed @@ -875,7 +879,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_shooterPoop, cmd_move_moveTasBlue, cmd_move_moveHangarBlue, - cmd_intakeShoot, + cmd_intakeReverse, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, cmd_move_moveEndBlue @@ -903,7 +907,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_move_moveSpeedyFromAltRed, cmd_move_moveTasFromSpeedyRed, cmd_move_moveHangarRed, - cmd_intakeShoot, + cmd_intakeReverse, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, cmd_move_moveEndRed, @@ -932,7 +936,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_move_moveSpeedyFromAltBlue, cmd_move_moveTasFromSpeedyBlue, cmd_move_moveHangarBlue, - cmd_intakeShoot, + cmd_intakeReverse, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, cmd_move_moveEndBlue, diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 38a3c85..0819857 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -181,10 +181,10 @@ namespace ShooterConstants{ constexpr static double hoodKIZ = 0; constexpr static double hoodKFF = 0.000156 * .5; - constexpr static double hoodMaxV = 8000; + constexpr static double hoodMaxV = 10000; //8000 constexpr static double hoodMinV = 0; - constexpr static double hoodMaxAccel = hoodMaxV * 1; - constexpr static double hoodAllowedError = 0; + constexpr static double hoodMaxAccel = hoodMaxV * 4; // *1 + constexpr static double hoodAllowedError = 0.2; constexpr static double hoodTop = 5; // constexpr static double hoodAuto = 6; From cb0471149061cd2a23a4cd2dadbe7f306dd237a4 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Tue, 12 Apr 2022 08:53:05 -0500 Subject: [PATCH 18/62] fixed main auto bugs, have to tune points still --- Competition/src/main/cpp/ValorAuto.cpp | 72 +++++++++++++++++++++----- 1 file changed, 60 insertions(+), 12 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 0639c52..f22cc5c 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -832,6 +832,43 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).375), cmd_intakeShoot ); + + frc2::SequentialCommandGroup *shoot2Red = new frc2::SequentialCommandGroup(); + shoot2Red->AddCommands + (cmd_set2ballOdometryRed, + cmd_intakeOne, + frc2::WaitCommand((units::second_t).125), + cmd_intakeClearDeque, + cmd_intakeAuto, + cmd_shooterAuto, + cmd_move_moveMarvinRed, + cmd_intakeDisable, + cmd_move_moveMarvinShootRed, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t).5), + cmd_intakeDisable + ); + + frc2::SequentialCommandGroup *shoot2Blue = new frc2::SequentialCommandGroup(); + shoot2Blue->AddCommands + (cmd_set2ballOdometryBlue, + cmd_intakeOne, + frc2::WaitCommand((units::second_t).125), + cmd_intakeClearDeque, + cmd_intakeAuto, + cmd_shooterAuto, + cmd_move_moveMarvinBlue, + cmd_intakeDisable, + cmd_move_moveMarvinShootBlue, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t).5), + cmd_intakeDisable + ); + frc2::SequentialCommandGroup *shoot2RedAlt = new frc2::SequentialCommandGroup(); shoot2RedAlt->AddCommands @@ -849,14 +886,16 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeOne, - cmd_turretHomeMid, - cmd_shooterPoop, cmd_move_moveTasRed, cmd_move_moveHangarRed, cmd_intakeReverse, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, - cmd_move_moveEndRed + cmd_move_moveEndRed, + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).125), + cmd_shooterAuto ); frc2::SequentialCommandGroup *shoot2BlueAlt = new frc2::SequentialCommandGroup(); @@ -875,14 +914,16 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeOne, - cmd_turretHomeMid, - cmd_shooterPoop, cmd_move_moveTasBlue, cmd_move_moveHangarBlue, cmd_intakeReverse, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, - cmd_move_moveEndBlue + cmd_move_moveEndBlue, + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).125), + cmd_shooterAuto ); frc2::SequentialCommandGroup *shoot2Def2Red = new frc2::SequentialCommandGroup(); @@ -902,8 +943,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeOne, - cmd_turretHomeMid, - cmd_shooterPoop, cmd_move_moveSpeedyFromAltRed, cmd_move_moveTasFromSpeedyRed, cmd_move_moveHangarRed, @@ -911,7 +950,10 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, cmd_move_moveEndRed, - cmd_turretHomeRight + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).125), + cmd_shooterAuto ); frc2::SequentialCommandGroup *shoot2Def2Blue = new frc2::SequentialCommandGroup(); @@ -931,8 +973,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeOne, - cmd_turretHomeMid, - cmd_shooterPoop, cmd_move_moveSpeedyFromAltBlue, cmd_move_moveTasFromSpeedyBlue, cmd_move_moveHangarBlue, @@ -940,9 +980,15 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, cmd_move_moveEndBlue, - cmd_turretHomeRight + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).125), + cmd_shooterAuto ); + m_chooser.AddOption("RED 2 ball", shoot2Red); + m_chooser.AddOption("Blue 2 ball", shoot2Blue); + m_chooser.AddOption("RED 2 ball + 1 Defensive", shoot2RedAlt); m_chooser.AddOption("Blue 2 ball + 1 Defensive", shoot2BlueAlt); @@ -957,6 +1003,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder m_chooser.AddOption("BLUE 3 ball auto", shoot3Blue); m_chooser.AddOption("BLUE 5 ball auto", shoot5Blue); + m_chooser.SetDefaultOption("RED 2 ball", shoot2Red); + frc::SmartDashboard::PutData(&m_chooser); } From 102c59a6ffbe53ba522daf273f6b814a66fcc8db Mon Sep 17 00:00:00 2001 From: Michael Ray Date: Wed, 13 Apr 2022 18:24:29 -0500 Subject: [PATCH 19/62] Start working on auto-tracking --- Competition/src/main/cpp/Robot.cpp | 6 ++-- Competition/src/main/cpp/RobotContainer.cpp | 2 +- .../src/main/cpp/subsystems/TurretTracker.cpp | 29 ++++++++++--------- Competition/src/main/include/RobotContainer.h | 2 +- .../main/include/subsystems/TurretTracker.h | 1 + 5 files changed, 22 insertions(+), 18 deletions(-) diff --git a/Competition/src/main/cpp/Robot.cpp b/Competition/src/main/cpp/Robot.cpp index 6bf65ee..8482c9f 100644 --- a/Competition/src/main/cpp/Robot.cpp +++ b/Competition/src/main/cpp/Robot.cpp @@ -41,7 +41,7 @@ void Robot::DisabledInit() { m_container.m_shooter.resetState(); m_container.m_drivetrain.resetState(); - m_container.m_lift.resetState(); + // m_container.m_lift.resetState(); m_container.m_turretTracker.resetState(); //m_container.m_feeder.resetState(); //just added, not tested @@ -70,7 +70,7 @@ void Robot::AutonomousInit() { m_container.m_feeder.robotMode = ValorSubsystem::RobotMode::AUTO; m_container.m_drivetrain.robotMode = ValorSubsystem::RobotMode::AUTO; m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::AUTO; - m_container.m_lift.robotMode = ValorSubsystem::RobotMode::AUTO; + // m_container.m_lift.robotMode = ValorSubsystem::RobotMode::AUTO; m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::AUTO; m_container.m_drivetrain.pullSwerveModuleZeroReference(); @@ -94,7 +94,7 @@ void Robot::TeleopInit() { m_container.m_feeder.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_drivetrain.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::TELEOP; - m_container.m_lift.robotMode = ValorSubsystem::RobotMode::TELEOP; + // m_container.m_lift.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::TELEOP; } diff --git a/Competition/src/main/cpp/RobotContainer.cpp b/Competition/src/main/cpp/RobotContainer.cpp index 492fe48..7c5f190 100644 --- a/Competition/src/main/cpp/RobotContainer.cpp +++ b/Competition/src/main/cpp/RobotContainer.cpp @@ -19,7 +19,7 @@ void RobotContainer::ConfigureButtonBindings() { m_feeder.setControllers(&m_GamepadOperator, &m_GamepadDriver); m_drivetrain.setController(&m_GamepadDriver); m_shooter.setControllers(&m_GamepadOperator, &m_GamepadDriver); - m_lift.setController(&m_GamepadOperator); + // m_lift.setController(&m_GamepadOperator); } frc2::Command* RobotContainer::GetAutonomousCommand() { diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index 6663412..bf1f4da 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -35,31 +35,34 @@ void TurretTracker::analyzeDashboard() { } void TurretTracker::assignOutputs() { - // state.cachedVX = drivetrain->getKinematics().ToChassisSpeeds().vx.to(); - // state.cachedVY = drivetrain->getKinematics().ToChassisSpeeds().vy.to(); - // state.cachedVT = drivetrain->getKinematics().ToChassisSpeeds().omega.to(); double tv = shooter->state.tv; + double turretPos = shooter->turretEncoder.GetPosition(); + double robotHeading = drivetrain->getPose_m().Rotation().Degrees().to(); + double x = drivetrain->getPose_m().X().to(); + double y = drivetrain->getPose_m().Y().to(); + double tx = shooter->state.tx; if (tv == 1) { - state.cachedTx = shooter->state.tx; // 0.75 = limeligh KP - state.target = (-state.cachedTx * 0.75) + shooter->turretEncoder.GetPosition(); - - state.cachedHeading = drivetrain->getPose_m().Rotation().Degrees().to(); - state.cachedX = drivetrain->getPose_m().X().to(); - state.cachedY = drivetrain->getPose_m().Y().to(); - state.cachedTurretPos = shooter->turretEncoder.GetPosition(); + state.target = (-state.cachedTx * 0.75) + turretPos; - // state.target = -1 * drivetrain->getPose_m().Rotation().Degrees().to() + state.cachedTurretPos; + // state.target = -1 * robotHeading + state.cachedTurretPos; // atan2(drivetrain->getKinematics().ToChassisSpeeds().vx.to(()), drivetrain->getPose_m().X()); + state.cachedHeading = robotHeading; + state.cachedX = x; + state.cachedY = y; + state.cachedTx = tx; + state.cachedTurretPos = turretPos; + + state.destinationTurretHeading = robotHeading + turretPos - 90 - tx; } else { if (table->GetBoolean("Use Turret Shoot", false)) - state.target = -1 * drivetrain->getPose_m().Rotation().Degrees().to() + state.cachedTurretPos - state.cachedTx; + state.target = state.destinationTurretHeading - robotHeading + 90 + tx; else - state.target = shooter->turretEncoder.GetPosition(); + state.target = turretPos; } if (state.target < -90) { diff --git a/Competition/src/main/include/RobotContainer.h b/Competition/src/main/include/RobotContainer.h index 964efca..88f5681 100644 --- a/Competition/src/main/include/RobotContainer.h +++ b/Competition/src/main/include/RobotContainer.h @@ -33,7 +33,7 @@ class RobotContainer { Drivetrain m_drivetrain; Shooter m_shooter; Feeder m_feeder; - Lift m_lift; + // Lift m_lift; TurretTracker m_turretTracker; private: diff --git a/Competition/src/main/include/subsystems/TurretTracker.h b/Competition/src/main/include/subsystems/TurretTracker.h index 615a6ab..96fb704 100644 --- a/Competition/src/main/include/subsystems/TurretTracker.h +++ b/Competition/src/main/include/subsystems/TurretTracker.h @@ -45,6 +45,7 @@ class TurretTracker : public ValorSubsystem double cachedVT; double cachedTurretPos; + double destinationTurretHeading; } state; From 69786a9775de3903ccc10834ab74c23c5424f4c5 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Wed, 13 Apr 2022 18:34:43 -0500 Subject: [PATCH 20/62] opened up turret range --- Competition/src/main/cpp/subsystems/Shooter.cpp | 9 ++++++++- Competition/src/main/cpp/subsystems/TurretTracker.cpp | 8 ++++---- Competition/src/main/include/Constants.h | 6 +++--- Competition/src/main/include/subsystems/Shooter.h | 1 + 4 files changed, 16 insertions(+), 8 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index 9cf32bc..5d8c20d 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -176,6 +176,7 @@ void Shooter::assessInputs() state.xButtonPressed = operatorController->GetXButtonPressed(); state.bButton = operatorController->GetBButtonPressed(); state.driverLeftTrigger = driverController->GetLeftTriggerAxis() > 0.9; + state.driverDPadUp = driverController->GetPOV() == OIConstants::dpadUp; //Turret if (std::abs(state.leftStickX) > ShooterConstants::kDeadband) { @@ -188,7 +189,7 @@ void Shooter::assessInputs() state.turretState = TurretState::TURRET_TRACK; } - if (state.yButton) { + if (state.yButton || state.driverDPadUp) { state.turretState = TurretState::TURRET_HOME_MID; } @@ -290,6 +291,8 @@ void Shooter::analyzeDashboard() table->PutNumber("Turret target", state.turretTarget); table->PutNumber("Turret Desired", state.turretDesired); + table->PutNumber("Turret Output", state.turretOutput); + table->PutNumber("left stick x", state.leftStickX); table->PutNumber("flywheel power", state.flywheelHigh); @@ -336,6 +339,10 @@ void Shooter::assignOutputs() if (std::abs(state.leftStickX) < ShooterConstants::pDeadband) { state.turretOutput = 0; } + if( (turretEncoder.GetPosition() > 160 && state.turretOutput > ShooterConstants::pDeadband) || + (turretEncoder.GetPosition() < 20 && state.turretOutput < -1 * ShooterConstants::pDeadband) ){ + state.turretOutput = 0; + } turret.Set(state.turretOutput); } //HOME diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index bf1f4da..fea88c3 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -72,11 +72,11 @@ void TurretTracker::assignOutputs() { state.target -= 360; } - if (state.target < 0) { - state.target = 0; + if (state.target < -20) { + state.target = -20; } - else if (state.target > 180) { - state.target = 180; + else if (state.target > 200) { + state.target = 200; } shooter->assignTurret(state.target); diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 0819857..8ca2268 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -158,7 +158,7 @@ namespace ShooterConstants{ constexpr static double flywheelPrimedValue = 0.46; constexpr static double flywheelAutoValue = 0.405; //can change to .4 constexpr static double flywheelDefaultValue = 0.45; //.375 - constexpr static double flywheelPoopValue = 0.3; + constexpr static double flywheelPoopValue = 0;//0.3; constexpr static double flywheelLaunchpadValue = .455; constexpr static double flywheelSpeeds[] = {.372, .38125, .372}; //.387, .39125 @@ -214,8 +214,8 @@ namespace ShooterConstants{ constexpr static double homePositionMid = 90; constexpr static double homePositionLeft = 180; constexpr static double homePositionRight = 0; - constexpr static double turretLimitLeft = 180; - constexpr static double turretLimitRight = 0; + constexpr static double turretLimitLeft = 180 + 20; + constexpr static double turretLimitRight = 0 - 20; constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500 constexpr static double hubX = 0; diff --git a/Competition/src/main/include/subsystems/Shooter.h b/Competition/src/main/include/subsystems/Shooter.h index 4b446b6..27005d0 100644 --- a/Competition/src/main/include/subsystems/Shooter.h +++ b/Competition/src/main/include/subsystems/Shooter.h @@ -96,6 +96,7 @@ class Shooter : public ValorSubsystem bool bButton; bool driverLeftTrigger; + bool driverDPadUp; bool driverLastLeftTrigger; double limelightDistance; From 46dae372a12a8e50f38963e66eba96eda2ed6809 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Wed, 13 Apr 2022 19:57:50 -0500 Subject: [PATCH 21/62] disabled wrapAround logic in auto --- Competition/src/main/cpp/Robot.cpp | 1 + Competition/src/main/cpp/RobotContainer.cpp | 2 +- Competition/src/main/cpp/ValorAuto.cpp | 49 ++++++++++++++----- .../src/main/cpp/subsystems/Shooter.cpp | 10 ++-- .../src/main/cpp/subsystems/TurretTracker.cpp | 14 +++++- Competition/src/main/include/Constants.h | 2 +- Competition/src/main/include/ValorAuto.h | 4 +- .../main/include/subsystems/TurretTracker.h | 3 ++ 8 files changed, 61 insertions(+), 24 deletions(-) diff --git a/Competition/src/main/cpp/Robot.cpp b/Competition/src/main/cpp/Robot.cpp index 8482c9f..7ece0c8 100644 --- a/Competition/src/main/cpp/Robot.cpp +++ b/Competition/src/main/cpp/Robot.cpp @@ -94,6 +94,7 @@ void Robot::TeleopInit() { m_container.m_feeder.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_drivetrain.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::TELEOP; + m_container.m_shooter.state.turretState = m_container.m_shooter.TURRET_TRACK; // m_container.m_lift.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::TELEOP; diff --git a/Competition/src/main/cpp/RobotContainer.cpp b/Competition/src/main/cpp/RobotContainer.cpp index 7c5f190..b74f830 100644 --- a/Competition/src/main/cpp/RobotContainer.cpp +++ b/Competition/src/main/cpp/RobotContainer.cpp @@ -7,7 +7,7 @@ #include "RobotContainer.h" -RobotContainer::RobotContainer() : m_auto(&m_drivetrain, &m_shooter, &m_feeder) { +RobotContainer::RobotContainer() : m_auto(&m_drivetrain, &m_shooter, &m_feeder, &m_turretTracker) { ConfigureButtonBindings(); m_shooter.setDrivetrain(&m_drivetrain); m_turretTracker.setDrivetrain(&m_drivetrain); diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index f22cc5c..4a23c01 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -2,10 +2,11 @@ #include //See https://github.com/wpilibsuite/allwpilib/blob/v2022.1.1/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp -ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder) : +ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder, TurretTracker *_turretTracker) : drivetrain(_drivetrain), shooter(_shooter), - feeder(_feeder) + feeder(_feeder), + turretTracker(_turretTracker) { frc::TrajectoryConfig config(units::velocity::meters_per_second_t{SwerveConstants::AUTO_MAX_SPEED_MPS}, units::acceleration::meters_per_second_squared_t{SwerveConstants::AUTO_MAX_ACCEL_MPSS}); @@ -138,6 +139,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::InstantCommand cmd_intakeClearDeque = frc2::InstantCommand( [&] { feeder->resetDeque();} ); frc2::InstantCommand cmd_intakeShoot = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_SHOOT; } ); frc2::InstantCommand cmd_intakeReverse = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_REVERSE; } ); + frc2::InstantCommand cmd_turretNoWrapAround = frc2::InstantCommand( [&] { turretTracker->disableWrapAround(); } ); + frc2::InstantCommand cmd_turretWrapAround = frc2::InstantCommand( [&] { turretTracker->enableWrapAround(); } ); frc2::InstantCommand cmd_setOdometryRed = frc2::InstantCommand( [&] { drivetrain->resetOdometry(startPoseRed); @@ -711,6 +714,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shoot3Red->AddCommands (cmd_setOdometryRed, cmd_shooterAuto, + cmd_turretNoWrapAround, cmd_intakeOne, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, @@ -728,12 +732,15 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).5), frc2::WaitCommand((units::second_t).2), cmd_intakeShoot, - frc2::WaitCommand((units::second_t).5) + frc2::WaitCommand((units::second_t).5), + cmd_intakeDisable, + cmd_turretWrapAround ); frc2::SequentialCommandGroup *shoot3Blue = new frc2::SequentialCommandGroup(); shoot3Blue->AddCommands (cmd_setOdometryBlue, cmd_shooterAuto, + cmd_turretNoWrapAround, cmd_intakeOne, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, @@ -751,7 +758,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).5), frc2::WaitCommand((units::second_t).2), cmd_intakeShoot, - frc2::WaitCommand((units::second_t).5) + frc2::WaitCommand((units::second_t).5), + cmd_intakeDisable, + cmd_turretWrapAround ); frc2::SequentialCommandGroup *shoot5Red = new frc2::SequentialCommandGroup(); @@ -759,6 +768,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder (cmd_setOdometryRed, cmd_turretDisable, cmd_shooterAuto, + cmd_turretNoWrapAround, cmd_intakeOne, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, @@ -790,13 +800,15 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_move_moveShootRed, cmd_shooterAuto, frc2::WaitCommand((units::second_t).375), - cmd_intakeShoot + cmd_intakeShoot, + cmd_turretWrapAround ); frc2::SequentialCommandGroup *shoot5Blue = new frc2::SequentialCommandGroup(); shoot5Blue->AddCommands (cmd_setOdometryBlue, cmd_turretDisable, cmd_shooterAuto, + cmd_turretNoWrapAround, cmd_intakeOne, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, @@ -830,13 +842,15 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_move_moveShootBlue, cmd_shooterAuto, frc2::WaitCommand((units::second_t).375), - cmd_intakeShoot + cmd_intakeShoot, + cmd_turretWrapAround ); frc2::SequentialCommandGroup *shoot2Red = new frc2::SequentialCommandGroup(); shoot2Red->AddCommands (cmd_set2ballOdometryRed, cmd_intakeOne, + cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -848,13 +862,15 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).5), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), - cmd_intakeDisable + cmd_intakeDisable, + cmd_turretWrapAround ); frc2::SequentialCommandGroup *shoot2Blue = new frc2::SequentialCommandGroup(); shoot2Blue->AddCommands (cmd_set2ballOdometryBlue, cmd_intakeOne, + cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -866,7 +882,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).5), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), - cmd_intakeDisable + cmd_intakeDisable, + cmd_turretWrapAround ); @@ -874,6 +891,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shoot2RedAlt->AddCommands (cmd_set2ballOdometryRed, cmd_intakeOne, + cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -895,13 +913,15 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), - cmd_shooterAuto + cmd_shooterAuto, + cmd_turretWrapAround ); frc2::SequentialCommandGroup *shoot2BlueAlt = new frc2::SequentialCommandGroup(); shoot2BlueAlt->AddCommands (cmd_set2ballOdometryBlue, cmd_intakeOne, + cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -923,7 +943,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), - cmd_shooterAuto + cmd_shooterAuto, + cmd_turretWrapAround ); frc2::SequentialCommandGroup *shoot2Def2Red = new frc2::SequentialCommandGroup(); @@ -932,6 +953,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeOne, cmd_turretHomeLeft, cmd_shooterAuto, + cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -953,7 +975,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), - cmd_shooterAuto + cmd_shooterAuto, + cmd_turretWrapAround ); frc2::SequentialCommandGroup *shoot2Def2Blue = new frc2::SequentialCommandGroup(); @@ -962,6 +985,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeOne, cmd_turretHomeLeft, cmd_shooterAuto, + cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -983,7 +1007,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), - cmd_shooterAuto + cmd_shooterAuto, + cmd_turretWrapAround ); m_chooser.AddOption("RED 2 ball", shoot2Red); diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index 5d8c20d..e9a2641 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -182,17 +182,13 @@ void Shooter::assessInputs() if (std::abs(state.leftStickX) > ShooterConstants::kDeadband) { state.turretState = TurretState::TURRET_MANUAL; // Operator control } - else if (state.bButton){ - state.turretState = TurretState::TURRET_TRACK; // Not moving + else if (state.yButton || state.driverDPadUp) { + state.turretState = TurretState::TURRET_HOME_MID; } - else if(state.turretState == TurretState::TURRET_MANUAL || (fabs(ShooterConstants::homePositionMid - turretEncoder.GetPosition()) < ShooterConstants::turretAllowedError * 10 && state.turretState == TurretState::TURRET_HOME_MID)){ + else if (!table->GetBoolean("Pit Disable", false)){ state.turretState = TurretState::TURRET_TRACK; } - if (state.yButton || state.driverDPadUp) { - state.turretState = TurretState::TURRET_HOME_MID; - } - //Hood if(state.aButton){ diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index fea88c3..cb08726 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -15,7 +15,7 @@ TurretTracker::TurretTracker() : ValorSubsystem() void TurretTracker::init() { initTable("TurretTracker"); - table->PutBoolean("Use Turret Shoot", false); + table->PutBoolean("Use Turret Shoot", true); } void TurretTracker::setDrivetrain(Drivetrain *dt){ @@ -34,6 +34,16 @@ void TurretTracker::analyzeDashboard() { } +void TurretTracker::disableWrapAround(){ + table->PutBoolean("Use Turret Shoot", false); +} + +void TurretTracker::enableWrapAround(){ + table->PutBoolean("Use Turret Shoot", true); +} + + + void TurretTracker::assignOutputs() { double tv = shooter->state.tv; @@ -59,7 +69,7 @@ void TurretTracker::assignOutputs() { state.destinationTurretHeading = robotHeading + turretPos - 90 - tx; } else { - if (table->GetBoolean("Use Turret Shoot", false)) + if (table->GetBoolean("Use Turret Shoot", true)) state.target = state.destinationTurretHeading - robotHeading + 90 + tx; else state.target = turretPos; diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 8ca2268..21e06d4 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -158,7 +158,7 @@ namespace ShooterConstants{ constexpr static double flywheelPrimedValue = 0.46; constexpr static double flywheelAutoValue = 0.405; //can change to .4 constexpr static double flywheelDefaultValue = 0.45; //.375 - constexpr static double flywheelPoopValue = 0;//0.3; + constexpr static double flywheelPoopValue = 0.3; constexpr static double flywheelLaunchpadValue = .455; constexpr static double flywheelSpeeds[] = {.372, .38125, .372}; //.387, .39125 diff --git a/Competition/src/main/include/ValorAuto.h b/Competition/src/main/include/ValorAuto.h index 4b9c49c..e98a0dd 100644 --- a/Competition/src/main/include/ValorAuto.h +++ b/Competition/src/main/include/ValorAuto.h @@ -19,13 +19,14 @@ #include "subsystems/Drivetrain.h" #include "subsystems/Shooter.h" #include "subsystems/Feeder.h" +#include "subsystems/TurretTracker.h" #ifndef VALOR_AUTO_H #define VALOR_AUTO_H class ValorAuto { public: - ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder); + ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder, TurretTracker *_turretTracker); frc2::Command* getCurrentAuto(); @@ -33,6 +34,7 @@ class ValorAuto { Drivetrain *drivetrain; Shooter *shooter; Feeder *feeder; + TurretTracker *turretTracker; frc::SendableChooser m_chooser; }; diff --git a/Competition/src/main/include/subsystems/TurretTracker.h b/Competition/src/main/include/subsystems/TurretTracker.h index 96fb704..e68d708 100644 --- a/Competition/src/main/include/subsystems/TurretTracker.h +++ b/Competition/src/main/include/subsystems/TurretTracker.h @@ -29,6 +29,9 @@ class TurretTracker : public ValorSubsystem void analyzeDashboard(); void assignOutputs(); + void disableWrapAround(); + void enableWrapAround(); + struct x { double target; From 5e649a85416a1b0a59a793bc21e2863d312d4b42 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Fri, 15 Apr 2022 20:14:10 -0500 Subject: [PATCH 22/62] removed wrap around in auto, started tuning 2+2 --- Competition/src/main/cpp/ValorAuto.cpp | 20 +++++++++---------- .../src/main/cpp/subsystems/TurretTracker.cpp | 8 ++++---- Competition/src/main/include/Constants.h | 4 ++-- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 4a23c01..eed8ba4 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -66,20 +66,20 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d marvinShootRed = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); frc::Pose2d marvinShootBlue = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); - frc::Pose2d marvinShootAltRed = frc::Pose2d(units::meter_t(3.35), units::meter_t(4), frc::Rotation2d(-90_deg)); - frc::Pose2d marvinShootAltBlue = frc::Pose2d(units::meter_t(3.35), units::meter_t(4), frc::Rotation2d(-90_deg)); + frc::Pose2d marvinShootAltRed = frc::Pose2d(units::meter_t(3.35), units::meter_t(5), frc::Rotation2d(-90_deg)); + frc::Pose2d marvinShootAltBlue = frc::Pose2d(units::meter_t(3.35), units::meter_t(5), frc::Rotation2d(-90_deg)); frc::Pose2d tasRed = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.8), frc::Rotation2d(60_deg)); frc::Pose2d tasBlue = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.8), frc::Rotation2d(60_deg)); - frc::Translation2d tasToSpeedyConstrainRed = frc::Translation2d(.75_m, 2.5_m); //1.2_m in case we need to push it more towards wall - frc::Translation2d tasToSpeedyConstrainBlue = frc::Translation2d(.75_m, 2.5_m); //1.2_m in case we need to push it more towards wall + frc::Translation2d tasToSpeedyConstrainRed = frc::Translation2d(5_m, 7.5_m); //1.2_m in case we need to push it more towards wall + frc::Translation2d tasToSpeedyConstrainBlue = frc::Translation2d(5_m, 7.5_m); //1.2_m in case we need to push it more towards wall - frc::Pose2d hangarSpotRed = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(190_deg)); - frc::Pose2d hangarSpotBlue = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(190_deg)); + frc::Pose2d hangarSpotRed = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(170_deg)); + frc::Pose2d hangarSpotBlue = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(170_deg)); - frc::Pose2d speedyRed = frc::Pose2d(3.35_m, 2.8_m, frc::Rotation2d(-60_deg)); //originally 0 - frc::Pose2d speedyBlue = frc::Pose2d(3.35_m, 2.8_m, frc::Rotation2d(-60_deg)); //originally 0 + frc::Pose2d speedyRed = frc::Pose2d(3.35_m, 3.2_m, frc::Rotation2d(-60_deg)); //originally 0 + frc::Pose2d speedyBlue = frc::Pose2d(3.35_m, 3.2_m, frc::Rotation2d(-60_deg)); //originally 0 frc::Pose2d trenchEndRed = frc::Pose2d(3.75_m, 3_m, frc::Rotation2d(135_deg)); frc::Pose2d trenchEndBlue = frc::Pose2d(3.75_m, 3_m, frc::Rotation2d(135_deg)); @@ -332,12 +332,12 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder auto moveTasFromSpeedyRed = frc::TrajectoryGenerator::GenerateTrajectory( speedyRed, - {}, + {tasToSpeedyConstrainRed}, tasRed, reverseConfig); auto moveTasFromSpeedyBlue = frc::TrajectoryGenerator::GenerateTrajectory( speedyBlue, - {}, + {tasToSpeedyConstrainBlue}, tasBlue, reverseConfig); diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index cb08726..03c0f42 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -82,11 +82,11 @@ void TurretTracker::assignOutputs() { state.target -= 360; } - if (state.target < -20) { - state.target = -20; + if (state.target < -16) { + state.target = -16; } - else if (state.target > 200) { - state.target = 200; + else if (state.target > 196) { + state.target = 196; } shooter->assignTurret(state.target); diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 21e06d4..d37f41b 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -214,8 +214,8 @@ namespace ShooterConstants{ constexpr static double homePositionMid = 90; constexpr static double homePositionLeft = 180; constexpr static double homePositionRight = 0; - constexpr static double turretLimitLeft = 180 + 20; - constexpr static double turretLimitRight = 0 - 20; + constexpr static double turretLimitLeft = 180 + 16; + constexpr static double turretLimitRight = 0 - 16; constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500 constexpr static double hubX = 0; From 67e2583e927dbca4525aff10dc17d842ac69f7e8 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Fri, 15 Apr 2022 22:00:34 -0500 Subject: [PATCH 23/62] added trench point in auto, need to test tomorrow morning --- Competition/src/main/cpp/ValorAuto.cpp | 109 ++++++++++++++++++++++++- 1 file changed, 108 insertions(+), 1 deletion(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index eed8ba4..25ca47d 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -78,6 +78,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d hangarSpotRed = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(170_deg)); frc::Pose2d hangarSpotBlue = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(170_deg)); + frc::Pose2d trenchSpotRed = frc::Pose2d(units::meter_t(4.5), units::meter_t(6.4), frc::Rotation2d(-43_deg)); + frc::Pose2d trenchSpotBlue = frc::Pose2d(units::meter_t(4.5), units::meter_t(6.4), frc::Rotation2d(-43_deg)); + frc::Pose2d speedyRed = frc::Pose2d(3.35_m, 3.2_m, frc::Rotation2d(-60_deg)); //originally 0 frc::Pose2d speedyBlue = frc::Pose2d(3.35_m, 3.2_m, frc::Rotation2d(-60_deg)); //originally 0 @@ -297,6 +300,29 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder hangarSpotBlue, reverseConfig); + auto moveTrenchRed = frc::TrajectoryGenerator::GenerateTrajectory( + hangarSpotRed, + {}, + trenchSpotRed, + reverseConfig); + auto moveTrenchBlue = frc::TrajectoryGenerator::GenerateTrajectory( + hangarSpotBlue, + {}, + trenchSpotBlue, + reverseConfig); + + auto moveMarvinFromTrenchRed = frc::TrajectoryGenerator::GenerateTrajectory( + trenchSpotRed, + {}, + marvinRed, + reverseConfig); + auto moveMarvinFromTrenchBlue = frc::TrajectoryGenerator::GenerateTrajectory( + trenchSpotBlue, + {}, + marvinBlue, + reverseConfig); + + auto moveEndRed = frc::TrajectoryGenerator::GenerateTrajectory( hangarSpotRed, {}, @@ -306,7 +332,18 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder hangarSpotBlue, {}, tasBlue, + config); + + auto moveEndFromMarvinRed = frc::TrajectoryGenerator::GenerateTrajectory( + marvinRed, + {}, + tasRed, reverseConfig); + auto moveEndFromMarvinBlue = frc::TrajectoryGenerator::GenerateTrajectory( + marvinBlue, + {}, + tasBlue, + config); auto moveSpeedyFromTasRed = frc::TrajectoryGenerator::GenerateTrajectory( tasRed, @@ -604,6 +641,70 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); + + frc2::SwerveControllerCommand<4> cmd_move_moveTrenchRed( + moveTrenchRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveTrenchBlue( + moveTrenchBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + + frc2::SwerveControllerCommand<4> cmd_move_moveMarvinFromTrenchRed( + moveMarvinFromTrenchRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveHangarBlue( + moveMarvinFromTrenchBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + + frc2::SwerveControllerCommand<4> cmd_move_moveEndFromMarvinRed( + moveEndFromMarvinRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveEndFromMarvinBlue( + moveEndFromMarvinBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveSpeedyFromTasRed( moveSpeedyFromTasRed, [&] () { return drivetrain->getPose_m(); }, @@ -906,10 +1007,16 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeOne, cmd_move_moveTasRed, cmd_move_moveHangarRed, + cmd_move_moveTrenchRed, cmd_intakeReverse, frc2::WaitCommand((units::second_t)1), cmd_intakeDisable, - cmd_move_moveEndRed, + cmd_move_moveMarvinFromTrenchRed, + cmd_move_moveEndFromMarvinRed, + // cmd_intakeReverse, + // frc2::WaitCommand((units::second_t)1), + // cmd_intakeDisable, + // cmd_move_moveEndRed, cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), From 8535de83370ee857bdf2e9a41ba340c27a668b9a Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Sat, 16 Apr 2022 09:05:41 -0500 Subject: [PATCH 24/62] fixed a copypasta issue --- Competition/src/main/cpp/ValorAuto.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 25ca47d..e8ffa96 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -673,7 +673,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); - frc2::SwerveControllerCommand<4> cmd_move_moveHangarBlue( + frc2::SwerveControllerCommand<4> cmd_move_moveMarvinFromTrenchBlue( moveMarvinFromTrenchBlue, [&] () { return drivetrain->getPose_m(); }, drivetrain->getKinematics(), From f601b36aad2e849b30a195e4ff51a53569c95092 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Sat, 16 Apr 2022 11:18:22 -0500 Subject: [PATCH 25/62] fixed lift and auto tuning --- Competition/src/main/cpp/Robot.cpp | 8 ++++---- Competition/src/main/cpp/RobotContainer.cpp | 2 +- Competition/src/main/cpp/ValorAuto.cpp | 9 ++++----- Competition/src/main/cpp/subsystems/TurretTracker.cpp | 8 ++++---- Competition/src/main/include/Constants.h | 8 ++++---- Competition/src/main/include/RobotContainer.h | 2 +- 6 files changed, 18 insertions(+), 19 deletions(-) diff --git a/Competition/src/main/cpp/Robot.cpp b/Competition/src/main/cpp/Robot.cpp index 7ece0c8..003f98b 100644 --- a/Competition/src/main/cpp/Robot.cpp +++ b/Competition/src/main/cpp/Robot.cpp @@ -37,11 +37,11 @@ void Robot::DisabledInit() { m_container.m_drivetrain.robotMode = ValorSubsystem::RobotMode::DISABLED; m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::DISABLED; m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::DISABLED; - //m_container.m_lift.robotMode = ValorSubsystem::RobotMode::DISABLED; //just added, not tested + m_container.m_lift.robotMode = ValorSubsystem::RobotMode::DISABLED; //just added, not tested m_container.m_shooter.resetState(); m_container.m_drivetrain.resetState(); - // m_container.m_lift.resetState(); + m_container.m_lift.resetState(); m_container.m_turretTracker.resetState(); //m_container.m_feeder.resetState(); //just added, not tested @@ -70,7 +70,7 @@ void Robot::AutonomousInit() { m_container.m_feeder.robotMode = ValorSubsystem::RobotMode::AUTO; m_container.m_drivetrain.robotMode = ValorSubsystem::RobotMode::AUTO; m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::AUTO; - // m_container.m_lift.robotMode = ValorSubsystem::RobotMode::AUTO; + m_container.m_lift.robotMode = ValorSubsystem::RobotMode::AUTO; m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::AUTO; m_container.m_drivetrain.pullSwerveModuleZeroReference(); @@ -95,7 +95,7 @@ void Robot::TeleopInit() { m_container.m_drivetrain.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_shooter.state.turretState = m_container.m_shooter.TURRET_TRACK; - // m_container.m_lift.robotMode = ValorSubsystem::RobotMode::TELEOP; + m_container.m_lift.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::TELEOP; } diff --git a/Competition/src/main/cpp/RobotContainer.cpp b/Competition/src/main/cpp/RobotContainer.cpp index b74f830..d0b55a0 100644 --- a/Competition/src/main/cpp/RobotContainer.cpp +++ b/Competition/src/main/cpp/RobotContainer.cpp @@ -19,7 +19,7 @@ void RobotContainer::ConfigureButtonBindings() { m_feeder.setControllers(&m_GamepadOperator, &m_GamepadDriver); m_drivetrain.setController(&m_GamepadDriver); m_shooter.setControllers(&m_GamepadOperator, &m_GamepadDriver); - // m_lift.setController(&m_GamepadOperator); + m_lift.setController(&m_GamepadOperator); } frc2::Command* RobotContainer::GetAutonomousCommand() { diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index e8ffa96..c526feb 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -78,8 +78,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d hangarSpotRed = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(170_deg)); frc::Pose2d hangarSpotBlue = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(170_deg)); - frc::Pose2d trenchSpotRed = frc::Pose2d(units::meter_t(4.5), units::meter_t(6.4), frc::Rotation2d(-43_deg)); - frc::Pose2d trenchSpotBlue = frc::Pose2d(units::meter_t(4.5), units::meter_t(6.4), frc::Rotation2d(-43_deg)); + frc::Pose2d trenchSpotRed = frc::Pose2d(units::meter_t(4.5), units::meter_t(5.4), frc::Rotation2d(-43_deg)); + frc::Pose2d trenchSpotBlue = frc::Pose2d(units::meter_t(4.5), units::meter_t(5.4), frc::Rotation2d(-43_deg)); frc::Pose2d speedyRed = frc::Pose2d(3.35_m, 3.2_m, frc::Rotation2d(-60_deg)); //originally 0 frc::Pose2d speedyBlue = frc::Pose2d(3.35_m, 3.2_m, frc::Rotation2d(-60_deg)); //originally 0 @@ -301,12 +301,12 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder reverseConfig); auto moveTrenchRed = frc::TrajectoryGenerator::GenerateTrajectory( - hangarSpotRed, + tasRed, {}, trenchSpotRed, reverseConfig); auto moveTrenchBlue = frc::TrajectoryGenerator::GenerateTrajectory( - hangarSpotBlue, + tasBlue, {}, trenchSpotBlue, reverseConfig); @@ -1006,7 +1006,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).5), cmd_intakeOne, cmd_move_moveTasRed, - cmd_move_moveHangarRed, cmd_move_moveTrenchRed, cmd_intakeReverse, frc2::WaitCommand((units::second_t)1), diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index 03c0f42..0d64b97 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -82,11 +82,11 @@ void TurretTracker::assignOutputs() { state.target -= 360; } - if (state.target < -16) { - state.target = -16; + if (state.target < -12) { + state.target = -12; } - else if (state.target > 196) { - state.target = 196; + else if (state.target > 192) { + state.target = 192; } shooter->assignTurret(state.target); diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index d37f41b..e467145 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -214,8 +214,8 @@ namespace ShooterConstants{ constexpr static double homePositionMid = 90; constexpr static double homePositionLeft = 180; constexpr static double homePositionRight = 0; - constexpr static double turretLimitLeft = 180 + 16; - constexpr static double turretLimitRight = 0 - 16; + constexpr static double turretLimitLeft = 180 + 12; + constexpr static double turretLimitRight = 0 - 12; constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500 constexpr static double hubX = 0; @@ -259,7 +259,7 @@ namespace LiftConstants{ constexpr static int MAIN_FIRST_POSITION = 62000; constexpr static int MAIN_SECOND_POSITION = 78500; - constexpr static int MAIN_THIRD_POSITION = 103000; + constexpr static int MAIN_THIRD_POSITION = 98000; constexpr static int MAIN_DOWN_POSITION = 125; constexpr static int MAIN_BOTTOM_POSITION = 0; constexpr static int MAIN_SLOW_UP_POSITION = 5500; @@ -270,7 +270,7 @@ namespace LiftConstants{ constexpr static double rotateForwardLimit = 40; constexpr static double rotateReverseLimit = 0; - constexpr static double extendForwardLimit = 103000; + constexpr static double extendForwardLimit = 98000; constexpr static double extendReverseLimit = 125; constexpr static double pivotGearRatio = 1 / 95.67; diff --git a/Competition/src/main/include/RobotContainer.h b/Competition/src/main/include/RobotContainer.h index 88f5681..964efca 100644 --- a/Competition/src/main/include/RobotContainer.h +++ b/Competition/src/main/include/RobotContainer.h @@ -33,7 +33,7 @@ class RobotContainer { Drivetrain m_drivetrain; Shooter m_shooter; Feeder m_feeder; - // Lift m_lift; + Lift m_lift; TurretTracker m_turretTracker; private: From 6bf6a0c0e7d74334b869acb76ab942d80236dba8 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Sat, 16 Apr 2022 16:09:49 -0500 Subject: [PATCH 26/62] auto tuning for 2+2 --- Competition/src/main/cpp/ValorAuto.cpp | 198 +++++++++++++++--- .../src/main/cpp/subsystems/Shooter.cpp | 4 +- .../src/main/cpp/subsystems/TurretTracker.cpp | 8 +- Competition/src/main/include/Constants.h | 16 +- 4 files changed, 179 insertions(+), 47 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index c526feb..db7bc54 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -78,8 +78,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d hangarSpotRed = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(170_deg)); frc::Pose2d hangarSpotBlue = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(170_deg)); - frc::Pose2d trenchSpotRed = frc::Pose2d(units::meter_t(4.5), units::meter_t(5.4), frc::Rotation2d(-43_deg)); - frc::Pose2d trenchSpotBlue = frc::Pose2d(units::meter_t(4.5), units::meter_t(5.4), frc::Rotation2d(-43_deg)); + frc::Pose2d trenchSpotRed = frc::Pose2d(units::meter_t(6.6), units::meter_t(5), frc::Rotation2d(-27_deg)); + frc::Pose2d trenchSpotBlue = frc::Pose2d(units::meter_t(6.6), units::meter_t(5), frc::Rotation2d(-27_deg)); frc::Pose2d speedyRed = frc::Pose2d(3.35_m, 3.2_m, frc::Rotation2d(-60_deg)); //originally 0 frc::Pose2d speedyBlue = frc::Pose2d(3.35_m, 3.2_m, frc::Rotation2d(-60_deg)); //originally 0 @@ -87,8 +87,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d trenchEndRed = frc::Pose2d(3.75_m, 3_m, frc::Rotation2d(135_deg)); frc::Pose2d trenchEndBlue = frc::Pose2d(3.75_m, 3_m, frc::Rotation2d(135_deg)); - frc::Pose2d endPose2ballRed = frc::Pose2d(10_m, 10_m, frc::Rotation2d(0_deg)); - frc::Pose2d endPose2ballBlue = frc::Pose2d(10_m, 10_m, frc::Rotation2d(0_deg)); + frc::Pose2d endPose2BallRed = frc::Pose2d(7.069_m, 7.3_m, frc::Rotation2d(60_deg)); + frc::Pose2d endPose2BallBlue = frc::Pose2d(7.069_m, 7.3_m, frc::Rotation2d(60_deg)); frc2::InstantCommand cmd_printHeading = frc2::InstantCommand( [&] { @@ -157,12 +157,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::InstantCommand cmd_set2ballOdometryBlue = frc2::InstantCommand( [&] { drivetrain->resetOdometry(startPose2ballBlue); }); - frc2::InstantCommand cmd_setEnd2ballRed = frc2::InstantCommand( [&] { - drivetrain->resetOdometry(endPose2ballRed); - }); - frc2::InstantCommand cmd_setEnd2ballBlue = frc2::InstantCommand( [&] { - drivetrain->resetOdometry(endPose2ballBlue); - }); auto moveBugsRed = frc::TrajectoryGenerator::GenerateTrajectory( startPoseRed, @@ -332,19 +326,41 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder hangarSpotBlue, {}, tasBlue, - config); + reverseConfig); auto moveEndFromMarvinRed = frc::TrajectoryGenerator::GenerateTrajectory( marvinRed, {}, tasRed, - reverseConfig); + config); auto moveEndFromMarvinBlue = frc::TrajectoryGenerator::GenerateTrajectory( marvinBlue, {}, tasBlue, config); + auto moveEndFromTrenchRed = frc::TrajectoryGenerator::GenerateTrajectory( + trenchSpotRed, + {}, + endPose2BallRed, + reverseConfig); + auto moveEndFromTrenchBlue = frc::TrajectoryGenerator::GenerateTrajectory( + trenchSpotBlue, + {}, + endPose2BallBlue, + reverseConfig); + + auto moveEndFromTrenchNoCoastRed = frc::TrajectoryGenerator::GenerateTrajectory( + trenchSpotRed, + {}, + tasRed, + reverseConfig); + auto moveEndFromTrenchNoCoastBlue = frc::TrajectoryGenerator::GenerateTrajectory( + trenchSpotBlue, + {}, + tasBlue, + reverseConfig); + auto moveSpeedyFromTasRed = frc::TrajectoryGenerator::GenerateTrajectory( tasRed, {}, @@ -369,12 +385,12 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder auto moveTasFromSpeedyRed = frc::TrajectoryGenerator::GenerateTrajectory( speedyRed, - {tasToSpeedyConstrainRed}, + {}, tasRed, reverseConfig); auto moveTasFromSpeedyBlue = frc::TrajectoryGenerator::GenerateTrajectory( speedyBlue, - {tasToSpeedyConstrainBlue}, + {}, tasBlue, reverseConfig); @@ -705,6 +721,48 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder {drivetrain} ); + frc2::SwerveControllerCommand<4> cmd_move_moveEndFromTrenchRed( + moveEndFromTrenchRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveEndFromTrenchBlue( + moveEndFromTrenchBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + + frc2::SwerveControllerCommand<4> cmd_move_moveEndFromTrenchNoCoastRed( + moveEndFromTrenchNoCoastRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveEndFromTrenchNoCoastBlue( + moveEndFromTrenchNoCoastBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveSpeedyFromTasRed( moveSpeedyFromTasRed, [&] () { return drivetrain->getPose_m(); }, @@ -1001,21 +1059,17 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeDisable, cmd_move_moveMarvinShootRed, cmd_turretTrack, - frc2::WaitCommand((units::second_t).5), + frc2::WaitCommand((units::second_t)1), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeOne, cmd_move_moveTasRed, cmd_move_moveTrenchRed, cmd_intakeReverse, - frc2::WaitCommand((units::second_t)1), + frc2::WaitCommand((units::second_t).3), cmd_intakeDisable, - cmd_move_moveMarvinFromTrenchRed, - cmd_move_moveEndFromMarvinRed, - // cmd_intakeReverse, - // frc2::WaitCommand((units::second_t)1), - // cmd_intakeDisable, - // cmd_move_moveEndRed, + frc2::WaitCommand((units::second_t)2), + cmd_move_moveEndFromTrenchRed, cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), @@ -1036,16 +1090,17 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeDisable, cmd_move_moveMarvinShootBlue, cmd_turretTrack, - frc2::WaitCommand((units::second_t).5), + frc2::WaitCommand((units::second_t)1), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeOne, cmd_move_moveTasBlue, - cmd_move_moveHangarBlue, + cmd_move_moveTrenchBlue, cmd_intakeReverse, - frc2::WaitCommand((units::second_t)1), + frc2::WaitCommand((units::second_t).3), cmd_intakeDisable, - cmd_move_moveEndBlue, + frc2::WaitCommand((units::second_t)2), + cmd_move_moveEndFromTrenchBlue, cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), @@ -1067,17 +1122,19 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeDisable, cmd_move_moveMarvinShootAltRed, cmd_turretTrack, - frc2::WaitCommand((units::second_t).5), + frc2::WaitCommand((units::second_t)1), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeOne, cmd_move_moveSpeedyFromAltRed, cmd_move_moveTasFromSpeedyRed, - cmd_move_moveHangarRed, + cmd_intakeDisable, + cmd_move_moveTrenchRed, cmd_intakeReverse, - frc2::WaitCommand((units::second_t)1), + frc2::WaitCommand((units::second_t).3), cmd_intakeDisable, - cmd_move_moveEndRed, + frc2::WaitCommand((units::second_t)2), + cmd_move_moveEndFromTrenchRed, cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), @@ -1099,17 +1156,89 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeDisable, cmd_move_moveMarvinShootAltBlue, cmd_turretTrack, - frc2::WaitCommand((units::second_t).5), + frc2::WaitCommand((units::second_t)1), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), cmd_intakeOne, cmd_move_moveSpeedyFromAltBlue, cmd_move_moveTasFromSpeedyBlue, - cmd_move_moveHangarBlue, + cmd_intakeDisable, + cmd_move_moveTrenchBlue, cmd_intakeReverse, + frc2::WaitCommand((units::second_t).3), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)2), + cmd_move_moveEndFromTrenchBlue, + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).125), + cmd_shooterAuto, + cmd_turretWrapAround + ); + + + + frc2::SequentialCommandGroup *shoot2Def2RedNoCoast = new frc2::SequentialCommandGroup(); + shoot2Def2RedNoCoast->AddCommands + (cmd_set2ballOdometryRed, + cmd_intakeOne, + cmd_turretHomeLeft, + cmd_shooterAuto, + cmd_turretNoWrapAround, + frc2::WaitCommand((units::second_t).125), + cmd_intakeClearDeque, + cmd_intakeAuto, + cmd_move_moveMarvinRed, + cmd_intakeDisable, + cmd_move_moveMarvinShootAltRed, + cmd_turretTrack, frc2::WaitCommand((units::second_t)1), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t).5), + cmd_intakeOne, + cmd_move_moveSpeedyFromAltRed, + cmd_move_moveTasFromSpeedyRed, cmd_intakeDisable, - cmd_move_moveEndBlue, + cmd_move_moveTrenchRed, + cmd_intakeReverse, + frc2::WaitCommand((units::second_t).3), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)2), + cmd_move_moveEndFromTrenchNoCoastRed, + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).125), + cmd_shooterAuto, + cmd_turretWrapAround + ); + + frc2::SequentialCommandGroup *shoot2Def2BlueNoCoast = new frc2::SequentialCommandGroup(); + shoot2Def2BlueNoCoast->AddCommands + (cmd_set2ballOdometryBlue, + cmd_intakeOne, + cmd_turretHomeLeft, + cmd_shooterAuto, + cmd_turretNoWrapAround, + frc2::WaitCommand((units::second_t).125), + cmd_intakeClearDeque, + cmd_intakeAuto, + cmd_move_moveMarvinBlue, + cmd_intakeDisable, + cmd_move_moveMarvinShootAltBlue, + cmd_turretTrack, + frc2::WaitCommand((units::second_t)1), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t).5), + cmd_intakeOne, + cmd_move_moveSpeedyFromAltBlue, + cmd_move_moveTasFromSpeedyBlue, + cmd_intakeDisable, + cmd_move_moveTrenchBlue, + cmd_intakeReverse, + frc2::WaitCommand((units::second_t).3), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)2), + cmd_move_moveEndFromTrenchNoCoastBlue, cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), @@ -1126,6 +1255,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder m_chooser.AddOption("RED 2 ball + 2 Defensive", shoot2Def2Red); m_chooser.AddOption("Blue 2 ball + 2 Defensive", shoot2Def2Blue); + m_chooser.AddOption("RED 2 ball + 2 Defensive :: NO COAST", shoot2Def2RedNoCoast); + m_chooser.AddOption("Blue 2 ball + 2 Defensive :: NO COAST", shoot2Def2BlueNoCoast); + m_chooser.AddOption("RED 3 ball auto", shoot3Red); diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index e9a2641..4ce1d97 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -225,8 +225,8 @@ void Shooter::analyzeDashboard() double xDist = deltaH / tan(angle * MathConstants::toRadians); state.distanceToHub = xDist; - if (state.distanceToHub < 1.6) - state.distanceToHub = 1.6; + if (state.distanceToHub < 1.0) + state.distanceToHub = 1.0; table->PutNumber("x distance to hub", xDist); } diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index 0d64b97..7e75271 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -82,11 +82,11 @@ void TurretTracker::assignOutputs() { state.target -= 360; } - if (state.target < -12) { - state.target = -12; + if (state.target < -10.5) { + state.target = -10.5; } - else if (state.target > 192) { - state.target = 192; + else if (state.target > 190.5) { + state.target = 190.5; } shooter->assignTurret(state.target); diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index e467145..f096686 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -117,12 +117,12 @@ namespace ShooterConstants{ constexpr static int CAN_ID_TURRET = 12; constexpr static int CAN_ID_HOOD = 15; - constexpr static double aPower_1x = 0.114; - constexpr static double bPower_1x = -0.36; - constexpr static double cPower_1x = 0.705; - constexpr static double aHood_1x = 12.5; - constexpr static double bHood_1x = -32.7; - constexpr static double cHood_1x = 23.1; + constexpr static double aPower_1x = 0.0656; + constexpr static double bPower_1x = -0.171; + constexpr static double cPower_1x = 0.538; + constexpr static double aHood_1x = 7.38; + constexpr static double bHood_1x = -11.5; + constexpr static double cHood_1x = 3.73; constexpr static double aPower_2x = 0.165; constexpr static double bPower_2x = -0.432; @@ -214,8 +214,8 @@ namespace ShooterConstants{ constexpr static double homePositionMid = 90; constexpr static double homePositionLeft = 180; constexpr static double homePositionRight = 0; - constexpr static double turretLimitLeft = 180 + 12; - constexpr static double turretLimitRight = 0 - 12; + constexpr static double turretLimitLeft = 180 + 10.5; + constexpr static double turretLimitRight = 0 - 10.5; constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500 constexpr static double hubX = 0; From 936cc8f78ecf2dfe29c6b695c4dbdff4f56a2b26 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Sat, 16 Apr 2022 17:37:07 -0500 Subject: [PATCH 27/62] tuned shooter, new LOBF looks good --- Competition/src/main/cpp/subsystems/Feeder.cpp | 6 +++--- Competition/src/main/cpp/subsystems/Shooter.cpp | 11 ++++++++++- Competition/src/main/include/Constants.h | 2 +- Competition/src/main/include/subsystems/Shooter.h | 1 + 4 files changed, 15 insertions(+), 5 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 7ce18c7..b50c914 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -93,9 +93,9 @@ void Feeder::assessInputs() else if (state.driver_rightBumperPressed) { state.feederState = FeederState::FEEDER_REGULAR_INTAKE; //standard intake } - else if (state.driver_leftTriggerPressed) { - state.feederState = FeederState::FEEDER_CURRENT_INTAKE; //includes current/banner sensing - } + // else if (state.driver_leftTriggerPressed) { + // state.feederState = FeederState::FEEDER_CURRENT_INTAKE; //includes current/banner sensing + // } else { state.feederState = FeederState::FEEDER_DISABLE; } diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index 4ce1d97..31531bc 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -177,12 +177,13 @@ void Shooter::assessInputs() state.bButton = operatorController->GetBButtonPressed(); state.driverLeftTrigger = driverController->GetLeftTriggerAxis() > 0.9; state.driverDPadUp = driverController->GetPOV() == OIConstants::dpadUp; + state.driverDPadLeft = driverController->GetPOV() == OIConstants::dpadLeft; //Turret if (std::abs(state.leftStickX) > ShooterConstants::kDeadband) { state.turretState = TurretState::TURRET_MANUAL; // Operator control } - else if (state.yButton || state.driverDPadUp) { + else if (state.yButton || state.driverLeftTrigger) { state.turretState = TurretState::TURRET_HOME_MID; } else if (!table->GetBoolean("Pit Disable", false)){ @@ -441,4 +442,12 @@ void Shooter::assignOutputs() void Shooter::assignTurret(double tg) { state.turretDesired = tg; + if(state.driverDPadLeft){ + if (state.turretDesired < 90){ + state.turretDesired += 15; + } + else{ + state.turretDesired -= 15; + } + } } diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index f096686..5f1d4ec 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -119,7 +119,7 @@ namespace ShooterConstants{ constexpr static double aPower_1x = 0.0656; constexpr static double bPower_1x = -0.171; - constexpr static double cPower_1x = 0.538; + constexpr static double cPower_1x = 0.54; constexpr static double aHood_1x = 7.38; constexpr static double bHood_1x = -11.5; constexpr static double cHood_1x = 3.73; diff --git a/Competition/src/main/include/subsystems/Shooter.h b/Competition/src/main/include/subsystems/Shooter.h index 27005d0..1f37920 100644 --- a/Competition/src/main/include/subsystems/Shooter.h +++ b/Competition/src/main/include/subsystems/Shooter.h @@ -97,6 +97,7 @@ class Shooter : public ValorSubsystem bool driverLeftTrigger; bool driverDPadUp; + bool driverDPadLeft; bool driverLastLeftTrigger; double limelightDistance; From d0440078d6e6adaa9c97af4c497149a769a2a800 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Sat, 16 Apr 2022 17:54:46 -0500 Subject: [PATCH 28/62] fixed 2+1 to not go over the line --- Competition/src/main/cpp/ValorAuto.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index db7bc54..b76a920 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -1069,7 +1069,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).3), cmd_intakeDisable, frc2::WaitCommand((units::second_t)2), - cmd_move_moveEndFromTrenchRed, + cmd_move_moveEndFromTrenchNoCoastRed, cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), @@ -1100,7 +1100,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).3), cmd_intakeDisable, frc2::WaitCommand((units::second_t)2), - cmd_move_moveEndFromTrenchBlue, + cmd_move_moveEndFromTrenchNoCoastBlue, cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), From 12a717fe8c96502e5ab7c4655fe8418b6b1ca291 Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Mon, 18 Apr 2022 17:27:30 -0500 Subject: [PATCH 29/62] all the changes I made during the first hour --- Competition/src/main/cpp/ValorAuto.cpp | 8 ++++---- Competition/src/main/cpp/subsystems/Shooter.cpp | 10 +--------- Competition/src/main/cpp/subsystems/TurretTracker.cpp | 4 ++++ Competition/src/main/include/subsystems/Shooter.h | 2 +- 4 files changed, 10 insertions(+), 14 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index b76a920..ae955bf 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -1252,11 +1252,11 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder m_chooser.AddOption("RED 2 ball + 1 Defensive", shoot2RedAlt); m_chooser.AddOption("Blue 2 ball + 1 Defensive", shoot2BlueAlt); - m_chooser.AddOption("RED 2 ball + 2 Defensive", shoot2Def2Red); - m_chooser.AddOption("Blue 2 ball + 2 Defensive", shoot2Def2Blue); + m_chooser.AddOption("RED 2 ball + 2 Defensive :: ELIMS", shoot2Def2Red); + m_chooser.AddOption("Blue 2 ball + 2 Defensive :: ELIMS", shoot2Def2Blue); - m_chooser.AddOption("RED 2 ball + 2 Defensive :: NO COAST", shoot2Def2RedNoCoast); - m_chooser.AddOption("Blue 2 ball + 2 Defensive :: NO COAST", shoot2Def2BlueNoCoast); + m_chooser.AddOption("RED 2 ball + 2 Defensive :: QUALS", shoot2Def2RedNoCoast); + m_chooser.AddOption("Blue 2 ball + 2 Defensive :: QUALS", shoot2Def2BlueNoCoast); diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index 31531bc..dd3e88a 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -177,7 +177,7 @@ void Shooter::assessInputs() state.bButton = operatorController->GetBButtonPressed(); state.driverLeftTrigger = driverController->GetLeftTriggerAxis() > 0.9; state.driverDPadUp = driverController->GetPOV() == OIConstants::dpadUp; - state.driverDPadLeft = driverController->GetPOV() == OIConstants::dpadLeft; + state.driverBButton = driverController->GetBButton(); //Turret if (std::abs(state.leftStickX) > ShooterConstants::kDeadband) { @@ -442,12 +442,4 @@ void Shooter::assignOutputs() void Shooter::assignTurret(double tg) { state.turretDesired = tg; - if(state.driverDPadLeft){ - if (state.turretDesired < 90){ - state.turretDesired += 15; - } - else{ - state.turretDesired -= 15; - } - } } diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index 7e75271..acdcf67 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -57,6 +57,10 @@ void TurretTracker::assignOutputs() { // 0.75 = limeligh KP state.target = (-state.cachedTx * 0.75) + turretPos; + if(shooter-> state.driverBButton){ + state.target += 15; + } + // state.target = -1 * robotHeading + state.cachedTurretPos; // atan2(drivetrain->getKinematics().ToChassisSpeeds().vx.to(()), drivetrain->getPose_m().X()); diff --git a/Competition/src/main/include/subsystems/Shooter.h b/Competition/src/main/include/subsystems/Shooter.h index 1f37920..17db95c 100644 --- a/Competition/src/main/include/subsystems/Shooter.h +++ b/Competition/src/main/include/subsystems/Shooter.h @@ -97,7 +97,7 @@ class Shooter : public ValorSubsystem bool driverLeftTrigger; bool driverDPadUp; - bool driverDPadLeft; + bool driverBButton; bool driverLastLeftTrigger; double limelightDistance; From d99c37dcb21b402acd3007c0c862fc08190496cd Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Mon, 18 Apr 2022 17:36:47 -0500 Subject: [PATCH 30/62] made better way of enabling & disabling turretWrapAround --- Competition/src/main/cpp/Robot.cpp | 2 + Competition/src/main/cpp/ValorAuto.cpp | 52 +++++++------------------- 2 files changed, 15 insertions(+), 39 deletions(-) diff --git a/Competition/src/main/cpp/Robot.cpp b/Competition/src/main/cpp/Robot.cpp index 003f98b..c65398b 100644 --- a/Competition/src/main/cpp/Robot.cpp +++ b/Competition/src/main/cpp/Robot.cpp @@ -72,6 +72,7 @@ void Robot::AutonomousInit() { m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::AUTO; m_container.m_lift.robotMode = ValorSubsystem::RobotMode::AUTO; m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::AUTO; + m_container.m_turretTracker.disableWrapAround(); m_container.m_drivetrain.pullSwerveModuleZeroReference(); } @@ -97,6 +98,7 @@ void Robot::TeleopInit() { m_container.m_shooter.state.turretState = m_container.m_shooter.TURRET_TRACK; m_container.m_lift.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::TELEOP; + m_container.m_turretTracker.enableWrapAround(); } diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index ae955bf..0f8c090 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -142,9 +142,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::InstantCommand cmd_intakeClearDeque = frc2::InstantCommand( [&] { feeder->resetDeque();} ); frc2::InstantCommand cmd_intakeShoot = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_SHOOT; } ); frc2::InstantCommand cmd_intakeReverse = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_REVERSE; } ); - frc2::InstantCommand cmd_turretNoWrapAround = frc2::InstantCommand( [&] { turretTracker->disableWrapAround(); } ); - frc2::InstantCommand cmd_turretWrapAround = frc2::InstantCommand( [&] { turretTracker->enableWrapAround(); } ); - + frc2::InstantCommand cmd_setOdometryRed = frc2::InstantCommand( [&] { drivetrain->resetOdometry(startPoseRed); }); @@ -873,7 +871,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shoot3Red->AddCommands (cmd_setOdometryRed, cmd_shooterAuto, - cmd_turretNoWrapAround, cmd_intakeOne, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, @@ -892,14 +889,12 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).2), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), - cmd_intakeDisable, - cmd_turretWrapAround + cmd_intakeDisable ); frc2::SequentialCommandGroup *shoot3Blue = new frc2::SequentialCommandGroup(); shoot3Blue->AddCommands (cmd_setOdometryBlue, cmd_shooterAuto, - cmd_turretNoWrapAround, cmd_intakeOne, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, @@ -918,8 +913,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).2), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), - cmd_intakeDisable, - cmd_turretWrapAround + cmd_intakeDisable ); frc2::SequentialCommandGroup *shoot5Red = new frc2::SequentialCommandGroup(); @@ -927,7 +921,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder (cmd_setOdometryRed, cmd_turretDisable, cmd_shooterAuto, - cmd_turretNoWrapAround, cmd_intakeOne, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, @@ -959,15 +952,13 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_move_moveShootRed, cmd_shooterAuto, frc2::WaitCommand((units::second_t).375), - cmd_intakeShoot, - cmd_turretWrapAround + cmd_intakeShoot ); frc2::SequentialCommandGroup *shoot5Blue = new frc2::SequentialCommandGroup(); shoot5Blue->AddCommands (cmd_setOdometryBlue, cmd_turretDisable, cmd_shooterAuto, - cmd_turretNoWrapAround, cmd_intakeOne, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, @@ -1001,15 +992,13 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_move_moveShootBlue, cmd_shooterAuto, frc2::WaitCommand((units::second_t).375), - cmd_intakeShoot, - cmd_turretWrapAround + cmd_intakeShoot ); frc2::SequentialCommandGroup *shoot2Red = new frc2::SequentialCommandGroup(); shoot2Red->AddCommands (cmd_set2ballOdometryRed, cmd_intakeOne, - cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -1021,15 +1010,13 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).5), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), - cmd_intakeDisable, - cmd_turretWrapAround + cmd_intakeDisable ); frc2::SequentialCommandGroup *shoot2Blue = new frc2::SequentialCommandGroup(); shoot2Blue->AddCommands (cmd_set2ballOdometryBlue, cmd_intakeOne, - cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -1041,8 +1028,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::WaitCommand((units::second_t).5), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), - cmd_intakeDisable, - cmd_turretWrapAround + cmd_intakeDisable ); @@ -1050,7 +1036,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shoot2RedAlt->AddCommands (cmd_set2ballOdometryRed, cmd_intakeOne, - cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -1073,15 +1058,13 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), - cmd_shooterAuto, - cmd_turretWrapAround + cmd_shooterAuto ); frc2::SequentialCommandGroup *shoot2BlueAlt = new frc2::SequentialCommandGroup(); shoot2BlueAlt->AddCommands (cmd_set2ballOdometryBlue, cmd_intakeOne, - cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -1104,8 +1087,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), - cmd_shooterAuto, - cmd_turretWrapAround + cmd_shooterAuto ); frc2::SequentialCommandGroup *shoot2Def2Red = new frc2::SequentialCommandGroup(); @@ -1114,7 +1096,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeOne, cmd_turretHomeLeft, cmd_shooterAuto, - cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -1138,8 +1119,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), - cmd_shooterAuto, - cmd_turretWrapAround + cmd_shooterAuto ); frc2::SequentialCommandGroup *shoot2Def2Blue = new frc2::SequentialCommandGroup(); @@ -1148,7 +1128,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeOne, cmd_turretHomeLeft, cmd_shooterAuto, - cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -1172,8 +1151,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), - cmd_shooterAuto, - cmd_turretWrapAround + cmd_shooterAuto ); @@ -1184,7 +1162,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeOne, cmd_turretHomeLeft, cmd_shooterAuto, - cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -1208,8 +1185,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), - cmd_shooterAuto, - cmd_turretWrapAround + cmd_shooterAuto ); frc2::SequentialCommandGroup *shoot2Def2BlueNoCoast = new frc2::SequentialCommandGroup(); @@ -1218,7 +1194,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeOne, cmd_turretHomeLeft, cmd_shooterAuto, - cmd_turretNoWrapAround, frc2::WaitCommand((units::second_t).125), cmd_intakeClearDeque, cmd_intakeAuto, @@ -1242,8 +1217,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), - cmd_shooterAuto, - cmd_turretWrapAround + cmd_shooterAuto ); m_chooser.AddOption("RED 2 ball", shoot2Red); From 61152d8b4cfb19ff3f3ebaa0a0af591371f387e0 Mon Sep 17 00:00:00 2001 From: j a Date: Wed, 20 Apr 2022 22:21:12 -0500 Subject: [PATCH 31/62] wednesday code --- Competition/src/main/cpp/Robot.cpp | 5 +++++ Competition/src/main/cpp/subsystems/Feeder.cpp | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/Competition/src/main/cpp/Robot.cpp b/Competition/src/main/cpp/Robot.cpp index c65398b..c4c5b1c 100644 --- a/Competition/src/main/cpp/Robot.cpp +++ b/Competition/src/main/cpp/Robot.cpp @@ -7,6 +7,8 @@ #include "Robot.h" + + #include #include @@ -96,6 +98,9 @@ void Robot::TeleopInit() { m_container.m_drivetrain.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_shooter.state.turretState = m_container.m_shooter.TURRET_TRACK; + m_container.m_shooter.state.hoodState = m_container.m_shooter.HOOD_TRACK; + m_container.m_shooter.state.flywheelState = m_container.m_shooter.FLYWHEEL_TRACK; + m_container.m_lift.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::TELEOP; m_container.m_turretTracker.enableWrapAround(); diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index b50c914..792a56c 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -32,7 +32,7 @@ void Feeder::init() motor_intake.SetInverted(false); motor_intake.EnableVoltageCompensation(false); motor_intake.ConfigVoltageCompSaturation(10); - motor_intake.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60, 80, .75)); + motor_intake.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 45 , 70, .75)); //potentially could do 40 60 motor_stage.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); motor_stage.SetNeutralMode(ctre::phoenix::motorcontrol::Coast); From a9c30957f252fd4ec75e13bd86a4c442411c72a9 Mon Sep 17 00:00:00 2001 From: j a Date: Fri, 22 Apr 2022 21:42:39 -0500 Subject: [PATCH 32/62] friday code --- Competition/src/main/cpp/ValorAuto.cpp | 104 +++++++++--------- .../src/main/cpp/subsystems/Drivetrain.cpp | 2 +- .../src/main/cpp/subsystems/Feeder.cpp | 2 +- .../src/main/cpp/subsystems/Shooter.cpp | 2 +- .../src/main/cpp/subsystems/TurretTracker.cpp | 2 +- Competition/src/main/include/Constants.h | 18 +-- 6 files changed, 65 insertions(+), 65 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 0f8c090..e5e4778 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -69,8 +69,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d marvinShootAltRed = frc::Pose2d(units::meter_t(3.35), units::meter_t(5), frc::Rotation2d(-90_deg)); frc::Pose2d marvinShootAltBlue = frc::Pose2d(units::meter_t(3.35), units::meter_t(5), frc::Rotation2d(-90_deg)); - frc::Pose2d tasRed = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.8), frc::Rotation2d(60_deg)); - frc::Pose2d tasBlue = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.8), frc::Rotation2d(60_deg)); + frc::Pose2d tasRed = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.9), frc::Rotation2d(55_deg)); + frc::Pose2d tasBlue = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.9), frc::Rotation2d(55_deg)); frc::Translation2d tasToSpeedyConstrainRed = frc::Translation2d(5_m, 7.5_m); //1.2_m in case we need to push it more towards wall frc::Translation2d tasToSpeedyConstrainBlue = frc::Translation2d(5_m, 7.5_m); //1.2_m in case we need to push it more towards wall @@ -872,9 +872,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder (cmd_setOdometryRed, cmd_shooterAuto, cmd_intakeOne, - frc2::WaitCommand((units::second_t).125), - cmd_intakeClearDeque, - cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_move_moveBugsRed, cmd_intakeDisable, cmd_move_movePreDaffyRed, @@ -896,9 +896,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder (cmd_setOdometryBlue, cmd_shooterAuto, cmd_intakeOne, - frc2::WaitCommand((units::second_t).125), - cmd_intakeClearDeque, - cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_move_moveBugsBlue, cmd_intakeDisable, cmd_move_movePreDaffyBlue, @@ -922,9 +922,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretDisable, cmd_shooterAuto, cmd_intakeOne, - frc2::WaitCommand((units::second_t).125), - cmd_intakeClearDeque, - cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_move_moveBugsRed, cmd_intakeDisable, cmd_move_moveBackBugsRed, @@ -960,9 +960,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_turretDisable, cmd_shooterAuto, cmd_intakeOne, - frc2::WaitCommand((units::second_t).125), - cmd_intakeClearDeque, - cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_move_moveBugsBlue, cmd_intakeDisable, cmd_move_moveBackBugsBlue, @@ -999,9 +999,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shoot2Red->AddCommands (cmd_set2ballOdometryRed, cmd_intakeOne, - frc2::WaitCommand((units::second_t).125), - cmd_intakeClearDeque, - cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_shooterAuto, cmd_move_moveMarvinRed, cmd_intakeDisable, @@ -1017,9 +1017,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shoot2Blue->AddCommands (cmd_set2ballOdometryBlue, cmd_intakeOne, - frc2::WaitCommand((units::second_t).125), - cmd_intakeClearDeque, - cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_shooterAuto, cmd_move_moveMarvinBlue, cmd_intakeDisable, @@ -1036,9 +1036,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shoot2RedAlt->AddCommands (cmd_set2ballOdometryRed, cmd_intakeOne, - frc2::WaitCommand((units::second_t).125), - cmd_intakeClearDeque, - cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_shooterAuto, cmd_move_moveMarvinRed, cmd_intakeDisable, @@ -1051,13 +1051,13 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_move_moveTasRed, cmd_move_moveTrenchRed, cmd_intakeReverse, - frc2::WaitCommand((units::second_t).3), + frc2::WaitCommand((units::second_t).5), cmd_intakeDisable, - frc2::WaitCommand((units::second_t)2), + frc2::WaitCommand((units::second_t)1.8), cmd_move_moveEndFromTrenchNoCoastRed, cmd_turretHomeRight, cmd_turretTrack, - frc2::WaitCommand((units::second_t).125), + frc2::WaitCommand((units::second_t).2), cmd_shooterAuto ); @@ -1065,9 +1065,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder shoot2BlueAlt->AddCommands (cmd_set2ballOdometryBlue, cmd_intakeOne, - frc2::WaitCommand((units::second_t).125), - cmd_intakeClearDeque, - cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_shooterAuto, cmd_move_moveMarvinBlue, cmd_intakeDisable, @@ -1080,13 +1080,13 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_move_moveTasBlue, cmd_move_moveTrenchBlue, cmd_intakeReverse, - frc2::WaitCommand((units::second_t).3), + frc2::WaitCommand((units::second_t).5), cmd_intakeDisable, - frc2::WaitCommand((units::second_t)2), + frc2::WaitCommand((units::second_t)1.8), cmd_move_moveEndFromTrenchNoCoastBlue, cmd_turretHomeRight, cmd_turretTrack, - frc2::WaitCommand((units::second_t).125), + frc2::WaitCommand((units::second_t).2), cmd_shooterAuto ); @@ -1096,9 +1096,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeOne, cmd_turretHomeLeft, cmd_shooterAuto, - frc2::WaitCommand((units::second_t).125), - cmd_intakeClearDeque, - cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_move_moveMarvinRed, cmd_intakeDisable, cmd_move_moveMarvinShootAltRed, @@ -1112,9 +1112,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeDisable, cmd_move_moveTrenchRed, cmd_intakeReverse, - frc2::WaitCommand((units::second_t).3), + frc2::WaitCommand((units::second_t).5), cmd_intakeDisable, - frc2::WaitCommand((units::second_t)2), + frc2::WaitCommand((units::second_t)1.8), cmd_move_moveEndFromTrenchRed, cmd_turretHomeRight, cmd_turretTrack, @@ -1128,9 +1128,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeOne, cmd_turretHomeLeft, cmd_shooterAuto, - frc2::WaitCommand((units::second_t).125), - cmd_intakeClearDeque, - cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_move_moveMarvinBlue, cmd_intakeDisable, cmd_move_moveMarvinShootAltBlue, @@ -1144,9 +1144,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeDisable, cmd_move_moveTrenchBlue, cmd_intakeReverse, - frc2::WaitCommand((units::second_t).3), + frc2::WaitCommand((units::second_t).5), cmd_intakeDisable, - frc2::WaitCommand((units::second_t)2), + frc2::WaitCommand((units::second_t)1.8), cmd_move_moveEndFromTrenchBlue, cmd_turretHomeRight, cmd_turretTrack, @@ -1162,9 +1162,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeOne, cmd_turretHomeLeft, cmd_shooterAuto, - frc2::WaitCommand((units::second_t).125), - cmd_intakeClearDeque, - cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_move_moveMarvinRed, cmd_intakeDisable, cmd_move_moveMarvinShootAltRed, @@ -1178,9 +1178,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeDisable, cmd_move_moveTrenchRed, cmd_intakeReverse, - frc2::WaitCommand((units::second_t).3), + frc2::WaitCommand((units::second_t).5), cmd_intakeDisable, - frc2::WaitCommand((units::second_t)2), + frc2::WaitCommand((units::second_t)1.8), cmd_move_moveEndFromTrenchNoCoastRed, cmd_turretHomeRight, cmd_turretTrack, @@ -1194,9 +1194,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeOne, cmd_turretHomeLeft, cmd_shooterAuto, - frc2::WaitCommand((units::second_t).125), - cmd_intakeClearDeque, - cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_move_moveMarvinBlue, cmd_intakeDisable, cmd_move_moveMarvinShootAltBlue, @@ -1210,9 +1210,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeDisable, cmd_move_moveTrenchBlue, cmd_intakeReverse, - frc2::WaitCommand((units::second_t).3), + frc2::WaitCommand((units::second_t).5), cmd_intakeDisable, - frc2::WaitCommand((units::second_t)2), + frc2::WaitCommand((units::second_t)1.8), cmd_move_moveEndFromTrenchNoCoastBlue, cmd_turretHomeRight, cmd_turretTrack, diff --git a/Competition/src/main/cpp/subsystems/Drivetrain.cpp b/Competition/src/main/cpp/subsystems/Drivetrain.cpp index 5e76539..2b16056 100644 --- a/Competition/src/main/cpp/subsystems/Drivetrain.cpp +++ b/Competition/src/main/cpp/subsystems/Drivetrain.cpp @@ -238,7 +238,7 @@ void Drivetrain::assignOutputs() // mathematics). Xbox controllers return positive values when you pull to // the right by default. - double rot = std::abs(state.rightStickX) > OIConstants::kDeadbandX ? -1 * state.rightStickX * state.rightStickX * state.rightStickX : 0; + double rot = std::abs(state.rightStickX) > .06 ? -1 * state.rightStickX * state.rightStickX * state.rightStickX : 0; units::meters_per_second_t xSpeedMPS = units::meters_per_second_t{xSpeed * SwerveConstants::DRIVE_MAX_SPEED_MPS}; units::meters_per_second_t ySpeedMPS = units::meters_per_second_t{ySpeed * SwerveConstants::DRIVE_MAX_SPEED_MPS}; diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 792a56c..6834f1e 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -32,7 +32,7 @@ void Feeder::init() motor_intake.SetInverted(false); motor_intake.EnableVoltageCompensation(false); motor_intake.ConfigVoltageCompSaturation(10); - motor_intake.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 45 , 70, .75)); //potentially could do 40 60 + motor_intake.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60 , 80, .75)); //potentially could do 40 60 motor_stage.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); motor_stage.SetNeutralMode(ctre::phoenix::motorcontrol::Coast); diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index dd3e88a..5c7840f 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -183,7 +183,7 @@ void Shooter::assessInputs() if (std::abs(state.leftStickX) > ShooterConstants::kDeadband) { state.turretState = TurretState::TURRET_MANUAL; // Operator control } - else if (state.yButton || state.driverLeftTrigger) { + else if (state.yButton || state.driverBButton) { state.turretState = TurretState::TURRET_HOME_MID; } else if (!table->GetBoolean("Pit Disable", false)){ diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index acdcf67..07ff841 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -57,7 +57,7 @@ void TurretTracker::assignOutputs() { // 0.75 = limeligh KP state.target = (-state.cachedTx * 0.75) + turretPos; - if(shooter-> state.driverBButton){ + if(shooter-> state.driverLeftTrigger){ state.target += 15; } diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 5f1d4ec..e07d887 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -117,12 +117,12 @@ namespace ShooterConstants{ constexpr static int CAN_ID_TURRET = 12; constexpr static int CAN_ID_HOOD = 15; - constexpr static double aPower_1x = 0.0656; - constexpr static double bPower_1x = -0.171; - constexpr static double cPower_1x = 0.54; - constexpr static double aHood_1x = 7.38; - constexpr static double bHood_1x = -11.5; - constexpr static double cHood_1x = 3.73; + constexpr static double aPower_1x = 0.0553; + constexpr static double bPower_1x = -0.101; + constexpr static double cPower_1x = 0.471; + constexpr static double aHood_1x = 6.29; + constexpr static double bHood_1x = -7.48; + constexpr static double cHood_1x = .993; constexpr static double aPower_2x = 0.165; constexpr static double bPower_2x = -0.432; @@ -136,13 +136,13 @@ namespace ShooterConstants{ constexpr static double hubHeight = 2.64; constexpr static double limelightHeight = .6075; - constexpr static double flywheelKP1 = 0.088; //1 + constexpr static double flywheelKP1 = 0.0905; //0.088 -> 0.091 constexpr static double flywheelKI1 = 0; constexpr static double flywheelKD1 = 0; constexpr static double flywheelKIZ1 = 0; constexpr static double flywheelKFF1 = 0.04; - constexpr static double flywheelKP0 = 0.088; //.1 + constexpr static double flywheelKP0 = 0.0905; //0.088 -> 0.091 constexpr static double flywheelKI0 = 0; constexpr static double flywheelKD0 = 0; constexpr static double flywheelKIZ0 = 0; @@ -241,7 +241,7 @@ namespace FeederConstants{ constexpr static double DEFAULT_FEEDER_SPEED_REVERSE = -1.0; constexpr static int CACHE_SIZE = 20; - constexpr static double JAM_CURRENT = 20; + constexpr static double JAM_CURRENT = 22; } namespace MathConstants{ From f9c17656d0a653ef9bdd30e36e94fa7828ad5d9a Mon Sep 17 00:00:00 2001 From: j a Date: Sun, 24 Apr 2022 21:25:03 -0500 Subject: [PATCH 33/62] final worlds code --- .../src/main/cpp/subsystems/Drivetrain.cpp | 2 +- .../src/main/cpp/subsystems/TurretTracker.cpp | 4 ++-- .../src/main/cpp/subsystems/ValorSwerve.cpp | 2 +- Competition/src/main/include/Constants.h | 15 ++++++++++++--- 4 files changed, 16 insertions(+), 7 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Drivetrain.cpp b/Competition/src/main/cpp/subsystems/Drivetrain.cpp index 2b16056..526093e 100644 --- a/Competition/src/main/cpp/subsystems/Drivetrain.cpp +++ b/Competition/src/main/cpp/subsystems/Drivetrain.cpp @@ -144,7 +144,7 @@ void Drivetrain::assessInputs() state.xButtonPressed = driverController->GetXButton(); state.yButtonPressed = driverController->GetYButton(); - state.startButtonPressed = driverController->GetStartButtonPressed(); + state.startButtonPressed = false;//driverController->GetStartButtonPressed(); state.stickPressed = std::abs(state.leftStickY) > 0.05 || std::abs(state.leftStickX) > 0.05 || diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index 07ff841..b4a7ad5 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -86,8 +86,8 @@ void TurretTracker::assignOutputs() { state.target -= 360; } - if (state.target < -10.5) { - state.target = -10.5; + if (state.target < -7) { + state.target = -7; } else if (state.target > 190.5) { state.target = 190.5; diff --git a/Competition/src/main/cpp/subsystems/ValorSwerve.cpp b/Competition/src/main/cpp/subsystems/ValorSwerve.cpp index a5e04a9..edba2cb 100644 --- a/Competition/src/main/cpp/subsystems/ValorSwerve.cpp +++ b/Competition/src/main/cpp/subsystems/ValorSwerve.cpp @@ -116,7 +116,7 @@ void ValorSwerve::loadAndSetAzimuthZeroReference() //azimuthSetpoint = fmod(azimuthSetpoint, SwerveConstants::AZIMUTH_COUNTS_PER_REV / SwerveConstants::AZIMUTH_GEAR_RATIO); // Set the azimuth offset to the calculated setpoint (which will take over in teleop) - azimuthFalcon->SetSelectedSensorPosition(azimuthSetpoint, 0, 10); + azimuthFalcon->SetSelectedSensorPosition(0, 0, 10); //std::cout << "pulled pospition from file" << std::endl; } diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index e07d887..1d4b0ef 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -117,7 +117,16 @@ namespace ShooterConstants{ constexpr static int CAN_ID_TURRET = 12; constexpr static int CAN_ID_HOOD = 15; - constexpr static double aPower_1x = 0.0553; + //new power + // constexpr static double aPower_1x = 0.0485; + // constexpr static double bPower_1x = -0.076; + // constexpr static double cPower_1x = 0.448; + // constexpr static double aHood_1x = 6.29; + // constexpr static double bHood_1x = -7.48; + // constexpr static double cHood_1x = .993; + + //current power + constexpr static double aPower_1x = 0.0553; constexpr static double bPower_1x = -0.101; constexpr static double cPower_1x = 0.471; constexpr static double aHood_1x = 6.29; @@ -136,7 +145,7 @@ namespace ShooterConstants{ constexpr static double hubHeight = 2.64; constexpr static double limelightHeight = .6075; - constexpr static double flywheelKP1 = 0.0905; //0.088 -> 0.091 + constexpr static double flywheelKP1 = 0.09025; //0.088 -> 0.091 constexpr static double flywheelKI1 = 0; constexpr static double flywheelKD1 = 0; constexpr static double flywheelKIZ1 = 0; @@ -215,7 +224,7 @@ namespace ShooterConstants{ constexpr static double homePositionLeft = 180; constexpr static double homePositionRight = 0; constexpr static double turretLimitLeft = 180 + 10.5; - constexpr static double turretLimitRight = 0 - 10.5; + constexpr static double turretLimitRight = 0 - 7; constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500 constexpr static double hubX = 0; From 13b408700b605917c90a1ff269cb4e0731c2cbd1 Mon Sep 17 00:00:00 2001 From: janhelgeson <58052864+janhelgeson@users.noreply.github.com> Date: Mon, 25 Apr 2022 09:24:51 -0500 Subject: [PATCH 34/62] Update Feeder.cpp removed space --- Competition/src/main/cpp/subsystems/Feeder.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 6834f1e..906ef73 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -32,7 +32,7 @@ void Feeder::init() motor_intake.SetInverted(false); motor_intake.EnableVoltageCompensation(false); motor_intake.ConfigVoltageCompSaturation(10); - motor_intake.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60 , 80, .75)); //potentially could do 40 60 + motor_intake.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60, 80, .75)); //potentially could do 40 60 motor_stage.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); motor_stage.SetNeutralMode(ctre::phoenix::motorcontrol::Coast); From 614ddecbc0479d047de1f267effa6274655bd10b Mon Sep 17 00:00:00 2001 From: Michael Ray Date: Thu, 2 Jun 2022 17:34:55 -0500 Subject: [PATCH 35/62] feat: Added ValorGamepad class Override the frc::XboxController with a new ValorGamepad class This will inherit all the same methods as XboxController, but also provide additional methods for utilizing a deadband on the X and Y sticks, as well as a deadband on the triggers. Prevent caching all the values in each subsystem's state struct --- Competition/src/main/cpp/ValorGamepad.cpp | 111 ++++++++++++++++++ .../src/main/cpp/subsystems/Drivetrain.cpp | 37 ++---- .../src/main/cpp/subsystems/Feeder.cpp | 24 +--- Competition/src/main/cpp/subsystems/Lift.cpp | 41 +++---- .../src/main/cpp/subsystems/Shooter.cpp | 46 ++------ .../src/main/cpp/subsystems/TurretTracker.cpp | 2 +- Competition/src/main/include/RobotContainer.h | 6 +- Competition/src/main/include/ValorGamepad.h | 51 ++++++++ .../src/main/include/subsystems/Drivetrain.h | 22 +--- .../src/main/include/subsystems/Feeder.h | 19 +-- .../src/main/include/subsystems/Lift.h | 16 +-- .../src/main/include/subsystems/Shooter.h | 22 +--- 12 files changed, 220 insertions(+), 177 deletions(-) create mode 100644 Competition/src/main/cpp/ValorGamepad.cpp create mode 100644 Competition/src/main/include/ValorGamepad.h diff --git a/Competition/src/main/cpp/ValorGamepad.cpp b/Competition/src/main/cpp/ValorGamepad.cpp new file mode 100644 index 0000000..132bf56 --- /dev/null +++ b/Competition/src/main/cpp/ValorGamepad.cpp @@ -0,0 +1,111 @@ +#include "ValorGamepad.h" + +#include + +#define DPAD_UP 0 +#define DPAD_DOWN 180 +#define DPAD_RIGHT 90 +#define DPAD_LEFT 270 + +#define DEADBAND_X 0.05f +#define DEADBAND_Y 0.1f +#define DEADBAND_TRIGGER 0.05f + +ValorGamepad::ValorGamepad(int id) : + frc::XboxController(id), + deadbandX(DEADBAND_X), + deadbandY(DEADBAND_Y) +{ +} + +void ValorGamepad::setDeadbandX(double deadband) +{ + deadbandX = deadband; +} + +void ValorGamepad::setDeadbandY(double deadband) +{ + deadbandY = deadband; +} + +double ValorGamepad::deadband(double input, double deadband, int polynomial) +{ + return std::fabs(input) > deadband ? std::pow(input, polynomial) : 0; +} + +double ValorGamepad::leftStickX(int polynomial) +{ + return -deadband(GetLeftX(), deadbandX, polynomial); +} + +bool ValorGamepad::leftStickXActive(int polynomial) +{ + return leftStickX(polynomial) != 0; +} + +double ValorGamepad::leftStickY(int polynomial) +{ + return -deadband(GetLeftY(), deadbandY, polynomial); +} + +bool ValorGamepad::leftStickYActive(int polynomial) +{ + return leftStickY(polynomial) != 0; +} + +double ValorGamepad::rightStickX(int polynomial) +{ + return -deadband(GetRightX(), deadbandX, polynomial); +} + +bool ValorGamepad::rightStickXActive(int polynomial) +{ + return rightStickX(polynomial) != 0; +} + +double ValorGamepad::rightStickY(int polynomial) +{ + return -deadband(GetRightY(), deadbandY, polynomial); +} + +bool ValorGamepad::rightStickYActive(int polynomial) +{ + return rightStickY(polynomial) != 0; +} + +double ValorGamepad::leftTrigger() +{ + return GetLeftTriggerAxis() > DEADBAND_TRIGGER ? GetLeftTriggerAxis() : 0; +} + +bool ValorGamepad::leftTriggerActive() +{ + return leftTrigger() != 0; +} + +double ValorGamepad::rightTrigger() +{ + return GetRightTriggerAxis() > DEADBAND_TRIGGER ? GetRightTriggerAxis() : 0; +} + +bool ValorGamepad::rightTriggerActive() +{ + return rightTrigger() != 0; +} + +bool ValorGamepad::DPadUp() +{ + return GetPOV() == DPAD_UP; +} +bool ValorGamepad::DPadDown() +{ + return GetPOV() == DPAD_DOWN; +} +bool ValorGamepad::DPadLeft() +{ + return GetPOV() == DPAD_LEFT; +} +bool ValorGamepad::DPadRight() +{ + return GetPOV() == DPAD_RIGHT; +} \ No newline at end of file diff --git a/Competition/src/main/cpp/subsystems/Drivetrain.cpp b/Competition/src/main/cpp/subsystems/Drivetrain.cpp index 526093e..b490d0f 100644 --- a/Competition/src/main/cpp/subsystems/Drivetrain.cpp +++ b/Competition/src/main/cpp/subsystems/Drivetrain.cpp @@ -101,7 +101,7 @@ void Drivetrain::init() //std::cout <<"init drivetrain" << std::endl; } -void Drivetrain::setController(frc::XboxController *controller) +void Drivetrain::setController(ValorGamepad *controller) { driverController = controller; } @@ -133,22 +133,9 @@ void Drivetrain::assessInputs() return; } - // driver inputs - state.leftStickX = driverController->GetLeftX(); - state.leftStickY = driverController->GetLeftY(); - state.rightStickX = driverController->GetRightX(); - state.rightStickY = driverController->GetRightY(); - - state.bButtonPressed = driverController->GetBButton(); - state.aButtonPressed = driverController->GetAButton(); - state.xButtonPressed = driverController->GetXButton(); - state.yButtonPressed = driverController->GetYButton(); - - state.startButtonPressed = false;//driverController->GetStartButtonPressed(); - - state.stickPressed = std::abs(state.leftStickY) > 0.05 || - std::abs(state.leftStickX) > 0.05 || - std::abs(state.rightStickX) > 0.05; + state.stickPressed = driverController->leftStickYActive() || + driverController->leftStickXActive() || + driverController->rightStickXActive(); //state.dPadDownPressed = driverController->GetPOV(frc::GenericHID::) @@ -157,9 +144,6 @@ void Drivetrain::assessInputs() void Drivetrain::analyzeDashboard() { - state.backButtonPressed = driverController->GetBackButtonPressed(); - - table->PutNumber("Robot X", getPose_m().X().to()); table->PutNumber("Robot Y", getPose_m().Y().to()); table->PutNumber("Robot Theta", getPose_m().Rotation().Degrees().to()); @@ -206,7 +190,7 @@ void Drivetrain::analyzeDashboard() swerveModules[2]->getState(), swerveModules[3]->getState()); - if (state.backButtonPressed){ + if (driverController->GetBackButtonPressed()) { resetGyro(); } } @@ -226,25 +210,24 @@ void Drivetrain::assignOutputs() { // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. - double xSpeed = std::abs(state.leftStickY) > OIConstants::kDeadbandY ? fabs(state.leftStickY) * -state.leftStickY : 0 ; + double xSpeed = driverController->leftStickY(2); // Get the y speed or sideways/strafe speed. We are inverting this because // we want a positive value when we pull to the left. Xbox controllers // return positive values when you pull to the right by default. - double ySpeed = std::abs(state.leftStickX) > OIConstants::kDeadbandX ? fabs(state.leftStickX) * -state.leftStickX : 0; + double ySpeed = driverController->leftStickX(2); // Get the rate of angular rotation. We are inverting this because we want a // positive value when we pull to the left (remember, CCW is positive in // mathematics). Xbox controllers return positive values when you pull to // the right by default. - - double rot = std::abs(state.rightStickX) > .06 ? -1 * state.rightStickX * state.rightStickX * state.rightStickX : 0; + double rot = driverController->rightStickX(3); units::meters_per_second_t xSpeedMPS = units::meters_per_second_t{xSpeed * SwerveConstants::DRIVE_MAX_SPEED_MPS}; units::meters_per_second_t ySpeedMPS = units::meters_per_second_t{ySpeed * SwerveConstants::DRIVE_MAX_SPEED_MPS}; units::radians_per_second_t rotRPS = units::radians_per_second_t{rot * SwerveConstants::ROTATION_MAX_SPEED_RPS}; - if(state.aButtonPressed){ + if(driverController->GetAButton()){ double magnitude = std::sqrt(std::pow(xSpeed, 2) + std::pow(ySpeed, 2)); xSpeed /= magnitude; ySpeed /= magnitude; @@ -271,7 +254,7 @@ void Drivetrain::assignOutputs() rotRPS = units::radians_per_second_t{-limeTable->GetNumber("tx", 0.0) * DriveConstants::LIMELIGHT_KP * limeTable->GetNumber("tv", 0) * SwerveConstants::ROTATION_MAX_SPEED_RPS}; } - if (state.startButtonPressed){ + if (driverController->GetStartButtonPressed()) { // x0y0 = frc::Pose2d(8.514_m, 1.771_m, frc::Rotation2d(182.1_deg)); // goZeroZero = frc::TrajectoryGenerator::GenerateTrajectory( diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 906ef73..81b2e4c 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -56,7 +56,7 @@ void Feeder::init() } -void Feeder::setControllers(frc::XboxController *controllerO, frc::XboxController *controllerD) +void Feeder::setControllers(ValorGamepad *controllerO, ValorGamepad *controllerD) { driverController = controllerD; operatorController = controllerO; @@ -68,34 +68,18 @@ void Feeder::assessInputs() { return; } - - // driver inputs - - state.driver_leftBumperPressed = driverController->GetLeftBumper(); - state.driver_rightBumperPressed = driverController->GetRightBumper(); - - state.driver_rightTriggerPressed = driverController->GetRightTriggerAxis() > OIConstants::kDeadBandTrigger; - state.driver_leftTriggerPressed = driverController->GetLeftTriggerAxis() > OIConstants::kDeadBandTrigger; - - - // operator inputs - - state.operator_leftBumperPressed = operatorController->GetLeftBumper(); - if (state.driver_rightTriggerPressed || state.operator_leftBumperPressed) { + if (driverController->rightTrigger() || operatorController->GetLeftBumper()) { state.feederState = FeederState::FEEDER_SHOOT; //intake and feeder run state.spiked = false; } - else if (state.driver_leftBumperPressed) { + else if (driverController->GetLeftBumper()) { state.feederState = FeederState::FEEDER_REVERSE; state.spiked = false; } - else if (state.driver_rightBumperPressed) { + else if (driverController->GetRightBumper()) { state.feederState = FeederState::FEEDER_REGULAR_INTAKE; //standard intake } - // else if (state.driver_leftTriggerPressed) { - // state.feederState = FeederState::FEEDER_CURRENT_INTAKE; //includes current/banner sensing - // } else { state.feederState = FeederState::FEEDER_DISABLE; } diff --git a/Competition/src/main/cpp/subsystems/Lift.cpp b/Competition/src/main/cpp/subsystems/Lift.cpp index d339244..3c21d0b 100644 --- a/Competition/src/main/cpp/subsystems/Lift.cpp +++ b/Competition/src/main/cpp/subsystems/Lift.cpp @@ -74,7 +74,7 @@ void Lift::init() setupCommands(); } -void Lift::setController(frc::XboxController *controller) +void Lift::setController(ValorGamepad *controller) { operatorController = controller; } @@ -185,28 +185,18 @@ void Lift::assessInputs() return; } - state.rightStickY = -1 * operatorController->GetRightY(); - - state.dPadLeftPressed = operatorController->GetPOV() == OIConstants::dpadLeft; - state.dPadRightPressed = operatorController->GetPOV() == OIConstants::dpadRight; - state.dPadDownPressed = operatorController->GetPOV() == OIConstants::dpadDown; - state.dPadUpPressed = operatorController->GetPOV() == OIConstants::dpadUp; - - state.leftTriggerPressed = operatorController->GetLeftTriggerAxis() > LiftConstants::kDeadBandTrigger; - state.rightTriggerPressed = operatorController->GetRightTriggerAxis() > LiftConstants::kDeadBandTrigger; - - if((std::abs(state.rightStickY) > OIConstants::kDeadbandY) && liftSequenceUp.IsScheduled()){ + if(operatorController->rightStickYActive() && liftSequenceUp.IsScheduled()){ liftSequenceUp.Cancel(); } - if((std::abs(state.rightStickY) > OIConstants::kDeadbandY) && liftSequenceDown.IsScheduled()){ + if(operatorController->rightStickYActive() && liftSequenceDown.IsScheduled()){ liftSequenceDown.Cancel(); } - if (state.dPadLeftPressed && leadMainMotor.GetSelectedSensorPosition() > LiftConstants::rotateNoLowerThreshold) + if (operatorController->DPadLeft() && leadMainMotor.GetSelectedSensorPosition() > LiftConstants::rotateNoLowerThreshold) { state.liftstateRotate = LiftRotateState::LIFT_ROTATE_EXTEND; } - else if (state.dPadRightPressed && leadMainMotor.GetSelectedSensorPosition() > LiftConstants::rotateNoLowerThreshold) + else if (operatorController->DPadRight() && leadMainMotor.GetSelectedSensorPosition() > LiftConstants::rotateNoLowerThreshold) { state.liftstateRotate = LiftRotateState::LIFT_ROTATE_RETRACT; } @@ -215,11 +205,11 @@ void Lift::assessInputs() state.liftstateRotate = LiftRotateState::LIFT_ROTATE_DISABLED; } - if (std::abs(state.rightStickY) > OIConstants::kDeadbandY) + if (operatorController->rightStickYActive()) { state.liftstateMain = LiftMainState::LIFT_MAIN_ENABLE; } - else if (state.leftTriggerPressed && state.rightTriggerPressed) { + else if (operatorController->leftTriggerActive() && operatorController->rightTriggerActive()) { state.liftstateMain = LiftMainState::LIFT_MAIN_FIRSTPOSITION; } else if(!liftSequenceUp.IsScheduled() && !liftSequenceDown.IsScheduled()) @@ -227,10 +217,10 @@ void Lift::assessInputs() state.liftstateMain = LiftMainState::LIFT_MAIN_DISABLED; } - if (state.dPadDownPressed){ + if (operatorController->DPadDown()){ liftSequenceDown.Schedule(); } - else if (state.dPadUpPressed) { + else if (operatorController->DPadUp()) { liftSequenceUp.Schedule(); } @@ -276,8 +266,9 @@ void Lift::analyzeDashboard() void Lift::assignOutputs() { if (state.liftstateRotate == LiftRotateState::LIFT_ROTATE_DISABLED && - !(state.liftstateMain == LiftMainState::LIFT_MAIN_ENABLE && - state.rightStickY < (-1 * OIConstants::kDeadbandY))) + !(state.liftstateMain == LiftMainState::LIFT_MAIN_ENABLE && + operatorController->rightStickYActive() && + operatorController->rightStickY() < 0)) { rotateMotor.Set(0); } @@ -300,11 +291,11 @@ void Lift::assignOutputs() leadMainMotor.Set(0); } else if (state.liftstateMain == LiftMainState::LIFT_MAIN_ENABLE) { - if(state.rightStickY > OIConstants::kDeadbandY){ - leadMainMotor.Set(state.rightStickY * LiftConstants::DEFAULT_MAIN_EXTEND_SPD); + if (operatorController->rightStickYActive() && operatorController->rightStickY() > 0) { + leadMainMotor.Set(operatorController->rightStickY() * LiftConstants::DEFAULT_MAIN_EXTEND_SPD); } - else if(state.rightStickY < (-1 * OIConstants::kDeadbandY)){ - leadMainMotor.Set(state.rightStickY * LiftConstants::DEFAULT_MAIN_RETRACT_SPD); + else if (operatorController->rightStickYActive() && operatorController->rightStickY() < 0) { + leadMainMotor.Set(operatorController->rightStickY() * LiftConstants::DEFAULT_MAIN_RETRACT_SPD); rotateMotor.Set(-0.2); } } diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index 5c7840f..a1f8fcf 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -150,7 +150,7 @@ void Shooter::resetEncoder(){ hoodEncoder.SetPosition(0); } -void Shooter::setControllers(frc::XboxController *controllerO, frc::XboxController *controllerD) +void Shooter::setControllers(ValorGamepad *controllerO, ValorGamepad *controllerD) { operatorController = controllerO; driverController = controllerD; @@ -167,49 +167,30 @@ void Shooter::assessInputs() { return; } - state.startButton = operatorController->GetStartButtonPressed(); - state.backButton = operatorController->GetBackButtonPressed(); - state.rightBumper = operatorController->GetRightBumper(); - state.leftStickX = -operatorController->GetLeftX(); - state.aButton = operatorController->GetAButtonPressed(); - state.yButton = operatorController->GetYButton(); - state.xButtonPressed = operatorController->GetXButtonPressed(); - state.bButton = operatorController->GetBButtonPressed(); - state.driverLeftTrigger = driverController->GetLeftTriggerAxis() > 0.9; - state.driverDPadUp = driverController->GetPOV() == OIConstants::dpadUp; - state.driverBButton = driverController->GetBButton(); //Turret - if (std::abs(state.leftStickX) > ShooterConstants::kDeadband) { + if (operatorController->leftStickXActive()) { state.turretState = TurretState::TURRET_MANUAL; // Operator control } - else if (state.yButton || state.driverBButton) { + else if (operatorController->GetYButton() || driverController->GetBButton()) { state.turretState = TurretState::TURRET_HOME_MID; } else if (!table->GetBoolean("Pit Disable", false)){ state.turretState = TurretState::TURRET_TRACK; } - //Hood + //Hood & Flywheel - if(state.aButton){ + if(operatorController->GetAButtonPressed()){ state.hoodState = HoodState::HOOD_DOWN; // Low position - } - else if(state.xButtonPressed){ - state.hoodState = HoodState::HOOD_POOP; - } - else if (state.bButton){ - state.hoodState = HoodState::HOOD_TRACK; // High position - } - - //Flywheel - if (state.aButton){ state.flywheelState = FlywheelState::FLYWHEEL_DEFAULT; // Lower speed } - else if (state.xButtonPressed){ + else if(operatorController->GetXButtonPressed()){ + state.hoodState = HoodState::HOOD_POOP; state.flywheelState = FlywheelState::FLYWHEEL_POOP; } - else if (state.bButton){ + else if (operatorController->GetBButtonPressed()){ + state.hoodState = HoodState::HOOD_TRACK; // High position state.flywheelState = FlywheelState::FLYWHEEL_TRACK; // Higher speed } @@ -290,8 +271,6 @@ void Shooter::analyzeDashboard() table->PutNumber("Turret Output", state.turretOutput); - table->PutNumber("left stick x", state.leftStickX); - table->PutNumber("flywheel power", state.flywheelHigh); table->PutNumber("hood high", state.hoodHigh); @@ -330,12 +309,7 @@ void Shooter::assignOutputs() //MANUAL state.turretTarget = 0; if (state.turretState == TurretState::TURRET_MANUAL) { - state.turretOutput = std::pow(state.leftStickX, 3) * ShooterConstants::TURRET_SPEED_MULTIPLIER; - - // Minimum power deadband - if (std::abs(state.leftStickX) < ShooterConstants::pDeadband) { - state.turretOutput = 0; - } + state.turretOutput = operatorController->leftStickX(3) * ShooterConstants::TURRET_SPEED_MULTIPLIER; if( (turretEncoder.GetPosition() > 160 && state.turretOutput > ShooterConstants::pDeadband) || (turretEncoder.GetPosition() < 20 && state.turretOutput < -1 * ShooterConstants::pDeadband) ){ state.turretOutput = 0; diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index b4a7ad5..efcf3e9 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -57,7 +57,7 @@ void TurretTracker::assignOutputs() { // 0.75 = limeligh KP state.target = (-state.cachedTx * 0.75) + turretPos; - if(shooter-> state.driverLeftTrigger){ + if(shooter->driverController->leftTriggerActive()) { state.target += 15; } diff --git a/Competition/src/main/include/RobotContainer.h b/Competition/src/main/include/RobotContainer.h index 964efca..ab554c6 100644 --- a/Competition/src/main/include/RobotContainer.h +++ b/Competition/src/main/include/RobotContainer.h @@ -17,7 +17,7 @@ #include "subsystems/Feeder.h" #include "subsystems/Lift.h" #include "subsystems/TurretTracker.h" -#include +#include "ValorGamepad.h" #ifndef ROBOT_CONTAINER_H #define ROBOT_CONTAINER_H @@ -27,8 +27,8 @@ class RobotContainer { RobotContainer(); frc2::Command* GetAutonomousCommand(); - frc::XboxController m_GamepadDriver{OIConstants::GAMEPAD_BASE_LOCATION}; - frc::XboxController m_GamepadOperator{OIConstants::GAMEPAD_OPERATOR_LOCATION}; + ValorGamepad m_GamepadDriver{OIConstants::GAMEPAD_BASE_LOCATION}; + ValorGamepad m_GamepadOperator{OIConstants::GAMEPAD_OPERATOR_LOCATION}; Drivetrain m_drivetrain; Shooter m_shooter; diff --git a/Competition/src/main/include/ValorGamepad.h b/Competition/src/main/include/ValorGamepad.h new file mode 100644 index 0000000..20f76be --- /dev/null +++ b/Competition/src/main/include/ValorGamepad.h @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#ifndef VALORGAMEPAD_H +#define VALORGAMEPAD_H + +class ValorGamepad : public frc::XboxController +{ +public: + + ValorGamepad(int); + + void setDeadbandX(double); + void setDeadbandY(double); + + double leftStickX(int polynomial=1); + bool leftStickXActive(int polynomial=1); + double leftStickY(int polynomial=1); + bool leftStickYActive(int polynomial=1); + + double rightStickX(int polynomial=1); + bool rightStickXActive(int polynomial=1); + double rightStickY(int polynomial=1); + bool rightStickYActive(int polynomial=1); + + double rightTrigger(); + bool rightTriggerActive(); + double leftTrigger(); + bool leftTriggerActive(); + + bool DPadUp(); + bool DPadDown(); + bool DPadLeft(); + bool DPadRight(); + +private: + double deadband(double, double, int); + + double deadbandX; + double deadbandY; +}; + +#endif \ No newline at end of file diff --git a/Competition/src/main/include/subsystems/Drivetrain.h b/Competition/src/main/include/subsystems/Drivetrain.h index 7d4f743..30e6c51 100644 --- a/Competition/src/main/include/subsystems/Drivetrain.h +++ b/Competition/src/main/include/subsystems/Drivetrain.h @@ -10,11 +10,11 @@ #include "ValorSubsystem.h" #include "Constants.h" #include "ValorSwerve.h" +#include "ValorGamepad.h" #include #include "AHRS.h" #include "ctre/phoenix/sensors/WPI_Pigeon2.h" -#include #include #include #include @@ -45,7 +45,7 @@ class Drivetrain : public ValorSubsystem ~Drivetrain(); void init(); - void setController(frc::XboxController *controller); + void setController(ValorGamepad *controller); void assessInputs(); void analyzeDashboard(); @@ -55,23 +55,7 @@ class Drivetrain : public ValorSubsystem struct x { - double leftStickX; - double leftStickY; - double rightStickX; - double rightStickY; - - bool backButtonPressed; - bool startButtonPressed; bool stickPressed; - - bool bButtonPressed; - bool aButtonPressed; - bool xButtonPressed; - bool yButtonPressed; - - bool dPadUpPressed; - bool dPadDownPressed; - bool tracking; bool saveToFileDebouncer; @@ -156,7 +140,7 @@ class Drivetrain : public ValorSubsystem void configSwerveModule(int); - frc::XboxController *driverController; + ValorGamepad *driverController; std::vector swerveModules; std::vector azimuthMotors; diff --git a/Competition/src/main/include/subsystems/Feeder.h b/Competition/src/main/include/subsystems/Feeder.h index 416f974..544d4b1 100644 --- a/Competition/src/main/include/subsystems/Feeder.h +++ b/Competition/src/main/include/subsystems/Feeder.h @@ -9,9 +9,8 @@ #include "ValorSubsystem.h" #include "Constants.h" -#include +#include "ValorGamepad.h" -#include #include #include @@ -24,7 +23,7 @@ class Feeder : public ValorSubsystem Feeder(); void init(); - void setControllers(frc::XboxController *controllerO, frc::XboxController *controllerD); + void setControllers(ValorGamepad *controllerO, ValorGamepad *controllerD); void assessInputs(); void analyzeDashboard(); @@ -42,16 +41,6 @@ class Feeder : public ValorSubsystem struct x { - bool driver_rightBumperPressed; - - bool operator_bButtonPressed; - bool operator_aButtonPressed; - - bool driver_leftBumperPressed; - bool operator_leftBumperPressed; - - bool driver_rightTriggerPressed; - bool driver_leftTriggerPressed; bool bannerTripped; bool previousBanner; @@ -81,8 +70,8 @@ class Feeder : public ValorSubsystem void resetDeque(); private: - frc::XboxController *driverController; - frc::XboxController *operatorController; + ValorGamepad *driverController; + ValorGamepad *operatorController; WPI_TalonFX motor_intake; WPI_TalonFX motor_stage; diff --git a/Competition/src/main/include/subsystems/Lift.h b/Competition/src/main/include/subsystems/Lift.h index 6f9ed9a..d977227 100644 --- a/Competition/src/main/include/subsystems/Lift.h +++ b/Competition/src/main/include/subsystems/Lift.h @@ -2,7 +2,7 @@ #include "ValorSubsystem.h" #include "Constants.h" -#include +#include "ValorGamepad.h" #include #include "ValorSubsystem.h" @@ -26,7 +26,7 @@ class Lift : public ValorSubsystem Lift(); void init(); - void setController(frc::XboxController *controller); + void setController(ValorGamepad *controller); void assessInputs(); void analyzeDashboard(); @@ -67,16 +67,6 @@ class Lift : public ValorSubsystem LiftRotateState liftstateRotate; LiftAutomationState liftStateAutomation; - bool dPadUpPressed; - bool dPadDownPressed; - bool dPadLeftPressed; - bool dPadRightPressed; - - bool leftTriggerPressed; - bool rightTriggerPressed; - - double rightStickY; - double powerRetract; double powerExtend; double powerMain; @@ -93,7 +83,7 @@ class Lift : public ValorSubsystem void setupCommands(); private: - frc::XboxController *operatorController; + ValorGamepad *operatorController; WPI_TalonFX leadMainMotor; diff --git a/Competition/src/main/include/subsystems/Shooter.h b/Competition/src/main/include/subsystems/Shooter.h index 17db95c..c8d5595 100644 --- a/Competition/src/main/include/subsystems/Shooter.h +++ b/Competition/src/main/include/subsystems/Shooter.h @@ -11,13 +11,12 @@ #include "Constants.h" #include "ValorSwerve.h" #include "Drivetrain.h" +#include "ValorGamepad.h" #include #include #include -#include - #include #include #include @@ -36,7 +35,7 @@ class Shooter : public ValorSubsystem Shooter(); void init(); - void setControllers(frc::XboxController *controllerO, frc::XboxController *controllerD); + void setControllers(ValorGamepad *controllerO, ValorGamepad *controllerD); void setDrivetrain(Drivetrain * dt); void assessInputs(); @@ -85,19 +84,6 @@ class Shooter : public ValorSubsystem TurretState turretState; TurretState lastTurretState; - double leftStickX; - - bool backButton; - bool startButton; - bool rightBumper; - bool aButton; - bool yButton; - bool xButtonPressed; - bool bButton; - - bool driverLeftTrigger; - bool driverDPadUp; - bool driverBButton; bool driverLastLeftTrigger; double limelightDistance; @@ -151,8 +137,8 @@ class Shooter : public ValorSubsystem rev::SparkMaxRelativeEncoder hoodEncoder = hood.GetEncoder(); rev::SparkMaxPIDController hoodPidController = hood.GetPIDController(); - frc::XboxController *operatorController; - frc::XboxController *driverController; + ValorGamepad *operatorController; + ValorGamepad *driverController; std::shared_ptr limeTable; std::shared_ptr liftTable; From d9f913d26555def76fd241156595ff344aef5be6 Mon Sep 17 00:00:00 2001 From: Michael Ray Date: Wed, 1 Jun 2022 22:58:26 -0500 Subject: [PATCH 36/62] feat: Add new ValorSensor construct Allow sensors to be their own object, which simplifies the logic in the subsystems. This also allows us to re-use common sensor types year over year. For example the current sensing circular buffer on our intake is a good sensor to use in the future, therefore create a ValorCurrentSensor that can be used by multiple subsystems --- Competition/src/main/cpp/ValorAuto.cpp | 2 +- .../main/cpp/sensors/ValorCurrentSensor.cpp | 42 ++++++++++ .../main/cpp/sensors/ValorDebounceSensor.cpp | 24 ++++++ .../src/main/cpp/subsystems/Feeder.cpp | 76 ++++++------------- .../main/include/sensors/ValorCurrentSensor.h | 33 ++++++++ .../include/sensors/ValorDebounceSensor.h | 30 ++++++++ .../src/main/include/sensors/ValorSensor.h | 33 ++++++++ .../src/main/include/subsystems/Feeder.h | 25 ++---- 8 files changed, 194 insertions(+), 71 deletions(-) create mode 100644 Competition/src/main/cpp/sensors/ValorCurrentSensor.cpp create mode 100644 Competition/src/main/cpp/sensors/ValorDebounceSensor.cpp create mode 100644 Competition/src/main/include/sensors/ValorCurrentSensor.h create mode 100644 Competition/src/main/include/sensors/ValorDebounceSensor.h create mode 100644 Competition/src/main/include/sensors/ValorSensor.h diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index e5e4778..a145360 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -139,7 +139,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::InstantCommand cmd_intakeOne = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_REGULAR_INTAKE; } ); frc2::InstantCommand cmd_intakeDisable = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_DISABLE; } ); frc2::InstantCommand cmd_intakeAuto = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_CURRENT_INTAKE; } ); - frc2::InstantCommand cmd_intakeClearDeque = frc2::InstantCommand( [&] { feeder->resetDeque();} ); + frc2::InstantCommand cmd_intakeClearDeque = frc2::InstantCommand( [&] { feeder->resetIntakeSensor();} ); frc2::InstantCommand cmd_intakeShoot = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_SHOOT; } ); frc2::InstantCommand cmd_intakeReverse = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_REVERSE; } ); diff --git a/Competition/src/main/cpp/sensors/ValorCurrentSensor.cpp b/Competition/src/main/cpp/sensors/ValorCurrentSensor.cpp new file mode 100644 index 0000000..d422d75 --- /dev/null +++ b/Competition/src/main/cpp/sensors/ValorCurrentSensor.cpp @@ -0,0 +1,42 @@ +#include "sensors/ValorCurrentSensor.h" + +#define CACHE_SIZE 20 +#define DEFAULT_SPIKE_VALUE 22 + +ValorCurrentSensor::ValorCurrentSensor() : + spikedSetpoint(DEFAULT_SPIKE_VALUE) +{ + reset(); +} + +void ValorCurrentSensor::setSpikeSetpoint(double _setpoint) +{ + spikedSetpoint = _setpoint; +} + +bool ValorCurrentSensor::spiked() { + return currState > spikedSetpoint; +} + +void ValorCurrentSensor::reset() { + cache.clear(); + for (int i = 0; i < CACHE_SIZE; i++) { + cache.push_back(0); + } + prevState = 0; + currState = 0; +} + +void ValorCurrentSensor::calculate() { + cache.pop_front(); + cache.push_back(getSensor()); + + // Calculate average current over the cache size, or circular buffer window + double sum = 0; + for (int i = 0; i < CACHE_SIZE; i++) { + sum += cache.at(i); + } + + currState = sum / CACHE_SIZE; + prevState = currState; +} \ No newline at end of file diff --git a/Competition/src/main/cpp/sensors/ValorDebounceSensor.cpp b/Competition/src/main/cpp/sensors/ValorDebounceSensor.cpp new file mode 100644 index 0000000..2126102 --- /dev/null +++ b/Competition/src/main/cpp/sensors/ValorDebounceSensor.cpp @@ -0,0 +1,24 @@ +#include "sensors/ValorDebounceSensor.h" + +ValorDebounceSensor::ValorDebounceSensor() +{ +} + +void ValorDebounceSensor::reset() +{ + prevState = false; + currState = false; + isSpiked = false; +} + +bool ValorDebounceSensor::spiked() +{ + return isSpiked; +} + +void ValorDebounceSensor::calculate() +{ + currState = getSensor(); + isSpiked = currState != prevState; + prevState = currState; +} diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 81b2e4c..22e7f91 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -18,7 +18,6 @@ Feeder::Feeder() : ValorSubsystem(), motor_intake(FeederConstants::MOTOR_INTAKE_CAN_ID, "baseCAN"), motor_stage(FeederConstants::MOTOR_STAGE_CAN_ID, "baseCAN"), banner(FeederConstants::BANNER_DIO_PORT) - { frc2::CommandScheduler::GetInstance().RegisterSubsystem(this); init(); @@ -53,9 +52,16 @@ void Feeder::init() table->PutBoolean("Banner: ", 0); resetState(); + currentSensor.setGetter([this]() { return motor_intake.GetOutputCurrent(); }); + debounceSensor.setGetter([this]() { return !banner.Get(); }); } +void Feeder::resetIntakeSensor() +{ + currentSensor.reset(); +} + void Feeder::setControllers(ValorGamepad *controllerO, ValorGamepad *controllerD) { driverController = controllerD; @@ -71,11 +77,11 @@ void Feeder::assessInputs() if (driverController->rightTrigger() || operatorController->GetLeftBumper()) { state.feederState = FeederState::FEEDER_SHOOT; //intake and feeder run - state.spiked = false; + resetIntakeSensor(); } else if (driverController->GetLeftBumper()) { state.feederState = FeederState::FEEDER_REVERSE; - state.spiked = false; + resetIntakeSensor(); } else if (driverController->GetRightBumper()) { state.feederState = FeederState::FEEDER_REGULAR_INTAKE; //standard intake @@ -87,27 +93,26 @@ void Feeder::assessInputs() void Feeder::analyzeDashboard() { + // Calculate instantaneous current + currentSensor.calculate(); + // Calculate banner sensor trigger + debounceSensor.calculate(); + state.reversed = table->GetBoolean("Reverse Feeder?", false); state.intakeReverseSpeed = table->GetNumber("Intake Reverse Speed", FeederConstants::DEFAULT_INTAKE_SPEED_REVERSE); state.feederReverseSpeed = table->GetNumber("Feeder Reverse Speed", FeederConstants::DEFAULT_FEEDER_SPEED_REVERSE); state.intakeForwardSpeed = table->GetNumber("Intake Forward Speed", FeederConstants::DEFAULT_INTAKE_SPEED_FORWARD); state.feederForwardSpeedDefault = table->GetNumber("Feeder Forward Speed Default", FeederConstants::DEFAULT_FEEDER_SPEED_FORWARD_DEFAULT); state.feederForwardSpeedShoot = table->GetNumber("Feeder Forward Speed Shoot", FeederConstants::DEFAULT_FEEDER_SPEED_FORWARD_SHOOT); - state.spikeCurrent = table->GetNumber("Intake Spike Current", FeederConstants::JAM_CURRENT); - - table->PutNumber("Average Amps", state.instCurrent); - table->PutBoolean("Spiked: ", state.spiked); - table->PutBoolean("Banner: ", state.bannerTripped); + table->PutNumber("Average Amps", currentSensor.getSensor()); + table->PutBoolean("Spiked: ", currentSensor.spiked()); + table->PutBoolean("Banner: ", debounceSensor.getSensor()); table->PutNumber("current feeder state", state.feederState); - // Calculate instantaneous current - calcCurrent(); } void Feeder::assignOutputs() { - state.bannerTripped = !banner.Get(); - state.currentBanner = state.bannerTripped; if (state.feederState == FeederState::FEEDER_DISABLE) { motor_intake.Set(0); @@ -126,25 +131,17 @@ void Feeder::assignOutputs() motor_stage.Set(0); } else if (state.feederState == FeederState::FEEDER_CURRENT_INTAKE) { //includes currrent sensing - if (state.bannerTripped) { - if (state.currentBanner && !state.previousBanner) { - resetDeque(); - state.spiked = false; + if (debounceSensor.getSensor()) { + if (debounceSensor.spiked()) { + resetIntakeSensor(); } - if (state.spiked) { + if (currentSensor.spiked()) { motor_intake.Set(0); motor_stage.Set(0); } else { - if (state.instCurrent > state.spikeCurrent && state.bannerTripped) { - motor_intake.Set(0); - motor_stage.Set(0); - state.spiked = true; - } - else { - motor_intake.Set(state.intakeForwardSpeed); - motor_stage.Set(0); - } + motor_intake.Set(state.intakeForwardSpeed); + motor_stage.Set(0); } } else { @@ -156,35 +153,10 @@ void Feeder::assignOutputs() motor_intake.Set(0); motor_stage.Set(0); } - state.previousBanner = state.currentBanner; -} - -void Feeder::calcCurrent() { - state.current_cache.pop_front(); - state.current_cache.push_back(motor_intake.GetOutputCurrent()); - - // Calculate average current over the cache size, or circular buffer window - double sum = 0; - for (int i = 0; i < FeederConstants::CACHE_SIZE; i++) { - sum += state.current_cache.at(i); - } - state.instCurrent = sum / FeederConstants::CACHE_SIZE; -} - -void Feeder::resetDeque() { - state.current_cache.clear(); - for (int i = 0; i < FeederConstants::CACHE_SIZE; i++) { - state.current_cache.push_back(0); - } - state.spiked = false; } void Feeder::resetState() { state.feederState = FeederState::FEEDER_DISABLE; - - state.spiked = false; - state.previousBanner = false; - - resetDeque(); + resetIntakeSensor(); } diff --git a/Competition/src/main/include/sensors/ValorCurrentSensor.h b/Competition/src/main/include/sensors/ValorCurrentSensor.h new file mode 100644 index 0000000..3f899f1 --- /dev/null +++ b/Competition/src/main/include/sensors/ValorCurrentSensor.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "sensors/ValorSensor.h" +#include +#include + +#ifndef VALORCURRENTSENSOR_H +#define VALORCURRENTSENSOR_H + +class ValorCurrentSensor : public ValorSensor +{ +public: + ValorCurrentSensor(); + + void reset(); + void calculate(); + + bool spiked(); + void setSpikeSetpoint(double); + +private: + std::deque cache; + double spikedSetpoint; +}; + +#endif; \ No newline at end of file diff --git a/Competition/src/main/include/sensors/ValorDebounceSensor.h b/Competition/src/main/include/sensors/ValorDebounceSensor.h new file mode 100644 index 0000000..2f9f3f8 --- /dev/null +++ b/Competition/src/main/include/sensors/ValorDebounceSensor.h @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "sensors/ValorSensor.h" +#include + +#ifndef VALORDEBOUNCESENSOR_H +#define VALORDEBOUNCESENSOR_H + +class ValorDebounceSensor : public ValorSensor +{ +public: + ValorDebounceSensor(); + + void reset(); + void calculate(); + + bool spiked(); + +private: + bool isSpiked; +}; + +#endif; \ No newline at end of file diff --git a/Competition/src/main/include/sensors/ValorSensor.h b/Competition/src/main/include/sensors/ValorSensor.h new file mode 100644 index 0000000..c6786de --- /dev/null +++ b/Competition/src/main/include/sensors/ValorSensor.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#ifndef VALORSENSOR_H +#define VALORSENSOR_H + +template +class ValorSensor +{ +public: + ValorSensor() {} + + virtual void reset() = 0; + virtual void calculate() = 0; + + void setGetter(std::function _lambda) { sensorLambda = _lambda; } + T getSensor() { return sensorLambda(); } + +protected: + std::function sensorLambda; + T currState; + T prevState; +}; + +#endif \ No newline at end of file diff --git a/Competition/src/main/include/subsystems/Feeder.h b/Competition/src/main/include/subsystems/Feeder.h index 544d4b1..49c5ebb 100644 --- a/Competition/src/main/include/subsystems/Feeder.h +++ b/Competition/src/main/include/subsystems/Feeder.h @@ -14,6 +14,9 @@ #include #include +#include "sensors/ValorCurrentSensor.h" +#include "sensors/ValorDebounceSensor.h" + #ifndef FEEDER_H #define FEEDER_H @@ -42,32 +45,19 @@ class Feeder : public ValorSubsystem struct x { - bool bannerTripped; - bool previousBanner; - bool currentBanner; - bool reversed; - - bool spiked; double intakeForwardSpeed; double intakeReverseSpeed; - double spikeCurrent; double feederForwardSpeedDefault; double feederForwardSpeedShoot; double feederReverseSpeed; - - //int current_cache_index; - //std::vector current_cache; - std::deque current_cache; - - double instCurrent; FeederState feederState; } state; -void resetDeque(); + void resetIntakeSensor(); private: ValorGamepad *driverController; @@ -76,11 +66,10 @@ void resetDeque(); WPI_TalonFX motor_intake; WPI_TalonFX motor_stage; - frc::DigitalInput banner; + ValorCurrentSensor currentSensor; + ValorDebounceSensor debounceSensor; - void calcCurrent(); - - + frc::DigitalInput banner; }; From fcbbe91fb93f8de9365a4488287d7df07bdbaabc Mon Sep 17 00:00:00 2001 From: Michael Ray Date: Wed, 1 Jun 2022 23:29:11 -0500 Subject: [PATCH 37/62] fix: Protect against null lambdas in ValorSensor If someone forgets to set the getter function for a ValorSensor (which inevitably will happen), then return 0. If the getter function is indeed set, then use it when getting the sensor --- Competition/src/main/include/sensors/ValorSensor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Competition/src/main/include/sensors/ValorSensor.h b/Competition/src/main/include/sensors/ValorSensor.h index c6786de..48da482 100644 --- a/Competition/src/main/include/sensors/ValorSensor.h +++ b/Competition/src/main/include/sensors/ValorSensor.h @@ -22,7 +22,7 @@ class ValorSensor virtual void calculate() = 0; void setGetter(std::function _lambda) { sensorLambda = _lambda; } - T getSensor() { return sensorLambda(); } + T getSensor() { return sensorLambda ? sensorLambda() : 0; } protected: std::function sensorLambda; From 4836d3db14340c6b0714248a193bc19dfc8c48bb Mon Sep 17 00:00:00 2001 From: Michael Ray Date: Fri, 26 Aug 2022 17:51:10 -0400 Subject: [PATCH 38/62] Fix gamepad bug: Even polynomial on a negative base yields incorrect results --- Competition/src/main/cpp/ValorGamepad.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/Competition/src/main/cpp/ValorGamepad.cpp b/Competition/src/main/cpp/ValorGamepad.cpp index 132bf56..fb69ae4 100644 --- a/Competition/src/main/cpp/ValorGamepad.cpp +++ b/Competition/src/main/cpp/ValorGamepad.cpp @@ -28,9 +28,16 @@ void ValorGamepad::setDeadbandY(double deadband) deadbandY = deadband; } +// Get the sign of an input +template int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + double ValorGamepad::deadband(double input, double deadband, int polynomial) { - return std::fabs(input) > deadband ? std::pow(input, polynomial) : 0; + // If input is negative and polynomial is even, output would be positive which is incorrect + // Therefore if polynomial is even, multiply pow by the sign of the input + return std::fabs(input) > deadband ? ((polynomial % 2 == 0 ? sgn(input) : 1) * std::pow(input, polynomial)) : 0; } double ValorGamepad::leftStickX(int polynomial) From b22b2dd07ef778637376ef8932c046afd596cf1a Mon Sep 17 00:00:00 2001 From: Michael Ray Date: Fri, 26 Aug 2022 16:02:50 -0400 Subject: [PATCH 39/62] Implement Super Poop While holding left trigger, shoot the ball towards our hanger zone --- .../src/main/cpp/subsystems/TurretTracker.cpp | 32 ++++++++++++++----- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index efcf3e9..c0fbd66 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -57,20 +57,13 @@ void TurretTracker::assignOutputs() { // 0.75 = limeligh KP state.target = (-state.cachedTx * 0.75) + turretPos; - if(shooter->driverController->leftTriggerActive()) { - state.target += 15; - } - - // state.target = -1 * robotHeading + state.cachedTurretPos; - // atan2(drivetrain->getKinematics().ToChassisSpeeds().vx.to(()), drivetrain->getPose_m().X()); - state.cachedHeading = robotHeading; state.cachedX = x; state.cachedY = y; state.cachedTx = tx; state.cachedTurretPos = turretPos; - state.destinationTurretHeading = robotHeading + turretPos - 90 - tx; + state.destinationTurretHeading = robotHeading + turretPos - 90 - state.cachedTx; } else { if (table->GetBoolean("Use Turret Shoot", true)) @@ -79,6 +72,29 @@ void TurretTracker::assignOutputs() { state.target = turretPos; } + // Super Poop + if (shooter->driverController->leftTriggerActive()) { + double wrappedExistingHeading = state.destinationTurretHeading; + + // Wrap to positive numbers + if (wrappedExistingHeading < 0) + wrappedExistingHeading += 360; + + // Case structure for robot locations on the field + double superPoopHeading = 90; + if (wrappedExistingHeading <= 45) + superPoopHeading += 2 * 90.0 / 4; + else if (wrappedExistingHeading > 45 && wrappedExistingHeading <= 135) + superPoopHeading += 1 * 90.0 / 4; + else if (wrappedExistingHeading > 135 && wrappedExistingHeading <= 225) + superPoopHeading += 3 * 90.0 / 4; + else + superPoopHeading += 90; + + // Convert heading to turret angle + state.target = superPoopHeading - robotHeading + 90 + state.cachedTx; + } + if (state.target < -90) { state.target += 360; } From cda046441d39514dff08877f6bc6f55aa194b64e Mon Sep 17 00:00:00 2001 From: CooperNelson16 Date: Mon, 29 Aug 2022 19:11:20 -0500 Subject: [PATCH 40/62] re-implemented mag encoder start button functionality, made turret limits smaller --- Competition/src/main/cpp/subsystems/Drivetrain.cpp | 4 ++-- Competition/src/main/cpp/subsystems/ValorSwerve.cpp | 2 +- Competition/src/main/include/Constants.h | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Drivetrain.cpp b/Competition/src/main/cpp/subsystems/Drivetrain.cpp index b490d0f..9c3889a 100644 --- a/Competition/src/main/cpp/subsystems/Drivetrain.cpp +++ b/Competition/src/main/cpp/subsystems/Drivetrain.cpp @@ -97,8 +97,8 @@ void Drivetrain::init() // } resetState(); - //pullSwerveModuleZeroReference(); - //std::cout <<"init drivetrain" << std::endl; + // pullSwerveModuleZeroReference(); + // std::cout <<"init drivetrain" << std::endl; } void Drivetrain::setController(ValorGamepad *controller) diff --git a/Competition/src/main/cpp/subsystems/ValorSwerve.cpp b/Competition/src/main/cpp/subsystems/ValorSwerve.cpp index edba2cb..a5e04a9 100644 --- a/Competition/src/main/cpp/subsystems/ValorSwerve.cpp +++ b/Competition/src/main/cpp/subsystems/ValorSwerve.cpp @@ -116,7 +116,7 @@ void ValorSwerve::loadAndSetAzimuthZeroReference() //azimuthSetpoint = fmod(azimuthSetpoint, SwerveConstants::AZIMUTH_COUNTS_PER_REV / SwerveConstants::AZIMUTH_GEAR_RATIO); // Set the azimuth offset to the calculated setpoint (which will take over in teleop) - azimuthFalcon->SetSelectedSensorPosition(0, 0, 10); + azimuthFalcon->SetSelectedSensorPosition(azimuthSetpoint, 0, 10); //std::cout << "pulled pospition from file" << std::endl; } diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 1d4b0ef..3af3efd 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -223,7 +223,7 @@ namespace ShooterConstants{ constexpr static double homePositionMid = 90; constexpr static double homePositionLeft = 180; constexpr static double homePositionRight = 0; - constexpr static double turretLimitLeft = 180 + 10.5; + constexpr static double turretLimitLeft = 180 + 8.5; constexpr static double turretLimitRight = 0 - 7; constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500 From 49e8c2d65a6227318d7d37d5b5d61e120c7356e7 Mon Sep 17 00:00:00 2001 From: CooperNelson16 Date: Wed, 7 Sep 2022 18:48:03 -0500 Subject: [PATCH 41/62] all of new intake code minus position setting in assignoutputs of feeder --- Competition/src/main/cpp/subsystems/Feeder.cpp | 18 ++++++++++++++++++ .../src/main/cpp/subsystems/Shooter.cpp | 15 +++++++++++++-- Competition/src/main/include/Constants.h | 6 +++++- .../src/main/include/subsystems/Feeder.h | 9 ++++++++- .../src/main/include/subsystems/Shooter.h | 2 ++ 5 files changed, 46 insertions(+), 4 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 22e7f91..bc19144 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -17,6 +17,9 @@ Feeder::Feeder() : ValorSubsystem(), operatorController(NULL), motor_intake(FeederConstants::MOTOR_INTAKE_CAN_ID, "baseCAN"), motor_stage(FeederConstants::MOTOR_STAGE_CAN_ID, "baseCAN"), + motor_rotateMain(FeederConstants::MOTOR_ROTATE_MAIN_CAN_ID, "baseCAN"), + motor_rotateFollow(FeederConstants::MOTOR_ROTATE_FOLLOW_CAN_ID, "baseCAN"), + banner(FeederConstants::BANNER_DIO_PORT) { frc2::CommandScheduler::GetInstance().RegisterSubsystem(this); @@ -39,6 +42,14 @@ void Feeder::init() motor_stage.EnableVoltageCompensation(true); motor_stage.ConfigVoltageCompSaturation(10); + motor_rotateMain.SetInverted(false); + motor_rotateMain.SetNeutralMode(Brake); + + motor_rotateFollow.SetInverted(true); + motor_rotateFollow.SetNeutralMode(Brake); + motor_rotateFollow.Follow(motor_rotateMain); + + table->PutBoolean("Reverse Feeder?", false); table->PutNumber("Intake Reverse Speed", FeederConstants::DEFAULT_INTAKE_SPEED_REVERSE); table->PutNumber("Feeder Reverse Speed", FeederConstants::DEFAULT_FEEDER_SPEED_REVERSE); @@ -79,6 +90,9 @@ void Feeder::assessInputs() state.feederState = FeederState::FEEDER_SHOOT; //intake and feeder run resetIntakeSensor(); } + else if (driverController->GetBButton()) { + state.feederState = FeederState::FEEDER_RETRACT; //Set Intake rotate target to upper setpoint + } else if (driverController->GetLeftBumper()) { state.feederState = FeederState::FEEDER_REVERSE; resetIntakeSensor(); @@ -117,11 +131,15 @@ void Feeder::assignOutputs() if (state.feederState == FeederState::FEEDER_DISABLE) { motor_intake.Set(0); motor_stage.Set(0); + fixme motor_rotateRight.Set(0);// set rotate down } else if (state.feederState == FeederState::FEEDER_SHOOT) { motor_intake.Set(state.intakeForwardSpeed); motor_stage.Set(state.feederForwardSpeedShoot); } + else if (state.feederState == FeederState::FEEDER_RETRACT){ + fixme // set rotation to be up; + } else if (state.feederState == Feeder::FEEDER_REVERSE) { motor_intake.Set(state.intakeReverseSpeed); motor_stage.Set(state.feederReverseSpeed); diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index a1f8fcf..45c3ed7 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -169,7 +169,15 @@ void Shooter::assessInputs() } //Turret - if (operatorController->leftStickXActive()) { + if (feederTable->GetNumber("Intake Encoder Value", 0) > ShooterConstants::turretRotateIntakeThreshold) { + if (turretEncoder.GetPosition() > ShooterConstants::homePositionMid) { + state.turretState = TurretState::TURRET_HOME_LEFT; + } + else { + state.turretState == TurretState::TURRET_HOME_RIGHT; + } + } + else if (operatorController->leftStickXActive()) { state.turretState = TurretState::TURRET_MANUAL; // Operator control } else if (operatorController->GetYButton() || driverController->GetBButton()) { @@ -247,7 +255,8 @@ void Shooter::analyzeDashboard() state.hoodLow = table->GetNumber("Hood Bottom Position", ShooterConstants::hoodBottom); state.hoodHigh = table->GetNumber("Hood Top Position", ShooterConstants::hoodTop); - + + if (liftTable->GetNumber("Lift Main Encoder Value", 0) > ShooterConstants::turretRotateLiftThreshold) { state.turretState = TurretState::TURRET_HOME_LEFT; limelightTrack(false); @@ -255,6 +264,8 @@ void Shooter::analyzeDashboard() state.flywheelState = FlywheelState::FLYWHEEL_DISABLE; } + + if (state.turretState == TurretState::TURRET_TRACK && state.lastTurretState != TurretState::TURRET_TRACK){ limelightTrack(true); } diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 3af3efd..4497993 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -97,7 +97,7 @@ namespace SwerveConstants { constexpr static double AUTO_MAX_ACCEL_MPSS = AUTO_MAX_SPEED_MPS * .6; // * 1 constexpr static double DRIVE_SLOW_SPEED_MPS = 1; - constexpr static double ROTATION_MAX_SPEED_RPS = 2*M_PI;// DRIVE_MAX_SPEED_MPS / std::hypot(module_diff / 2, module_diff / 2); + constexpr static double ROTATION_MAX_SPEED_RPS = 4 *M_PI;// DRIVE_MAX_SPEED_MPS / std::hypot(module_diff / 2, module_diff / 2); constexpr static double AUTO_MAX_ROTATION_RPS = 4 * M_PI; constexpr static double AUTO_MAX_ROTATION_ACCEL_RPSS = AUTO_MAX_ROTATION_RPS * .5; // * 1 constexpr static double ROTATION_SLOW_SPEED_RPS = 1*M_PI; @@ -227,6 +227,8 @@ namespace ShooterConstants{ constexpr static double turretLimitRight = 0 - 7; constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500 + constexpr static double turretRotateIntakeThreshold = 0; fixme // set to intake rotate up threshold + constexpr static double hubX = 0; constexpr static double hubY = 0; @@ -251,6 +253,8 @@ namespace FeederConstants{ constexpr static int CACHE_SIZE = 20; constexpr static double JAM_CURRENT = 22; + + } namespace MathConstants{ diff --git a/Competition/src/main/include/subsystems/Feeder.h b/Competition/src/main/include/subsystems/Feeder.h index 49c5ebb..cf69d61 100644 --- a/Competition/src/main/include/subsystems/Feeder.h +++ b/Competition/src/main/include/subsystems/Feeder.h @@ -39,7 +39,8 @@ class Feeder : public ValorSubsystem FEEDER_REVERSE, FEEDER_SHOOT, FEEDER_CURRENT_INTAKE, - FEEDER_REGULAR_INTAKE + FEEDER_REGULAR_INTAKE, + FEEDER_RETRACT }; struct x @@ -65,6 +66,12 @@ class Feeder : public ValorSubsystem WPI_TalonFX motor_intake; WPI_TalonFX motor_stage; + WPI_TalonFX motor_rotateRight; + WPI_TalonFX motor_rotateLeft; + + //fixme // create motor group if needed + + ValorCurrentSensor currentSensor; ValorDebounceSensor debounceSensor; diff --git a/Competition/src/main/include/subsystems/Shooter.h b/Competition/src/main/include/subsystems/Shooter.h index c8d5595..6c6730f 100644 --- a/Competition/src/main/include/subsystems/Shooter.h +++ b/Competition/src/main/include/subsystems/Shooter.h @@ -142,6 +142,8 @@ class Shooter : public ValorSubsystem std::shared_ptr limeTable; std::shared_ptr liftTable; + std::shared_ptr feederTable; + Drivetrain *odom; From 8941b2a6797f3cbcd6aebb3eff61c2845e6381d4 Mon Sep 17 00:00:00 2001 From: CooperNelson16 Date: Mon, 12 Sep 2022 19:01:44 -0500 Subject: [PATCH 42/62] mid testing --- .../src/main/cpp/subsystems/Feeder.cpp | 39 ++++++++++++++----- Competition/src/main/include/Constants.h | 5 ++- .../src/main/include/subsystems/Feeder.h | 4 +- 3 files changed, 36 insertions(+), 12 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index bc19144..72bd532 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -42,13 +42,34 @@ void Feeder::init() motor_stage.EnableVoltageCompensation(true); motor_stage.ConfigVoltageCompSaturation(10); - motor_rotateMain.SetInverted(false); - motor_rotateMain.SetNeutralMode(Brake); - - motor_rotateFollow.SetInverted(true); - motor_rotateFollow.SetNeutralMode(Brake); - motor_rotateFollow.Follow(motor_rotateMain); - + // motor_rotateMain.SetInverted(false); + // motor_rotateMain.SetNeutralMode(Brake); + + // motor_rotateFollow.SetInverted(true); + // motor_rotateFollow.SetNeutralMode(Brake); + // motor_rotateFollow.Follow(motor_rotateMain); + motor_rotateMain.ConfigForwardSoftLimitThreshold(LiftConstants::extendForwardLimit); + motor_rotateMain.ConfigReverseSoftLimitThreshold(LiftConstants::extendReverseLimit); + + motor_rotateMain.ConfigForwardSoftLimitEnable(true); + motor_rotateMain.ConfigReverseSoftLimitEnable(true); + + motor_rotateMain.SetSelectedSensorPosition(0); + motor_rotateMain.SetInverted(true); + motor_rotateFollow.Follow(motor_rotateMain); + + motor_rotateMain.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); + motor_rotateFollow.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); + + motor_rotateMain.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); + motor_rotateMain.ConfigAllowableClosedloopError(0, 0); + motor_rotateMain.Config_IntegralZone(0, 0); + motor_rotateMain.Config_kF(0, LiftConstants::main_KF); + motor_rotateMain.Config_kD(0, LiftConstants::main_KD); + motor_rotateMain.Config_kI(0, LiftConstants::main_KI); + motor_rotateMain.Config_kP(0, LiftConstants::main_KP); + motor_rotateMain.ConfigMotionAcceleration(LiftConstants::MAIN_MOTION_ACCELERATION); + motor_rotateMain.ConfigMotionCruiseVelocity(LiftConstants::MAIN_MOTION_CRUISE_VELOCITY); table->PutBoolean("Reverse Feeder?", false); table->PutNumber("Intake Reverse Speed", FeederConstants::DEFAULT_INTAKE_SPEED_REVERSE); @@ -131,14 +152,14 @@ void Feeder::assignOutputs() if (state.feederState == FeederState::FEEDER_DISABLE) { motor_intake.Set(0); motor_stage.Set(0); - fixme motor_rotateRight.Set(0);// set rotate down + motor_rotateMain.Set(0);// set rotate down fixme } else if (state.feederState == FeederState::FEEDER_SHOOT) { motor_intake.Set(state.intakeForwardSpeed); motor_stage.Set(state.feederForwardSpeedShoot); } else if (state.feederState == FeederState::FEEDER_RETRACT){ - fixme // set rotation to be up; + motor_rotateMain.Set(0); // set rotation to be up fixme } else if (state.feederState == Feeder::FEEDER_REVERSE) { motor_intake.Set(state.intakeReverseSpeed); diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 4497993..c76d977 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -227,7 +227,7 @@ namespace ShooterConstants{ constexpr static double turretLimitRight = 0 - 7; constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500 - constexpr static double turretRotateIntakeThreshold = 0; fixme // set to intake rotate up threshold + constexpr static double turretRotateIntakeThreshold = 12000; // fixme set to intake rotate up threshold constexpr static double hubX = 0; constexpr static double hubY = 0; @@ -242,6 +242,9 @@ namespace FeederConstants{ constexpr static int MOTOR_INTAKE_CAN_ID = 9; //PDH slot 15 constexpr static int MOTOR_STAGE_CAN_ID = 10; + constexpr static int MOTOR_ROTATE_MAIN_CAN_ID = 20; + constexpr static int MOTOR_ROTATE_FOLLOW_CAN_ID = 21; + constexpr static int BANNER_DIO_PORT = 5; constexpr static double DEFAULT_INTAKE_SPEED_FORWARD = 0.7; diff --git a/Competition/src/main/include/subsystems/Feeder.h b/Competition/src/main/include/subsystems/Feeder.h index cf69d61..00e4f58 100644 --- a/Competition/src/main/include/subsystems/Feeder.h +++ b/Competition/src/main/include/subsystems/Feeder.h @@ -66,8 +66,8 @@ class Feeder : public ValorSubsystem WPI_TalonFX motor_intake; WPI_TalonFX motor_stage; - WPI_TalonFX motor_rotateRight; - WPI_TalonFX motor_rotateLeft; + WPI_TalonFX motor_rotateMain; + WPI_TalonFX motor_rotateFollow; //fixme // create motor group if needed From 62a417aae9002baa9c686e73c9420370b20dcebd Mon Sep 17 00:00:00 2001 From: CooperNelson16 Date: Wed, 14 Sep 2022 12:07:19 -0500 Subject: [PATCH 43/62] intake rotation ready to test intake retract --- .../src/main/cpp/subsystems/Feeder.cpp | 34 +++++++++---------- Competition/src/main/include/Constants.h | 21 +++++++++++- .../src/main/include/subsystems/Feeder.h | 6 +++- 3 files changed, 42 insertions(+), 19 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 72bd532..f085336 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -42,20 +42,18 @@ void Feeder::init() motor_stage.EnableVoltageCompensation(true); motor_stage.ConfigVoltageCompSaturation(10); - // motor_rotateMain.SetInverted(false); - // motor_rotateMain.SetNeutralMode(Brake); + - // motor_rotateFollow.SetInverted(true); - // motor_rotateFollow.SetNeutralMode(Brake); - // motor_rotateFollow.Follow(motor_rotateMain); - motor_rotateMain.ConfigForwardSoftLimitThreshold(LiftConstants::extendForwardLimit); - motor_rotateMain.ConfigReverseSoftLimitThreshold(LiftConstants::extendReverseLimit); + + motor_rotateMain.ConfigForwardSoftLimitThreshold(FeederConstants::rotateForwardLimit); + motor_rotateMain.ConfigReverseSoftLimitThreshold(FeederConstants::rotateReverseLimit); motor_rotateMain.ConfigForwardSoftLimitEnable(true); motor_rotateMain.ConfigReverseSoftLimitEnable(true); motor_rotateMain.SetSelectedSensorPosition(0); - motor_rotateMain.SetInverted(true); + motor_rotateMain.SetInverted(false); + motor_rotateFollow.SetInverted(true); motor_rotateFollow.Follow(motor_rotateMain); motor_rotateMain.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); @@ -64,12 +62,13 @@ void Feeder::init() motor_rotateMain.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); motor_rotateMain.ConfigAllowableClosedloopError(0, 0); motor_rotateMain.Config_IntegralZone(0, 0); - motor_rotateMain.Config_kF(0, LiftConstants::main_KF); - motor_rotateMain.Config_kD(0, LiftConstants::main_KD); - motor_rotateMain.Config_kI(0, LiftConstants::main_KI); - motor_rotateMain.Config_kP(0, LiftConstants::main_KP); - motor_rotateMain.ConfigMotionAcceleration(LiftConstants::MAIN_MOTION_ACCELERATION); - motor_rotateMain.ConfigMotionCruiseVelocity(LiftConstants::MAIN_MOTION_CRUISE_VELOCITY); + + motor_rotateMain.Config_kF(0, FeederConstants::main_KF); + motor_rotateMain.Config_kD(0, FeederConstants::main_KD); + motor_rotateMain.Config_kI(0, FeederConstants::main_KI); + motor_rotateMain.Config_kP(0, FeederConstants::main_KP); + motor_rotateMain.ConfigMotionAcceleration(FeederConstants::MAIN_MOTION_ACCELERATION); + motor_rotateMain.ConfigMotionCruiseVelocity(FeederConstants::MAIN_MOTION_CRUISE_VELOCITY); table->PutBoolean("Reverse Feeder?", false); table->PutNumber("Intake Reverse Speed", FeederConstants::DEFAULT_INTAKE_SPEED_REVERSE); @@ -112,7 +111,7 @@ void Feeder::assessInputs() resetIntakeSensor(); } else if (driverController->GetBButton()) { - state.feederState = FeederState::FEEDER_RETRACT; //Set Intake rotate target to upper setpoint + state.feederState = FeederState::FEEDER_RETRACT; //Set Intake rotate to upper setpoint } else if (driverController->GetLeftBumper()) { state.feederState = FeederState::FEEDER_REVERSE; @@ -144,6 +143,7 @@ void Feeder::analyzeDashboard() table->PutBoolean("Spiked: ", currentSensor.spiked()); table->PutBoolean("Banner: ", debounceSensor.getSensor()); table->PutNumber("current feeder state", state.feederState); + } void Feeder::assignOutputs() @@ -152,14 +152,14 @@ void Feeder::assignOutputs() if (state.feederState == FeederState::FEEDER_DISABLE) { motor_intake.Set(0); motor_stage.Set(0); - motor_rotateMain.Set(0);// set rotate down fixme + motor_rotateMain.Set(ControlMode::MotionMagic, FeederConstants::rotateForwardLimit); } else if (state.feederState == FeederState::FEEDER_SHOOT) { motor_intake.Set(state.intakeForwardSpeed); motor_stage.Set(state.feederForwardSpeedShoot); } else if (state.feederState == FeederState::FEEDER_RETRACT){ - motor_rotateMain.Set(0); // set rotation to be up fixme + motor_rotateMain.Set(ControlMode::MotionMagic, FeederConstants::rotateReverseLimit); // set rotation to be up fixme } else if (state.feederState == Feeder::FEEDER_REVERSE) { motor_intake.Set(state.intakeReverseSpeed); diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index c76d977..a911d3d 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -227,7 +227,7 @@ namespace ShooterConstants{ constexpr static double turretLimitRight = 0 - 7; constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500 - constexpr static double turretRotateIntakeThreshold = 12000; // fixme set to intake rotate up threshold + constexpr static double turretRotateIntakeThreshold = 2; // fixme set to intake rotate up threshold constexpr static double hubX = 0; constexpr static double hubY = 0; @@ -257,6 +257,25 @@ namespace FeederConstants{ constexpr static int CACHE_SIZE = 20; constexpr static double JAM_CURRENT = 22; + + constexpr static double rotateForwardLimit = 10; + constexpr static double rotateReverseLimit = 1; + + constexpr static double rotateGearRatio = 1 / 29.16667; //fixme + + + + //change to intake numbers fixme + constexpr static double main_KF = 0.05; + constexpr static double main_KD = 0.0; + constexpr static double main_KI = 0.0; + constexpr static double main_KP = 0.1; + + //change to intake numbers fixme + constexpr static double MAIN_MOTION_CRUISE_VELOCITY = 15000; + constexpr static double MAIN_MOTION_ACCELERATION = MAIN_MOTION_CRUISE_VELOCITY * 7; + + } diff --git a/Competition/src/main/include/subsystems/Feeder.h b/Competition/src/main/include/subsystems/Feeder.h index 00e4f58..32074e5 100644 --- a/Competition/src/main/include/subsystems/Feeder.h +++ b/Competition/src/main/include/subsystems/Feeder.h @@ -17,6 +17,9 @@ #include "sensors/ValorCurrentSensor.h" #include "sensors/ValorDebounceSensor.h" +#include + + #ifndef FEEDER_H #define FEEDER_H @@ -55,11 +58,13 @@ class Feeder : public ValorSubsystem double feederForwardSpeedShoot; double feederReverseSpeed; + FeederState feederState; } state; void resetIntakeSensor(); + private: ValorGamepad *driverController; ValorGamepad *operatorController; @@ -69,7 +74,6 @@ class Feeder : public ValorSubsystem WPI_TalonFX motor_rotateMain; WPI_TalonFX motor_rotateFollow; - //fixme // create motor group if needed From f26bab3b4233cb9051751257dd86f965d71b6428 Mon Sep 17 00:00:00 2001 From: CooperNelson16 Date: Wed, 14 Sep 2022 16:36:17 -0500 Subject: [PATCH 44/62] ready to test with values inserted intake retraction --- Competition/src/main/cpp/subsystems/Feeder.cpp | 6 +++--- Competition/src/main/include/Constants.h | 14 ++++++-------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index f085336..e91e707 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -52,8 +52,8 @@ void Feeder::init() motor_rotateMain.ConfigReverseSoftLimitEnable(true); motor_rotateMain.SetSelectedSensorPosition(0); - motor_rotateMain.SetInverted(false); - motor_rotateFollow.SetInverted(true); + motor_rotateMain.SetInverted(false); // needs to be tested + motor_rotateFollow.SetInverted(true); // needs to be tested motor_rotateFollow.Follow(motor_rotateMain); motor_rotateMain.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); @@ -159,7 +159,7 @@ void Feeder::assignOutputs() motor_stage.Set(state.feederForwardSpeedShoot); } else if (state.feederState == FeederState::FEEDER_RETRACT){ - motor_rotateMain.Set(ControlMode::MotionMagic, FeederConstants::rotateReverseLimit); // set rotation to be up fixme + motor_rotateMain.Set(ControlMode::MotionMagic, FeederConstants::rotateReverseLimit); // set rotation to be up } else if (state.feederState == Feeder::FEEDER_REVERSE) { motor_intake.Set(state.intakeReverseSpeed); diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index a911d3d..e6a57ff 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -227,7 +227,7 @@ namespace ShooterConstants{ constexpr static double turretLimitRight = 0 - 7; constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500 - constexpr static double turretRotateIntakeThreshold = 2; // fixme set to intake rotate up threshold + constexpr static double turretRotateIntakeThreshold = 829.63; //set to intake rotate up threshold constexpr static double hubX = 0; constexpr static double hubY = 0; @@ -257,23 +257,21 @@ namespace FeederConstants{ constexpr static int CACHE_SIZE = 20; constexpr static double JAM_CURRENT = 22; + constexpr static double tickToDegree = ((4200.0 /144.0) * 2048.0 ) / 360.0; - constexpr static double rotateForwardLimit = 10; - constexpr static double rotateReverseLimit = 1; + constexpr static double rotateForwardLimit = 10 * tickToDegree; + constexpr static double rotateReverseLimit = 0; - constexpr static double rotateGearRatio = 1 / 29.16667; //fixme - //change to intake numbers fixme constexpr static double main_KF = 0.05; constexpr static double main_KD = 0.0; constexpr static double main_KI = 0.0; constexpr static double main_KP = 0.1; - //change to intake numbers fixme - constexpr static double MAIN_MOTION_CRUISE_VELOCITY = 15000; - constexpr static double MAIN_MOTION_ACCELERATION = MAIN_MOTION_CRUISE_VELOCITY * 7; + constexpr static double MAIN_MOTION_CRUISE_VELOCITY = 50; + constexpr static double MAIN_MOTION_ACCELERATION = MAIN_MOTION_CRUISE_VELOCITY * 1; From fdfcc6e7541a921926a265bbaa4be1a445953731 Mon Sep 17 00:00:00 2001 From: CooperNelson16 Date: Wed, 14 Sep 2022 17:12:04 -0500 Subject: [PATCH 45/62] changed constants values for testing, remove turret rotate encoder limit and changed to half of forward rotate limit for intake --- Competition/src/main/cpp/subsystems/Shooter.cpp | 2 +- Competition/src/main/include/Constants.h | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index 45c3ed7..14ad4e4 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -169,7 +169,7 @@ void Shooter::assessInputs() } //Turret - if (feederTable->GetNumber("Intake Encoder Value", 0) > ShooterConstants::turretRotateIntakeThreshold) { + if (feederTable->GetNumber("Intake Encoder Value", 0) < FeederConstants::rotateForwardLimit / 2) { if (turretEncoder.GetPosition() > ShooterConstants::homePositionMid) { state.turretState = TurretState::TURRET_HOME_LEFT; } diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index e6a57ff..f1b1d22 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -227,7 +227,6 @@ namespace ShooterConstants{ constexpr static double turretLimitRight = 0 - 7; constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500 - constexpr static double turretRotateIntakeThreshold = 829.63; //set to intake rotate up threshold constexpr static double hubX = 0; constexpr static double hubY = 0; @@ -270,7 +269,7 @@ namespace FeederConstants{ constexpr static double main_KI = 0.0; constexpr static double main_KP = 0.1; - constexpr static double MAIN_MOTION_CRUISE_VELOCITY = 50; + constexpr static double MAIN_MOTION_CRUISE_VELOCITY = 4000; constexpr static double MAIN_MOTION_ACCELERATION = MAIN_MOTION_CRUISE_VELOCITY * 1; From 85bebfd8deb2b8fb95cbe83f3f60e620e912e2ea Mon Sep 17 00:00:00 2001 From: CooperNelson16 Date: Wed, 14 Sep 2022 18:20:46 -0500 Subject: [PATCH 46/62] set x driver button to clench, behavior for intake retract and deploy working, changed max speed values to usable values --- Competition/src/main/cpp/subsystems/Feeder.cpp | 12 +++++------- Competition/src/main/cpp/subsystems/Shooter.cpp | 3 ++- Competition/src/main/cpp/subsystems/ValorSwerve.cpp | 2 +- Competition/src/main/include/Constants.h | 9 +++------ 4 files changed, 11 insertions(+), 15 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index e91e707..cc7c19c 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -41,10 +41,7 @@ void Feeder::init() motor_stage.SetInverted(true); motor_stage.EnableVoltageCompensation(true); motor_stage.ConfigVoltageCompSaturation(10); - - - - + motor_rotateMain.ConfigForwardSoftLimitThreshold(FeederConstants::rotateForwardLimit); motor_rotateMain.ConfigReverseSoftLimitThreshold(FeederConstants::rotateReverseLimit); @@ -52,8 +49,8 @@ void Feeder::init() motor_rotateMain.ConfigReverseSoftLimitEnable(true); motor_rotateMain.SetSelectedSensorPosition(0); - motor_rotateMain.SetInverted(false); // needs to be tested - motor_rotateFollow.SetInverted(true); // needs to be tested + motor_rotateMain.SetInverted(true); // needs to be tested + motor_rotateFollow.SetInverted(false); // needs to be tested motor_rotateFollow.Follow(motor_rotateMain); motor_rotateMain.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); @@ -110,7 +107,7 @@ void Feeder::assessInputs() state.feederState = FeederState::FEEDER_SHOOT; //intake and feeder run resetIntakeSensor(); } - else if (driverController->GetBButton()) { + else if (driverController->GetXButton()) { state.feederState = FeederState::FEEDER_RETRACT; //Set Intake rotate to upper setpoint } else if (driverController->GetLeftBumper()) { @@ -144,6 +141,7 @@ void Feeder::analyzeDashboard() table->PutBoolean("Banner: ", debounceSensor.getSensor()); table->PutNumber("current feeder state", state.feederState); + table->PutNumber("Intake Encoder Value", motor_rotateMain.GetSelectedSensorPosition()); } void Feeder::assignOutputs() diff --git a/Competition/src/main/cpp/subsystems/Shooter.cpp b/Competition/src/main/cpp/subsystems/Shooter.cpp index 14ad4e4..ca80512 100644 --- a/Competition/src/main/cpp/subsystems/Shooter.cpp +++ b/Competition/src/main/cpp/subsystems/Shooter.cpp @@ -29,6 +29,7 @@ void Shooter::init() { limeTable = nt::NetworkTableInstance::GetDefault().GetTable("limelight"); liftTable = nt::NetworkTableInstance::GetDefault().GetTable("Lift"); + feederTable = nt::NetworkTableInstance::GetDefault().GetTable("Feeder"); initTable("Shooter"); table->PutBoolean("Zero Turret", false); @@ -174,7 +175,7 @@ void Shooter::assessInputs() state.turretState = TurretState::TURRET_HOME_LEFT; } else { - state.turretState == TurretState::TURRET_HOME_RIGHT; + state.turretState = TurretState::TURRET_HOME_RIGHT; } } else if (operatorController->leftStickXActive()) { diff --git a/Competition/src/main/cpp/subsystems/ValorSwerve.cpp b/Competition/src/main/cpp/subsystems/ValorSwerve.cpp index a5e04a9..edba2cb 100644 --- a/Competition/src/main/cpp/subsystems/ValorSwerve.cpp +++ b/Competition/src/main/cpp/subsystems/ValorSwerve.cpp @@ -116,7 +116,7 @@ void ValorSwerve::loadAndSetAzimuthZeroReference() //azimuthSetpoint = fmod(azimuthSetpoint, SwerveConstants::AZIMUTH_COUNTS_PER_REV / SwerveConstants::AZIMUTH_GEAR_RATIO); // Set the azimuth offset to the calculated setpoint (which will take over in teleop) - azimuthFalcon->SetSelectedSensorPosition(azimuthSetpoint, 0, 10); + azimuthFalcon->SetSelectedSensorPosition(0, 0, 10); //std::cout << "pulled pospition from file" << std::endl; } diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index f1b1d22..2ff0e10 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -258,19 +258,16 @@ namespace FeederConstants{ constexpr static double tickToDegree = ((4200.0 /144.0) * 2048.0 ) / 360.0; - constexpr static double rotateForwardLimit = 10 * tickToDegree; + constexpr static double rotateForwardLimit = 20 * tickToDegree; constexpr static double rotateReverseLimit = 0; - - - constexpr static double main_KF = 0.05; constexpr static double main_KD = 0.0; constexpr static double main_KI = 0.0; constexpr static double main_KP = 0.1; - constexpr static double MAIN_MOTION_CRUISE_VELOCITY = 4000; - constexpr static double MAIN_MOTION_ACCELERATION = MAIN_MOTION_CRUISE_VELOCITY * 1; + constexpr static double MAIN_MOTION_CRUISE_VELOCITY = 15000; + constexpr static double MAIN_MOTION_ACCELERATION = MAIN_MOTION_CRUISE_VELOCITY * 5; From 85d2c576a294e690c2544fda1a40d12a27e8c75c Mon Sep 17 00:00:00 2001 From: CooperNelson16 Date: Wed, 14 Sep 2022 19:54:42 -0500 Subject: [PATCH 47/62] changed shooter LOBF, changed resting forward intake point to 4 degrees and forward limit to 27, added voltage compensation --- Competition/src/main/cpp/subsystems/Feeder.cpp | 10 +++++++++- Competition/src/main/include/Constants.h | 15 ++++++++------- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index cc7c19c..8409315 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -48,6 +48,14 @@ void Feeder::init() motor_rotateMain.ConfigForwardSoftLimitEnable(true); motor_rotateMain.ConfigReverseSoftLimitEnable(true); + motor_rotateMain.EnableVoltageCompensation(true); + motor_rotateMain.ConfigVoltageCompSaturation(10); + motor_rotateMain.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 25, 60, 1)); //potentially could do 40 60 + + motor_rotateFollow.EnableVoltageCompensation(true); + motor_rotateFollow.ConfigVoltageCompSaturation(10); + motor_rotateFollow.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 25, 60, 1)); //potentially could do 40 60 + motor_rotateMain.SetSelectedSensorPosition(0); motor_rotateMain.SetInverted(true); // needs to be tested motor_rotateFollow.SetInverted(false); // needs to be tested @@ -157,7 +165,7 @@ void Feeder::assignOutputs() motor_stage.Set(state.feederForwardSpeedShoot); } else if (state.feederState == FeederState::FEEDER_RETRACT){ - motor_rotateMain.Set(ControlMode::MotionMagic, FeederConstants::rotateReverseLimit); // set rotation to be up + motor_rotateMain.Set(ControlMode::MotionMagic, FeederConstants::rotateUpSetPoint); // set rotation to be up } else if (state.feederState == Feeder::FEEDER_REVERSE) { motor_intake.Set(state.intakeReverseSpeed); diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 2ff0e10..f13fcb9 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -126,12 +126,12 @@ namespace ShooterConstants{ // constexpr static double cHood_1x = .993; //current power - constexpr static double aPower_1x = 0.0553; - constexpr static double bPower_1x = -0.101; - constexpr static double cPower_1x = 0.471; - constexpr static double aHood_1x = 6.29; - constexpr static double bHood_1x = -7.48; - constexpr static double cHood_1x = .993; + constexpr static double aPower_1x = 0.0708; + constexpr static double bPower_1x = -0.188; + constexpr static double cPower_1x = 0.56; + constexpr static double aHood_1x = 9.63; + constexpr static double bHood_1x = -16.2; + constexpr static double cHood_1x = 6.16; constexpr static double aPower_2x = 0.165; constexpr static double bPower_2x = -0.432; @@ -258,8 +258,9 @@ namespace FeederConstants{ constexpr static double tickToDegree = ((4200.0 /144.0) * 2048.0 ) / 360.0; - constexpr static double rotateForwardLimit = 20 * tickToDegree; + constexpr static double rotateForwardLimit = 27 * tickToDegree; constexpr static double rotateReverseLimit = 0; + constexpr static double rotateUpSetPoint = 4 * tickToDegree; constexpr static double main_KF = 0.05; constexpr static double main_KD = 0.0; From 9ab5f299cdc0c8e6bb502f19d028a65971ffdfee Mon Sep 17 00:00:00 2001 From: CooperNelson16 Date: Fri, 16 Sep 2022 16:27:44 -0500 Subject: [PATCH 48/62] changed forward intake retract point to 1 degree --- Competition/src/main/include/Constants.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index f13fcb9..a2c3f33 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -260,7 +260,7 @@ namespace FeederConstants{ constexpr static double rotateForwardLimit = 27 * tickToDegree; constexpr static double rotateReverseLimit = 0; - constexpr static double rotateUpSetPoint = 4 * tickToDegree; + constexpr static double rotateUpSetPoint = 1 * tickToDegree; constexpr static double main_KF = 0.05; constexpr static double main_KD = 0.0; From 9b31a3ca16c46394954bddff2d5f07106be530de Mon Sep 17 00:00:00 2001 From: j a Date: Mon, 29 Aug 2022 21:36:20 -0500 Subject: [PATCH 49/62] test quintic holonomic auto. needs testing --- Competition/src/main/cpp/ValorAuto.cpp | 30 +++++++++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index a145360..417d37c 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -402,7 +402,18 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder {}, trenchEndBlue, config); - + + std::vector points; + points.push_back(frc::Pose2d{0_m, 0_m, frc::Rotation2d(0_deg)}); + points.push_back(frc::Pose2d{2_m, 0_m, frc::Rotation2d(90_deg)}); + points.push_back(frc::Pose2d{2_m, 2_m, frc::Rotation2d(180_deg)}); + points.push_back(frc::Pose2d{0_m, 2_m, frc::Rotation2d(270_deg)}); + points.push_back(frc::Pose2d{0_m, 0_m, frc::Rotation2d(0_deg)}); + + auto testHolonomic = frc::TrajectoryGenerator::GenerateTrajectory( + points, + config); + frc2::SwerveControllerCommand<4> cmd_move_moveBugsRed( moveBugsRed, [&] () { return drivetrain->getPose_m(); }, @@ -865,6 +876,17 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); + + frc2::SwerveControllerCommand<4> cmd_testHolonomic( + testHolonomic, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); frc2::SequentialCommandGroup *shoot3Red = new frc2::SequentialCommandGroup(); @@ -1220,6 +1242,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_shooterAuto ); + frc2::SequentialCommandGroup *testHolonomicSquare = new frc2::SequentialCommandGroup(); + testHolonomicSquare->AddCommands(cmd_testHolonomic); + m_chooser.AddOption("RED 2 ball", shoot2Red); m_chooser.AddOption("Blue 2 ball", shoot2Blue); @@ -1242,6 +1267,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder m_chooser.SetDefaultOption("RED 2 ball", shoot2Red); + + m_chooser.AddOption("Test Holonomic", testHolonomicSquare); + frc::SmartDashboard::PutData(&m_chooser); } From d61c155693675ee76f84e37384c10b442b40e000 Mon Sep 17 00:00:00 2001 From: j a Date: Wed, 7 Sep 2022 18:01:09 -0500 Subject: [PATCH 50/62] working 2ball holonomic. May hit pillar --- Competition/src/main/cpp/ValorAuto.cpp | 93 ++++++++++++++++++++++++-- 1 file changed, 87 insertions(+), 6 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 417d37c..23d26e0 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -63,6 +63,12 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d marvinRed = frc::Pose2d(units::meter_t(4), units::meter_t(6.9), frc::Rotation2d(137_deg)); frc::Pose2d marvinBlue = frc::Pose2d(units::meter_t(4), units::meter_t(6.9), frc::Rotation2d(137_deg)); + frc::Pose2d postPreMarvinRed = frc::Pose2d(units::meter_t(4), units::meter_t(6.9), frc::Rotation2d(27_deg)); + frc::Pose2d postPreMarvinBlue = frc::Pose2d(units::meter_t(4), units::meter_t(6.9), frc::Rotation2d(27_deg)); + + frc::Pose2d preMarvinRed = frc::Pose2d(units::meter_t(2.5), units::meter_t(5.45), frc::Rotation2d(27_deg)); + frc::Pose2d preMarvinBlue = frc::Pose2d(units::meter_t(2.5), units::meter_t(5.45), frc::Rotation2d(27_deg)); + frc::Pose2d marvinShootRed = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); frc::Pose2d marvinShootBlue = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); @@ -88,7 +94,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d trenchEndBlue = frc::Pose2d(3.75_m, 3_m, frc::Rotation2d(135_deg)); frc::Pose2d endPose2BallRed = frc::Pose2d(7.069_m, 7.3_m, frc::Rotation2d(60_deg)); - frc::Pose2d endPose2BallBlue = frc::Pose2d(7.069_m, 7.3_m, frc::Rotation2d(60_deg)); + frc::Pose2d endPose2BallBlue = frc::Pose2d(7.069_m, 7.3_m, frc::Rotation2d(60_deg)); frc2::InstantCommand cmd_printHeading = frc2::InstantCommand( [&] { @@ -155,6 +161,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::InstantCommand cmd_set2ballOdometryBlue = frc2::InstantCommand( [&] { drivetrain->resetOdometry(startPose2ballBlue); }); + frc2::InstantCommand cmd_setOdometryZero = frc2::InstantCommand( [&] { + drivetrain->resetOdometry(frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg))); + }); auto moveBugsRed = frc::TrajectoryGenerator::GenerateTrajectory( startPoseRed, @@ -238,6 +247,24 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder {}, marvinRed, config); + + std::vector preMarvinPoints; + preMarvinPoints.push_back(startPose2ballRed); + preMarvinPoints.push_back(preMarvinRed); + preMarvinPoints.push_back(marvinShootRed);// + + auto movePreMarvinRed = frc::TrajectoryGenerator::GenerateTrajectory( + preMarvinPoints, + config); + + std::vector preMarvintoMarvinPoints; + preMarvinPoints.push_back(preMarvinRed); + preMarvinPoints.push_back(marvinShootRed); + + auto movePreMarvintoMarvinRed = frc::TrajectoryGenerator::GenerateTrajectory( + preMarvinPoints, + config); + auto moveMarvinBlue = frc::TrajectoryGenerator::GenerateTrajectory( startPose2ballBlue, {}, @@ -405,10 +432,11 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder std::vector points; points.push_back(frc::Pose2d{0_m, 0_m, frc::Rotation2d(0_deg)}); - points.push_back(frc::Pose2d{2_m, 0_m, frc::Rotation2d(90_deg)}); - points.push_back(frc::Pose2d{2_m, 2_m, frc::Rotation2d(180_deg)}); - points.push_back(frc::Pose2d{0_m, 2_m, frc::Rotation2d(270_deg)}); - points.push_back(frc::Pose2d{0_m, 0_m, frc::Rotation2d(0_deg)}); + points.push_back(frc::Pose2d{4_m, 0_m, frc::Rotation2d(0_deg)}); + points.push_back(frc::Pose2d{4_m, 4_m, frc::Rotation2d(0_deg)}); + points.push_back(frc::Pose2d{2_m, 4_m, frc::Rotation2d(90_deg)}); + points.push_back(frc::Pose2d{0_m, 4_m, frc::Rotation2d(180_deg)}); + //points.push_back(frc::Pose2d{0_m, 0_m, frc::Rotation2d(0_deg)}); auto testHolonomic = frc::TrajectoryGenerator::GenerateTrajectory( points, @@ -571,6 +599,29 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); + + frc2::SwerveControllerCommand<4> cmd_movePreMarvinRed( + movePreMarvinRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + + frc2::SwerveControllerCommand<4> cmd_movePreMarvintoMarvinRed( + movePreMarvintoMarvinRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveMarvinBlue( moveMarvinBlue, [&] () { return drivetrain->getPose_m(); }, @@ -1035,6 +1086,30 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeDisable ); + frc2::SequentialCommandGroup *shoot2RedNew = new frc2::SequentialCommandGroup(); + shoot2RedNew->AddCommands + (cmd_set2ballOdometryRed, + //cmd_intakeOne, + cmd_intakeClearDeque, + cmd_nextBall, + cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + cmd_shooterAuto, + cmd_movePreMarvinRed, + //cmd_movePreMarvintoMarvinRed, + cmd_intakeDisable, + //cmd_move_moveMarvinShootRed, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)0.75), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)0.5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)0.75), + cmd_intakeDisable + ); + frc2::SequentialCommandGroup *shoot2Blue = new frc2::SequentialCommandGroup(); shoot2Blue->AddCommands (cmd_set2ballOdometryBlue, @@ -1243,7 +1318,10 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder ); frc2::SequentialCommandGroup *testHolonomicSquare = new frc2::SequentialCommandGroup(); - testHolonomicSquare->AddCommands(cmd_testHolonomic); + testHolonomicSquare->AddCommands( + cmd_setOdometryZero, + cmd_testHolonomic + ); m_chooser.AddOption("RED 2 ball", shoot2Red); m_chooser.AddOption("Blue 2 ball", shoot2Blue); @@ -1265,6 +1343,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder m_chooser.AddOption("BLUE 3 ball auto", shoot3Blue); m_chooser.AddOption("BLUE 5 ball auto", shoot5Blue); + + m_chooser.AddOption("RED 2 ball new", shoot2RedNew); + m_chooser.SetDefaultOption("RED 2 ball", shoot2Red); From 1d739a7cdc9966c08d50a9e27b1c7e24f395f703 Mon Sep 17 00:00:00 2001 From: j a Date: Wed, 7 Sep 2022 18:31:17 -0500 Subject: [PATCH 51/62] 3 ball chezy --- Competition/src/main/cpp/ValorAuto.cpp | 62 +++++++++++++++++--------- 1 file changed, 41 insertions(+), 21 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 23d26e0..7d48a12 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -66,8 +66,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d postPreMarvinRed = frc::Pose2d(units::meter_t(4), units::meter_t(6.9), frc::Rotation2d(27_deg)); frc::Pose2d postPreMarvinBlue = frc::Pose2d(units::meter_t(4), units::meter_t(6.9), frc::Rotation2d(27_deg)); - frc::Pose2d preMarvinRed = frc::Pose2d(units::meter_t(2.5), units::meter_t(5.45), frc::Rotation2d(27_deg)); - frc::Pose2d preMarvinBlue = frc::Pose2d(units::meter_t(2.5), units::meter_t(5.45), frc::Rotation2d(27_deg)); + frc::Pose2d preMarvinRed = frc::Pose2d(units::meter_t(3.5), units::meter_t(5.9), frc::Rotation2d(27_deg)); + frc::Pose2d preMarvinBlue = frc::Pose2d(units::meter_t(3.5), units::meter_t(5.9), frc::Rotation2d(27_deg)); frc::Pose2d marvinShootRed = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); frc::Pose2d marvinShootBlue = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); @@ -248,21 +248,22 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder marvinRed, config); - std::vector preMarvinPoints; - preMarvinPoints.push_back(startPose2ballRed); - preMarvinPoints.push_back(preMarvinRed); - preMarvinPoints.push_back(marvinShootRed);// + std::vector preMarvinPointsRed; + preMarvinPointsRed.push_back(startPose2ballRed); + preMarvinPointsRed.push_back(preMarvinRed); + preMarvinPointsRed.push_back(marvinShootRed); + + std::vector preMarvinPointsBlue; + preMarvinPointsBlue.push_back(startPose2ballBlue); + preMarvinPointsBlue.push_back(preMarvinBlue); + preMarvinPointsBlue.push_back(marvinShootBlue); auto movePreMarvinRed = frc::TrajectoryGenerator::GenerateTrajectory( - preMarvinPoints, + preMarvinPointsRed, config); - std::vector preMarvintoMarvinPoints; - preMarvinPoints.push_back(preMarvinRed); - preMarvinPoints.push_back(marvinShootRed); - - auto movePreMarvintoMarvinRed = frc::TrajectoryGenerator::GenerateTrajectory( - preMarvinPoints, + auto movePreMarvinBlue = frc::TrajectoryGenerator::GenerateTrajectory( + preMarvinPointsBlue, config); auto moveMarvinBlue = frc::TrajectoryGenerator::GenerateTrajectory( @@ -611,8 +612,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder {drivetrain} ); - frc2::SwerveControllerCommand<4> cmd_movePreMarvintoMarvinRed( - movePreMarvintoMarvinRed, + frc2::SwerveControllerCommand<4> cmd_movePreMarvinBlue( + movePreMarvinBlue, [&] () { return drivetrain->getPose_m(); }, drivetrain->getKinematics(), frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), @@ -1086,19 +1087,37 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeDisable ); - frc2::SequentialCommandGroup *shoot2RedNew = new frc2::SequentialCommandGroup(); - shoot2RedNew->AddCommands + frc2::SequentialCommandGroup *shoot3ChezRed = new frc2::SequentialCommandGroup(); + shoot3ChezRed->AddCommands (cmd_set2ballOdometryRed, - //cmd_intakeOne, cmd_intakeClearDeque, cmd_nextBall, cmd_intakeAuto, frc2::WaitCommand((units::second_t).2), cmd_shooterAuto, cmd_movePreMarvinRed, - //cmd_movePreMarvintoMarvinRed, cmd_intakeDisable, - //cmd_move_moveMarvinShootRed, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)0.75), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)0.5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)0.75), + cmd_intakeDisable + ); + + frc2::SequentialCommandGroup *shoot3ChezBlue = new frc2::SequentialCommandGroup(); + shoot3ChezBlue->AddCommands + (cmd_set2ballOdometryRed, + cmd_intakeClearDeque, + cmd_nextBall, + cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + cmd_shooterAuto, + cmd_movePreMarvinBlue, + cmd_intakeDisable, cmd_turretTrack, frc2::WaitCommand((units::second_t).5), cmd_intakeShoot, @@ -1344,7 +1363,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder m_chooser.AddOption("BLUE 5 ball auto", shoot5Blue); - m_chooser.AddOption("RED 2 ball new", shoot2RedNew); + m_chooser.AddOption("RED 3 ball Chez", shoot3ChezRed); + m_chooser.AddOption("BLUE 3 ball Chez", shoot3ChezBlue); m_chooser.SetDefaultOption("RED 2 ball", shoot2Red); From 4fd2f6eb8ea75d7bb8dbc4eea82d33639a244383 Mon Sep 17 00:00:00 2001 From: j a Date: Mon, 12 Sep 2022 18:13:47 -0500 Subject: [PATCH 52/62] Holonomic testing --- Competition/src/main/cpp/ValorAuto.cpp | 82 +++++++++++++++++++++++++- 1 file changed, 80 insertions(+), 2 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 7d48a12..cb06857 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -66,8 +66,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d postPreMarvinRed = frc::Pose2d(units::meter_t(4), units::meter_t(6.9), frc::Rotation2d(27_deg)); frc::Pose2d postPreMarvinBlue = frc::Pose2d(units::meter_t(4), units::meter_t(6.9), frc::Rotation2d(27_deg)); - frc::Pose2d preMarvinRed = frc::Pose2d(units::meter_t(3.5), units::meter_t(5.9), frc::Rotation2d(27_deg)); - frc::Pose2d preMarvinBlue = frc::Pose2d(units::meter_t(3.5), units::meter_t(5.9), frc::Rotation2d(27_deg)); + frc::Pose2d preMarvinRed = frc::Pose2d(units::meter_t(2.75), units::meter_t(5.9), frc::Rotation2d(55_deg)); + frc::Pose2d preMarvinBlue = frc::Pose2d(units::meter_t(2.75), units::meter_t(5.9), frc::Rotation2d(55_deg)); frc::Pose2d marvinShootRed = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); frc::Pose2d marvinShootBlue = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); @@ -78,6 +78,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d tasRed = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.9), frc::Rotation2d(55_deg)); frc::Pose2d tasBlue = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.9), frc::Rotation2d(55_deg)); + frc::Pose2d tasPMarvinRed = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.9), frc::Rotation2d(35_deg)); + frc::Pose2d tasPMarvinBlue = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.9), frc::Rotation2d(35_deg)); + frc::Translation2d tasToSpeedyConstrainRed = frc::Translation2d(5_m, 7.5_m); //1.2_m in case we need to push it more towards wall frc::Translation2d tasToSpeedyConstrainBlue = frc::Translation2d(5_m, 7.5_m); //1.2_m in case we need to push it more towards wall @@ -266,6 +269,23 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder preMarvinPointsBlue, config); + std::vector PMarvinTazPointsRed; + PMarvinTazPointsRed.push_back(marvinShootRed); + PMarvinTazPointsRed.push_back(tasPMarvinRed); + + auto movePMarvinTasRed = frc::TrajectoryGenerator::GenerateTrajectory( + PMarvinTazPointsRed, + config); + + std::vector TazHangarPointsRed; + TazHangarPointsRed.push_back(tasPMarvinRed); + TazHangarPointsRed.push_back(hangarSpotRed); + + auto moveTasHangarRed = frc::TrajectoryGenerator::GenerateTrajectory( + TazHangarPointsRed, + reverseConfig); + + auto moveMarvinBlue = frc::TrajectoryGenerator::GenerateTrajectory( startPose2ballBlue, {}, @@ -623,6 +643,28 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder {drivetrain} ); + frc2::SwerveControllerCommand<4> cmd_movePMarvinTasRed( + movePMarvinTasRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + + frc2::SwerveControllerCommand<4> cmd_moveTasHangarRed( + moveTasHangarRed, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveMarvinBlue( moveMarvinBlue, [&] () { return drivetrain->getPose_m(); }, @@ -1129,6 +1171,40 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeDisable ); + frc2::SequentialCommandGroup *shoot3pick1ChezRed = new frc2::SequentialCommandGroup(); + shoot3pick1ChezRed->AddCommands + (cmd_set2ballOdometryRed, + cmd_intakeClearDeque, + cmd_nextBall, + cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + cmd_shooterAuto, + cmd_movePreMarvinRed, + cmd_intakeDisable, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)0.5), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)0.5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)0.5), + cmd_intakeAuto, + cmd_movePMarvinTasRed, + cmd_moveTasHangarRed, + cmd_intakeReverse, + frc2::WaitCommand((units::second_t).5), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)1.8), + cmd_intakeAuto, + cmd_move_moveEndFromTrenchBlue, + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).125), + cmd_intakeShoot, + cmd_shooterAuto + ); + frc2::SequentialCommandGroup *shoot2Blue = new frc2::SequentialCommandGroup(); shoot2Blue->AddCommands (cmd_set2ballOdometryBlue, @@ -1366,6 +1442,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder m_chooser.AddOption("RED 3 ball Chez", shoot3ChezRed); m_chooser.AddOption("BLUE 3 ball Chez", shoot3ChezBlue); + m_chooser.AddOption("RED 3 ball + Pick 1 Chez", shoot3pick1ChezRed); + m_chooser.SetDefaultOption("RED 2 ball", shoot2Red); From c494256bbf1aa1932ef794accb448712e4feee01 Mon Sep 17 00:00:00 2001 From: j a Date: Mon, 12 Sep 2022 19:23:55 -0500 Subject: [PATCH 53/62] hard coded one motor to be not inverted --- Competition/src/main/cpp/subsystems/Drivetrain.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Competition/src/main/cpp/subsystems/Drivetrain.cpp b/Competition/src/main/cpp/subsystems/Drivetrain.cpp index 9c3889a..baf5d8e 100644 --- a/Competition/src/main/cpp/subsystems/Drivetrain.cpp +++ b/Competition/src/main/cpp/subsystems/Drivetrain.cpp @@ -62,7 +62,11 @@ void Drivetrain::configSwerveModule(int i) driveMotors.push_back(new WPI_TalonFX(DriveConstants::DRIVE_CANS[i], "baseCAN")); driveMotors[i]->ConfigFactoryDefault(); + if (i == 0) { + driveMotors[i]->SetInverted(true); + } else { driveMotors[i]->SetInverted(false); + } driveMotors[i]->ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); driveMotors[i]->SetNeutralMode(NeutralMode::Coast); driveMotors[i]->ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60, 80, .75)); From 4bbb7f8324239bd7dc6be1427f4ae2c63a0dfc04 Mon Sep 17 00:00:00 2001 From: CooperNelson16 Date: Fri, 16 Sep 2022 17:51:46 -0500 Subject: [PATCH 54/62] removed hardcode inversion on one swerve module --- Competition/src/main/cpp/subsystems/Drivetrain.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Drivetrain.cpp b/Competition/src/main/cpp/subsystems/Drivetrain.cpp index baf5d8e..81c9c31 100644 --- a/Competition/src/main/cpp/subsystems/Drivetrain.cpp +++ b/Competition/src/main/cpp/subsystems/Drivetrain.cpp @@ -62,11 +62,8 @@ void Drivetrain::configSwerveModule(int i) driveMotors.push_back(new WPI_TalonFX(DriveConstants::DRIVE_CANS[i], "baseCAN")); driveMotors[i]->ConfigFactoryDefault(); - if (i == 0) { - driveMotors[i]->SetInverted(true); - } else { driveMotors[i]->SetInverted(false); - } + driveMotors[i]->ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); driveMotors[i]->SetNeutralMode(NeutralMode::Coast); driveMotors[i]->ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60, 80, .75)); From 8ebf9f5f5dbe0b213b7e6ad968d4ab8f18f52cc4 Mon Sep 17 00:00:00 2001 From: j a Date: Mon, 19 Sep 2022 19:43:09 -0500 Subject: [PATCH 55/62] superPoop changed to 4 --- Competition/src/main/cpp/subsystems/TurretTracker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index c0fbd66..8c3a881 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -83,7 +83,7 @@ void TurretTracker::assignOutputs() { // Case structure for robot locations on the field double superPoopHeading = 90; if (wrappedExistingHeading <= 45) - superPoopHeading += 2 * 90.0 / 4; + superPoopHeading += 4 * 90.0 / 4; else if (wrappedExistingHeading > 45 && wrappedExistingHeading <= 135) superPoopHeading += 1 * 90.0 / 4; else if (wrappedExistingHeading > 135 && wrappedExistingHeading <= 225) From a83529772901dfbbeebda847568032753921d375 Mon Sep 17 00:00:00 2001 From: j a Date: Mon, 19 Sep 2022 19:44:11 -0500 Subject: [PATCH 56/62] rough working 3 ball + 1. need blue version --- Competition/src/main/cpp/ValorAuto.cpp | 16 +++++++--------- .../src/main/cpp/subsystems/ValorSwerve.cpp | 2 +- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index cb06857..fb3c983 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -78,8 +78,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d tasRed = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.9), frc::Rotation2d(55_deg)); frc::Pose2d tasBlue = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.9), frc::Rotation2d(55_deg)); - frc::Pose2d tasPMarvinRed = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.9), frc::Rotation2d(35_deg)); - frc::Pose2d tasPMarvinBlue = frc::Pose2d(units::meter_t(6.069), units::meter_t(7.9), frc::Rotation2d(35_deg)); + frc::Pose2d tasPMarvinRed = frc::Pose2d(units::meter_t(6.269), units::meter_t(8.1), frc::Rotation2d(35_deg)); + frc::Pose2d tasPMarvinBlue = frc::Pose2d(units::meter_t(6.269), units::meter_t(8.1), frc::Rotation2d(35_deg)); frc::Translation2d tasToSpeedyConstrainRed = frc::Translation2d(5_m, 7.5_m); //1.2_m in case we need to push it more towards wall frc::Translation2d tasToSpeedyConstrainBlue = frc::Translation2d(5_m, 7.5_m); //1.2_m in case we need to push it more towards wall @@ -96,8 +96,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d trenchEndRed = frc::Pose2d(3.75_m, 3_m, frc::Rotation2d(135_deg)); frc::Pose2d trenchEndBlue = frc::Pose2d(3.75_m, 3_m, frc::Rotation2d(135_deg)); - frc::Pose2d endPose2BallRed = frc::Pose2d(7.069_m, 7.3_m, frc::Rotation2d(60_deg)); - frc::Pose2d endPose2BallBlue = frc::Pose2d(7.069_m, 7.3_m, frc::Rotation2d(60_deg)); + frc::Pose2d endPose2BallRed = frc::Pose2d(7.069_m, 7.3_m, frc::Rotation2d(20_deg)); + frc::Pose2d endPose2BallBlue = frc::Pose2d(7.069_m, 7.3_m, frc::Rotation2d(20_deg)); frc2::InstantCommand cmd_printHeading = frc2::InstantCommand( [&] { @@ -1182,11 +1182,11 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_movePreMarvinRed, cmd_intakeDisable, cmd_turretTrack, - frc2::WaitCommand((units::second_t).5), + frc2::WaitCommand((units::second_t).1), cmd_intakeShoot, frc2::WaitCommand((units::second_t)0.5), cmd_intakeDisable, - frc2::WaitCommand((units::second_t)0.5), + frc2::WaitCommand((units::second_t)0.2), cmd_intakeShoot, frc2::WaitCommand((units::second_t)0.5), cmd_intakeAuto, @@ -1194,10 +1194,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_moveTasHangarRed, cmd_intakeReverse, frc2::WaitCommand((units::second_t).5), - cmd_intakeDisable, - frc2::WaitCommand((units::second_t)1.8), cmd_intakeAuto, - cmd_move_moveEndFromTrenchBlue, + cmd_move_moveEndFromTrenchRed, cmd_turretHomeRight, cmd_turretTrack, frc2::WaitCommand((units::second_t).125), diff --git a/Competition/src/main/cpp/subsystems/ValorSwerve.cpp b/Competition/src/main/cpp/subsystems/ValorSwerve.cpp index edba2cb..c268e69 100644 --- a/Competition/src/main/cpp/subsystems/ValorSwerve.cpp +++ b/Competition/src/main/cpp/subsystems/ValorSwerve.cpp @@ -116,7 +116,7 @@ void ValorSwerve::loadAndSetAzimuthZeroReference() //azimuthSetpoint = fmod(azimuthSetpoint, SwerveConstants::AZIMUTH_COUNTS_PER_REV / SwerveConstants::AZIMUTH_GEAR_RATIO); // Set the azimuth offset to the calculated setpoint (which will take over in teleop) - azimuthFalcon->SetSelectedSensorPosition(0, 0, 10); + azimuthFalcon->SetSelectedSensorPosition(0); //std::cout << "pulled pospition from file" << std::endl; } From 4b8c9502f56b0f5f0eabbf354e4fb866b7ea5696 Mon Sep 17 00:00:00 2001 From: j a Date: Mon, 19 Sep 2022 19:53:44 -0500 Subject: [PATCH 57/62] Blue version of 3ball pick 1 chezy --- Competition/src/main/cpp/ValorAuto.cpp | 71 ++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index fb3c983..13bf811 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -285,6 +285,22 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder TazHangarPointsRed, reverseConfig); + std::vector PMarvinTazPointsBlue; + PMarvinTazPointsBlue.push_back(marvinShootBlue); + PMarvinTazPointsBlue.push_back(tasPMarvinBlue); + + auto movePMarvinTasBlue = frc::TrajectoryGenerator::GenerateTrajectory( + PMarvinTazPointsBlue, + config); + + std::vector TazHangarPointsBlue; + TazHangarPointsBlue.push_back(tasPMarvinBlue); + TazHangarPointsBlue.push_back(hangarSpotBlue); + + auto moveTasHangarBlue = frc::TrajectoryGenerator::GenerateTrajectory( + TazHangarPointsBlue, + reverseConfig); + auto moveMarvinBlue = frc::TrajectoryGenerator::GenerateTrajectory( startPose2ballBlue, @@ -665,6 +681,28 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder {drivetrain} ); + frc2::SwerveControllerCommand<4> cmd_movePMarvinTasBlue( + movePMarvinTasBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + + frc2::SwerveControllerCommand<4> cmd_moveTasHangarBlue( + moveTasHangarBlue, + [&] () { return drivetrain->getPose_m(); }, + drivetrain->getKinematics(), + frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), + frc2::PIDController(DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY), + thetaController, + [this] (auto states) { drivetrain->setModuleStates(states); }, + {drivetrain} + ); + frc2::SwerveControllerCommand<4> cmd_move_moveMarvinBlue( moveMarvinBlue, [&] () { return drivetrain->getPose_m(); }, @@ -1203,6 +1241,38 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_shooterAuto ); + frc2::SequentialCommandGroup *shoot3pick1ChezBlue = new frc2::SequentialCommandGroup(); + shoot3pick1ChezBlue->AddCommands + (cmd_set2ballOdometryRed, + cmd_intakeClearDeque, + cmd_nextBall, + cmd_intakeAuto, + frc2::WaitCommand((units::second_t).2), + cmd_shooterAuto, + cmd_movePreMarvinBlue, + cmd_intakeDisable, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).1), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)0.5), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)0.2), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)0.5), + cmd_intakeAuto, + cmd_movePMarvinTasBlue, + cmd_moveTasHangarBlue, + cmd_intakeReverse, + frc2::WaitCommand((units::second_t).5), + cmd_intakeAuto, + cmd_move_moveEndFromTrenchBlue, + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).125), + cmd_intakeShoot, + cmd_shooterAuto + ); + frc2::SequentialCommandGroup *shoot2Blue = new frc2::SequentialCommandGroup(); shoot2Blue->AddCommands (cmd_set2ballOdometryBlue, @@ -1441,6 +1511,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder m_chooser.AddOption("BLUE 3 ball Chez", shoot3ChezBlue); m_chooser.AddOption("RED 3 ball + Pick 1 Chez", shoot3pick1ChezRed); + m_chooser.AddOption("BLUE 3 ball + Pick 1 Chez", shoot3pick1ChezBlue); m_chooser.SetDefaultOption("RED 2 ball", shoot2Red); From a42c44dc67a8fa3cf9d7cc06bf701e26709a980d Mon Sep 17 00:00:00 2001 From: j a Date: Sat, 24 Sep 2022 10:54:26 -0500 Subject: [PATCH 58/62] added more time to 3 ball hanger kickout --- Competition/src/main/cpp/ValorAuto.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 13bf811..9e4deaa 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -1231,7 +1231,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_movePMarvinTasRed, cmd_moveTasHangarRed, cmd_intakeReverse, - frc2::WaitCommand((units::second_t).5), + frc2::WaitCommand((units::second_t)1), cmd_intakeAuto, cmd_move_moveEndFromTrenchRed, cmd_turretHomeRight, @@ -1263,7 +1263,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_movePMarvinTasBlue, cmd_moveTasHangarBlue, cmd_intakeReverse, - frc2::WaitCommand((units::second_t).5), + frc2::WaitCommand((units::second_t)1), cmd_intakeAuto, cmd_move_moveEndFromTrenchBlue, cmd_turretHomeRight, From 09b0c0e8b60f531db4dd6904856fdd511e8d2221 Mon Sep 17 00:00:00 2001 From: j a Date: Sat, 24 Sep 2022 12:55:23 -0500 Subject: [PATCH 59/62] moved3ball shoot point towards hanger --- Competition/src/main/cpp/ValorAuto.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 9e4deaa..86e7756 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -69,8 +69,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d preMarvinRed = frc::Pose2d(units::meter_t(2.75), units::meter_t(5.9), frc::Rotation2d(55_deg)); frc::Pose2d preMarvinBlue = frc::Pose2d(units::meter_t(2.75), units::meter_t(5.9), frc::Rotation2d(55_deg)); - frc::Pose2d marvinShootRed = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); - frc::Pose2d marvinShootBlue = frc::Pose2d(units::meter_t(5), units::meter_t(7.5), frc::Rotation2d(27_deg)); + frc::Pose2d marvinShootRed = frc::Pose2d(units::meter_t(4.5), units::meter_t(7.2), frc::Rotation2d(27_deg)); + frc::Pose2d marvinShootBlue = frc::Pose2d(units::meter_t(4.5), units::meter_t(7.2), frc::Rotation2d(27_deg)); frc::Pose2d marvinShootAltRed = frc::Pose2d(units::meter_t(3.35), units::meter_t(5), frc::Rotation2d(-90_deg)); frc::Pose2d marvinShootAltBlue = frc::Pose2d(units::meter_t(3.35), units::meter_t(5), frc::Rotation2d(-90_deg)); @@ -1235,10 +1235,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeAuto, cmd_move_moveEndFromTrenchRed, cmd_turretHomeRight, - cmd_turretTrack, + cmd_shooterAuto, frc2::WaitCommand((units::second_t).125), - cmd_intakeShoot, - cmd_shooterAuto + cmd_intakeShoot ); frc2::SequentialCommandGroup *shoot3pick1ChezBlue = new frc2::SequentialCommandGroup(); @@ -1267,10 +1266,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_intakeAuto, cmd_move_moveEndFromTrenchBlue, cmd_turretHomeRight, - cmd_turretTrack, + cmd_shooterAuto, frc2::WaitCommand((units::second_t).125), - cmd_intakeShoot, - cmd_shooterAuto + cmd_intakeShoot ); frc2::SequentialCommandGroup *shoot2Blue = new frc2::SequentialCommandGroup(); From 5f45c13e8606120d36dc3c6582f2999269f8cbab Mon Sep 17 00:00:00 2001 From: j a Date: Sat, 24 Sep 2022 14:04:30 -0500 Subject: [PATCH 60/62] moved 3ball shoot spot in -y --- Competition/src/main/cpp/ValorAuto.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 86e7756..619a36a 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -69,8 +69,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc::Pose2d preMarvinRed = frc::Pose2d(units::meter_t(2.75), units::meter_t(5.9), frc::Rotation2d(55_deg)); frc::Pose2d preMarvinBlue = frc::Pose2d(units::meter_t(2.75), units::meter_t(5.9), frc::Rotation2d(55_deg)); - frc::Pose2d marvinShootRed = frc::Pose2d(units::meter_t(4.5), units::meter_t(7.2), frc::Rotation2d(27_deg)); - frc::Pose2d marvinShootBlue = frc::Pose2d(units::meter_t(4.5), units::meter_t(7.2), frc::Rotation2d(27_deg)); + frc::Pose2d marvinShootRed = frc::Pose2d(units::meter_t(4.5), units::meter_t(7), frc::Rotation2d(27_deg)); + frc::Pose2d marvinShootBlue = frc::Pose2d(units::meter_t(4.5), units::meter_t(7), frc::Rotation2d(27_deg)); frc::Pose2d marvinShootAltRed = frc::Pose2d(units::meter_t(3.35), units::meter_t(5), frc::Rotation2d(-90_deg)); frc::Pose2d marvinShootAltBlue = frc::Pose2d(units::meter_t(3.35), units::meter_t(5), frc::Rotation2d(-90_deg)); From b15a9239b26b95ff6ba62d705e98c434d9852a88 Mon Sep 17 00:00:00 2001 From: j a Date: Tue, 27 Sep 2022 11:22:52 -0500 Subject: [PATCH 61/62] red 2 ball state fix --- Competition/src/main/cpp/ValorAuto.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index 619a36a..6464b4d 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -1160,7 +1160,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_move_moveMarvinRed, cmd_intakeDisable, cmd_move_moveMarvinShootRed, - cmd_turretTrack, + cmd_shooterAuto, frc2::WaitCommand((units::second_t).5), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), @@ -1282,7 +1282,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder cmd_move_moveMarvinBlue, cmd_intakeDisable, cmd_move_moveMarvinShootBlue, - cmd_turretTrack, + cmd_shooterAuto, frc2::WaitCommand((units::second_t).5), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), From 77a8f91c8bffe0394b6f642b4843707e6e0954d7 Mon Sep 17 00:00:00 2001 From: j a Date: Sat, 24 Sep 2022 11:21:13 -0500 Subject: [PATCH 62/62] changed intake rotation to lead lead set up. halfed rotation acceleration --- .../src/main/cpp/subsystems/Feeder.cpp | 81 ++++++++++++------- Competition/src/main/include/Constants.h | 2 +- .../src/main/include/subsystems/Feeder.h | 4 +- 3 files changed, 53 insertions(+), 34 deletions(-) diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 8409315..c78f24a 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -17,8 +17,8 @@ Feeder::Feeder() : ValorSubsystem(), operatorController(NULL), motor_intake(FeederConstants::MOTOR_INTAKE_CAN_ID, "baseCAN"), motor_stage(FeederConstants::MOTOR_STAGE_CAN_ID, "baseCAN"), - motor_rotateMain(FeederConstants::MOTOR_ROTATE_MAIN_CAN_ID, "baseCAN"), - motor_rotateFollow(FeederConstants::MOTOR_ROTATE_FOLLOW_CAN_ID, "baseCAN"), + motor_rotateRight(FeederConstants::MOTOR_ROTATE_MAIN_CAN_ID, "baseCAN"), + motor_rotateLeft(FeederConstants::MOTOR_ROTATE_FOLLOW_CAN_ID, "baseCAN"), banner(FeederConstants::BANNER_DIO_PORT) { @@ -42,38 +42,55 @@ void Feeder::init() motor_stage.EnableVoltageCompensation(true); motor_stage.ConfigVoltageCompSaturation(10); - motor_rotateMain.ConfigForwardSoftLimitThreshold(FeederConstants::rotateForwardLimit); - motor_rotateMain.ConfigReverseSoftLimitThreshold(FeederConstants::rotateReverseLimit); + motor_rotateRight.ConfigForwardSoftLimitThreshold(FeederConstants::rotateForwardLimit); + motor_rotateRight.ConfigReverseSoftLimitThreshold(FeederConstants::rotateReverseLimit); - motor_rotateMain.ConfigForwardSoftLimitEnable(true); - motor_rotateMain.ConfigReverseSoftLimitEnable(true); + motor_rotateLeft.ConfigForwardSoftLimitThreshold(FeederConstants::rotateForwardLimit); + motor_rotateLeft.ConfigReverseSoftLimitThreshold(FeederConstants::rotateReverseLimit); - motor_rotateMain.EnableVoltageCompensation(true); - motor_rotateMain.ConfigVoltageCompSaturation(10); - motor_rotateMain.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 25, 60, 1)); //potentially could do 40 60 + motor_rotateRight.ConfigForwardSoftLimitEnable(true); + motor_rotateRight.ConfigReverseSoftLimitEnable(true); - motor_rotateFollow.EnableVoltageCompensation(true); - motor_rotateFollow.ConfigVoltageCompSaturation(10); - motor_rotateFollow.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 25, 60, 1)); //potentially could do 40 60 + motor_rotateLeft.ConfigForwardSoftLimitEnable(true); + motor_rotateLeft.ConfigReverseSoftLimitEnable(true); - motor_rotateMain.SetSelectedSensorPosition(0); - motor_rotateMain.SetInverted(true); // needs to be tested - motor_rotateFollow.SetInverted(false); // needs to be tested - motor_rotateFollow.Follow(motor_rotateMain); + motor_rotateRight.EnableVoltageCompensation(true); + motor_rotateRight.ConfigVoltageCompSaturation(10); + motor_rotateRight.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 25, 60, 1)); //potentially could do 40 60 - motor_rotateMain.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); - motor_rotateFollow.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); + motor_rotateLeft.EnableVoltageCompensation(true); + motor_rotateLeft.ConfigVoltageCompSaturation(10); + motor_rotateLeft.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 25, 60, 1)); //potentially could do 40 60 - motor_rotateMain.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); - motor_rotateMain.ConfigAllowableClosedloopError(0, 0); - motor_rotateMain.Config_IntegralZone(0, 0); - - motor_rotateMain.Config_kF(0, FeederConstants::main_KF); - motor_rotateMain.Config_kD(0, FeederConstants::main_KD); - motor_rotateMain.Config_kI(0, FeederConstants::main_KI); - motor_rotateMain.Config_kP(0, FeederConstants::main_KP); - motor_rotateMain.ConfigMotionAcceleration(FeederConstants::MAIN_MOTION_ACCELERATION); - motor_rotateMain.ConfigMotionCruiseVelocity(FeederConstants::MAIN_MOTION_CRUISE_VELOCITY); + motor_rotateRight.SetSelectedSensorPosition(0); + motor_rotateLeft.SetSelectedSensorPosition(0); + motor_rotateRight.SetInverted(true); + motor_rotateLeft.SetInverted(false); + + motor_rotateRight.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); + motor_rotateLeft.SetNeutralMode(ctre::phoenix::motorcontrol::Brake); + + motor_rotateRight.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); + motor_rotateRight.ConfigAllowableClosedloopError(0, 0); + motor_rotateRight.Config_IntegralZone(0, 0); + + motor_rotateRight.Config_kF(0, FeederConstants::main_KF); + motor_rotateRight.Config_kD(0, FeederConstants::main_KD); + motor_rotateRight.Config_kI(0, FeederConstants::main_KI); + motor_rotateRight.Config_kP(0, FeederConstants::main_KP); + motor_rotateRight.ConfigMotionAcceleration(FeederConstants::MAIN_MOTION_ACCELERATION); + motor_rotateRight.ConfigMotionCruiseVelocity(FeederConstants::MAIN_MOTION_CRUISE_VELOCITY); + + motor_rotateLeft.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10); + motor_rotateLeft.ConfigAllowableClosedloopError(0, 0); + motor_rotateLeft.Config_IntegralZone(0, 0); + + motor_rotateLeft.Config_kF(0, FeederConstants::main_KF); + motor_rotateLeft.Config_kD(0, FeederConstants::main_KD); + motor_rotateLeft.Config_kI(0, FeederConstants::main_KI); + motor_rotateLeft.Config_kP(0, FeederConstants::main_KP); + motor_rotateLeft.ConfigMotionAcceleration(FeederConstants::MAIN_MOTION_ACCELERATION); + motor_rotateLeft.ConfigMotionCruiseVelocity(FeederConstants::MAIN_MOTION_CRUISE_VELOCITY); table->PutBoolean("Reverse Feeder?", false); table->PutNumber("Intake Reverse Speed", FeederConstants::DEFAULT_INTAKE_SPEED_REVERSE); @@ -149,7 +166,7 @@ void Feeder::analyzeDashboard() table->PutBoolean("Banner: ", debounceSensor.getSensor()); table->PutNumber("current feeder state", state.feederState); - table->PutNumber("Intake Encoder Value", motor_rotateMain.GetSelectedSensorPosition()); + table->PutNumber("Intake Encoder Value", motor_rotateRight.GetSelectedSensorPosition()); } void Feeder::assignOutputs() @@ -158,14 +175,16 @@ void Feeder::assignOutputs() if (state.feederState == FeederState::FEEDER_DISABLE) { motor_intake.Set(0); motor_stage.Set(0); - motor_rotateMain.Set(ControlMode::MotionMagic, FeederConstants::rotateForwardLimit); + motor_rotateRight.Set(ControlMode::MotionMagic, FeederConstants::rotateForwardLimit); + motor_rotateLeft.Set(ControlMode::MotionMagic, FeederConstants::rotateForwardLimit); } else if (state.feederState == FeederState::FEEDER_SHOOT) { motor_intake.Set(state.intakeForwardSpeed); motor_stage.Set(state.feederForwardSpeedShoot); } else if (state.feederState == FeederState::FEEDER_RETRACT){ - motor_rotateMain.Set(ControlMode::MotionMagic, FeederConstants::rotateUpSetPoint); // set rotation to be up + motor_rotateRight.Set(ControlMode::MotionMagic, FeederConstants::rotateUpSetPoint); // set rotation to be up + motor_rotateLeft.Set(ControlMode::MotionMagic, FeederConstants::rotateUpSetPoint); } else if (state.feederState == Feeder::FEEDER_REVERSE) { motor_intake.Set(state.intakeReverseSpeed); diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index a2c3f33..f15f6ca 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -268,7 +268,7 @@ namespace FeederConstants{ constexpr static double main_KP = 0.1; constexpr static double MAIN_MOTION_CRUISE_VELOCITY = 15000; - constexpr static double MAIN_MOTION_ACCELERATION = MAIN_MOTION_CRUISE_VELOCITY * 5; + constexpr static double MAIN_MOTION_ACCELERATION = MAIN_MOTION_CRUISE_VELOCITY * 5 * 0.5; //halfed accel diff --git a/Competition/src/main/include/subsystems/Feeder.h b/Competition/src/main/include/subsystems/Feeder.h index 32074e5..9bd0965 100644 --- a/Competition/src/main/include/subsystems/Feeder.h +++ b/Competition/src/main/include/subsystems/Feeder.h @@ -71,8 +71,8 @@ class Feeder : public ValorSubsystem WPI_TalonFX motor_intake; WPI_TalonFX motor_stage; - WPI_TalonFX motor_rotateMain; - WPI_TalonFX motor_rotateFollow; + WPI_TalonFX motor_rotateRight; + WPI_TalonFX motor_rotateLeft;