working auto changes

Author: AntarcticaByToto

Competition/src/main/cpp/ValorAuto.cpp

@@ -51,11 +51,11 @@ ValorAuto::ValorAuto(Drivetrain* _drivetrain, Intake* _intake, Shooter* _shooter
 
     // SHOOT 3 MOVE 5 OFFSET POSITION
     auto traj_move5_offset = frc::TrajectoryGenerator::GenerateTrajectory(frc::Pose2d(0_m, 0_m,frc::Rotation2d(0_deg)),
-                                                                    { frc::Translation2d(2_m, 0.6_m)},
-                                                                    frc::Pose2d(5.6_m, 1_m, frc::Rotation2d(0_deg)),
+                                                                    { frc::Translation2d(2_m, 0.5_m)},
+                                                                    frc::Pose2d(5.6_m, 0.9_m, frc::Rotation2d(0_deg)),
                                                                     kTrajectoryConfigForward);
 
-    auto traj_reverse_move5_offset = frc::TrajectoryGenerator::GenerateTrajectory(frc::Pose2d(5.6_m, 1_m, frc::Rotation2d(0_deg)),
+    auto traj_reverse_move5_offset = frc::TrajectoryGenerator::GenerateTrajectory(frc::Pose2d(5.6_m, 0.9_m, frc::Rotation2d(0_deg)),
                                                                     { },
                                                                     frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)),
                                                                     kTrajectoryConfigReverse);
@@ -104,6 +104,39 @@ ValorAuto::ValorAuto(Drivetrain* _drivetrain, Intake* _intake, Shooter* _shooter
                                     [&] (auto left, auto right) { drivetrain->TankDriveVolts(left, right); },
                                     {drivetrain});
 
+    frc2::RamseteCommand cmd_move5move5_first(traj_move5,
+                                    [&] () { return drivetrain->GetPose(); },
+                                    frc::RamseteController(RamseteConstants::kRamseteB, RamseteConstants::kRamseteZeta),
+                                    kSimpleMotorFeedforward,
+                                    kDriveKinematics,
+                                    [&] { return drivetrain->GetWheelSpeeds(); },
+                                    frc2::PIDController(RamseteConstants::kPDriveVel, 0, 0),
+                                    frc2::PIDController(RamseteConstants::kPDriveVel, 0, 0),
+                                    [&] (auto left, auto right) { drivetrain->TankDriveVolts(left, right); },
+                                    {drivetrain});
+
+    frc2::RamseteCommand cmd_reverse_move5move5(traj_reverse_move5,
+                                    [&] () { return drivetrain->GetPose(); },
+                                    frc::RamseteController(RamseteConstants::kRamseteB, RamseteConstants::kRamseteZeta),
+                                    kSimpleMotorFeedforward,
+                                    kDriveKinematics,
+                                    [&] { return drivetrain->GetWheelSpeeds(); },
+                                    frc2::PIDController(RamseteConstants::kPDriveVel, 0, 0),
+                                    frc2::PIDController(RamseteConstants::kPDriveVel, 0, 0),
+                                    [&] (auto left, auto right) { drivetrain->TankDriveVolts(left, right); },
+                                    {drivetrain});
+
+    frc2::RamseteCommand cmd_move5move5_second(traj_move5,
+                                    [&] () { return drivetrain->GetPose(); },
+                                    frc::RamseteController(RamseteConstants::kRamseteB, RamseteConstants::kRamseteZeta),
+                                    kSimpleMotorFeedforward,
+                                    kDriveKinematics,
+                                    [&] { return drivetrain->GetWheelSpeeds(); },
+                                    frc2::PIDController(RamseteConstants::kPDriveVel, 0, 0),
+                                    frc2::PIDController(RamseteConstants::kPDriveVel, 0, 0),
+                                    [&] (auto left, auto right) { drivetrain->TankDriveVolts(left, right); },
+                                    {drivetrain});
+
     frc2::SequentialCommandGroup *shoot3move5 = new frc2::SequentialCommandGroup();
     shoot3move5->AddCommands(cmd_spoolUp,
                              frc2::WaitCommand((units::second_t)1.5),
@@ -122,7 +155,8 @@ ValorAuto::ValorAuto(Drivetrain* _drivetrain, Intake* _intake, Shooter* _shooter
     autos["Shoot3Move5"] = shoot3move5;
 
     frc2::SequentialCommandGroup *shoot3move5Offset = new frc2::SequentialCommandGroup();
-    shoot3move5Offset->AddCommands(cmd_spoolUp,
+    shoot3move5Offset->AddCommands(cmd_track,
+                             cmd_spoolUp,
                              frc2::WaitCommand((units::second_t)1.5),
                              cmd_intake,
                              cmd_shoot,
@@ -141,6 +175,7 @@ ValorAuto::ValorAuto(Drivetrain* _drivetrain, Intake* _intake, Shooter* _shooter
     frc2::SequentialCommandGroup *shoot3 = new frc2::SequentialCommandGroup();
     shoot3->AddCommands(cmd_spoolUp,
                         frc2::WaitCommand((units::second_t)2),
+                        cmd_intake,
                         cmd_shoot,
                         frc2::WaitCommand((units::second_t)3),
                         cmd_stopShoot,
@@ -154,15 +189,15 @@ ValorAuto::ValorAuto(Drivetrain* _drivetrain, Intake* _intake, Shooter* _shooter
                              cmd_shoot,
                              frc2::WaitCommand((units::second_t)1.5),
                              cmd_stopShoot,
-                             std::move(cmd_move5),
-                             std::move(cmd_reverse_move5),
+                             std::move(cmd_move5move5_first),
+                             std::move(cmd_reverse_move5move5),
                              cmd_track,
-                            frc2::WaitCommand((units::second_t)1),
+                             frc2::WaitCommand((units::second_t)1),
                              cmd_shoot,
                              frc2::WaitCommand((units::second_t)2),
                              cmd_stopShoot,
                              cmd_spoolDown,
-                             std::move(cmd_move5));
+                             std::move(cmd_move5move5_second));
     autos["Shoot3Move5Move5"] = shoot3move5move5;
 }