| #include "stdio.h" |
| |
| #include "aos/aos_core.h" |
| #include "aos/common/control_loop/Timing.h" |
| #include "aos/common/time.h" |
| #include "frc971/autonomous/auto.q.h" |
| #include "frc971/control_loops/DriveTrain.q.h" |
| #include "frc971/control_loops/wrist/wrist_motor.q.h" |
| #include "frc971/control_loops/index/index_motor.q.h" |
| #include "frc971/control_loops/shooter/shooter_motor.q.h" |
| #include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h" |
| |
| using ::aos::time::Time; |
| |
| namespace frc971 { |
| namespace autonomous { |
| |
| void SetShooterVelocity(double velocity) { |
| control_loops::shooter.goal.MakeWithBuilder() |
| .velocity(velocity).Send(); |
| } |
| |
| void StartIntake() { |
| control_loops::intake.goal.MakeWithBuilder() |
| .goal_state(2).Send(); |
| } |
| |
| void PreloadIntake() { |
| control_loops::intake.goal.MakeWithBuilder() |
| .goal_state(3).Send(); |
| } |
| |
| void ShootIntake() { |
| control_loops::intake.goal.MakeWithBuilder() |
| .goal_state(4).Send(); |
| } |
| |
| void WaitForShooter() { |
| control_loops::shooter.status.FetchLatest(); |
| while (!control_loops::shooter.status->ready) { |
| control_loops::shooter.status.FetchNextBlocking(); |
| } |
| } |
| |
| // TODO(aschuh): Send the number of discs shot in the status message. |
| void ShootNDiscs(int n) { |
| control_loops::intake.status.FetchLatest(); |
| while (!control_loops::intake.status->ready) { |
| control_loops::intake.status.FetchNextBlocking(); |
| } |
| } |
| |
| bool ShouldExitAuto() { |
| ::frc971::autonomous::autonomous.FetchLatest(); |
| return !::frc971::autonomous::autonomous->run_auto; |
| } |
| |
| // TODO(aschuh): Drivetrain needs a profile somewhere. |
| // Here isn't the best spot. It should be in another thread/process |
| |
| void HandleAuto() { |
| SetShooterVelocity(200.0); |
| PreloadIntake(); |
| |
| WaitForShooter(); |
| ShootIntake(); |
| |
| // Wait for the wrist and angle adjust to finish zeroing before shooting. |
| // Sigh, constants go where? |
| |
| control_loops::drivetrain.goal.MakeWithBuilder() |
| .control_loop_driving(true) |
| .left_goal(0.0) |
| .right_goal(0.0).Send(); |
| } |
| |
| } // namespace autonomous |
| } // namespace frc971 |