Reorganize directory structure for third robot.
I added a "bot3" directory, so that we can have all the third
robot specific stuff live there.
Most of these changes were just copying files from frc971. Many
of the files have no modifications except changed paths in the
include directives, changed paths in gyp files, and changed
namespace names. Some of the code is new, however, for instance,
I modified motor_writer to control the motors and pistons on
the third robot, as well as added a new "rollers" queue to
transfer data pertaining to these.
I also updated the build system so that builds for the normal
robot code and the third robot code would in no way conflict
with each other. Finally, I figured out how I want to handle
the constants, although I haven't put actual values for
that in yet.
I cleaned up the python code, adding in the small changes to
that which Comran wanted.
Change-Id: I04e17dc8b17a980338b718a78e348ea647ec060b
diff --git a/bot3/autonomous/auto.cc b/bot3/autonomous/auto.cc
new file mode 100644
index 0000000..05d84b1
--- /dev/null
+++ b/bot3/autonomous/auto.cc
@@ -0,0 +1,117 @@
+#include <stdio.h>
+
+#include <memory>
+
+#include "aos/common/util/phased_loop.h"
+#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/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/queues/other_sensors.q.h"
+
+using ::aos::time::Time;
+
+namespace bot3 {
+namespace autonomous {
+
+namespace time = ::aos::time;
+
+static double left_initial_position, right_initial_position;
+
+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 StopDrivetrain() {
+ LOG(INFO, "Stopping the drivetrain\n");
+ control_loops::drivetrain.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ .left_goal(left_initial_position)
+ .left_velocity_goal(0)
+ .right_goal(right_initial_position)
+ .right_velocity_goal(0)
+ .quickturn(false)
+ .Send();
+}
+
+void ResetDrivetrain() {
+ LOG(INFO, "resetting the drivetrain\n");
+ control_loops::drivetrain.goal.MakeWithBuilder()
+ .control_loop_driving(false)
+ .highgear(false)
+ .steering(0.0)
+ .throttle(0.0)
+ .left_goal(left_initial_position)
+ .left_velocity_goal(0)
+ .right_goal(right_initial_position)
+ .right_velocity_goal(0)
+ .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;
+
+ 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();
+ }
+ left_initial_position -= side_offset;
+ right_initial_position += side_offset;
+ LOG(INFO, "Done moving\n");
+}
+
+void InitializeEncoders() {
+ control_loops::drivetrain.status.FetchLatest();
+ while (!control_loops::drivetrain.status.get()) {
+ LOG(WARNING,
+ "No previous drivetrain position packet, trying to fetch again\n");
+ control_loops::drivetrain.status.FetchNextBlocking();
+ }
+ left_initial_position =
+ control_loops::drivetrain.status->filtered_left_position;
+ right_initial_position =
+ control_loops::drivetrain.status->filtered_right_position;
+
+}
+
+void HandleAuto() {
+ //TODO (danielp): Implement this.
+}
+
+} // namespace autonomous
+} // namespace bot3