Auto mode actually works now. Tested, and it should work when the hardware works.
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index 0efab8d..e330cfe 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -21,11 +21,12 @@
'../input/input.gyp:sensor_receiver',
'../input/input.gyp:GyroReader',
'../input/input.gyp:AutoMode',
+ #'../input/input.gyp:AutoMode',
'../../vision/vision.gyp:OpenCVWorkTask',
'../../vision/vision.gyp:GoalMaster',
'../output/output.gyp:MotorWriter',
'../output/output.gyp:CameraServer',
- 'camera/camera.gyp:frc971',
+ #'camera/camera.gyp:frc971',
'../../gyro_board/src/libusb-driver/libusb-driver.gyp:get',
],
'copies': [
diff --git a/frc971/atom_code/scripts/start_list.txt b/frc971/atom_code/scripts/start_list.txt
index e759946..cb3397b 100644
--- a/frc971/atom_code/scripts/start_list.txt
+++ b/frc971/atom_code/scripts/start_list.txt
@@ -2,7 +2,6 @@
JoystickReader
sensor_receiver
drivetrain
-AutoMode
BinaryLogReader
CRIOLogReader
CameraReader
@@ -12,3 +11,4 @@
wrist
index
shooter
+auto
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 99d0598..c7400d8 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -3,8 +3,11 @@
#include "aos/aos_core.h"
#include "aos/common/control_loop/Timing.h"
#include "aos/common/time.h"
+#include "aos/common/util/trapezoid_profile.h"
#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/DriveTrain.q.h"
+#include "aos/common/messages/RobotState.q.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/drivetrain/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"
@@ -15,63 +18,249 @@
namespace frc971 {
namespace autonomous {
+bool ShouldExitAuto() {
+ ::frc971::autonomous::autonomous.FetchLatest();
+ bool ans = !::frc971::autonomous::autonomous->run_auto;
+ if (ans) {
+ LOG(INFO, "Time to exit auto mode\n");
+ }
+ return ans;
+}
+
void SetShooterVelocity(double velocity) {
+ LOG(INFO, "Setting shooter velocity to %f\n", velocity);
control_loops::shooter.goal.MakeWithBuilder()
.velocity(velocity).Send();
}
-void StartIntake() {
- control_loops::intake.goal.MakeWithBuilder()
+void SetWristGoal(double goal) {
+ LOG(INFO, "Setting wrist to %f\n", goal);
+ control_loops::wrist.goal.MakeWithBuilder()
+ .goal(goal).Send();
+}
+
+void SetAngle_AdjustGoal(double goal) {
+ LOG(INFO, "Setting angle adjust to %f\n", goal);
+ control_loops::angle_adjust.goal.MakeWithBuilder()
+ .goal(goal).Send();
+}
+
+void StartIndex() {
+ LOG(INFO, "Starting index\n");
+ control_loops::index_loop.goal.MakeWithBuilder()
.goal_state(2).Send();
}
-void PreloadIntake() {
- control_loops::intake.goal.MakeWithBuilder()
+void PreloadIndex() {
+ LOG(INFO, "Preloading index\n");
+ control_loops::index_loop.goal.MakeWithBuilder()
.goal_state(3).Send();
}
-void ShootIntake() {
- control_loops::intake.goal.MakeWithBuilder()
+void ShootIndex() {
+ LOG(INFO, "Shooting index\n");
+ control_loops::index_loop.goal.MakeWithBuilder()
.goal_state(4).Send();
}
+void WaitForWrist() {
+ LOG(INFO, "Waiting for the wrist\n");
+ control_loops::wrist.status.FetchLatest();
+
+ while (!control_loops::wrist.status.get()) {
+ LOG(WARNING, "No previous wrist packet, trying to fetch again\n");
+ control_loops::wrist.status.FetchNextBlocking();
+ }
+
+ while (!control_loops::wrist.status->done) {
+ control_loops::wrist.status.FetchNextBlocking();
+ LOG(DEBUG, "Got a new wrist status packet\n");
+ if (ShouldExitAuto()) return;
+ }
+ LOG(INFO, "Done waiting for the wrist\n");
+}
+ // Index_loop has no FetchNextBlocking
+void WaitForIndex() {
+ LOG(INFO, "Waiting for the indexer to be ready to intake\n");
+ control_loops::index_loop.status.FetchLatest();
+
+ while (!control_loops::index_loop.status.get()) {
+ LOG(WARNING, "No previous index packet, trying to fetch again\n");
+ control_loops::index_loop.status.FetchNextBlocking();
+ }
+
+ while (!control_loops::index_loop.status->ready_to_intake) {
+ control_loops::index_loop.status.FetchNextBlocking();
+ if (ShouldExitAuto()) return;
+ }
+ LOG(INFO, "Indexer ready to intake\n");
+}
+
+void WaitForAngle_Adjust() {
+ LOG(INFO, "Waiting for the angle adjuster to finish\n");
+ control_loops::angle_adjust.status.FetchLatest();
+
+ while (!control_loops::angle_adjust.status.get()) {
+ LOG(WARNING, "No previous angle adjust packet, trying to fetch again\n");
+ control_loops::angle_adjust.status.FetchNextBlocking();
+ }
+
+ while (!control_loops::angle_adjust.status->done) {
+ control_loops::angle_adjust.status.FetchNextBlocking();
+ if (ShouldExitAuto()) return;
+ }
+ LOG(INFO, "Angle adjuster done\n");
+}
+
void WaitForShooter() {
+ LOG(INFO, "Waiting for the shooter to spin up\n");
control_loops::shooter.status.FetchLatest();
- while (!control_loops::shooter.status->ready) {
+
+ while (!control_loops::shooter.status.get()) {
+ LOG(WARNING, "No previous shooteracket, trying to fetch again\n");
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();
+ while (!control_loops::shooter.status->ready) {
+ control_loops::shooter.status.FetchNextBlocking();
+ if (ShouldExitAuto()) return;
}
+ LOG(INFO, "Shooter ready to shoot\n");
}
-bool ShouldExitAuto() {
- ::frc971::autonomous::autonomous.FetchLatest();
- return !::frc971::autonomous::autonomous->run_auto;
+void ShootNDiscs(int n) {
+ LOG(INFO, "Waiting until %d discs have been shot\n", n);
+ control_loops::index_loop.status.FetchLatest();
+
+ while (!control_loops::index_loop.status.get()) {
+ LOG(WARNING, "No previous index_loop packet, trying to fetch again\n");
+ control_loops::index_loop.status.FetchNextBlocking();
+ }
+
+ int final_disc_count = control_loops::index_loop.status->shot_disc_count + n;
+ LOG(DEBUG, "Disc count should be %d when done\n", final_disc_count);
+ while (final_disc_count > control_loops::index_loop.status->shot_disc_count) {
+ control_loops::index_loop.status.FetchNextBlocking();
+ if (ShouldExitAuto()) return;
+ }
+ LOG(INFO, "Shot %d discs\n", n);
}
-// 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?
-
+void StopDrivetrain() {
+ LOG(INFO, "Stopping the drivetrain\n");
control_loops::drivetrain.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .left_goal(0.0)
- .right_goal(0.0).Send();
+ .control_loop_driving(false)
+ .highgear(false)
+ .steering(0.0)
+ .throttle(0.0)
+ .quickturn(false)
+ .Send();
+}
+
+void SetDriveGoal(double yoffset) {
+ LOG(INFO, "Going to move %f\n", yoffset);
+
+ // Measured conversion to get the distance right.
+ yoffset *= 0.73;
+ LOG(INFO, "Going to move an adjusted %f\n", yoffset);
+ ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
+ ::Eigen::Matrix<double, 2, 1> driveTrainState;
+ const double goal_velocity = 0.0;
+ const double epsilon = 0.01;
+
+ profile.set_maximum_acceleration(2);
+ profile.set_maximum_velocity(0.8);
+
+ control_loops::drivetrain.position.FetchLatest();
+ while (!control_loops::drivetrain.position.get()) {
+ LOG(WARNING, "No previous drivetrain position packet, trying to fetch again\n");
+ control_loops::drivetrain.position.FetchNextBlocking();
+ }
+
+ const double left_initial_position =
+ control_loops::drivetrain.position->left_encoder;
+ const double right_initial_position =
+ control_loops::drivetrain.position->right_encoder;
+
+ while (true) {
+ ::aos::time::PhasedLoop10MS(5000); // wait until next 10ms tick
+ driveTrainState = profile.Update(yoffset, goal_velocity);
+
+ if (::std::abs(driveTrainState(0, 0) - yoffset) < epsilon) return;
+ if (ShouldExitAuto()) return;
+
+ LOG(DEBUG, "Driving left to %f, right to %f\n",
+ driveTrainState(0, 0) + left_initial_position,
+ driveTrainState(0, 0) + right_initial_position);
+ control_loops::drivetrain.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ .highgear(false)
+ .left_goal(driveTrainState(0, 0) + left_initial_position)
+ .right_goal(driveTrainState(0, 0) + right_initial_position)
+ .left_velocity_goal(driveTrainState(1, 0))
+ .right_velocity_goal(driveTrainState(1, 0))
+ .Send();
+ }
+ LOG(INFO, "Done moving\n");
+}
+
+ // start with N discs in the indexer
+void HandleAuto() {
+ LOG(INFO, "Handling auto mode\n");
+ double WRIST_UP;
+
+ ::aos::robot_state.FetchLatest();
+ if (::aos::robot_state.get() &&
+ !constants::wrist_hall_effect_start_angle(&WRIST_UP)) {
+ LOG(ERROR, "Constants not ready\n");
+ return;
+ }
+ WRIST_UP -= 0.4;
+ LOG(INFO, "Got constants\n");
+ const double WRIST_DOWN = -0.633;
+ const double ANGLE_ONE = 0.50;
+ const double ANGLE_TWO = 0.685;
+
+ StopDrivetrain();
+
+ SetWristGoal(WRIST_UP); // wrist must calibrate itself on power-up
+ SetAngle_AdjustGoal(ANGLE_ONE);
+ SetShooterVelocity(405.0);
+ PreloadIndex(); // spin to top and put 1 disc into loader
+
+ if (ShouldExitAuto()) return;
+ WaitForWrist();
+ if (ShouldExitAuto()) return;
+ WaitForAngle_Adjust();
+ ShootIndex(); // tilt up, shoot, repeat until empty
+ // calls WaitForShooter
+ ShootNDiscs(3); // ShootNDiscs returns if ShouldExitAuto
+ if (ShouldExitAuto()) return;
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.25));
+
+ SetWristGoal(WRIST_DOWN);
+ SetAngle_AdjustGoal(ANGLE_TWO);
+ SetShooterVelocity(375.0);
+ StartIndex(); // take in up to 4 discs
+
+ if (ShouldExitAuto()) return;
+ WaitForWrist(); // wrist must be down before moving
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.25));
+
+ if (ShouldExitAuto()) return;
+ WaitForIndex(); // ready to pick up discs
+
+ SetDriveGoal(0.75);
+ SetDriveGoal(2.75);
+ if (ShouldExitAuto()) return;
+
+ PreloadIndex();
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.4));
+ SetDriveGoal(-1.3);
+
+ if (ShouldExitAuto()) return;
+ WaitForAngle_Adjust();
+ ShootIndex();
}
} // namespace autonomous
diff --git a/frc971/autonomous/auto_main.cc b/frc971/autonomous/auto_main.cc
index 56b98fb..7102da9 100644
--- a/frc971/autonomous/auto_main.cc
+++ b/frc971/autonomous/auto_main.cc
@@ -13,18 +13,24 @@
::frc971::autonomous::autonomous.FetchLatest();
while (!::frc971::autonomous::autonomous.get()) {
- ::frc971::autonomous::autonomous.FetchNextBlocking();
+ ::frc971::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Got another auto packet\n");
}
while (true) {
while (!::frc971::autonomous::autonomous->run_auto) {
::frc971::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Got another auto packet\n");
}
+ LOG(INFO, "Starting auto mode\n");
::frc971::autonomous::HandleAuto();
+ LOG(INFO, "Auto mode exited, waiting for it to finish.\n");
while (::frc971::autonomous::autonomous->run_auto) {
::frc971::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Got another auto packet\n");
}
+ LOG(INFO, "Waiting for auto to start back up.\n");
}
::aos::Cleanup();
return 0;
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index 4892975..229ab40 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -31,10 +31,11 @@
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
'<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/frc971.gyp:common',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/common.gyp:timing',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
],
'export_dependent_settings': [
'<(AOS)/common/common.gyp:controls',
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index b160ae8..b40643e 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -7,11 +7,11 @@
#include "aos/atom_code/input/FRCComm.h"
#include "aos/atom_code/input/JoystickInput.h"
-#include "frc971/input/AutoMode.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/queues/Piston.q.h"
#include "frc971/control_loops/wrist/wrist_motor.q.h"
+#include "frc971/autonomous/auto.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"
@@ -40,11 +40,11 @@
if (Pressed(0, AUTONOMOUS)) {
if (PosEdge(0, ENABLED)){
LOG(INFO, "Starting auto mode\n");
- AutoMode.Start();
+ ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(true).Send();
}
if (NegEdge(0, ENABLED)) {
LOG(INFO, "Stopping auto mode\n");
- AutoMode.Stop();
+ ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(false).Send();
}
} else { // teleop
bool is_control_loop_driving = false;
@@ -104,6 +104,7 @@
}
// Where the wrist should be to pick up a frisbee.
+ // TODO(brians): Make these globally accessible and clean up auto.
static const double kWristPickup = -0.633;
static const double kWristNearGround = -0.4;
// Where the wrist gets stored when up.
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 47400d1..5ac253a 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -28,7 +28,8 @@
'<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
],
},
{
@@ -47,7 +48,6 @@
'<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
],
},
{
@@ -99,19 +99,5 @@
'<(AOS)/build/aos.gyp:logging',
],
},
- {
- 'target_name': 'AutoMode',
- 'type': 'executable',
- 'sources': [
- 'AutoMode.cc',
- ],
- 'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- 'actions',
-# TODO(brians) this shouldn't need to be here
- '<(AOS)/atom_code/atom_code.gyp:init',
- ],
- },
],
}