Copied over and modified drivetrain framework.
Sorry for the truly massive commit, there's not really a good way
to split it up, as then things will stop compiling.
Needless to say, stuff does compile but remains as of yet
untested.
Among the changes:
- Copied over constants for drivetrain_motor_plant from 2012.
- Completely (for now) disabled control loop driving, as Brian
said those loops would be too much work to tune.
- Put stuff in the proper namespaces and fixed the bazillion
resulting broken refs.
- Copied autonomous framework, but it currectly does nothing.
(I intend for this to change in the future.)
In it's current state, the work on this branch should render
the robot driveable, but that's about it.
diff --git a/bot3/atom_code/atom_code.gyp b/bot3/atom_code/atom_code.gyp
index 6dbcc77..337e298 100644
--- a/bot3/atom_code/atom_code.gyp
+++ b/bot3/atom_code/atom_code.gyp
@@ -5,11 +5,11 @@
'type': 'none',
'dependencies': [
'<(AOS)/build/aos_all.gyp:Atom',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_lib_test',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter',
- '<(DEPTH)/frc971/autonomous/autonomous.gyp:auto',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
+ #'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_lib_test',
+ #'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter',
+ '<(DEPTH)/bot3/autonomous/autonomous.gyp:auto',
'<(DEPTH)/bot3/input/input.gyp:joystick_reader',
'<(DEPTH)/bot3/input/input.gyp:gyro_reader',
#'../input/input.gyp:AutoMode',
diff --git a/bot3/autonomous/auto.cc b/bot3/autonomous/auto.cc
new file mode 100644
index 0000000..3167770
--- /dev/null
+++ b/bot3/autonomous/auto.cc
@@ -0,0 +1,26 @@
+#include "stdio.h"
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "bot3/autonomous/auto.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/constants.h"
+
+using ::aos::time::Time;
+
+namespace bot3 {
+namespace autonomous {
+
+// start with N discs in the indexer
+void HandleAuto() {
+ // TODO (danielp): Do something in auto.
+ // (That's why I left all the includes.)
+ LOG(INFO, "Auto mode is not currently implemented on this robot.\n");
+}
+
+} // namespace autonomous
+} // namespace bot3
diff --git a/bot3/autonomous/auto.h b/bot3/autonomous/auto.h
new file mode 100644
index 0000000..d8a85a6
--- /dev/null
+++ b/bot3/autonomous/auto.h
@@ -0,0 +1,11 @@
+#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.q b/bot3/autonomous/auto.q
new file mode 100644
index 0000000..3636d31
--- /dev/null
+++ b/bot3/autonomous/auto.q
@@ -0,0 +1,8 @@
+package bot3.autonomous;
+
+message AutoControl {
+ // True if auto mode should be running, false otherwise.
+ bool run_auto;
+};
+
+queue AutoControl autonomous;
diff --git a/bot3/autonomous/auto_main.cc b/bot3/autonomous/auto_main.cc
new file mode 100644
index 0000000..333a67a
--- /dev/null
+++ b/bot3/autonomous/auto_main.cc
@@ -0,0 +1,39 @@
+#include "stdio.h"
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "aos/atom_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "bot3/autonomous/auto.q.h"
+#include "bot3/autonomous/auto.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ ::bot3::autonomous::autonomous.FetchLatest();
+ while (!::bot3::autonomous::autonomous.get()) {
+ ::bot3::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Got another auto packet\n");
+ }
+
+ while (true) {
+ while (!::bot3::autonomous::autonomous->run_auto) {
+ ::bot3::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Got another auto packet\n");
+ }
+ LOG(INFO, "Starting auto mode\n");
+ ::bot3::autonomous::HandleAuto();
+
+ LOG(INFO, "Auto mode exited, waiting for it to finish.\n");
+ while (::bot3::autonomous::autonomous->run_auto) {
+ ::bot3::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..7e2d5da
--- /dev/null
+++ b/bot3/autonomous/autonomous.gyp
@@ -0,0 +1,51 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'auto_queue',
+ 'type': 'static_library',
+ 'sources': ['auto.q'],
+ 'variables': {
+ 'header_path': 'bot3/autonomous',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'auto_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'auto.cc',
+ ],
+ 'dependencies': [
+ 'auto_queue',
+ '<(AOS)/common/common.gyp:controls',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/bot3/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',
+ ],
+ },
+ {
+ 'target_name': 'auto',
+ 'type': 'executable',
+ 'sources': [
+ 'auto_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'auto_queue',
+ 'auto_lib',
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/drivetrain/drivetrain.cc b/bot3/control_loops/drivetrain/drivetrain.cc
new file mode 100644
index 0000000..b0cb8b4
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain.cc
@@ -0,0 +1,285 @@
+#include "bot3/control_loops/drivetrain/drivetrain.h"
+
+#include <stdio.h>
+#include <sched.h>
+#include <cmath>
+#include <memory>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/queue.h"
+#include "bot3/control_loops/drivetrain/drivetrain_motor_plant.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/queues/Piston.q.h"
+
+using frc971::sensors::gyro;
+using ::frc971::control_loops::shifters;
+
+namespace bot3 {
+namespace control_loops {
+
+// Width of the robot.
+const double width = 22.0 / 100.0 * 2.54;
+
+class DrivetrainMotorsSS {
+ public:
+ DrivetrainMotorsSS ()
+ : loop_(new StateFeedbackLoop<4, 2, 2>(MakeDrivetrainLoop())) {
+ _offset = 0;
+ _integral_offset = 0;
+ _left_goal = 0.0;
+ _right_goal = 0.0;
+ _raw_left = 0.0;
+ _raw_right = 0.0;
+ _control_loop_driving = false;
+ }
+ void SetGoal(double left, double left_velocity, double right, double right_velocity) {
+ _left_goal = left;
+ _right_goal = right;
+ loop_->R << left, left_velocity, right, right_velocity;
+ }
+ void SetRawPosition(double left, double right) {
+ _raw_right = right;
+ _raw_left = left;
+ loop_->Y << left, right;
+ }
+ void SetPosition(
+ double left, double right, double gyro, bool control_loop_driving) {
+ // Decay the offset quickly because this gyro is great.
+ _offset = (0.25) * (right - left - gyro * width) / 2.0 + 0.75 * _offset;
+ const double angle_error = (_right_goal - _left_goal) / width - (_raw_right - _offset - _raw_left - _offset) / width;
+ // TODO(aschuh): Add in the gyro.
+ _integral_offset = 0.0;
+ _offset = 0.0;
+ _gyro = gyro;
+ _control_loop_driving = control_loop_driving;
+ SetRawPosition(left, right);
+ LOG(DEBUG, "Left %f->%f Right %f->%f Gyro %f aerror %f ioff %f\n", left + _offset, _left_goal, right - _offset, _right_goal, gyro, angle_error, _integral_offset);
+ }
+
+ void Update(bool update_observer, bool stop_motors) {
+ loop_->Update(update_observer, stop_motors);
+ }
+
+ void SendMotors(Drivetrain::Output *output) {
+ if (output) {
+ output->left_voltage = loop_->U(0, 0);
+ output->right_voltage = loop_->U(1, 0);
+ }
+ }
+ void PrintMotors() const {
+ // LOG(DEBUG, "Left Power %f Right Power %f lg %f rg %f le %f re %f gyro %f\n", U[0], U[1], R[0], R[2], Y[0], Y[1], _gyro);
+ ::Eigen::Matrix<double, 4, 1> E = loop_->R - loop_->X_hat;
+ LOG(DEBUG, "E[0, 0]: %f E[1, 0] %f E[2, 0] %f E[3, 0] %f\n", E(0, 0), E(1, 0), E(2, 0), E(3, 0));
+ }
+
+ private:
+ ::std::unique_ptr<StateFeedbackLoop<4, 2, 2>> loop_;
+
+ double _integral_offset;
+ double _offset;
+ double _gyro;
+ double _left_goal;
+ double _right_goal;
+ double _raw_left;
+ double _raw_right;
+ bool _control_loop_driving;
+};
+
+class DrivetrainMotorsOL {
+ public:
+ DrivetrainMotorsOL() {
+ _old_wheel = 0.0;
+ _wheel = 0.0;
+ _throttle = 0.0;
+ _quickturn = false;
+ _highgear = true;
+ _neg_inertia_accumulator = 0.0;
+ _left_pwm = 0.0;
+ _right_pwm = 0.0;
+ }
+ void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
+ _wheel = wheel;
+ _throttle = throttle;
+ _quickturn = quickturn;
+ _highgear = highgear;
+ _left_pwm = 0.0;
+ _right_pwm = 0.0;
+ }
+ void Update(void) {
+ double overPower;
+ float sensitivity = 1.7;
+ float angular_power;
+ float linear_power;
+ double wheel;
+
+ double neg_inertia = _wheel - _old_wheel;
+ _old_wheel = _wheel;
+
+ double wheelNonLinearity;
+ if (_highgear) {
+ wheelNonLinearity = 0.1; // used to be csvReader->TURN_NONLIN_HIGH
+ // Apply a sin function that's scaled to make it feel better.
+ const double angular_range = M_PI / 2.0 * wheelNonLinearity;
+ wheel = sin(angular_range * _wheel) / sin(angular_range);
+ wheel = sin(angular_range * wheel) / sin(angular_range);
+ } else {
+ wheelNonLinearity = 0.2; // used to be csvReader->TURN_NONLIN_LOW
+ // Apply a sin function that's scaled to make it feel better.
+ const double angular_range = M_PI / 2.0 * wheelNonLinearity;
+ wheel = sin(angular_range * _wheel) / sin(angular_range);
+ wheel = sin(angular_range * wheel) / sin(angular_range);
+ wheel = sin(angular_range * wheel) / sin(angular_range);
+ }
+
+ static const double kThrottleDeadband = 0.05;
+ if (::std::abs(_throttle) < kThrottleDeadband) {
+ _throttle = 0;
+ } else {
+ _throttle = copysign((::std::abs(_throttle) - kThrottleDeadband) /
+ (1.0 - kThrottleDeadband), _throttle);
+ }
+
+ double neg_inertia_scalar;
+ if (_highgear) {
+ neg_inertia_scalar = 8.0; // used to be csvReader->NEG_INTERTIA_HIGH
+ sensitivity = 1.22; // used to be csvReader->SENSE_HIGH
+ } else {
+ if (wheel * neg_inertia > 0) {
+ neg_inertia_scalar = 5; // used to be csvReader->NEG_INERTIA_LOW_MORE
+ } else {
+ if (::std::abs(wheel) > 0.65) {
+ neg_inertia_scalar = 5; // used to be csvReader->NEG_INTERTIA_LOW_LESS_EXT
+ } else {
+ neg_inertia_scalar = 5; // used to be csvReader->NEG_INTERTIA_LOW_LESS
+ }
+ }
+ sensitivity = 1.24; // used to be csvReader->SENSE_LOW
+ }
+ double neg_inertia_power = neg_inertia * neg_inertia_scalar;
+ _neg_inertia_accumulator += neg_inertia_power;
+
+ wheel = wheel + _neg_inertia_accumulator;
+ if (_neg_inertia_accumulator > 1) {
+ _neg_inertia_accumulator -= 1;
+ } else if (_neg_inertia_accumulator < -1) {
+ _neg_inertia_accumulator += 1;
+ } else {
+ _neg_inertia_accumulator = 0;
+ }
+
+ linear_power = _throttle;
+
+ if (_quickturn) {
+ double qt_angular_power = wheel;
+ if (::std::abs(linear_power) < 0.2) {
+ if (qt_angular_power > 1) qt_angular_power = 1.0;
+ if (qt_angular_power < -1) qt_angular_power = -1.0;
+ } else {
+ qt_angular_power = 0.0;
+ }
+ overPower = 1.0;
+ if (_highgear) {
+ sensitivity = 1.0;
+ } else {
+ sensitivity = 1.0;
+ }
+ angular_power = wheel;
+ } else {
+ overPower = 0.0;
+ angular_power = ::std::abs(_throttle) * wheel * sensitivity;
+ }
+
+ _right_pwm = _left_pwm = linear_power;
+ _left_pwm += angular_power;
+ _right_pwm -= angular_power;
+
+ if (_left_pwm > 1.0) {
+ _right_pwm -= overPower*(_left_pwm - 1.0);
+ _left_pwm = 1.0;
+ } else if (_right_pwm > 1.0) {
+ _left_pwm -= overPower*(_right_pwm - 1.0);
+ _right_pwm = 1.0;
+ } else if (_left_pwm < -1.0) {
+ _right_pwm += overPower*(-1.0 - _left_pwm);
+ _left_pwm = -1.0;
+ } else if (_right_pwm < -1.0) {
+ _left_pwm += overPower*(-1.0 - _right_pwm);
+ _right_pwm = -1.0;
+ }
+ }
+
+ void SendMotors(Drivetrain::Output *output) {
+ LOG(DEBUG, "left pwm: %f right pwm: %f wheel: %f throttle: %f\n",
+ _left_pwm, _right_pwm, _wheel, _throttle);
+ if (output) {
+ output->left_voltage = _left_pwm * 12.0;
+ output->right_voltage = _right_pwm * 12.0;
+ }
+ if (_highgear) {
+ shifters.MakeWithBuilder().set(false).Send();
+ } else {
+ shifters.MakeWithBuilder().set(true).Send();
+ }
+ }
+
+ private:
+ double _old_wheel;
+ double _wheel;
+ double _throttle;
+ bool _quickturn;
+ bool _highgear;
+ double _neg_inertia_accumulator;
+ double _left_pwm;
+ double _right_pwm;
+};
+
+void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
+ const Drivetrain::Position *position,
+ Drivetrain::Output *output,
+ Drivetrain::Status * /*status*/) {
+ // TODO(aschuh): These should be members of the class.
+ static DrivetrainMotorsSS dt_closedloop;
+ static DrivetrainMotorsOL dt_openloop;
+
+ bool bad_pos = false;
+ if (position == NULL) {
+ LOG(WARNING, "no position\n");
+ bad_pos = true;
+ }
+
+ double wheel = goal->steering;
+ double throttle = goal->throttle;
+ bool quickturn = goal->quickturn;
+ bool highgear = goal->highgear;
+
+ bool control_loop_driving = goal->control_loop_driving;
+ double left_goal = goal->left_goal;
+ double right_goal = goal->right_goal;
+
+ dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal,
+ right_goal, goal->right_velocity_goal);
+ if (!bad_pos) {
+ const double left_encoder = position->left_encoder;
+ const double right_encoder = position->right_encoder;
+ if (gyro.FetchLatest()) {
+ dt_closedloop.SetPosition(left_encoder, right_encoder,
+ gyro->angle, control_loop_driving);
+ } else {
+ dt_closedloop.SetRawPosition(left_encoder, right_encoder);
+ }
+ }
+ dt_closedloop.Update(position, output == NULL);
+ //dt_closedloop.PrintMotors();
+ dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
+ dt_openloop.Update();
+ if (control_loop_driving) {
+ dt_closedloop.SendMotors(output);
+ } else {
+ dt_openloop.SendMotors(output);
+ }
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/drivetrain/drivetrain.gyp b/bot3/control_loops/drivetrain/drivetrain.gyp
new file mode 100644
index 0000000..17e5a09
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain.gyp
@@ -0,0 +1,68 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'drivetrain_loop',
+ 'type': 'static_library',
+ 'sources': ['drivetrain.q'],
+ 'variables': {
+ 'header_path': 'bot3/control_loops/drivetrain',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'drivetrain_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'drivetrain.cc',
+ 'drivetrain_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'drivetrain_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'drivetrain_loop',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'drivetrain_loop',
+ 'drivetrain_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'drivetrain_lib',
+ 'drivetrain_loop',
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/drivetrain/drivetrain.h b/bot3/control_loops/drivetrain/drivetrain.h
new file mode 100644
index 0000000..03b3bd0
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain.h
@@ -0,0 +1,32 @@
+#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+#define BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+
+namespace bot3 {
+namespace control_loops {
+
+class DrivetrainLoop
+ : public aos::control_loops::ControlLoop<control_loops::Drivetrain> {
+ public:
+ // Constructs a control loop which can take a Drivetrain or defaults to the
+ // drivetrain at frc971::control_loops::drivetrain
+ explicit DrivetrainLoop(
+ control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
+ : aos::control_loops::ControlLoop<control_loops::Drivetrain>(
+ my_drivetrain) {}
+
+ protected:
+ // Executes one cycle of the control loop.
+ virtual void RunIteration(
+ const control_loops::Drivetrain::Goal *goal,
+ const control_loops::Drivetrain::Position *position,
+ control_loops::Drivetrain::Output *output,
+ control_loops::Drivetrain::Status *status);
+};
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
diff --git a/bot3/control_loops/drivetrain/drivetrain.q b/bot3/control_loops/drivetrain/drivetrain.q
new file mode 100644
index 0000000..d082e13
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain.q
@@ -0,0 +1,40 @@
+package bot3.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group Drivetrain {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ float steering;
+ float throttle;
+ bool highgear;
+ bool quickturn;
+ bool control_loop_driving;
+ float left_goal;
+ float left_velocity_goal;
+ float right_goal;
+ float right_velocity_goal;
+ };
+
+ message Position {
+ double left_encoder;
+ double right_encoder;
+ };
+
+ message Output {
+ float left_voltage;
+ float right_voltage;
+ };
+
+ message Status {
+ bool is_done;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group Drivetrain drivetrain;
diff --git a/bot3/control_loops/drivetrain/drivetrain_lib_test.cc b/bot3/control_loops/drivetrain/drivetrain_lib_test.cc
new file mode 100644
index 0000000..931b9ad
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -0,0 +1,187 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.h"
+#include "bot3/control_loops/drivetrain/drivetrain_motor_plant.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/queues/GyroAngle.q.h"
+
+
+using ::aos::time::Time;
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+
+
+// Class which simulates the drivetrain and sends out queue messages containing the
+// position.
+class DrivetrainSimulation {
+ public:
+ // Constructs a motor simulation.
+ DrivetrainSimulation()
+ : drivetrain_plant_(
+ new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
+ my_drivetrain_loop_(".frc971.control_loops.drivetrain",
+ 0x8a8dde77, ".frc971.control_loops.drivetrain.goal",
+ ".frc971.control_loops.drivetrain.position",
+ ".frc971.control_loops.drivetrain.output",
+ ".frc971.control_loops.drivetrain.status") {
+ Reinitialize();
+ }
+
+ // Resets the plant.
+ void Reinitialize() {
+ drivetrain_plant_->X(0, 0) = 0.0;
+ drivetrain_plant_->X(1, 0) = 0.0;
+ drivetrain_plant_->Y = drivetrain_plant_->C() * drivetrain_plant_->X;
+ last_left_position_ = drivetrain_plant_->Y(0, 0);
+ last_right_position_ = drivetrain_plant_->Y(1, 0);
+ }
+
+ // Returns the position of the drivetrain.
+ double GetLeftPosition() const {
+ return drivetrain_plant_->Y(0, 0);
+ }
+ double GetRightPosition() const {
+ return drivetrain_plant_->Y(1, 0);
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ const double left_encoder = GetLeftPosition();
+ const double right_encoder = GetRightPosition();
+
+ ::aos::ScopedMessagePtr<control_loops::Drivetrain::Position> position =
+ my_drivetrain_loop_.position.MakeMessage();
+ position->left_encoder = left_encoder;
+ position->right_encoder = right_encoder;
+ position.Send();
+ }
+
+ // Simulates the drivetrain moving for one timestep.
+ void Simulate() {
+ last_left_position_ = drivetrain_plant_->Y(0, 0);
+ last_right_position_ = drivetrain_plant_->Y(1, 0);
+ EXPECT_TRUE(my_drivetrain_loop_.output.FetchLatest());
+ drivetrain_plant_->U << my_drivetrain_loop_.output->left_voltage,
+ my_drivetrain_loop_.output->right_voltage;
+ drivetrain_plant_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> drivetrain_plant_;
+ private:
+ Drivetrain my_drivetrain_loop_;
+ double last_left_position_;
+ double last_right_position_;
+};
+
+class DrivetrainTest : public ::testing::Test {
+ protected:
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ Drivetrain my_drivetrain_loop_;
+
+ // Create a loop and simulation plant.
+ DrivetrainLoop drivetrain_motor_;
+ DrivetrainSimulation drivetrain_motor_plant_;
+
+ DrivetrainTest() : my_drivetrain_loop_(".frc971.control_loops.drivetrain",
+ 0x8a8dde77,
+ ".frc971.control_loops.drivetrain.goal",
+ ".frc971.control_loops.drivetrain.position",
+ ".frc971.control_loops.drivetrain.output",
+ ".frc971.control_loops.drivetrain.status"),
+ drivetrain_motor_(&my_drivetrain_loop_),
+ drivetrain_motor_plant_() {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test, also for the gyro.
+ ::aos::robot_state.Clear();
+ ::frc971::sensors::gyro.Clear();
+ SendDSPacket(true);
+ }
+
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_drivetrain_loop_.goal.FetchLatest();
+ my_drivetrain_loop_.position.FetchLatest();
+ EXPECT_NEAR(my_drivetrain_loop_.goal->left_goal,
+ drivetrain_motor_plant_.GetLeftPosition(),
+ 1e-2);
+ EXPECT_NEAR(my_drivetrain_loop_.goal->right_goal,
+ drivetrain_motor_plant_.GetRightPosition(),
+ 1e-2);
+ }
+
+ virtual ~DrivetrainTest() {
+ ::aos::robot_state.Clear();
+ ::frc971::sensors::gyro.Clear();
+ }
+};
+
+// Tests that the drivetrain converges on a goal.
+TEST_F(DrivetrainTest, ConvergesCorrectly) {
+ my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ .left_goal(-1.0)
+ .right_goal(1.0).Send();
+ for (int i = 0; i < 200; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that it survives disabling.
+TEST_F(DrivetrainTest, SurvivesDisabling) {
+ my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ .left_goal(-1.0)
+ .right_goal(1.0).Send();
+ for (int i = 0; i < 500; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ if (i > 20 && i < 200) {
+ SendDSPacket(false);
+ } else {
+ SendDSPacket(true);
+ }
+ }
+ VerifyNearGoal();
+}
+
+// Tests surviving bad positions.
+TEST_F(DrivetrainTest, SurvivesBadPosition) {
+ my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ .left_goal(-1.0)
+ .right_goal(1.0).Send();
+ for (int i = 0; i < 500; ++i) {
+ if (i > 20 && i < 200) {
+ } else {
+ drivetrain_motor_plant_.SendPositionMessage();
+ }
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/drivetrain/drivetrain_main.cc b/bot3/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..5ef00bf
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,11 @@
+#include "bot3/control_loops/drivetrain/drivetrain.h"
+
+#include "aos/atom_code/init.h"
+
+int main() {
+ ::aos::Init();
+ bot3::control_loops::DrivetrainLoop drivetrain;
+ drivetrain.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/bot3/control_loops/drivetrain/drivetrain_motor_plant.cc b/bot3/control_loops/drivetrain/drivetrain_motor_plant.cc
new file mode 100644
index 0000000..1a6989a
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "bot3/control_loops/drivetrain/drivetrain_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.0095410093, 0.0, -0.0000167223, 0.0, 0.9096302600, 0.0, -0.0032396985, 0.0, -0.0000167223, 1.0, 0.0095410093, 0.0, -0.0032396985, 0.0, 0.9096302600;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.0000628338, 0.0000022892, 0.0123712263, 0.0004435007, 0.0000022892, 0.0000628338, 0.0004435007, 0.0123712263;
+ Eigen::Matrix<double, 2, 4> C;
+ C << 1, 0, 0, 0, 0, 0, 1, 0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0, 0, 0, 0;
+ Eigen::Matrix<double, 2, 1> U_max;
+ U_max << 12.0, 12.0;
+ Eigen::Matrix<double, 2, 1> U_min;
+ U_min << -12.0, -12.0;
+ return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 1.7496302600, -0.0032396985, 72.1296388532, -0.4369906587, -0.0032396985, 1.7496302600, -0.4369906586, 72.1296388532;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 242.8102455120, 19.7898401032, -8.7045950610, -0.9720464423, -8.7045950610, -0.9720464423, 242.8102455120, 19.7898401032;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainPlantCoefficients());
+}
+
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainPlantCoefficients());
+ return StateFeedbackPlant<4, 2, 2>(plants);
+}
+
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
+ ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
+ controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainController());
+ return StateFeedbackLoop<4, 2, 2>(controllers);
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/drivetrain/drivetrain_motor_plant.h b/bot3/control_loops/drivetrain/drivetrain_motor_plant.h
new file mode 100644
index 0000000..e511560
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainController();
+
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
diff --git a/bot3/input/gyro_board_reader.cc b/bot3/input/gyro_board_reader.cc
index 62c3657..a5f961f 100644
--- a/bot3/input/gyro_board_reader.cc
+++ b/bot3/input/gyro_board_reader.cc
@@ -7,11 +7,7 @@
#include "aos/common/control_loop/Timing.h"
#include "aos/common/time.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
#include "bot3/input/gyro_board_data.h"
#include "gyro_board/src/libusb-driver/libusb_wrap.h"
#include "frc971/queues/GyroAngle.q.h"
@@ -20,8 +16,7 @@
#define M_PI 3.14159265358979323846
#endif
-using ::frc971::control_loops::drivetrain;
-using ::frc971::control_loops::shooter;
+using ::bot3::control_loops::drivetrain;
using ::frc971::sensors::gyro;
namespace bot3 {
@@ -35,7 +30,7 @@
// TODO(daniel): This might have to change if I find out that the gear ratios are different.
inline double shooter_translate(int32_t in) {
- return static_cast<double>(in) / (32.0 /*cpr*/ * 4.0 /*quad*/) *
+ return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
(15.0 / 34.0) /*gears*/ * (2 * M_PI);
}
@@ -164,9 +159,7 @@
.left_encoder(-drivetrain_translate(data->left_drive))
.Send();
- shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->shooter))
- .Send();
+ //TODO (danielp): Add stuff for bot3 shooter.
}
::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
index 4dec23e..6553e3a 100644
--- a/bot3/input/gyro_sensor_receiver.cc
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -8,11 +8,7 @@
#include "aos/common/sensors/sensor_unpacker.h"
#include "aos/common/sensors/sensor_receiver.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/input/gyro_board_data.h"
#include "gyro_board/src/libusb-driver/libusb_wrap.h"
#include "frc971/queues/GyroAngle.q.h"
@@ -21,14 +17,11 @@
#define M_PI 3.14159265358979323846
#endif
-using ::frc971::control_loops::drivetrain;
-using ::frc971::control_loops::wrist;
-using ::frc971::control_loops::angle_adjust;
-using ::frc971::control_loops::shooter;
-using ::frc971::control_loops::index_loop;
+using ::bot3::control_loops::drivetrain;
using ::frc971::sensors::gyro;
+using ::frc971::GyroBoardData;
-namespace frc971 {
+namespace bot3 {
namespace {
inline double drivetrain_translate(int32_t in) {
@@ -114,41 +107,6 @@
.right_encoder(drivetrain_translate(data->right_drive))
.left_encoder(-drivetrain_translate(data->left_drive))
.Send();
-
- wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data->wrist))
- .hall_effect(!data->wrist_hall_effect)
- .calibration(wrist_translate(data->capture_wrist_rise))
- .Send();
-
- angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data->shooter_angle))
- .bottom_hall_effect(!data->angle_adjust_bottom_hall_effect)
- .middle_hall_effect(false)
- .bottom_calibration(angle_adjust_translate(
- data->capture_shooter_angle_rise))
- .middle_calibration(angle_adjust_translate(
- 0))
- .Send();
-
- shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->shooter))
- .Send();
-
- index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data->indexer))
- .top_disc_detect(!data->top_disc)
- .top_disc_posedge_count(top_rise_count_)
- .top_disc_posedge_position(index_translate(data->capture_top_rise))
- .top_disc_negedge_count(top_fall_count_)
- .top_disc_negedge_position(index_translate(data->capture_top_fall))
- .bottom_disc_detect(!data->bottom_disc)
- .bottom_disc_posedge_count(bottom_rise_count_)
- .bottom_disc_negedge_count(bottom_fall_count_)
- .bottom_disc_negedge_wait_position(index_translate(
- data->capture_bottom_fall_delay))
- .bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
- .Send();
}
private:
@@ -287,12 +245,12 @@
LibUSB libusb_;
};
-} // namespace frc971
+} // namespace bot3
int main() {
::aos::Init();
- ::frc971::GyroSensorUnpacker unpacker;
- ::frc971::GyroSensorReceiver receiver(&unpacker);
+ ::bot3::GyroSensorUnpacker unpacker;
+ ::bot3::GyroSensorReceiver receiver(&unpacker);
while (true) {
receiver.RunIteration();
}
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
index 8d75241..5beb3d0 100644
--- a/bot3/input/input.gyp
+++ b/bot3/input/input.gyp
@@ -10,13 +10,9 @@
'<(AOS)/atom_code/input/input.gyp:joystick_input',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
-
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
+ '<(DEPTH)/bot3/autonomous/autonomous.gyp:auto_queue',
],
},
{
@@ -26,12 +22,8 @@
'gyro_sensor_receiver.cc',
],
'dependencies': [
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
- '<(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',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
'<(EXTERNALS):libusb',
@@ -47,12 +39,8 @@
'gyro_board_reader.cc',
],
'dependencies': [
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
- '<(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',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
'<(EXTERNALS):libusb',
diff --git a/bot3/input/joystick_reader.cc b/bot3/input/joystick_reader.cc
index da8a8e2..5efe8b4 100644
--- a/bot3/input/joystick_reader.cc
+++ b/bot3/input/joystick_reader.cc
@@ -7,21 +7,16 @@
#include "aos/atom_code/input/joystick_input.h"
#include "aos/common/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/autonomous/auto.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/queues/Piston.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/queues/CameraTarget.q.h"
-using ::frc971::control_loops::drivetrain;
+using ::bot3::control_loops::drivetrain;
using ::frc971::control_loops::shifters;
using ::frc971::sensors::gyro;
-using ::frc971::control_loops::index_loop;
-using ::frc971::control_loops::shooter;
-using ::frc971::control_loops::hangers;
-using ::frc971::vision::target_angle;
+// using ::frc971::vision::target_angle;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
@@ -47,8 +42,6 @@
const ButtonLocation kForceFire(3, 12);
const ButtonLocation kForceIndexUp(3, 9), kForceIndexDown(3, 7);
-const ButtonLocation kDeployHangers(3, 1);
-
class Reader : public ::aos::input::JoystickInput {
public:
static const bool kWristAlwaysDown = false;
@@ -64,11 +57,11 @@
if (data.GetControlBit(ControlBit::kAutonomous)) {
if (data.PosEdge(ControlBit::kEnabled)){
LOG(INFO, "Starting auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder()
+ ::bot3::autonomous::autonomous.MakeWithBuilder()
.run_auto(true).Send();
} else if (data.NegEdge(ControlBit::kEnabled)) {
LOG(INFO, "Stopping auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder()
+ ::bot3::autonomous::autonomous.MakeWithBuilder()
.run_auto(false).Send();
}
} else { // teleop
@@ -78,9 +71,11 @@
const double wheel = data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
LOG(DEBUG, "wheel %f throttle %f\n", wheel, throttle);
- const double kThrottleGain = 1.0 / 2.5;
+ //const double kThrottleGain = 1.0 / 2.5;
if (data.IsPressed(kDriveControlLoopEnable1) ||
data.IsPressed(kDriveControlLoopEnable2)) {
+ LOG(INFO, "Control loop driving is currently not supported by this robot.\n");
+#if 0
static double distance = 0.0;
static double angle = 0.0;
static double filtered_goal_distance = 0.0;
@@ -99,7 +94,6 @@
//const double gyro_angle = Gyro.View().angle;
const double goal_theta = angle - wheel * 0.27;
const double goal_distance = distance + throttle * kThrottleGain;
- //TODO(danielp) Change this after a look in the CAD.
const double robot_width = 22.0 / 100.0 * 2.54;
const double kMaxVelocity = 0.6;
if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
@@ -115,6 +109,7 @@
is_high_gear = false;
LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
+#endif
}
if (!(drivetrain.goal.MakeWithBuilder()
.steering(wheel)
@@ -131,7 +126,7 @@
if (data.PosEdge(kShiftLow)) {
is_high_gear = true;
}
-
+#if 0
::aos::ScopedMessagePtr<frc971::control_loops::ShooterLoop::Goal> shooter_goal =
shooter.goal.MakeMessage();
shooter_goal->velocity = 0;
@@ -163,7 +158,7 @@
shooter_goal->velocity = 375;
}
- //TODO (daniel) Modify this for hopper.
+ //TODO (daniel) Modify this for hopper and shooter.
::aos::ScopedMessagePtr<frc971::control_loops::IndexLoop::Goal> index_goal =
index_loop.goal.MakeMessage();
if (data.IsPressed(kFire)) {
@@ -194,15 +189,8 @@
index_goal.Send();
shooter_goal.Send();
+#endif
}
-
- static int hanger_cycles = 0;
- if (data.IsPressed(kDeployHangers)) {
- ++hanger_cycles;
- } else {
- hanger_cycles = 0;
- }
- hangers.MakeWithBuilder().set(hanger_cycles >= 10).Send();
}
};
diff --git a/bot3/output/atom_motor_writer.cc b/bot3/output/atom_motor_writer.cc
index c2aeeb2..7d43b53 100644
--- a/bot3/output/atom_motor_writer.cc
+++ b/bot3/output/atom_motor_writer.cc
@@ -7,13 +7,10 @@
#include "aos/atom_code/init.h"
#include "frc971/queues/Piston.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-using ::frc971::control_loops::drivetrain;
+using ::bot3::control_loops::drivetrain;
using ::frc971::control_loops::shifters;
-using ::frc971::control_loops::shooter;
-using ::frc971::control_loops::hangers;
namespace bot3 {
namespace output {
@@ -50,20 +47,13 @@
SetSolenoid(2, !shifters->set);
}
- shooter.output.FetchLatest();
- if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
+ /*if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
SetPWMOutput(4, shooter.output->voltage / 12.0, kVictorBounds);
} else {
DisablePWMOutput(4);
LOG(WARNING, "shooter not new enough\n");
- }
- // TODO(danielp): Add stuff for intake.
-
- hangers.FetchLatest();
- if (hangers.get()) {
- SetSolenoid(4, hangers->set);
- SetSolenoid(5, hangers->set);
- }
+ }*/
+ // TODO(danielp): Add stuff for intake and shooter.
}
};
diff --git a/bot3/output/output.gyp b/bot3/output/output.gyp
index 139997b..219f501 100644
--- a/bot3/output/output.gyp
+++ b/bot3/output/output.gyp
@@ -33,11 +33,7 @@
'<(AOS)/atom_code/output/output.gyp:motor_output',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
- '<(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/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
],