Skip to content

Commit

Permalink
made better way of enabling & disabling turretWrapAround
Browse files Browse the repository at this point in the history
  • Loading branch information
@rohitalamgari @mray190
rohitalamgari authored and mray190 committed Apr 18, 2022
1 parent 12a717f commit d99c37d
Show file tree
Hide file tree
Showing 2 changed files with 15 additions and 39 deletions.
2 changes: 2 additions & 0 deletions Competition/src/main/cpp/Robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,7 @@ void Robot::AutonomousInit() {
m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::AUTO;
m_container.m_lift.robotMode = ValorSubsystem::RobotMode::AUTO;
m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::AUTO;
m_container.m_turretTracker.disableWrapAround();

m_container.m_drivetrain.pullSwerveModuleZeroReference();
}
Expand All @@ -97,6 +98,7 @@ void Robot::TeleopInit() {
m_container.m_shooter.state.turretState = m_container.m_shooter.TURRET_TRACK;
m_container.m_lift.robotMode = ValorSubsystem::RobotMode::TELEOP;
m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::TELEOP;
m_container.m_turretTracker.enableWrapAround();

}

Expand Down
52 changes: 13 additions & 39 deletions Competition/src/main/cpp/ValorAuto.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -142,9 +142,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
frc2::InstantCommand cmd_intakeClearDeque = frc2::InstantCommand( [&] { feeder->resetDeque();} );
frc2::InstantCommand cmd_intakeShoot = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_SHOOT; } );
frc2::InstantCommand cmd_intakeReverse = frc2::InstantCommand( [&] { feeder->state.feederState = Feeder::FeederState::FEEDER_REVERSE; } );
frc2::InstantCommand cmd_turretNoWrapAround = frc2::InstantCommand( [&] { turretTracker->disableWrapAround(); } );
frc2::InstantCommand cmd_turretWrapAround = frc2::InstantCommand( [&] { turretTracker->enableWrapAround(); } );


frc2::InstantCommand cmd_setOdometryRed = frc2::InstantCommand( [&] {
drivetrain->resetOdometry(startPoseRed);
});
Expand Down Expand Up @@ -873,7 +871,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
shoot3Red->AddCommands
(cmd_setOdometryRed,
cmd_shooterAuto,
cmd_turretNoWrapAround,
cmd_intakeOne,
frc2::WaitCommand((units::second_t).125),
cmd_intakeClearDeque,
Expand All @@ -892,14 +889,12 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
frc2::WaitCommand((units::second_t).2),
cmd_intakeShoot,
frc2::WaitCommand((units::second_t).5),
cmd_intakeDisable,
cmd_turretWrapAround
cmd_intakeDisable
);
frc2::SequentialCommandGroup *shoot3Blue = new frc2::SequentialCommandGroup();
shoot3Blue->AddCommands
(cmd_setOdometryBlue,
cmd_shooterAuto,
cmd_turretNoWrapAround,
cmd_intakeOne,
frc2::WaitCommand((units::second_t).125),
cmd_intakeClearDeque,
Expand All @@ -918,16 +913,14 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
frc2::WaitCommand((units::second_t).2),
cmd_intakeShoot,
frc2::WaitCommand((units::second_t).5),
cmd_intakeDisable,
cmd_turretWrapAround
cmd_intakeDisable
);

frc2::SequentialCommandGroup *shoot5Red = new frc2::SequentialCommandGroup();
shoot5Red->AddCommands
(cmd_setOdometryRed,
cmd_turretDisable,
cmd_shooterAuto,
cmd_turretNoWrapAround,
cmd_intakeOne,
frc2::WaitCommand((units::second_t).125),
cmd_intakeClearDeque,
Expand Down Expand Up @@ -959,15 +952,13 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
cmd_move_moveShootRed,
cmd_shooterAuto,
frc2::WaitCommand((units::second_t).375),
cmd_intakeShoot,
cmd_turretWrapAround
cmd_intakeShoot
);
frc2::SequentialCommandGroup *shoot5Blue = new frc2::SequentialCommandGroup();
shoot5Blue->AddCommands
(cmd_setOdometryBlue,
cmd_turretDisable,
cmd_shooterAuto,
cmd_turretNoWrapAround,
cmd_intakeOne,
frc2::WaitCommand((units::second_t).125),
cmd_intakeClearDeque,
Expand Down Expand Up @@ -1001,15 +992,13 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
cmd_move_moveShootBlue,
cmd_shooterAuto,
frc2::WaitCommand((units::second_t).375),
cmd_intakeShoot,
cmd_turretWrapAround
cmd_intakeShoot
);

frc2::SequentialCommandGroup *shoot2Red = new frc2::SequentialCommandGroup();
shoot2Red->AddCommands
(cmd_set2ballOdometryRed,
cmd_intakeOne,
cmd_turretNoWrapAround,
frc2::WaitCommand((units::second_t).125),
cmd_intakeClearDeque,
cmd_intakeAuto,
Expand All @@ -1021,15 +1010,13 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
frc2::WaitCommand((units::second_t).5),
cmd_intakeShoot,
frc2::WaitCommand((units::second_t).5),
cmd_intakeDisable,
cmd_turretWrapAround
cmd_intakeDisable
);

frc2::SequentialCommandGroup *shoot2Blue = new frc2::SequentialCommandGroup();
shoot2Blue->AddCommands
(cmd_set2ballOdometryBlue,
cmd_intakeOne,
cmd_turretNoWrapAround,
frc2::WaitCommand((units::second_t).125),
cmd_intakeClearDeque,
cmd_intakeAuto,
Expand All @@ -1041,16 +1028,14 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
frc2::WaitCommand((units::second_t).5),
cmd_intakeShoot,
frc2::WaitCommand((units::second_t).5),
cmd_intakeDisable,
cmd_turretWrapAround
cmd_intakeDisable
);


frc2::SequentialCommandGroup *shoot2RedAlt = new frc2::SequentialCommandGroup();
shoot2RedAlt->AddCommands
(cmd_set2ballOdometryRed,
cmd_intakeOne,
cmd_turretNoWrapAround,
frc2::WaitCommand((units::second_t).125),
cmd_intakeClearDeque,
cmd_intakeAuto,
Expand All @@ -1073,15 +1058,13 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
cmd_turretHomeRight,
cmd_turretTrack,
frc2::WaitCommand((units::second_t).125),
cmd_shooterAuto,
cmd_turretWrapAround
cmd_shooterAuto
);

frc2::SequentialCommandGroup *shoot2BlueAlt = new frc2::SequentialCommandGroup();
shoot2BlueAlt->AddCommands
(cmd_set2ballOdometryBlue,
cmd_intakeOne,
cmd_turretNoWrapAround,
frc2::WaitCommand((units::second_t).125),
cmd_intakeClearDeque,
cmd_intakeAuto,
Expand All @@ -1104,8 +1087,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
cmd_turretHomeRight,
cmd_turretTrack,
frc2::WaitCommand((units::second_t).125),
cmd_shooterAuto,
cmd_turretWrapAround
cmd_shooterAuto
);

frc2::SequentialCommandGroup *shoot2Def2Red = new frc2::SequentialCommandGroup();
Expand All @@ -1114,7 +1096,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
cmd_intakeOne,
cmd_turretHomeLeft,
cmd_shooterAuto,
cmd_turretNoWrapAround,
frc2::WaitCommand((units::second_t).125),
cmd_intakeClearDeque,
cmd_intakeAuto,
Expand All @@ -1138,8 +1119,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
cmd_turretHomeRight,
cmd_turretTrack,
frc2::WaitCommand((units::second_t).125),
cmd_shooterAuto,
cmd_turretWrapAround
cmd_shooterAuto
);

frc2::SequentialCommandGroup *shoot2Def2Blue = new frc2::SequentialCommandGroup();
Expand All @@ -1148,7 +1128,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
cmd_intakeOne,
cmd_turretHomeLeft,
cmd_shooterAuto,
cmd_turretNoWrapAround,
frc2::WaitCommand((units::second_t).125),
cmd_intakeClearDeque,
cmd_intakeAuto,
Expand All @@ -1172,8 +1151,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
cmd_turretHomeRight,
cmd_turretTrack,
frc2::WaitCommand((units::second_t).125),
cmd_shooterAuto,
cmd_turretWrapAround
cmd_shooterAuto
);


Expand All @@ -1184,7 +1162,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
cmd_intakeOne,
cmd_turretHomeLeft,
cmd_shooterAuto,
cmd_turretNoWrapAround,
frc2::WaitCommand((units::second_t).125),
cmd_intakeClearDeque,
cmd_intakeAuto,
Expand All @@ -1208,8 +1185,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
cmd_turretHomeRight,
cmd_turretTrack,
frc2::WaitCommand((units::second_t).125),
cmd_shooterAuto,
cmd_turretWrapAround
cmd_shooterAuto
);

frc2::SequentialCommandGroup *shoot2Def2BlueNoCoast = new frc2::SequentialCommandGroup();
Expand All @@ -1218,7 +1194,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
cmd_intakeOne,
cmd_turretHomeLeft,
cmd_shooterAuto,
cmd_turretNoWrapAround,
frc2::WaitCommand((units::second_t).125),
cmd_intakeClearDeque,
cmd_intakeAuto,
Expand All @@ -1242,8 +1217,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
cmd_turretHomeRight,
cmd_turretTrack,
frc2::WaitCommand((units::second_t).125),
cmd_shooterAuto,
cmd_turretWrapAround
cmd_shooterAuto
);

m_chooser.AddOption("RED 2 ball", shoot2Red);
Expand Down

0 comments on commit d99c37d

Please sign in to comment.