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
diff --git a/bot3/autonomous/auto.h b/bot3/autonomous/auto.h
new file mode 100644
index 0000000..896d22c
--- /dev/null
+++ b/bot3/autonomous/auto.h
@@ -0,0 +1,12 @@
+#ifndef BOT3_AUTONOMOUS_AUTO_H_
+#define BOT3_AUTONOMOUS_AUTO_H_
+
+namespace bot3 {
+namespace autonomous {
+
+void HandleAuto();
+
+} // namespace autonomous
+} // namespace bot3
+
+#endif // BOT3_AUTONOMOUS_AUTO_H_
diff --git a/bot3/autonomous/auto_main.cc b/bot3/autonomous/auto_main.cc
new file mode 100644
index 0000000..eb8f865
--- /dev/null
+++ b/bot3/autonomous/auto_main.cc
@@ -0,0 +1,42 @@
+#include <stdio.h>
+
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "bot3/autonomous/auto.h"
+#include "frc971/autonomous/auto.q.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ LOG(INFO, "Auto main started\n");
+ ::frc971::autonomous::autonomous.FetchLatest();
+ while (!::frc971::autonomous::autonomous.get()) {
+ ::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");
+ ::aos::time::Time start_time = ::aos::time::Time::Now();
+ ::bot3::autonomous::HandleAuto();
+
+ ::aos::time::Time elapsed_time = ::aos::time::Time::Now() - start_time;
+ LOG(INFO, "Auto mode exited in %f, waiting for it to finish.\n",
+ elapsed_time.ToSeconds());
+ 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/bot3/autonomous/autonomous.gyp b/bot3/autonomous/autonomous.gyp
new file mode 100644
index 0000000..b70d178
--- /dev/null
+++ b/bot3/autonomous/autonomous.gyp
@@ -0,0 +1,48 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'auto_queue',
+ 'type': 'static_library',
+ 'sources': [
+ '<(DEPTH)/frc971/autonomous/auto.q',
+ ],
+ 'variables': {
+ 'header_path': 'frc971/autonomous',
+ },
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'auto_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'auto.cc',
+ ],
+ 'dependencies': [
+ 'auto_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(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',
+ ],
+ },
+ {
+ 'target_name': 'auto',
+ 'type': 'executable',
+ 'sources': [
+ 'auto_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'auto_queue',
+ 'auto_lib',
+ ],
+ },
+ ],
+}