@@ -45,10 +45,10 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
4545 frc::Pose2d daffyBlue = frc::Pose2d (3 .55_m, 2 .15_m, frc::Rotation2d (115_deg));
4646
4747 frc::Translation2d porkyEntryRed = frc::Translation2d (1 .3_m, 3 .25_m); // 3.5
48- frc::Pose2d porkyRed = frc::Pose2d (-0 .35_m , 2 .1_m , frc::Rotation2d (212_deg));
48+ frc::Pose2d porkyRed = frc::Pose2d (-0 .45_m , 2 .35_m , frc::Rotation2d (212_deg));
4949 frc::Pose2d porkyStepBackRed = frc::Pose2d (.7_m, 3 .2_m, frc::Rotation2d (212_deg));
5050 frc::Translation2d porkyEntryBlue = frc::Translation2d (1 .3_m, 3 .25_m); // 3.5
51- frc::Pose2d porkyBlue = frc::Pose2d (-0 .35_m , 2 .1_m , frc::Rotation2d (212_deg));
51+ frc::Pose2d porkyBlue = frc::Pose2d (-0 .45_m , 2 .35_m , frc::Rotation2d (212_deg));
5252 frc::Pose2d porkyStepBackBlue = frc::Pose2d (.7_m, 3 .2_m, frc::Rotation2d (212_deg));
5353
5454 frc::Translation2d shootConstrainRed = frc::Translation2d (3 .15_m, 2_m); // 1.2_m in case we need to push it more towards wall
@@ -59,26 +59,29 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
5959 frc::Pose2d startPose2ballRed = frc::Pose2d (units::meter_t (5.75 ), units::meter_t (5.42 ), frc::Rotation2d (137_deg));
6060 frc::Pose2d startPose2ballBlue = frc::Pose2d (units::meter_t (5.75 ), units::meter_t (5.42 ), frc::Rotation2d (137_deg));
6161
62- frc::Pose2d marvinRed = frc::Pose2d (units::meter_t (3.7 ), units::meter_t (7.2 ), frc::Rotation2d (137_deg));
63- frc::Pose2d marvinBlue = frc::Pose2d (units::meter_t (4.5 ), units::meter_t (6.4 ), frc::Rotation2d (137_deg));
62+ frc::Pose2d marvinRed = frc::Pose2d (units::meter_t (4 ), units::meter_t (6.9 ), frc::Rotation2d (137_deg));
63+ frc::Pose2d marvinBlue = frc::Pose2d (units::meter_t (4 ), units::meter_t (6.9 ), frc::Rotation2d (137_deg));
6464
6565 frc::Pose2d marvinShootRed = frc::Pose2d (units::meter_t (5 ), units::meter_t (7.5 ), frc::Rotation2d (27_deg));
66- frc::Pose2d marvinShootBlue = frc::Pose2d (units::meter_t (5.2 ), units::meter_t (6.8 ), frc::Rotation2d (27_deg));
66+ frc::Pose2d marvinShootBlue = frc::Pose2d (units::meter_t (5 ), units::meter_t (7.5 ), frc::Rotation2d (27_deg));
6767
68- frc::Pose2d tasRed = frc::Pose2d (units::meter_t (5.869 ), units::meter_t (7.6 ), frc::Rotation2d (30_deg));
69- frc::Pose2d tasBlue = frc::Pose2d (units::meter_t (5.869 ), units::meter_t (7.6 ), frc::Rotation2d (30_deg));
68+ frc::Pose2d marvinShootAltRed = frc::Pose2d (units::meter_t (3.35 ), units::meter_t (4 ), frc::Rotation2d (-90_deg));
69+ frc::Pose2d marvinShootAltBlue = frc::Pose2d (units::meter_t (3.35 ), units::meter_t (4 ), frc::Rotation2d (-90_deg));
70+
71+ frc::Pose2d tasRed = frc::Pose2d (units::meter_t (6.069 ), units::meter_t (7.8 ), frc::Rotation2d (60_deg));
72+ frc::Pose2d tasBlue = frc::Pose2d (units::meter_t (6.069 ), units::meter_t (7.8 ), frc::Rotation2d (60_deg));
7073
7174 frc::Translation2d tasToSpeedyConstrainRed = frc::Translation2d (.75_m, 2 .5_m); // 1.2_m in case we need to push it more towards wall
7275 frc::Translation2d tasToSpeedyConstrainBlue = frc::Translation2d (.75_m, 2 .5_m); // 1.2_m in case we need to push it more towards wall
7376
7477 frc::Pose2d hangarSpotRed = frc::Pose2d (units::meter_t (4 ), units::meter_t (7.5 ), frc::Rotation2d (190_deg));
7578 frc::Pose2d hangarSpotBlue = frc::Pose2d (units::meter_t (4 ), units::meter_t (7.5 ), frc::Rotation2d (190_deg));
7679
77- frc::Pose2d speedyRed = frc::Pose2d (3 .45_m , 2 .15_m , frc::Rotation2d (-60_deg)); // originally 0
78- frc::Pose2d speedyBlue = frc::Pose2d (3 .45_m , 2 .15_m , frc::Rotation2d (-60_deg)); // originally 0
80+ frc::Pose2d speedyRed = frc::Pose2d (3 .35_m , 2 .8_m , frc::Rotation2d (-60_deg)); // originally 0
81+ frc::Pose2d speedyBlue = frc::Pose2d (3 .35_m , 2 .8_m , frc::Rotation2d (-60_deg)); // originally 0
7982
80- frc::Pose2d trenchEndRed = frc::Pose2d (3 .75_m, 2 .15_m , frc::Rotation2d (0_deg ));
81- frc::Pose2d trenchEndBlue = frc::Pose2d (3 .75_m, 2 .15_m , frc::Rotation2d (0_deg ));
83+ frc::Pose2d trenchEndRed = frc::Pose2d (3 .75_m, 3_m , frc::Rotation2d (135_deg ));
84+ frc::Pose2d trenchEndBlue = frc::Pose2d (3 .75_m, 3_m , frc::Rotation2d (135_deg ));
8285
8386 frc::Pose2d endPose2ballRed = frc::Pose2d (10_m, 10_m, frc::Rotation2d (0_deg));
8487 frc::Pose2d endPose2ballBlue = frc::Pose2d (10_m, 10_m, frc::Rotation2d (0_deg));
@@ -113,6 +116,9 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
113116 frc2::InstantCommand cmd_turretHomeLeft = frc2::InstantCommand ( [&] {
114117 shooter->state .turretState = Shooter::TurretState::TURRET_HOME_LEFT;
115118 } );
119+ frc2::InstantCommand cmd_turretHomeRight = frc2::InstantCommand ( [&] {
120+ shooter->state .turretState = Shooter::TurretState::TURRET_HOME_RIGHT;
121+ } );
116122
117123 frc2::InstantCommand cmd_turretDisable = frc2::InstantCommand ( [&] {
118124 shooter->state .turretState = Shooter::TurretState::TURRET_DISABLE;
@@ -121,6 +127,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
121127 frc2::InstantCommand cmd_intakeOne = frc2::InstantCommand ( [&] { feeder->state .feederState = Feeder::FeederState::FEEDER_REGULAR_INTAKE; } );
122128 frc2::InstantCommand cmd_intakeDisable = frc2::InstantCommand ( [&] { feeder->state .feederState = Feeder::FeederState::FEEDER_DISABLE; } );
123129 frc2::InstantCommand cmd_intakeAuto = frc2::InstantCommand ( [&] { feeder->state .feederState = Feeder::FeederState::FEEDER_CURRENT_INTAKE; } );
130+ frc2::InstantCommand cmd_intakeClearDeque = frc2::InstantCommand ( [&] { feeder->resetDeque ();} );
124131 frc2::InstantCommand cmd_intakeShoot = frc2::InstantCommand ( [&] { feeder->state .feederState = Feeder::FeederState::FEEDER_SHOOT; } );
125132 frc2::InstantCommand cmd_intakeReverse = frc2::InstantCommand ( [&] { feeder->state .feederState = Feeder::FeederState::FEEDER_REVERSE; } );
126133
@@ -244,6 +251,19 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
244251 config
245252 );
246253
254+ auto moveMarvinShootAltRed = frc::TrajectoryGenerator::GenerateTrajectory (
255+ marvinRed,
256+ {},
257+ marvinShootAltRed,
258+ config
259+ );
260+ auto moveMarvinShootAltBlue = frc::TrajectoryGenerator::GenerateTrajectory (
261+ marvinBlue,
262+ {},
263+ marvinShootAltBlue,
264+ config
265+ );
266+
247267 auto moveTasRed = frc::TrajectoryGenerator::GenerateTrajectory (
248268 marvinShootRed,
249269 {},
@@ -279,14 +299,36 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
279299
280300 auto moveSpeedyFromTasRed = frc::TrajectoryGenerator::GenerateTrajectory (
281301 tasRed,
282- {tasToSpeedyConstrainRed },
302+ {},
283303 speedyRed,
284304 reverseConfig);
285305 auto moveSpeedyFromTasBlue = frc::TrajectoryGenerator::GenerateTrajectory (
286306 tasBlue,
287- {tasToSpeedyConstrainBlue },
307+ {},
288308 speedyBlue,
289309 reverseConfig);
310+
311+ auto moveSpeedyFromAltRed = frc::TrajectoryGenerator::GenerateTrajectory (
312+ marvinShootAltRed,
313+ {},
314+ speedyRed,
315+ config);
316+ auto moveSpeedyFromAltBlue = frc::TrajectoryGenerator::GenerateTrajectory (
317+ marvinShootAltBlue,
318+ {},
319+ speedyBlue,
320+ config);
321+
322+ auto moveTasFromSpeedyRed = frc::TrajectoryGenerator::GenerateTrajectory (
323+ speedyRed,
324+ {},
325+ tasRed,
326+ reverseConfig);
327+ auto moveTasFromSpeedyBlue = frc::TrajectoryGenerator::GenerateTrajectory (
328+ speedyBlue,
329+ {},
330+ tasBlue,
331+ reverseConfig);
290332
291333 auto moveEndFromSpeedyRed = frc::TrajectoryGenerator::GenerateTrajectory (
292334 speedyRed,
@@ -487,6 +529,27 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
487529 [this ] (auto states) { drivetrain->setModuleStates (states); },
488530 {drivetrain}
489531 );
532+
533+ frc2::SwerveControllerCommand<4 > cmd_move_moveMarvinShootAltRed (
534+ moveMarvinShootAltRed,
535+ [&] () { return drivetrain->getPose_m (); },
536+ drivetrain->getKinematics (),
537+ frc2::PIDController (DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX),
538+ frc2::PIDController (DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY),
539+ thetaController,
540+ [this ] (auto states) { drivetrain->setModuleStates (states); },
541+ {drivetrain}
542+ );
543+ frc2::SwerveControllerCommand<4 > cmd_move_moveMarvinShootAltBlue (
544+ moveMarvinShootAltBlue,
545+ [&] () { return drivetrain->getPose_m (); },
546+ drivetrain->getKinematics (),
547+ frc2::PIDController (DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX),
548+ frc2::PIDController (DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY),
549+ thetaController,
550+ [this ] (auto states) { drivetrain->setModuleStates (states); },
551+ {drivetrain}
552+ );
490553
491554 frc2::SwerveControllerCommand<4 > cmd_move_moveTasRed (
492555 moveTasRed,
@@ -540,9 +603,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
540603 [this ] (auto states) { drivetrain->setModuleStates (states); },
541604 {drivetrain}
542605 );
543-
544- frc2::SwerveControllerCommand<4 > cmd_move_moveEndFromSpeedyRed (
545- moveEndFromSpeedyRed,
606+ frc2::SwerveControllerCommand<4 > cmd_move_moveSpeedyFromTasBlue (
607+ moveSpeedyFromTasBlue,
546608 [&] () { return drivetrain->getPose_m (); },
547609 drivetrain->getKinematics (),
548610 frc2::PIDController (DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX),
@@ -551,8 +613,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
551613 [this ] (auto states) { drivetrain->setModuleStates (states); },
552614 {drivetrain}
553615 );
554- frc2::SwerveControllerCommand<4 > cmd_move_moveSpeedyFromTasRed (
555- moveSpeedyFromTasBlue ,
616+ frc2::SwerveControllerCommand<4 > cmd_move_moveSpeedyFromAltRed (
617+ moveSpeedyFromAltRed ,
556618 [&] () { return drivetrain->getPose_m (); },
557619 drivetrain->getKinematics (),
558620 frc2::PIDController (DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX),
@@ -561,7 +623,49 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
561623 [this ] (auto states) { drivetrain->setModuleStates (states); },
562624 {drivetrain}
563625 );
564-
626+ frc2::SwerveControllerCommand<4 > cmd_move_moveSpeedyFromAltBlue (
627+ moveSpeedyFromAltBlue,
628+ [&] () { return drivetrain->getPose_m (); },
629+ drivetrain->getKinematics (),
630+ frc2::PIDController (DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX),
631+ frc2::PIDController (DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY),
632+ thetaController,
633+ [this ] (auto states) { drivetrain->setModuleStates (states); },
634+ {drivetrain}
635+ );
636+
637+ frc2::SwerveControllerCommand<4 > cmd_move_moveTasFromSpeedyRed (
638+ moveTasFromSpeedyRed,
639+ [&] () { return drivetrain->getPose_m (); },
640+ drivetrain->getKinematics (),
641+ frc2::PIDController (DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX),
642+ frc2::PIDController (DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY),
643+ thetaController,
644+ [this ] (auto states) { drivetrain->setModuleStates (states); },
645+ {drivetrain}
646+ );
647+ frc2::SwerveControllerCommand<4 > cmd_move_moveTasFromSpeedyBlue (
648+ moveTasFromSpeedyBlue,
649+ [&] () { return drivetrain->getPose_m (); },
650+ drivetrain->getKinematics (),
651+ frc2::PIDController (DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX),
652+ frc2::PIDController (DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY),
653+ thetaController,
654+ [this ] (auto states) { drivetrain->setModuleStates (states); },
655+ {drivetrain}
656+ );
657+
658+ frc2::SwerveControllerCommand<4 > cmd_move_moveEndFromSpeedyRed (
659+ moveEndFromSpeedyRed,
660+ [&] () { return drivetrain->getPose_m (); },
661+ drivetrain->getKinematics (),
662+ frc2::PIDController (DriveConstants::KPX, DriveConstants::KIX, DriveConstants::KDX),
663+ frc2::PIDController (DriveConstants::KPY, DriveConstants::KIY, DriveConstants::KDY),
664+ thetaController,
665+ [this ] (auto states) { drivetrain->setModuleStates (states); },
666+ {drivetrain}
667+ );
668+
565669 frc2::SwerveControllerCommand<4 > cmd_move_moveEndFromSpeedyBlue (
566670 moveEndFromSpeedyBlue,
567671 [&] () { return drivetrain->getPose_m (); },
@@ -659,13 +763,14 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
659763 cmd_turretDisable,
660764 cmd_intakeOne,
661765 cmd_move_movePorkyFromDaffyRed,
766+ cmd_turretHomeMid,
662767 cmd_move_moveStepBackRed,
768+ cmd_intakeClearDeque,
663769 cmd_intakeAuto,
664- frc2::WaitCommand ((units::second_t ).5 ),
665- cmd_turretHomeMid ,
770+ frc2::WaitCommand ((units::second_t ).25 ),
771+ cmd_turretTrack ,
666772 cmd_move_moveShootRed,
667- cmd_turretTrack, // if we lower the angle to 50 we could potentially cut tracking / lower time
668- frc2::WaitCommand ((units::second_t ).225 ),
773+ frc2::WaitCommand ((units::second_t ).375 ),
669774 cmd_intakeShoot
670775 );
671776 frc2::SequentialCommandGroup *shoot5Blue = new frc2::SequentialCommandGroup ();
@@ -693,13 +798,14 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
693798 cmd_turretDisable,
694799 cmd_intakeOne,
695800 cmd_move_movePorkyFromDaffyBlue,
801+ cmd_turretHomeMid,
696802 cmd_move_moveStepBackBlue,
803+ cmd_intakeClearDeque,
697804 cmd_intakeAuto,
698- frc2::WaitCommand ((units::second_t ).5 ),
699- cmd_turretHomeMid ,
805+ frc2::WaitCommand ((units::second_t ).25 ),
806+ cmd_turretTrack ,
700807 cmd_move_moveShootBlue,
701- cmd_turretTrack, // if we lower the angle to 50 we could potentially cut the tracking / wait
702- frc2::WaitCommand ((units::second_t ).225 ),
808+ frc2::WaitCommand ((units::second_t ).375 ),
703809 cmd_intakeShoot
704810 );
705811
@@ -749,51 +855,62 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
749855 shoot2Def2Red->AddCommands
750856 (cmd_set2ballOdometryRed,
751857 cmd_intakeOne,
858+ cmd_turretHomeLeft,
752859 cmd_shooterAuto,
860+ frc2::WaitCommand ((units::second_t ).125 ),
861+ cmd_intakeClearDeque,
862+ cmd_intakeAuto,
753863 cmd_move_moveMarvinRed,
754864 cmd_intakeDisable,
755- cmd_move_moveMarvinShootRed ,
865+ cmd_move_moveMarvinShootAltRed ,
756866 cmd_turretTrack,
757867 frc2::WaitCommand ((units::second_t ).5 ),
758868 cmd_intakeShoot,
759869 frc2::WaitCommand ((units::second_t ).5 ),
760- cmd_intakeAuto,
761- cmd_move_moveTasRed,
762- cmd_move_moveSpeedyFromTasRed,
763- cmd_turretHomeLeft,
764- cmd_shooterPoop,
765- frc2::WaitCommand ((units::second_t ).5 ),
766- cmd_intakeShoot,
870+ cmd_intakeOne,
871+ cmd_move_moveSpeedyFromAltRed,
872+ cmd_move_moveTasFromSpeedyRed,
873+ cmd_move_moveHangarRed,
874+ cmd_intakeReverse,
767875 frc2::WaitCommand ((units::second_t )1 ),
768876 cmd_intakeDisable,
769- cmd_move_moveEndFromSpeedyRed
877+ cmd_move_moveEndRed,
878+ cmd_turretHomeRight
770879 );
880+
771881 frc2::SequentialCommandGroup *shoot2Def2Blue = new frc2::SequentialCommandGroup ();
772882 shoot2Def2Blue->AddCommands
773883 (cmd_set2ballOdometryBlue,
774884 cmd_intakeOne,
885+ cmd_turretHomeLeft,
775886 cmd_shooterAuto,
887+ frc2::WaitCommand ((units::second_t ).125 ),
888+ cmd_intakeClearDeque,
889+ cmd_intakeAuto,
776890 cmd_move_moveMarvinBlue,
777891 cmd_intakeDisable,
778- cmd_move_moveMarvinShootBlue ,
892+ cmd_move_moveMarvinShootAltBlue ,
779893 cmd_turretTrack,
780894 frc2::WaitCommand ((units::second_t ).5 ),
781895 cmd_intakeShoot,
782896 frc2::WaitCommand ((units::second_t ).5 ),
783- cmd_intakeAuto,
784- cmd_move_moveTasBlue,
785- // cmd_move_moveSpeedyFromTasBlue,
786- cmd_turretHomeLeft,
787- cmd_shooterPoop,
788- frc2::WaitCommand ((units::second_t ).5 ),
789- cmd_intakeShoot,
897+ cmd_intakeOne,
898+ cmd_move_moveSpeedyFromAltBlue,
899+ cmd_move_moveTasFromSpeedyBlue,
900+ cmd_move_moveHangarBlue,
901+ cmd_intakeReverse,
790902 frc2::WaitCommand ((units::second_t )1 ),
791903 cmd_intakeDisable,
792- cmd_move_moveEndFromSpeedyBlue
904+ cmd_move_moveEndBlue,
905+ cmd_turretHomeRight
793906 );
794907
795- m_chooser.AddOption (" RED 2 ball auto alt" , shoot2RedAlt);
796- m_chooser.AddOption (" Blue 2 ball auto alt" , shoot2BlueAlt);
908+ m_chooser.AddOption (" RED 2 ball + 1 Defensive" , shoot2RedAlt);
909+ m_chooser.AddOption (" Blue 2 ball + 1 Defensive" , shoot2BlueAlt);
910+
911+ m_chooser.AddOption (" RED 2 ball + 2 Defensive" , shoot2Def2Red);
912+ m_chooser.AddOption (" Blue 2 ball + 2 Defensive" , shoot2Def2Blue);
913+
797914
798915
799916 m_chooser.AddOption (" RED 3 ball auto" , shoot3Red);
0 commit comments