blob: 99d0598e7b52c04b00af289d92f11b46eac1e0e0 [file] [log] [blame]
#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