Finally add auto mode.
It is pretty basic now, it just drives forward and puts a ball
in the low goal. This did involve copying over some of the
action stuff, though, and I fixed a couple silly issues in
the frc971 versions while I was at it.
Change-Id: Idc18ea63dd74ba8ba5405264f212ebe993084aa9
diff --git a/bot3/actions/actions.gyp b/bot3/actions/actions.gyp
new file mode 100644
index 0000000..55af1aa
--- /dev/null
+++ b/bot3/actions/actions.gyp
@@ -0,0 +1,99 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'action_client',
+ 'type': 'static_library',
+ 'sources': [
+ #'<(DEPTH)/frc971/actions/action_client.h',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ },
+ {
+ 'target_name': 'action_queue',
+ 'type': 'static_library',
+ 'sources': [
+ '<(DEPTH)/frc971/actions/action.q',
+ ],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'drivetrain_action_queue',
+ 'type': 'static_library',
+ 'sources': [
+ '<(DEPTH)/frc971/actions/drivetrain_action.q',
+ ],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
+ 'dependencies': [
+ 'action_queue',
+ ],
+ 'export_dependent_settings': [
+ 'action_queue',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'drivetrain_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'drivetrain_action.cc',
+ ],
+ 'dependencies': [
+ 'drivetrain_action_queue',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/build/aos.gyp:logging',
+ 'action_client',
+ 'action',
+ '<(EXTERNALS):eigen',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:polydrivetrain_plants',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ ],
+ 'export_dependent_settings': [
+ 'action',
+ 'drivetrain_action_queue',
+ 'action_client',
+ ],
+ },
+ {
+ 'target_name': 'action',
+ 'type': 'static_library',
+ 'sources': [
+ #'action.h',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_action',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'drivetrain_action_queue',
+ 'drivetrain_action_lib',
+ 'action',
+ ],
+ },
+ ],
+}
diff --git a/bot3/actions/drivetrain_action.cc b/bot3/actions/drivetrain_action.cc
new file mode 100644
index 0000000..93b3d46
--- /dev/null
+++ b/bot3/actions/drivetrain_action.cc
@@ -0,0 +1,164 @@
+#include <functional>
+#include <numeric>
+
+#include <Eigen/Dense>
+
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "aos/common/commonmath.h"
+
+#include "bot3/actions/drivetrain_action.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
+#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+
+namespace bot3 {
+namespace actions {
+
+DrivetrainAction::DrivetrainAction(::frc971::actions::DrivetrainActionQueueGroup* s)
+ : ::frc971::actions::ActionBase
+ <::frc971::actions::DrivetrainActionQueueGroup>(s) {}
+
+void DrivetrainAction::RunAction() {
+ static const auto K = control_loops::MakeDrivetrainLoop().K();
+
+ const double yoffset = action_q_->goal->y_offset;
+ const double turn_offset =
+ action_q_->goal->theta_offset * control_loops::kBot3TurnWidth / 2.0;
+ LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
+
+ // Measured conversion to get the distance right.
+ ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
+ ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(10));
+ const double goal_velocity = 0.0;
+ const double epsilon = 0.01;
+ ::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
+
+ profile.set_maximum_acceleration(action_q_->goal->maximum_acceleration);
+ profile.set_maximum_velocity(action_q_->goal->maximum_velocity);
+ turn_profile.set_maximum_acceleration(
+ 10.0 * control_loops::kBot3TurnWidth / 2.0);
+ turn_profile.set_maximum_velocity(3.0 * control_loops::kBot3TurnWidth / 2.0);
+
+ while (true) {
+ // wait until next 10ms tick
+ ::aos::time::PhasedLoop10MS(5000);
+
+ control_loops::drivetrain.status.FetchLatest();
+ if (control_loops::drivetrain.status.get()) {
+ const auto &status = *control_loops::drivetrain.status;
+ if (::std::abs(status.uncapped_left_voltage -
+ status.uncapped_right_voltage) >
+ 24) {
+ LOG(DEBUG, "spinning in place\n");
+ // They're more than 24V apart, so stop moving forwards and let it deal
+ // with spinning first.
+ profile.SetGoal(
+ (status.filtered_left_position + status.filtered_right_position) /
+ 2.0);
+ } else {
+ static const double divisor = K(0, 0) + K(0, 2);
+ double dx_left, dx_right;
+ if (status.uncapped_left_voltage > 12.0) {
+ dx_left = (status.uncapped_left_voltage - 12.0) / divisor;
+ } else if (status.uncapped_left_voltage < -12.0) {
+ dx_left = (status.uncapped_left_voltage + 12.0) / divisor;
+ } else {
+ dx_left = 0;
+ }
+ if (status.uncapped_right_voltage > 12.0) {
+ dx_right = (status.uncapped_right_voltage - 12.0) / divisor;
+ } else if (status.uncapped_right_voltage < -12.0) {
+ dx_right = (status.uncapped_right_voltage + 12.0) / divisor;
+ } else {
+ dx_right = 0;
+ }
+ double dx;
+ if (dx_left == 0 && dx_right == 0) {
+ dx = 0;
+ } else if (dx_left != 0 && dx_right != 0 &&
+ ::aos::sign(dx_left) != ::aos::sign(dx_right)) {
+ // Both saturating in opposite directions. Don't do anything.
+ dx = 0;
+ } else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
+ dx = dx_left;
+ } else {
+ dx = dx_right;
+ }
+ if (dx != 0) {
+ LOG(DEBUG, "adjusting goal by %f\n", dx);
+ profile.MoveGoal(-dx);
+ }
+ }
+ } else {
+ // If we ever get here, that's bad and we should just give up
+ LOG(FATAL, "no drivetrain status!\n");
+ }
+
+ const auto drive_profile_goal_state =
+ profile.Update(yoffset, goal_velocity);
+ const auto turn_profile_goal_state = turn_profile.Update(turn_offset, 0.0);
+ left_goal_state = drive_profile_goal_state - turn_profile_goal_state;
+ right_goal_state = drive_profile_goal_state + turn_profile_goal_state;
+
+ if (::std::abs(drive_profile_goal_state(0, 0) - yoffset) < epsilon &&
+ ::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
+ break;
+ }
+ if (ShouldCancel()) return;
+
+ LOG(DEBUG, "Driving left to %f, right to %f\n",
+ left_goal_state(0, 0) + action_q_->goal->left_initial_position,
+ right_goal_state(0, 0) + action_q_->goal->right_initial_position);
+ control_loops::drivetrain.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ .highgear(false)
+ .left_goal(left_goal_state(0, 0) + action_q_->goal->left_initial_position)
+ .right_goal(right_goal_state(0, 0) + action_q_->goal->right_initial_position)
+ .left_velocity_goal(left_goal_state(1, 0))
+ .right_velocity_goal(right_goal_state(1, 0))
+ .Send();
+ }
+ if (ShouldCancel()) return;
+ control_loops::drivetrain.status.FetchLatest();
+ while (!control_loops::drivetrain.status.get()) {
+ LOG(WARNING,
+ "No previous drivetrain status packet, trying to fetch again\n");
+ control_loops::drivetrain.status.FetchNextBlocking();
+ if (ShouldCancel()) return;
+ }
+ while (true) {
+ if (ShouldCancel()) return;
+ const double kPositionThreshold = 0.05;
+
+ const double left_error = ::std::abs(
+ control_loops::drivetrain.status->filtered_left_position -
+ (left_goal_state(0, 0) + action_q_->goal->left_initial_position));
+ const double right_error = ::std::abs(
+ control_loops::drivetrain.status->filtered_right_position -
+ (right_goal_state(0, 0) + action_q_->goal->right_initial_position));
+ const double velocity_error =
+ ::std::abs(control_loops::drivetrain.status->robot_speed);
+ if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
+ velocity_error < 0.2) {
+ break;
+ } else {
+ LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
+ velocity_error);
+ }
+ control_loops::drivetrain.status.FetchNextBlocking();
+ }
+ LOG(INFO, "Done moving\n");
+}
+
+::std::unique_ptr<::frc971::TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
+MakeDrivetrainAction() {
+ return ::std::unique_ptr<
+ ::frc971::TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>(
+ new ::frc971::TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>(
+ &::frc971::actions::drivetrain_action));
+}
+
+} // namespace actions
+} // namespace bot3
diff --git a/bot3/actions/drivetrain_action.h b/bot3/actions/drivetrain_action.h
new file mode 100644
index 0000000..fc4b837
--- /dev/null
+++ b/bot3/actions/drivetrain_action.h
@@ -0,0 +1,29 @@
+#ifndef BOT3_ACTIONS_DRIVETRAIN_ACTION_H_
+#define BOT3_ACTIONS_DRIVETRAIN_ACTION_H_
+
+#include <memory>
+
+#include "frc971/actions/action.h"
+#include "frc971/actions/action_client.h"
+#include "frc971/actions/drivetrain_action.q.h"
+
+namespace bot3 {
+namespace actions {
+
+class DrivetrainAction : public
+ ::frc971::actions::ActionBase<::frc971::actions::DrivetrainActionQueueGroup> {
+ public:
+ explicit DrivetrainAction(::frc971::actions::DrivetrainActionQueueGroup* s);
+
+ virtual void RunAction();
+};
+
+// Makes a new DrivetrainAction action.
+::std::unique_ptr<::frc971::TypedAction
+ < ::frc971::actions::DrivetrainActionQueueGroup>>
+MakeDrivetrainAction();
+
+} // namespace actions
+} // namespace bot3
+
+#endif
diff --git a/bot3/actions/drivetrain_action_main.cc b/bot3/actions/drivetrain_action_main.cc
new file mode 100644
index 0000000..d68a3bc
--- /dev/null
+++ b/bot3/actions/drivetrain_action_main.cc
@@ -0,0 +1,19 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "bot3/actions/drivetrain_action.h"
+#include "frc971/actions/drivetrain_action.q.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ bot3::actions::DrivetrainAction drivetrain(&::frc971::actions::drivetrain_action);
+ drivetrain.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/bot3/autonomous/auto.cc b/bot3/autonomous/auto.cc
index 05d84b1..ab9ffa5 100644
--- a/bot3/autonomous/auto.cc
+++ b/bot3/autonomous/auto.cc
@@ -6,9 +6,13 @@
#include "aos/common/time.h"
#include "aos/common/util/trapezoid_profile.h"
#include "aos/common/logging/logging.h"
-#include "aos/common/logging/queue_logging.h"
+#include "bot3/actions/drivetrain_action.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
+#include "bot3/control_loops/rollers/rollers.q.h"
+#include "frc971/actions/action_client.h"
+#include "frc971/actions/drivetrain_action.q.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/queues/other_sensors.q.h"
@@ -56,43 +60,35 @@
.Send();
}
-void DriveSpin(double radians) {
- LOG(INFO, "going to spin %f\n", radians);
-
- ::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;
- // in drivetrain "meters"
- const double kRobotWidth = 0.4544;
-
- profile.set_maximum_acceleration(1.5);
- profile.set_maximum_velocity(0.8);
-
- const double side_offset = kRobotWidth * radians / 2.0;
-
+void WaitUntilDoneOrCanceled(::frc971::Action *action) {
while (true) {
- ::aos::time::PhasedLoop10MS(5000); // wait until next 10ms tick
- driveTrainState = profile.Update(side_offset, goal_velocity);
-
- if (::std::abs(driveTrainState(0, 0) - side_offset) < epsilon) break;
- if (ShouldExitAuto()) return;
-
- LOG(DEBUG, "Driving left to %f, right to %f\n",
- left_initial_position - driveTrainState(0, 0),
- right_initial_position + driveTrainState(0, 0));
- control_loops::drivetrain.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(false)
- .left_goal(left_initial_position - driveTrainState(0, 0))
- .right_goal(right_initial_position + driveTrainState(0, 0))
- .left_velocity_goal(-driveTrainState(1, 0))
- .right_velocity_goal(driveTrainState(1, 0))
- .Send();
+ // Poll the running bit and auto done bits.
+ ::aos::time::PhasedLoop10MS(5000);
+ if (!action->Running() || ShouldExitAuto()) {
+ return;
+ }
}
- left_initial_position -= side_offset;
- right_initial_position += side_offset;
- LOG(INFO, "Done moving\n");
+}
+
+::std::unique_ptr<::frc971::TypedAction
+ < ::frc971::actions::DrivetrainActionQueueGroup>>
+SetDriveGoal(double distance, bool slow_acceleration,
+ double maximum_velocity = 1.7, double theta = 0) {
+ LOG(INFO, "Driving to %f\n", distance);
+ auto drivetrain_action = actions::MakeDrivetrainAction();
+ drivetrain_action->GetGoal()->left_initial_position = left_initial_position;
+ drivetrain_action->GetGoal()->right_initial_position = right_initial_position;
+ drivetrain_action->GetGoal()->y_offset = distance;
+ drivetrain_action->GetGoal()->theta_offset = theta;
+ drivetrain_action->GetGoal()->maximum_velocity = maximum_velocity;
+ drivetrain_action->GetGoal()->maximum_acceleration =
+ slow_acceleration ? 2.5 : 3.0;
+ drivetrain_action->Start();
+ left_initial_position +=
+ distance - theta * control_loops::kBot3TurnWidth / 2.0;
+ right_initial_position +=
+ distance + theta * control_loops::kBot3TurnWidth / 2.0;
+ return ::std::move(drivetrain_action);
}
void InitializeEncoders() {
@@ -109,8 +105,72 @@
}
+void StopRollers() {
+ control_loops::rollers.goal.MakeWithBuilder()
+ .intake(0)
+ .low_spit(0)
+ .human_player(false)
+ .Send();
+}
+
+void SpitBallFront() {
+ control_loops::rollers.goal.MakeWithBuilder()
+ .intake(0)
+ .low_spit(1)
+ .human_player(false)
+ .Send();
+ time::SleepFor(time::Time::InSeconds(1));
+ StopRollers();
+}
+
+void IntakeBallBack() {
+ control_loops::rollers.goal.MakeWithBuilder()
+ .intake(-1)
+ .low_spit(0)
+ .human_player(false)
+ .Send();
+ time::SleepFor(time::Time::InSeconds(1.5));
+ StopRollers();
+}
+
+void ScoreBall(const double distance, const double velocity) {
+ // Drive to the goal, score, and drive back.
+ {
+ // Drive forward.
+ auto drivetrain_action = SetDriveGoal(distance,
+ false, velocity);
+ LOG(INFO, "Waiting until drivetrain is finished.\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ time::SleepFor(time::Time::InSeconds(0.5));
+ if (ShouldExitAuto()) return;
+ }
+ {
+ LOG(INFO, "Spitting ball.\n");
+ SpitBallFront();
+ time::SleepFor(time::Time::InSeconds(0.5));
+ if (ShouldExitAuto()) return;
+ }
+ {
+ // Drive back.
+ LOG(INFO, "Driving back.\n");
+ auto drivetrain_action = SetDriveGoal(-distance,
+ false, velocity);
+ LOG(INFO, "Waiting until drivetrain is finished.\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
+}
+
void HandleAuto() {
- //TODO (danielp): Implement this.
+ constexpr double kDriveDistance = 4.86;
+ constexpr double kAutoVelocity = 2.5;
+
+ if (ShouldExitAuto()) return;
+ ResetDrivetrain();
+ InitializeEncoders();
+ if (ShouldExitAuto()) return;
+
+ ScoreBall(kDriveDistance, kAutoVelocity);
}
} // namespace autonomous
diff --git a/bot3/autonomous/autonomous.gyp b/bot3/autonomous/autonomous.gyp
index b70d178..01d95d0 100644
--- a/bot3/autonomous/autonomous.gyp
+++ b/bot3/autonomous/autonomous.gyp
@@ -21,12 +21,14 @@
'auto_queue',
'<(AOS)/common/controls/controls.gyp:control_loop',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/rollers/rollers.gyp:rollers_loop',
+ '<(DEPTH)/bot3/actions/actions.gyp:action_client',
+ '<(DEPTH)/bot3/actions/actions.gyp:drivetrain_action_lib',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/util/util.gyp:phased_loop',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
'<(AOS)/build/aos.gyp:logging',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'<(AOS)/common/controls/controls.gyp:control_loop',
diff --git a/bot3/prime/prime.gyp b/bot3/prime/prime.gyp
index ebc0965..986f9fa 100644
--- a/bot3/prime/prime.gyp
+++ b/bot3/prime/prime.gyp
@@ -11,6 +11,7 @@
'../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
'../control_loops/rollers/rollers.gyp:rollers',
'../autonomous/autonomous.gyp:auto',
+ '../actions/actions.gyp:drivetrain_action',
'../input/input.gyp:joystick_reader',
'../output/output.gyp:motor_writer',
'../input/input.gyp:sensor_receiver',
diff --git a/bot3/prime/start_list.txt b/bot3/prime/start_list.txt
index 8606b0b..6b08ba0 100644
--- a/bot3/prime/start_list.txt
+++ b/bot3/prime/start_list.txt
@@ -2,7 +2,9 @@
motor_writer
joystick_reader
drivetrain
+drivetrain_action
auto
sensor_receiver
joystick_proxy
rollers
+auto
diff --git a/frc971/actions/action.h b/frc971/actions/action.h
index 1fc50ea..1243e58 100644
--- a/frc971/actions/action.h
+++ b/frc971/actions/action.h
@@ -9,8 +9,6 @@
#include "aos/common/logging/logging.h"
#include "aos/common/time.h"
-#include "frc971/constants.h"
-
namespace frc971 {
namespace actions {
diff --git a/frc971/actions/actions.gyp b/frc971/actions/actions.gyp
index 0ddd61c..ff18204 100644
--- a/frc971/actions/actions.gyp
+++ b/frc971/actions/actions.gyp
@@ -140,12 +140,10 @@
],
'dependencies': [
'<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/common.gyp:time',
],
'export_dependent_settings': [
'<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/common.gyp:time',
],
},
diff --git a/frc971/actions/catch_action.cc b/frc971/actions/catch_action.cc
index 350b6f6..7b67645 100644
--- a/frc971/actions/catch_action.cc
+++ b/frc971/actions/catch_action.cc
@@ -1,3 +1,4 @@
+#include <complex>
#include <functional>
#include "aos/common/logging/logging.h"
diff --git a/frc971/actions/drivetrain_action.h b/frc971/actions/drivetrain_action.h
index b164e22..0a256b8 100644
--- a/frc971/actions/drivetrain_action.h
+++ b/frc971/actions/drivetrain_action.h
@@ -1,3 +1,6 @@
+#ifndef FRC971_ACTIONS_DRIVETRAIN_ACTION_H_
+#define FRC971_ACTIONS_DRIVETRAIN_ACTION_H_
+
#include <memory>
#include "frc971/actions/drivetrain_action.q.h"
@@ -20,3 +23,5 @@
} // namespace actions
} // namespace frc971
+
+#endif
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index af29c46..29722e4 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -193,8 +193,7 @@
left_initial_position +=
distance - theta * constants::GetValues().turn_width / 2.0;
right_initial_position +=
- distance + theta * constants::GetValues().turn_width / 2. -
- theta * constants::GetValues().turn_width / 2.00;
+ distance + theta * constants::GetValues().turn_width / 2.0;
return ::std::move(drivetrain_action);
}