diff --git a/Competition/src/main/cpp/Robot.cpp b/Competition/src/main/cpp/Robot.cpp index 6bf65ee..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 @@ -37,7 +39,7 @@ 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(); @@ -72,6 +74,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(); } @@ -94,8 +97,13 @@ 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_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/RobotContainer.cpp b/Competition/src/main/cpp/RobotContainer.cpp index 492fe48..d0b55a0 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 405640a..6464b4d 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}); @@ -45,11 +46,11 @@ 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 porkyStepBackRed = frc::Pose2d(.3_m, 2.8_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 porkyStepBackBlue = frc::Pose2d(.3_m, 2.8_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 frc::Pose2d shootRed = frc::Pose2d(6_m, 1.2_m, frc::Rotation2d(53_deg)); // lower angle to 50 in case of time @@ -59,23 +60,48 @@ 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 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 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 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 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 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)); + + 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.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 + + 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(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 + + 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(20_deg)); + frc::Pose2d endPose2BallBlue = frc::Pose2d(7.069_m, 7.3_m, frc::Rotation2d(20_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( [&] { @@ -88,14 +114,32 @@ 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; } ); + frc2::InstantCommand cmd_shooterTarmac = frc2::InstantCommand( [&] { + 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; } ); + 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; @@ -104,8 +148,10 @@ 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->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; } ); + frc2::InstantCommand cmd_setOdometryRed = frc2::InstantCommand( [&] { drivetrain->resetOdometry(startPoseRed); }); @@ -115,11 +161,11 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::InstantCommand cmd_set2ballOdometryRed = frc2::InstantCommand( [&] { drivetrain->resetOdometry(startPose2ballRed); }); - frc2::InstantCommand cmd_setEnd2ballRed = frc2::InstantCommand( [&] { - drivetrain->resetOdometry(endPose2ballRed); + frc2::InstantCommand cmd_set2ballOdometryBlue = frc2::InstantCommand( [&] { + drivetrain->resetOdometry(startPose2ballBlue); }); - frc2::InstantCommand cmd_setEnd2ballBlue = frc2::InstantCommand( [&] { - drivetrain->resetOdometry(endPose2ballBlue); + frc2::InstantCommand cmd_setOdometryZero = frc2::InstantCommand( [&] { + drivetrain->resetOdometry(frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg))); }); auto moveBugsRed = frc::TrajectoryGenerator::GenerateTrajectory( @@ -205,24 +251,234 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder marvinRed, config); - auto moveMarvinRedShoot = frc::TrajectoryGenerator::GenerateTrajectory( + 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( + preMarvinPointsRed, + config); + + auto movePreMarvinBlue = frc::TrajectoryGenerator::GenerateTrajectory( + 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); + + 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, + {}, + marvinBlue, + config); + + auto moveMarvinShootRed = frc::TrajectoryGenerator::GenerateTrajectory( marvinRed, {}, marvinShootRed, config ); + auto moveMarvinShootBlue = frc::TrajectoryGenerator::GenerateTrajectory( + marvinBlue, + {}, + marvinShootBlue, + config + ); + + auto moveMarvinShootAltRed = frc::TrajectoryGenerator::GenerateTrajectory( + marvinRed, + {}, + marvinShootAltRed, + config + ); + auto moveMarvinShootAltBlue = frc::TrajectoryGenerator::GenerateTrajectory( + marvinBlue, + {}, + marvinShootAltBlue, + config + ); - auto moveOppBallRed = frc::TrajectoryGenerator::GenerateTrajectory( + auto moveTasRed = frc::TrajectoryGenerator::GenerateTrajectory( marvinShootRed, {}, - oppRemoveRed, + tasRed, config); + auto moveTasBlue = frc::TrajectoryGenerator::GenerateTrajectory( + marvinShootBlue, + {}, + tasBlue, + config); + auto moveHangarRed = frc::TrajectoryGenerator::GenerateTrajectory( - oppRemoveRed, + tasRed, + {}, + hangarSpotRed, + reverseConfig); + auto moveHangarBlue = frc::TrajectoryGenerator::GenerateTrajectory( + tasBlue, + {}, + hangarSpotBlue, + reverseConfig); + + auto moveTrenchRed = frc::TrajectoryGenerator::GenerateTrajectory( + tasRed, + {}, + trenchSpotRed, + reverseConfig); + auto moveTrenchBlue = frc::TrajectoryGenerator::GenerateTrajectory( + tasBlue, + {}, + trenchSpotBlue, + reverseConfig); + + auto moveMarvinFromTrenchRed = frc::TrajectoryGenerator::GenerateTrajectory( + trenchSpotRed, + {}, + marvinRed, + reverseConfig); + auto moveMarvinFromTrenchBlue = frc::TrajectoryGenerator::GenerateTrajectory( + trenchSpotBlue, {}, + marvinBlue, + reverseConfig); + + + auto moveEndRed = frc::TrajectoryGenerator::GenerateTrajectory( hangarSpotRed, + {}, + tasRed, + reverseConfig); + auto moveEndBlue = frc::TrajectoryGenerator::GenerateTrajectory( + hangarSpotBlue, + {}, + tasBlue, + reverseConfig); + + auto moveEndFromMarvinRed = frc::TrajectoryGenerator::GenerateTrajectory( + marvinRed, + {}, + tasRed, + 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, + {}, + speedyRed, + reverseConfig); + auto moveSpeedyFromTasBlue = frc::TrajectoryGenerator::GenerateTrajectory( + tasBlue, + {}, + 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, + {}, + trenchEndRed, + config); + auto moveEndFromSpeedyBlue = frc::TrajectoryGenerator::GenerateTrajectory( + speedyBlue, + {}, + trenchEndBlue, + config); + + std::vector points; + 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, + config); + frc2::SwerveControllerCommand<4> cmd_move_moveBugsRed( moveBugsRed, [&] () { return drivetrain->getPose_m(); }, @@ -380,9 +636,86 @@ 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_movePreMarvinBlue( + movePreMarvinBlue, + [&] () { 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_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_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(); }, + 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), @@ -391,9 +724,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); - - frc2::SwerveControllerCommand<4> cmd_move_moveOppRemoveRed( - moveOppBallRed, + frc2::SwerveControllerCommand<4> cmd_move_moveMarvinShootBlue( + moveMarvinShootBlue, [&] () { return drivetrain->getPose_m(); }, drivetrain->getKinematics(), frc2::PIDController(DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX), @@ -402,8 +734,19 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder [this] (auto states) { drivetrain->setModuleStates(states); }, {drivetrain} ); - frc2::SwerveControllerCommand<4> cmd_move_moveHangarRed( - moveHangarRed, + + 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), @@ -413,53 +756,331 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder {drivetrain} ); + 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::SequentialCommandGroup *shoot3Red = new frc2::SequentialCommandGroup(); - shoot3Red->AddCommands - (cmd_setOdometryRed, - cmd_shooterAuto, - cmd_intakeAuto, - cmd_move_moveBugsRed, - cmd_move_movePreDaffyRed, - cmd_turretTrack, - frc2::WaitCommand((units::second_t).5), - cmd_intakeShoot, - frc2::WaitCommand((units::second_t)1.0), - cmd_nextBall, - cmd_intakeAuto, - cmd_move_moveDaffyFromPredaffyRed, - frc2::WaitCommand((units::second_t).5), - frc2::WaitCommand((units::second_t).2), - cmd_intakeShoot, - frc2::WaitCommand((units::second_t).5) + frc2::SwerveControllerCommand<4> cmd_move_moveTasBlue( + moveTasBlue, + [&] () { 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 *shoot3Blue = new frc2::SequentialCommandGroup(); - shoot3Blue->AddCommands - (cmd_setOdometryBlue, - cmd_shooterAuto, - cmd_intakeAuto, - cmd_move_moveBugsBlue, - cmd_move_movePreDaffyBlue, - cmd_turretTrack, - frc2::WaitCommand((units::second_t).5), - cmd_intakeShoot, - frc2::WaitCommand((units::second_t)1.0), - cmd_nextBall, - cmd_intakeAuto, - cmd_move_moveDaffyFromPredaffyBlue, - frc2::WaitCommand((units::second_t).5), - frc2::WaitCommand((units::second_t).2), - cmd_intakeShoot, - frc2::WaitCommand((units::second_t).5) + + frc2::SwerveControllerCommand<4> cmd_move_moveHangarRed( + moveHangarRed, + [&] () { 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( + 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::SequentialCommandGroup *shoot5Red = new frc2::SequentialCommandGroup(); - shoot5Red->AddCommands - (cmd_setOdometryRed, - cmd_turretDisable, + 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_moveMarvinFromTrenchBlue( + 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_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(); }, + 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_moveSpeedyFromTasBlue( + 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_moveSpeedyFromAltRed( + moveSpeedyFromAltRed, + [&] () { 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_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(); }, + 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(), + 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_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::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(); + shoot3Red->AddCommands + (cmd_setOdometryRed, cmd_shooterAuto, + cmd_intakeOne, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, + cmd_move_moveBugsRed, + cmd_intakeDisable, + cmd_move_movePreDaffyRed, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)1.0), + cmd_nextBall, cmd_intakeAuto, + cmd_move_moveDaffyFromPredaffyRed, + frc2::WaitCommand((units::second_t).5), + frc2::WaitCommand((units::second_t).2), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t).5), + cmd_intakeDisable + ); + frc2::SequentialCommandGroup *shoot3Blue = new frc2::SequentialCommandGroup(); + shoot3Blue->AddCommands + (cmd_setOdometryBlue, + cmd_shooterAuto, + cmd_intakeOne, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, + cmd_move_moveBugsBlue, + cmd_intakeDisable, + cmd_move_movePreDaffyBlue, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)1.0), + cmd_nextBall, + cmd_intakeAuto, + cmd_move_moveDaffyFromPredaffyBlue, + frc2::WaitCommand((units::second_t).5), + frc2::WaitCommand((units::second_t).2), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t).5), + cmd_intakeDisable + ); + + frc2::SequentialCommandGroup *shoot5Red = new frc2::SequentialCommandGroup(); + shoot5Red->AddCommands + (cmd_setOdometryRed, + cmd_turretDisable, + cmd_shooterAuto, + cmd_intakeOne, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_move_moveBugsRed, + cmd_intakeDisable, cmd_move_moveBackBugsRed, cmd_intakeDisable, cmd_move_movePreDaffyRed, @@ -467,23 +1088,24 @@ 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_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_shooterTarmac, 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), + cmd_shooterAuto, + frc2::WaitCommand((units::second_t).375), cmd_intakeShoot ); frc2::SequentialCommandGroup *shoot5Blue = new frc2::SequentialCommandGroup(); @@ -491,8 +1113,12 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder (cmd_setOdometryBlue, cmd_turretDisable, cmd_shooterAuto, - cmd_intakeAuto, + cmd_intakeOne, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_move_moveBugsBlue, + cmd_intakeDisable, cmd_move_moveBackBugsBlue, cmd_intakeDisable, cmd_move_movePreDaffyBlue, @@ -508,36 +1134,368 @@ 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_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_shooterTarmac, 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), + cmd_shooterAuto, + 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).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, + cmd_shooterAuto, + cmd_move_moveMarvinRed, + cmd_intakeDisable, + cmd_move_moveMarvinShootRed, + cmd_shooterAuto, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t).5), + cmd_intakeDisable + ); + + frc2::SequentialCommandGroup *shoot3ChezRed = new frc2::SequentialCommandGroup(); + shoot3ChezRed->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.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, + 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 *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).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_movePMarvinTasRed, + cmd_moveTasHangarRed, + cmd_intakeReverse, + frc2::WaitCommand((units::second_t)1), + cmd_intakeAuto, + cmd_move_moveEndFromTrenchRed, + cmd_turretHomeRight, + cmd_shooterAuto, + frc2::WaitCommand((units::second_t).125), cmd_intakeShoot ); + + 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)1), + cmd_intakeAuto, + cmd_move_moveEndFromTrenchBlue, + cmd_turretHomeRight, + cmd_shooterAuto, + frc2::WaitCommand((units::second_t).125), + cmd_intakeShoot + ); + + frc2::SequentialCommandGroup *shoot2Blue = new frc2::SequentialCommandGroup(); + shoot2Blue->AddCommands + (cmd_set2ballOdometryBlue, + cmd_intakeOne, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, + cmd_shooterAuto, + cmd_move_moveMarvinBlue, + cmd_intakeDisable, + cmd_move_moveMarvinShootBlue, + cmd_shooterAuto, + frc2::WaitCommand((units::second_t).5), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t).5), + cmd_intakeDisable + ); + + frc2::SequentialCommandGroup *shoot2RedAlt = new frc2::SequentialCommandGroup(); shoot2RedAlt->AddCommands (cmd_set2ballOdometryRed, - cmd_intakeAuto, + cmd_intakeOne, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, cmd_shooterAuto, cmd_move_moveMarvinRed, cmd_intakeDisable, cmd_move_moveMarvinShootRed, cmd_turretTrack, + 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).5), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)1.8), + cmd_move_moveEndFromTrenchNoCoastRed, + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).2), + cmd_shooterAuto + ); + + frc2::SequentialCommandGroup *shoot2BlueAlt = new frc2::SequentialCommandGroup(); + shoot2BlueAlt->AddCommands + (cmd_set2ballOdometryBlue, + cmd_intakeOne, + frc2::WaitCommand((units::second_t).2), + // cmd_intakeClearDeque, + // cmd_intakeAuto, + cmd_shooterAuto, + cmd_move_moveMarvinBlue, + cmd_intakeDisable, + cmd_move_moveMarvinShootBlue, + cmd_turretTrack, + frc2::WaitCommand((units::second_t)1), cmd_intakeShoot, frc2::WaitCommand((units::second_t).5), - cmd_intakeAuto, - cmd_move_moveOppRemoveRed, - cmd_move_moveHangarRed, - cmd_intakeReverse + cmd_intakeOne, + cmd_move_moveTasBlue, + cmd_move_moveTrenchBlue, + cmd_intakeReverse, + frc2::WaitCommand((units::second_t).5), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)1.8), + cmd_move_moveEndFromTrenchNoCoastBlue, + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).2), + cmd_shooterAuto ); - m_chooser.AddOption("RED 2 ball auto alt", shoot2RedAlt); + frc2::SequentialCommandGroup *shoot2Def2Red = new frc2::SequentialCommandGroup(); + shoot2Def2Red->AddCommands + (cmd_set2ballOdometryRed, + cmd_intakeOne, + cmd_turretHomeLeft, + cmd_shooterAuto, + frc2::WaitCommand((units::second_t).2), + // 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_moveTrenchRed, + cmd_intakeReverse, + frc2::WaitCommand((units::second_t).5), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)1.8), + cmd_move_moveEndFromTrenchRed, + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).125), + cmd_shooterAuto + ); + + frc2::SequentialCommandGroup *shoot2Def2Blue = new frc2::SequentialCommandGroup(); + shoot2Def2Blue->AddCommands + (cmd_set2ballOdometryBlue, + cmd_intakeOne, + cmd_turretHomeLeft, + cmd_shooterAuto, + frc2::WaitCommand((units::second_t).2), + // 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).5), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)1.8), + cmd_move_moveEndFromTrenchBlue, + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).125), + cmd_shooterAuto + ); + + + + frc2::SequentialCommandGroup *shoot2Def2RedNoCoast = new frc2::SequentialCommandGroup(); + shoot2Def2RedNoCoast->AddCommands + (cmd_set2ballOdometryRed, + cmd_intakeOne, + cmd_turretHomeLeft, + cmd_shooterAuto, + frc2::WaitCommand((units::second_t).2), + // 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_moveTrenchRed, + cmd_intakeReverse, + frc2::WaitCommand((units::second_t).5), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)1.8), + cmd_move_moveEndFromTrenchNoCoastRed, + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).125), + cmd_shooterAuto + ); + + frc2::SequentialCommandGroup *shoot2Def2BlueNoCoast = new frc2::SequentialCommandGroup(); + shoot2Def2BlueNoCoast->AddCommands + (cmd_set2ballOdometryBlue, + cmd_intakeOne, + cmd_turretHomeLeft, + cmd_shooterAuto, + frc2::WaitCommand((units::second_t).2), + // 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).5), + cmd_intakeDisable, + frc2::WaitCommand((units::second_t)1.8), + cmd_move_moveEndFromTrenchNoCoastBlue, + cmd_turretHomeRight, + cmd_turretTrack, + frc2::WaitCommand((units::second_t).125), + cmd_shooterAuto + ); + + frc2::SequentialCommandGroup *testHolonomicSquare = new frc2::SequentialCommandGroup(); + testHolonomicSquare->AddCommands( + cmd_setOdometryZero, + cmd_testHolonomic + ); + + 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); + + 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 :: QUALS", shoot2Def2RedNoCoast); + m_chooser.AddOption("Blue 2 ball + 2 Defensive :: QUALS", shoot2Def2BlueNoCoast); + m_chooser.AddOption("RED 3 ball auto", shoot3Red); @@ -546,6 +1504,18 @@ 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 3 ball Chez", shoot3ChezRed); + 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); + + + m_chooser.AddOption("Test Holonomic", testHolonomicSquare); + frc::SmartDashboard::PutData(&m_chooser); } diff --git a/Competition/src/main/cpp/ValorGamepad.cpp b/Competition/src/main/cpp/ValorGamepad.cpp new file mode 100644 index 0000000..fb69ae4 --- /dev/null +++ b/Competition/src/main/cpp/ValorGamepad.cpp @@ -0,0 +1,118 @@ +#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; +} + +// 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) +{ + // 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) +{ + 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/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/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/Drivetrain.cpp b/Competition/src/main/cpp/subsystems/Drivetrain.cpp index fd2249c..81c9c31 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}), @@ -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() @@ -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,9 +60,10 @@ 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); driveMotors[i]->SetNeutralMode(NeutralMode::Coast); driveMotors[i]->ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60, 80, .75)); @@ -97,11 +98,11 @@ void Drivetrain::init() // } resetState(); - //pullSwerveModuleZeroReference(); - std::cout <<"init drivetrain" << std::endl; + // pullSwerveModuleZeroReference(); + // std::cout <<"init drivetrain" << std::endl; } -void Drivetrain::setController(frc::XboxController *controller) +void Drivetrain::setController(ValorGamepad *controller) { driverController = controller; } @@ -133,22 +134,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 = 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 +145,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 +191,7 @@ void Drivetrain::analyzeDashboard() swerveModules[2]->getState(), swerveModules[3]->getState()); - if (state.backButtonPressed){ + if (driverController->GetBackButtonPressed()) { resetGyro(); } } @@ -226,25 +211,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) > OIConstants::kDeadbandX ? -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; @@ -252,7 +236,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(); @@ -271,7 +255,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 e542f06..c78f24a 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -10,14 +10,17 @@ #include "subsystems/Feeder.h" +#include 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), - banner(FeederConstants::BANNER_DIO_PORT) + motor_intake(FeederConstants::MOTOR_INTAKE_CAN_ID, "baseCAN"), + motor_stage(FeederConstants::MOTOR_STAGE_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) { frc2::CommandScheduler::GetInstance().RegisterSubsystem(this); init(); @@ -26,13 +29,68 @@ 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_intake.EnableVoltageCompensation(false); + motor_intake.ConfigVoltageCompSaturation(10); + motor_intake.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60, 80, .75)); //potentially could do 40 60 - 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); + motor_stage.EnableVoltageCompensation(true); + motor_stage.ConfigVoltageCompSaturation(10); + + motor_rotateRight.ConfigForwardSoftLimitThreshold(FeederConstants::rotateForwardLimit); + motor_rotateRight.ConfigReverseSoftLimitThreshold(FeederConstants::rotateReverseLimit); + + motor_rotateLeft.ConfigForwardSoftLimitThreshold(FeederConstants::rotateForwardLimit); + motor_rotateLeft.ConfigReverseSoftLimitThreshold(FeederConstants::rotateReverseLimit); + + motor_rotateRight.ConfigForwardSoftLimitEnable(true); + motor_rotateRight.ConfigReverseSoftLimitEnable(true); + + motor_rotateLeft.ConfigForwardSoftLimitEnable(true); + motor_rotateLeft.ConfigReverseSoftLimitEnable(true); + + motor_rotateRight.EnableVoltageCompensation(true); + motor_rotateRight.ConfigVoltageCompSaturation(10); + motor_rotateRight.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 25, 60, 1)); //potentially could do 40 60 + + motor_rotateLeft.EnableVoltageCompensation(true); + motor_rotateLeft.ConfigVoltageCompSaturation(10); + motor_rotateLeft.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 25, 60, 1)); //potentially could do 40 60 + + 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); @@ -41,12 +99,23 @@ 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(); + currentSensor.setGetter([this]() { return motor_intake.GetOutputCurrent(); }); + debounceSensor.setGetter([this]() { return !banner.Get(); }); } -void Feeder::setControllers(frc::XboxController *controllerO, frc::XboxController *controllerD) +void Feeder::resetIntakeSensor() +{ + currentSensor.reset(); +} + +void Feeder::setControllers(ValorGamepad *controllerO, ValorGamepad *controllerD) { driverController = controllerD; operatorController = controllerO; @@ -58,70 +127,64 @@ 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; + resetIntakeSensor(); + } + else if (driverController->GetXButton()) { + state.feederState = FeederState::FEEDER_RETRACT; //Set Intake rotate to upper setpoint } - else if (state.driver_leftBumperPressed) { + else if (driverController->GetLeftBumper()) { state.feederState = FeederState::FEEDER_REVERSE; - state.spiked = false; + resetIntakeSensor(); } - 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; } - - // Calculate instantaneous current - calcCurrent(); } 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); + + table->PutNumber("Intake Encoder Value", motor_rotateRight.GetSelectedSensorPosition()); } void Feeder::assignOutputs() { - state.bannerTripped = !banner.Get(); - state.currentBanner = state.bannerTripped; if (state.feederState == FeederState::FEEDER_DISABLE) { motor_intake.Set(0); motor_stage.Set(0); + 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); - motor_stage.SetVoltage(10_V * state.feederForwardSpeedShoot); + motor_stage.Set(state.feederForwardSpeedShoot); + } + else if (state.feederState == FeederState::FEEDER_RETRACT){ + 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); @@ -132,25 +195,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 { @@ -162,35 +217,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); - } -} - - void Feeder::resetState() { state.feederState = FeederState::FEEDER_DISABLE; - - state.spiked = false; - state.previousBanner = false; - - resetDeque(); + resetIntakeSensor(); } diff --git a/Competition/src/main/cpp/subsystems/Lift.cpp b/Competition/src/main/cpp/subsystems/Lift.cpp index 32d92a5..3c21d0b 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); @@ -71,7 +74,7 @@ void Lift::init() setupCommands(); } -void Lift::setController(frc::XboxController *controller) +void Lift::setController(ValorGamepad *controller) { operatorController = controller; } @@ -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() @@ -182,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; } @@ -212,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()) @@ -224,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(); } @@ -261,6 +254,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); @@ -272,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); } @@ -296,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 d4cdca2..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); @@ -40,8 +41,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); @@ -113,6 +117,9 @@ void Shooter::init() hoodPidController.SetSmartMotionMinOutputVelocity(ShooterConstants::hoodMinV); hoodPidController.SetSmartMotionMaxAccel(ShooterConstants::hoodMaxAccel); hoodPidController.SetSmartMotionAllowedClosedLoopError(ShooterConstants::hoodAllowedError); + + state.pipeline = 0; + state.LoBFZoom = 1; resetState(); resetEncoder(); @@ -144,7 +151,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; @@ -161,60 +168,42 @@ 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; //Turret - if (std::abs(state.leftStickX) > ShooterConstants::kDeadband) { + if (feederTable->GetNumber("Intake Encoder Value", 0) < FeederConstants::rotateForwardLimit / 2) { + 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 (state.bButton){ - state.turretState = TurretState::TURRET_TRACK; // Not moving + else if (operatorController->GetYButton() || driverController->GetBButton()) { + state.turretState = TurretState::TURRET_HOME_MID; } - else if(state.turretState == TurretState::TURRET_MANUAL){ + 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 } 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() @@ -226,6 +215,10 @@ void Shooter::analyzeDashboard() double deltaH = ShooterConstants::hubHeight - ShooterConstants::limelightHeight; double xDist = deltaH / tan(angle * MathConstants::toRadians); state.distanceToHub = xDist; + + if (state.distanceToHub < 1.0) + state.distanceToHub = 1.0; + table->PutNumber("x distance to hub", xDist); } @@ -263,7 +256,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); @@ -271,6 +265,8 @@ void Shooter::analyzeDashboard() state.flywheelState = FlywheelState::FLYWHEEL_DISABLE; } + + if (state.turretState == TurretState::TURRET_TRACK && state.lastTurretState != TurretState::TURRET_TRACK){ limelightTrack(true); } @@ -285,18 +281,25 @@ void Shooter::analyzeDashboard() table->PutNumber("Turret target", state.turretTarget); table->PutNumber("Turret Desired", state.turretDesired); - table->PutNumber("left stick x", state.leftStickX); + table->PutNumber("Turret Output", state.turretOutput); table->PutNumber("flywheel power", state.flywheelHigh); table->PutNumber("hood high", state.hoodHigh); table->PutNumber("Turret State", state.turretState); - state.hoodC = table->GetNumber("Hood Y Int", ShooterConstants::cHood); - state.powerC = table->GetNumber("Power Y Int", ShooterConstants::cPower); + 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; @@ -318,10 +321,9 @@ 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 = 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; } turret.Set(state.turretOutput); @@ -376,7 +378,12 @@ 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 + 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; @@ -402,7 +409,14 @@ 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 + 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/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index 9f33a2a..8c3a881 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,28 +34,65 @@ void TurretTracker::analyzeDashboard() { } +void TurretTracker::disableWrapAround(){ + table->PutBoolean("Use Turret Shoot", false); +} + +void TurretTracker::enableWrapAround(){ + table->PutBoolean("Use Turret Shoot", true); +} + + + 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.target = (-state.cachedTx * 0.75) + turretPos; + + state.cachedHeading = robotHeading; + state.cachedX = x; + state.cachedY = y; + state.cachedTx = tx; + state.cachedTurretPos = turretPos; - 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.destinationTurretHeading = robotHeading + turretPos - 90 - state.cachedTx; } else { - if (table->GetBoolean("Use Turret Shoot", false)) - state.target = -1 * drivetrain->getPose_m().Rotation().Degrees().to() + state.cachedTurretPos - state.cachedTx; + if (table->GetBoolean("Use Turret Shoot", true)) + state.target = state.destinationTurretHeading - robotHeading + 90 + tx; else - state.target = shooter->turretEncoder.GetPosition(); + 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 += 4 * 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) { @@ -65,11 +102,11 @@ void TurretTracker::assignOutputs() { state.target -= 360; } - if (state.target < 0) { - state.target = 0; + if (state.target < -7) { + state.target = -7; } - else if (state.target > 180) { - state.target = 180; + else if (state.target > 190.5) { + state.target = 190.5; } shooter->assignTurret(state.target); diff --git a/Competition/src/main/cpp/subsystems/ValorSwerve.cpp b/Competition/src/main/cpp/subsystems/ValorSwerve.cpp index d586cca..c268e69 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() @@ -116,8 +116,8 @@ 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); - std::cout << "pulled pospition from file" << std::endl; + azimuthFalcon->SetSelectedSensorPosition(0); + //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 6caf194..f15f6ca 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; @@ -117,25 +117,41 @@ 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; + //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.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; + 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; constexpr static double hubHeight = 2.64; constexpr static double limelightHeight = .6075; - constexpr static double flywheelKP1 = 0.1; + 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; constexpr static double flywheelKFF1 = 0.04; - constexpr static double flywheelKP0 = 0.1 ; //.25 + 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; @@ -150,8 +166,8 @@ 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 flywheelDefaultValue = 0.45; //.375 + constexpr static double flywheelPoopValue = 0.3; constexpr static double flywheelLaunchpadValue = .455; constexpr static double flywheelSpeeds[] = {.372, .38125, .372}; //.387, .39125 @@ -174,10 +190,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; @@ -207,10 +223,11 @@ 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 + 8.5; + constexpr static double turretLimitRight = 0 - 7; constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500 + constexpr static double hubX = 0; constexpr static double hubY = 0; @@ -221,20 +238,40 @@ 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 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.9; - constexpr static double DEFAULT_INTAKE_SPEED_REVERSE = -0.9; + constexpr static double DEFAULT_INTAKE_SPEED_FORWARD = 0.7; + 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.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 double JAM_CURRENT = 30; + 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 = 27 * tickToDegree; + constexpr static double rotateReverseLimit = 0; + constexpr static double rotateUpSetPoint = 1 * tickToDegree; + + 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 = 15000; + constexpr static double MAIN_MOTION_ACCELERATION = MAIN_MOTION_CRUISE_VELOCITY * 5 * 0.5; //halfed accel + + + } namespace MathConstants{ @@ -252,7 +289,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; @@ -263,7 +300,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 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/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/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/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..48da482 --- /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 ? sensorLambda() : 0; } + +protected: + std::function sensorLambda; + T currState; + T prevState; +}; + +#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 f519125..9bd0965 100644 --- a/Competition/src/main/include/subsystems/Feeder.h +++ b/Competition/src/main/include/subsystems/Feeder.h @@ -9,12 +9,17 @@ #include "ValorSubsystem.h" #include "Constants.h" -#include +#include "ValorGamepad.h" -#include -#include +#include #include +#include "sensors/ValorCurrentSensor.h" +#include "sensors/ValorDebounceSensor.h" + +#include + + #ifndef FEEDER_H #define FEEDER_H @@ -24,7 +29,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(); @@ -37,61 +42,46 @@ class Feeder : public ValorSubsystem FEEDER_REVERSE, FEEDER_SHOOT, FEEDER_CURRENT_INTAKE, - FEEDER_REGULAR_INTAKE + FEEDER_REGULAR_INTAKE, + FEEDER_RETRACT }; 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; - 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 resetIntakeSensor(); private: - frc::XboxController *driverController; - frc::XboxController *operatorController; + ValorGamepad *driverController; + ValorGamepad *operatorController; + + WPI_TalonFX motor_intake; + WPI_TalonFX motor_stage; + WPI_TalonFX motor_rotateRight; + WPI_TalonFX motor_rotateLeft; + - rev::CANSparkMax motor_intake; - rev::CANSparkMax motor_stage; + + + ValorCurrentSensor currentSensor; + ValorDebounceSensor debounceSensor; frc::DigitalInput banner; - void calcCurrent(); - - void resetDeque(); }; #endif \ No newline at end of file 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 0bedbe2..6c6730f 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,17 +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 driverLastLeftTrigger; double limelightDistance; @@ -120,12 +108,18 @@ class Shooter : public ValorSubsystem int currentBall; int pipeline; + int LoBFZoom; + + double hoodB_1x; + double powerB_1x; + double hoodB_2x; + double powerB_2x; - double hoodB; - double powerB; + double powerC_1x; + double hoodC_1x; + double powerC_2x; + double hoodC_2x; - double powerC; - double hoodC; double tv; double tx; @@ -143,11 +137,13 @@ 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; + std::shared_ptr feederTable; + Drivetrain *odom; diff --git a/Competition/src/main/include/subsystems/TurretTracker.h b/Competition/src/main/include/subsystems/TurretTracker.h index 615a6ab..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; @@ -45,6 +48,7 @@ class TurretTracker : public ValorSubsystem double cachedVT; double cachedTurretPos; + double destinationTurretHeading; } state;