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/aos/build/build.py b/aos/build/build.py
index a36f69c..5caa8ee 100755
--- a/aos/build/build.py
+++ b/aos/build/build.py
@@ -615,7 +615,7 @@
# We don't have a compiler to use here.
continue
platforms.append(
- PrimeProcessor.Platform(architecture, compiler, debug, 'none'))
+ self.Platform(architecture, compiler, debug, 'none'))
for sanitizer in PrimeProcessor.SANITIZERS:
for compiler in ('gcc_4.8', 'clang'):
if compiler == 'gcc_4.8' and (sanitizer == 'undefined' or
@@ -627,7 +627,7 @@
# We already added sanitizer == 'none' above.
continue
platforms.append(
- PrimeProcessor.Platform('amd64', compiler, True, sanitizer))
+ self.Platform('amd64', compiler, True, sanitizer))
self.__platforms = frozenset(platforms)
if is_test:
@@ -754,6 +754,15 @@
self.do_check_installed(tuple(packages))
+class Bot3PrimeProcessor(PrimeProcessor):
+ """A very simple subclass of PrimeProcessor whose main function is to allow
+ the building of third robot targets in separate directories from those of
+ the main robot."""
+ class Platform(PrimeProcessor.Platform):
+ def __str__(self):
+ return 'bot3-%s' % (super(Bot3PrimeProcessor.Platform, self).__str__())
+
+
def strsignal(num):
# It ends up with SIGIOT instead otherwise, which is weird.
if num == signal.SIGABRT:
@@ -893,6 +902,9 @@
elif args.processor == 'prime':
processor = PrimeProcessor(args.action_name == 'tests',
args.action_name == 'deploy')
+ elif args.processor == 'bot3_prime':
+ processor = Bot3PrimeProcessor(args.action_name == 'tests',
+ args.action_name == 'deploy')
else:
print_help(1, message='Unknown processor "%s".' % args.processor)
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',
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/drivetrain/drivetrain.cc b/bot3/control_loops/drivetrain/drivetrain.cc
new file mode 100644
index 0000000..ae313e5
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain.cc
@@ -0,0 +1,746 @@
+#include "bot3/control_loops/drivetrain/drivetrain.h"
+
+#include <math.h>
+#include <stdio.h>
+#include <sched.h>
+
+#include <functional>
+#include <memory>
+
+#include "Eigen/Dense"
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/controls/polytope.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/logging/matrix_logging.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"
+#include "bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h"
+#include "bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "frc971/control_loops/coerce_goal.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/queues/other_sensors.q.h"
+
+using ::frc971::sensors::gyro_reading;
+using ::frc971::control_loops::DoCoerceGoal;
+using ::frc971::control_loops::CoerceGoal;
+
+namespace bot3 {
+namespace control_loops {
+
+class DrivetrainMotorsSS {
+ public:
+ class LimitedDrivetrainLoop : public StateFeedbackLoop<4, 2, 2> {
+ public:
+ LimitedDrivetrainLoop(StateFeedbackLoop<4, 2, 2> &&loop)
+ : StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
+ U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0,
+ -1, 0,
+ 0, 1,
+ 0, -1).finished(),
+ (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0,
+ 12.0, 12.0).finished()) {
+ ::aos::controls::HPolytope<0>::Init();
+ T << 1, -1, 1, 1;
+ T_inverse = T.inverse();
+ }
+
+ bool output_was_capped() const {
+ return output_was_capped_;
+ }
+
+ private:
+ virtual void CapU() {
+ const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
+
+ if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
+ output_was_capped_ = true;
+ LOG_MATRIX(DEBUG, "U at start", U());
+
+ Eigen::Matrix<double, 2, 2> position_K;
+ position_K << K(0, 0), K(0, 2),
+ K(1, 0), K(1, 2);
+ Eigen::Matrix<double, 2, 2> velocity_K;
+ velocity_K << K(0, 1), K(0, 3),
+ K(1, 1), K(1, 3);
+
+ Eigen::Matrix<double, 2, 1> position_error;
+ position_error << error(0, 0), error(2, 0);
+ const auto drive_error = T_inverse * position_error;
+ Eigen::Matrix<double, 2, 1> velocity_error;
+ velocity_error << error(1, 0), error(3, 0);
+ LOG_MATRIX(DEBUG, "error", error);
+
+ const auto &poly = U_Poly_;
+ const Eigen::Matrix<double, 4, 2> pos_poly_H =
+ poly.H() * position_K * T;
+ const Eigen::Matrix<double, 4, 1> pos_poly_k =
+ poly.k() - poly.H() * velocity_K * velocity_error;
+ const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+
+ Eigen::Matrix<double, 2, 1> adjusted_pos_error;
+ {
+ const auto &P = drive_error;
+
+ Eigen::Matrix<double, 1, 2> L45;
+ L45 << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
+ const double w45 = 0;
+
+ Eigen::Matrix<double, 1, 2> LH;
+ if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
+ LH << 0, 1;
+ } else {
+ LH << 1, 0;
+ }
+ const double wh = LH.dot(P);
+
+ Eigen::Matrix<double, 2, 2> standard;
+ standard << L45, LH;
+ Eigen::Matrix<double, 2, 1> W;
+ W << w45, wh;
+ const Eigen::Matrix<double, 2, 1> intersection =
+ standard.inverse() * W;
+
+ bool is_inside_h;
+ const auto adjusted_pos_error_h =
+ DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
+ const auto adjusted_pos_error_45 =
+ DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
+ if (pos_poly.IsInside(intersection)) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ if (is_inside_h) {
+ if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm()) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
+ }
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
+ }
+ }
+ }
+
+ LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
+ mutable_U() =
+ velocity_K * velocity_error + position_K * T * adjusted_pos_error;
+ LOG_MATRIX(DEBUG, "U is now", U());
+ } else {
+ output_was_capped_ = false;
+ }
+ }
+
+ const ::aos::controls::HPolytope<2> U_Poly_;
+ Eigen::Matrix<double, 2, 2> T, T_inverse;
+ bool output_was_capped_ = false;;
+ };
+
+ DrivetrainMotorsSS()
+ : loop_(new LimitedDrivetrainLoop(
+ MakeDrivetrainLoop())),
+ filtered_offset_(0.0),
+ gyro_(0.0),
+ left_goal_(0.0),
+ right_goal_(0.0),
+ raw_left_(0.0),
+ raw_right_(0.0) {
+ // Low gear on both.
+ loop_->set_controller_index(0);
+ }
+
+ void SetGoal(double left, double left_velocity, double right,
+ double right_velocity) {
+ left_goal_ = left;
+ right_goal_ = right;
+ loop_->mutable_R() << left, left_velocity, right, right_velocity;
+ }
+ void SetRawPosition(double left, double right) {
+ raw_right_ = right;
+ raw_left_ = left;
+ Eigen::Matrix<double, 2, 1> Y;
+ Y << left + filtered_offset_, right - filtered_offset_;
+ loop_->Correct(Y);
+ }
+ void SetPosition(double left, double right, double gyro) {
+ // Decay the offset quickly because this gyro is great.
+ const double offset =
+ (right - left - gyro * kBot3TurnWidth) / 2.0;
+ // TODO(brians): filtered_offset_ = offset first time around.
+ filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
+ gyro_ = gyro;
+ SetRawPosition(left, right);
+ }
+
+ void SetExternalMotors(double left_voltage, double right_voltage) {
+ loop_->mutable_U() << left_voltage, right_voltage;
+ }
+
+ void Update(bool stop_motors, bool enable_control_loop) {
+ if (enable_control_loop) {
+ loop_->Update(stop_motors);
+ } else {
+ if (stop_motors) {
+ loop_->mutable_U().setZero();
+ loop_->mutable_U_uncapped().setZero();
+ }
+ loop_->UpdateObserver();
+ }
+ ::Eigen::Matrix<double, 4, 1> E = loop_->R() - loop_->X_hat();
+ LOG_MATRIX(DEBUG, "E", E);
+ }
+
+ double GetEstimatedRobotSpeed() const {
+ // lets just call the average of left and right velocities close enough
+ return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
+ }
+
+ double GetEstimatedLeftEncoder() const {
+ return loop_->X_hat(0, 0);
+ }
+
+ double GetEstimatedRightEncoder() const {
+ return loop_->X_hat(2, 0);
+ }
+
+ bool OutputWasCapped() const {
+ return loop_->output_was_capped();
+ }
+
+ void SendMotors(Drivetrain::Output *output) const {
+ if (output) {
+ output->left_voltage = loop_->U(0, 0);
+ output->right_voltage = loop_->U(1, 0);
+ output->left_high = false;
+ output->right_high = false;
+ }
+ }
+
+ const LimitedDrivetrainLoop &loop() const { return *loop_; }
+
+ private:
+ ::std::unique_ptr<LimitedDrivetrainLoop> loop_;
+
+ double filtered_offset_;
+ double gyro_;
+ double left_goal_;
+ double right_goal_;
+ double raw_left_;
+ double raw_right_;
+};
+
+class PolyDrivetrain {
+ public:
+
+ enum Gear {
+ HIGH,
+ LOW,
+ SHIFTING_UP,
+ SHIFTING_DOWN
+ };
+ // Stall Torque in N m
+ static constexpr double kStallTorque = 2.42;
+ // Stall Current in Amps
+ static constexpr double kStallCurrent = 133;
+ // Free Speed in RPM. Used number from last year.
+ static constexpr double kFreeSpeed = 4650.0;
+ // Free Current in Amps
+ static constexpr double kFreeCurrent = 2.7;
+ // Moment of inertia of the drivetrain in kg m^2
+ // Just borrowed from last year.
+ static constexpr double J = 6.4;
+ // Mass of the robot, in kg.
+ static constexpr double m = 68;
+ // Radius of the robot, in meters (from last year).
+ static constexpr double rb = 0.617998644 / 2.0;
+ static constexpr double kWheelRadius = 0.04445;
+ // Resistance of the motor, divided by the number of motors.
+ static constexpr double kR = (12.0 / kStallCurrent / 4 + 0.03) / (0.93 * 0.93);
+ // Motor velocity constant
+ static constexpr double Kv =
+ ((kFreeSpeed / 60.0 * 2.0 * M_PI) / (12.0 - kR * kFreeCurrent));
+ // Torque constant
+ static constexpr double Kt = kStallTorque / kStallCurrent;
+
+ PolyDrivetrain()
+ : U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+ /*[*/ -1, 0 /*]*/,
+ /*[*/ 0, 1 /*]*/,
+ /*[*/ 0, -1 /*]]*/).finished(),
+ (Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
+ /*[*/ 12 /*]*/,
+ /*[*/ 12 /*]*/,
+ /*[*/ 12 /*]]*/).finished()),
+ loop_(new StateFeedbackLoop<2, 2, 2>(
+ MakeVelocityDrivetrainLoop())),
+ ttrust_(1.1),
+ wheel_(0.0),
+ throttle_(0.0),
+ quickturn_(false),
+ stale_count_(0),
+ position_time_delta_(0.01),
+ left_gear_(LOW),
+ right_gear_(LOW),
+ counter_(0) {
+
+ last_position_.Zero();
+ position_.Zero();
+ }
+ static bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
+
+ static double MotorSpeed(const ::frc971::constants::ShifterHallEffect &hall_effect,
+ double shifter_position, double velocity) {
+ // TODO(austin): G_high, G_low and kWheelRadius
+ const double avg_hall_effect =
+ (hall_effect.clear_high + hall_effect.clear_low) / 2.0;
+
+ if (shifter_position > avg_hall_effect) {
+ return velocity / kBot3HighGearRatio / kWheelRadius;
+ } else {
+ return velocity / kBot3LowGearRatio / kWheelRadius;
+ }
+ }
+
+ Gear ComputeGear(const ::frc971::constants::ShifterHallEffect &hall_effect,
+ double velocity, Gear current) {
+ const double low_omega = MotorSpeed(hall_effect, 0.0, ::std::abs(velocity));
+ const double high_omega =
+ MotorSpeed(hall_effect, 1.0, ::std::abs(velocity));
+
+ double high_torque = ((12.0 - high_omega / Kv) * Kt / kR);
+ double low_torque = ((12.0 - low_omega / Kv) * Kt / kR);
+ double high_power = high_torque * high_omega;
+ double low_power = low_torque * low_omega;
+
+ // TODO(aschuh): Do this right!
+ if ((current == HIGH || high_power > low_power + 160) &&
+ ::std::abs(velocity) > 0.14) {
+ return HIGH;
+ } else {
+ return LOW;
+ }
+ }
+
+ void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
+ const double kWheelNonLinearity = 0.3;
+ // Apply a sin function that's scaled to make it feel better.
+ const double angular_range = M_PI_2 * kWheelNonLinearity;
+ wheel_ = sin(angular_range * wheel) / sin(angular_range);
+ wheel_ = sin(angular_range * wheel_) / sin(angular_range);
+ quickturn_ = quickturn;
+
+ static const double kThrottleDeadband = 0.05;
+ if (::std::abs(throttle) < kThrottleDeadband) {
+ throttle_ = 0;
+ } else {
+ throttle_ = copysign((::std::abs(throttle) - kThrottleDeadband) /
+ (1.0 - kThrottleDeadband), throttle);
+ }
+
+ // TODO(austin): Fix the upshift logic to include states.
+ Gear requested_gear;
+ if (false) {
+ const double current_left_velocity =
+ (position_.left_encoder - last_position_.left_encoder) /
+ position_time_delta_;
+ const double current_right_velocity =
+ (position_.right_encoder - last_position_.right_encoder) /
+ position_time_delta_;
+
+ Gear left_requested =
+ ComputeGear(kBot3LeftDriveShifter, current_left_velocity, left_gear_);
+ Gear right_requested =
+ ComputeGear(kBot3RightDriveShifter, current_right_velocity, right_gear_);
+ requested_gear =
+ (left_requested == HIGH || right_requested == HIGH) ? HIGH : LOW;
+ } else {
+ requested_gear = highgear ? HIGH : LOW;
+ }
+
+ const Gear shift_up = SHIFTING_UP;
+ const Gear shift_down = SHIFTING_DOWN;
+
+ if (left_gear_ != requested_gear) {
+ if (IsInGear(left_gear_)) {
+ if (requested_gear == HIGH) {
+ left_gear_ = shift_up;
+ } else {
+ left_gear_ = shift_down;
+ }
+ } else {
+ if (requested_gear == HIGH && left_gear_ == SHIFTING_DOWN) {
+ left_gear_ = SHIFTING_UP;
+ } else if (requested_gear == LOW && left_gear_ == SHIFTING_UP) {
+ left_gear_ = SHIFTING_DOWN;
+ }
+ }
+ }
+ if (right_gear_ != requested_gear) {
+ if (IsInGear(right_gear_)) {
+ if (requested_gear == HIGH) {
+ right_gear_ = shift_up;
+ } else {
+ right_gear_ = shift_down;
+ }
+ } else {
+ if (requested_gear == HIGH && right_gear_ == SHIFTING_DOWN) {
+ right_gear_ = SHIFTING_UP;
+ } else if (requested_gear == LOW && right_gear_ == SHIFTING_UP) {
+ right_gear_ = SHIFTING_DOWN;
+ }
+ }
+ }
+ }
+ void SetPosition(const Drivetrain::Position *position) {
+ if (position == NULL) {
+ ++stale_count_;
+ } else {
+ last_position_ = position_;
+ position_ = *position;
+ position_time_delta_ = (stale_count_ + 1) * 0.01;
+ stale_count_ = 0;
+ }
+
+ if (position) {
+ GearLogging gear_logging;
+ // Switch to the correct controller.
+ const double left_middle_shifter_position =
+ (kBot3LeftDriveShifter.clear_high +
+ kBot3LeftDriveShifter.clear_low) / 2.0;
+ const double right_middle_shifter_position =
+ (kBot3RightDriveShifter.clear_high +
+ kBot3RightDriveShifter.clear_low) / 2.0;
+
+ if (position->left_shifter_position < left_middle_shifter_position) {
+ if (position->right_shifter_position < right_middle_shifter_position ||
+ right_gear_ == LOW) {
+ gear_logging.left_loop_high = false;
+ gear_logging.right_loop_high = false;
+ loop_->set_controller_index(gear_logging.controller_index = 0);
+ } else {
+ gear_logging.left_loop_high = false;
+ gear_logging.right_loop_high = true;
+ loop_->set_controller_index(gear_logging.controller_index = 1);
+ }
+ } else {
+ if (position->right_shifter_position < right_middle_shifter_position ||
+ left_gear_ == LOW) {
+ gear_logging.left_loop_high = true;
+ gear_logging.right_loop_high = false;
+ loop_->set_controller_index(gear_logging.controller_index = 2);
+ } else {
+ gear_logging.left_loop_high = true;
+ gear_logging.right_loop_high = true;
+ loop_->set_controller_index(gear_logging.controller_index = 3);
+ }
+ }
+
+ // TODO(austin): Constants.
+ if (position->left_shifter_position >
+ kBot3LeftDriveShifter.clear_high &&
+ left_gear_ == SHIFTING_UP) {
+ left_gear_ = HIGH;
+ }
+ if (position->left_shifter_position <
+ kBot3LeftDriveShifter.clear_low &&
+ left_gear_ == SHIFTING_DOWN) {
+ left_gear_ = LOW;
+ }
+ if (position->right_shifter_position >
+ kBot3RightDriveShifter.clear_high &&
+ right_gear_ == SHIFTING_UP) {
+ right_gear_ = HIGH;
+ }
+ if (position->right_shifter_position <
+ kBot3RightDriveShifter.clear_low &&
+ right_gear_ == SHIFTING_DOWN) {
+ right_gear_ = LOW;
+ }
+
+ gear_logging.left_state = left_gear_;
+ gear_logging.right_state = right_gear_;
+ LOG_STRUCT(DEBUG, "state", gear_logging);
+ }
+ }
+
+ double FilterVelocity(double throttle) {
+ const Eigen::Matrix<double, 2, 2> FF =
+ loop_->B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+
+ constexpr int kHighGearController = 3;
+ const Eigen::Matrix<double, 2, 2> FF_high =
+ loop_->controller(kHighGearController).plant.B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() -
+ loop_->controller(kHighGearController).plant.A());
+
+ ::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
+ int min_FF_sum_index;
+ const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
+ const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+ const double high_min_FF_sum = FF_high.col(0).sum();
+
+ const double adjusted_ff_voltage = ::aos::Clip(
+ throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
+ return ((adjusted_ff_voltage +
+ ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) / 2.0) /
+ (ttrust_ * min_K_sum + min_FF_sum));
+ }
+
+ double MaxVelocity() {
+ const Eigen::Matrix<double, 2, 2> FF =
+ loop_->B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+
+ constexpr int kHighGearController = 3;
+ const Eigen::Matrix<double, 2, 2> FF_high =
+ loop_->controller(kHighGearController).plant.B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() -
+ loop_->controller(kHighGearController).plant.A());
+
+ ::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
+ int min_FF_sum_index;
+ const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
+ //const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+ const double high_min_FF_sum = FF_high.col(0).sum();
+
+ const double adjusted_ff_voltage = ::aos::Clip(
+ 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
+ return adjusted_ff_voltage / min_FF_sum;
+ }
+
+ void Update() {
+ // TODO(austin): Observer for the current velocity instead of difference
+ // calculations.
+ ++counter_;
+ const double current_left_velocity =
+ (position_.left_encoder - last_position_.left_encoder) /
+ position_time_delta_;
+ const double current_right_velocity =
+ (position_.right_encoder - last_position_.right_encoder) /
+ position_time_delta_;
+ const double left_motor_speed =
+ MotorSpeed(kBot3LeftDriveShifter,
+ position_.left_shifter_position,
+ current_left_velocity);
+ const double right_motor_speed =
+ MotorSpeed(kBot3RightDriveShifter,
+ position_.right_shifter_position,
+ current_right_velocity);
+
+ {
+ CIMLogging logging;
+
+ // Reset the CIM model to the current conditions to be ready for when we
+ // shift.
+ if (IsInGear(left_gear_)) {
+ logging.left_in_gear = true;
+ } else {
+ logging.left_in_gear = false;
+ }
+ logging.left_motor_speed = left_motor_speed;
+ logging.left_velocity = current_left_velocity;
+ if (IsInGear(right_gear_)) {
+ logging.right_in_gear = true;
+ } else {
+ logging.right_in_gear = false;
+ }
+ logging.right_motor_speed = right_motor_speed;
+ logging.right_velocity = current_right_velocity;
+
+ LOG_STRUCT(DEBUG, "currently", logging);
+ }
+
+ if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
+ // FF * X = U (steady state)
+ const Eigen::Matrix<double, 2, 2> FF =
+ loop_->B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+
+ // Invert the plant to figure out how the velocity filter would have to
+ // work
+ // out in order to filter out the forwards negative inertia.
+ // This math assumes that the left and right power and velocity are
+ // equals,
+ // and that the plant is the same on the left and right.
+ const double fvel = FilterVelocity(throttle_);
+
+ const double sign_svel = wheel_ * ((fvel > 0.0) ? 1.0 : -1.0);
+ double steering_velocity;
+ if (quickturn_) {
+ steering_velocity = wheel_ * MaxVelocity();
+ } else {
+ steering_velocity = ::std::abs(fvel) * wheel_;
+ }
+ const double left_velocity = fvel - steering_velocity;
+ const double right_velocity = fvel + steering_velocity;
+
+ // Integrate velocity to get the position.
+ // This position is used to get integral control.
+ loop_->mutable_R() << left_velocity, right_velocity;
+
+ if (!quickturn_) {
+ // K * R = w
+ Eigen::Matrix<double, 1, 2> equality_k;
+ equality_k << 1 + sign_svel, -(1 - sign_svel);
+ const double equality_w = 0.0;
+
+ // Construct a constraint on R by manipulating the constraint on U
+ ::aos::controls::HPolytope<2> R_poly = ::aos::controls::HPolytope<2>(
+ U_Poly_.H() * (loop_->K() + FF),
+ U_Poly_.k() + U_Poly_.H() * loop_->K() * loop_->X_hat());
+
+ // Limit R back inside the box.
+ loop_->mutable_R() =
+ CoerceGoal(R_poly, equality_k, equality_w, loop_->R());
+ }
+
+ const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
+ const Eigen::Matrix<double, 2, 1> U_ideal =
+ loop_->K() * (loop_->R() - loop_->X_hat()) + FF_volts;
+
+ for (int i = 0; i < 2; i++) {
+ loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
+ }
+
+ // TODO(austin): Model this better.
+ // TODO(austin): Feed back?
+ loop_->mutable_X_hat() =
+ loop_->A() * loop_->X_hat() + loop_->B() * loop_->U();
+ } else {
+ // Any motor is not in gear. Speed match.
+ ::Eigen::Matrix<double, 1, 1> R_left;
+ ::Eigen::Matrix<double, 1, 1> R_right;
+ R_left(0, 0) = left_motor_speed;
+ R_right(0, 0) = right_motor_speed;
+
+ const double wiggle =
+ (static_cast<double>((counter_ % 20) / 10) - 0.5) * 5.0;
+
+ loop_->mutable_U(0, 0) = ::aos::Clip(
+ (R_left / Kv)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
+ loop_->mutable_U(1, 0) = ::aos::Clip(
+ (R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
+ loop_->mutable_U() *= 12.0 / position_.battery_voltage;
+ }
+ }
+
+ void SendMotors(Drivetrain::Output *output) {
+ if (output != NULL) {
+ output->left_voltage = loop_->U(0, 0);
+ output->right_voltage = loop_->U(1, 0);
+ output->left_high = left_gear_ == HIGH || left_gear_ == SHIFTING_UP;
+ output->right_high = right_gear_ == HIGH || right_gear_ == SHIFTING_UP;
+ }
+ }
+
+ private:
+ const ::aos::controls::HPolytope<2> U_Poly_;
+
+ ::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;
+
+ const double ttrust_;
+ double wheel_;
+ double throttle_;
+ bool quickturn_;
+ int stale_count_;
+ double position_time_delta_;
+ Gear left_gear_;
+ Gear right_gear_;
+ Drivetrain::Position last_position_;
+ Drivetrain::Position position_;
+ int counter_;
+};
+constexpr double PolyDrivetrain::kStallTorque;
+constexpr double PolyDrivetrain::kStallCurrent;
+constexpr double PolyDrivetrain::kFreeSpeed;
+constexpr double PolyDrivetrain::kFreeCurrent;
+constexpr double PolyDrivetrain::J;
+constexpr double PolyDrivetrain::m;
+constexpr double PolyDrivetrain::rb;
+constexpr double PolyDrivetrain::kWheelRadius;
+constexpr double PolyDrivetrain::kR;
+constexpr double PolyDrivetrain::Kv;
+constexpr double PolyDrivetrain::Kt;
+
+
+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 PolyDrivetrain dt_openloop;
+
+ bool bad_pos = false;
+ if (position == nullptr) {
+ LOG_INTERVAL(no_position_);
+ bad_pos = true;
+ }
+ no_position_.Print();
+
+ 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_reading.FetchLatest()) {
+ LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+ dt_closedloop.SetPosition(left_encoder, right_encoder,
+ gyro_reading->angle);
+ } else {
+ dt_closedloop.SetRawPosition(left_encoder, right_encoder);
+ }
+ }
+ dt_openloop.SetPosition(position);
+ dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
+ dt_openloop.Update();
+
+ if (control_loop_driving) {
+ dt_closedloop.Update(output == NULL, true);
+ dt_closedloop.SendMotors(output);
+ } else {
+ dt_openloop.SendMotors(output);
+ if (output) {
+ dt_closedloop.SetExternalMotors(output->left_voltage,
+ output->right_voltage);
+ }
+ dt_closedloop.Update(output == NULL, false);
+ }
+
+ // set the output status of the control loop state
+ if (status) {
+ bool done = false;
+ if (goal) {
+ done = ((::std::abs(goal->left_goal -
+ dt_closedloop.GetEstimatedLeftEncoder()) <
+ kBot3DrivetrainDoneDistance) &&
+ (::std::abs(goal->right_goal -
+ dt_closedloop.GetEstimatedRightEncoder()) <
+ kBot3DrivetrainDoneDistance));
+ }
+ status->is_done = done;
+ status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
+ status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
+ status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
+ status->output_was_capped = dt_closedloop.OutputWasCapped();
+ status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
+ status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
+ }
+}
+
+} // 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..d17cdf2
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain.gyp
@@ -0,0 +1,102 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'drivetrain_loop',
+ 'type': 'static_library',
+ 'sources': ['drivetrain.q'],
+ 'variables': {
+ 'header_path': 'bot3/control_loops/drivetrain',
+ },
+ 'dependencies': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'polydrivetrain_plants',
+ 'type': 'static_library',
+ 'sources': [
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'drivetrain_dog_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_constants',
+ 'type': 'static_library',
+ 'sources': [
+ #'drivetrain_constants.h'
+ ],
+ 'dependencies': [
+ 'polydrivetrain_plants',
+ ],
+ 'export_dependent_settings': [
+ 'polydrivetrain_plants',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'drivetrain.cc',
+ 'polydrivetrain_cim_plant.cc',
+ ],
+ 'dependencies': [
+ 'drivetrain_loop',
+ 'drivetrain_constants',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/controls/controls.gyp:polytope',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/util/util.gyp:log_interval',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/logging/logging.gyp:matrix_logging',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/controls/controls.gyp:polytope',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ 'drivetrain_loop',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'drivetrain_loop',
+ 'drivetrain_lib',
+ '<(AOS)/common/controls/controls.gyp:control_loop_test',
+ '<(AOS)/common/network/network.gyp:team_number',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_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..b2bed8c
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain.h
@@ -0,0 +1,43 @@
+#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+#define BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+
+#include "Eigen/Dense"
+
+#include "aos/common/controls/polytope.h"
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/controls/polytope.h"
+#include "aos/common/util/log_interval.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+
+namespace bot3 {
+namespace control_loops {
+
+class DrivetrainLoop
+ : public aos::controls::ControlLoop<control_loops::Drivetrain, true, false> {
+ public:
+ // Constructs a control loop which can take a Drivetrain or defaults to the
+ // drivetrain at bot3::control_loops::drivetrain
+ explicit DrivetrainLoop(
+ control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
+ : aos::controls::ControlLoop<control_loops::Drivetrain, true, false>(
+ my_drivetrain) {
+ ::aos::controls::HPolytope<0>::Init();
+ }
+
+ 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);
+
+ typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
+ SimpleLogInterval no_position_ = SimpleLogInterval(
+ ::aos::time::Time::InSeconds(0.25), WARNING, "no position");
+};
+
+} // 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..f4fe3d5
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain.q
@@ -0,0 +1,70 @@
+package bot3.control_loops;
+
+import "aos/common/controls/control_loops.q";
+
+struct GearLogging {
+ int8_t controller_index;
+ bool left_loop_high;
+ bool right_loop_high;
+ int8_t left_state;
+ int8_t right_state;
+};
+
+struct CIMLogging {
+ bool left_in_gear;
+ bool right_in_gear;
+ double left_motor_speed;
+ double right_motor_speed;
+ double left_velocity;
+ double right_velocity;
+};
+
+queue_group Drivetrain {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ double steering;
+ double throttle;
+ bool highgear;
+ bool quickturn;
+ bool control_loop_driving;
+ double left_goal;
+ double left_velocity_goal;
+ double right_goal;
+ double right_velocity_goal;
+ };
+
+ message Position {
+ double left_encoder;
+ double right_encoder;
+ double left_shifter_position;
+ double right_shifter_position;
+ double battery_voltage;
+ };
+
+ message Output {
+ double left_voltage;
+ double right_voltage;
+ bool left_high;
+ bool right_high;
+ };
+
+ message Status {
+ double robot_speed;
+ double filtered_left_position;
+ double filtered_right_position;
+
+ double uncapped_left_voltage;
+ double uncapped_right_voltage;
+ bool output_was_capped;
+
+ 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_constants.h b/bot3/control_loops/drivetrain/drivetrain_constants.h
new file mode 100644
index 0000000..9d49d59
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_constants.h
@@ -0,0 +1,25 @@
+#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_CONSTANTS_H_
+#define BOT3_CONTROL_LOOPS_DRIVETRAIN_CONSTANTS_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/shifter_hall_effect.h"
+
+namespace bot3 {
+namespace control_loops {
+
+// TODO(danielp): Figure out the real values for these constants.
+constexpr ::frc971::constants::ShifterHallEffect kBot3LeftDriveShifter =
+ {555, 657, 660, 560, 0.2, 0.7};
+constexpr ::frc971::constants::ShifterHallEffect kBot3RightDriveShifter =
+ {555, 660, 644, 552, 0.2, 0.7};
+
+constexpr double kBot3TurnWidth = 0.5;
+constexpr double kBot3DrivetrainDoneDistance = 0.02;
+
+constexpr double kBot3HighGearRatio = 18.0 / 60.0 * 18.0 / 50.0;
+constexpr double kBot3LowGearRatio = 28.0 / 50.0 * 18.0 / 50.0;
+
+} // control_loops
+} // bot3
+
+#endif
diff --git a/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
new file mode 100644
index 0000000..80d69bf
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -0,0 +1,125 @@
+#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00751678417107, 0.0, 0.000244815974033, 0.0, 0.548849954683, 0.0, 0.0396601987617, 0.0, 0.000244815974033, 1.0, 0.00751678417107, 0.0, 0.0396601987617, 0.0, 0.548849954683;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.000357151105465, -3.52109126974e-05, 0.0648871256127, -0.0057041694345, -3.52109126974e-05, 0.000357151105465, -0.0057041694345, 0.0648871256127;
+ 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);
+}
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00751193529517, 0.0, 3.41705796399e-05, 0.0, 0.547617329816, 0.0, 0.00612721792429, 0.0, 0.000291766839985, 1.0, 0.00965616939349, 0.0, 0.0523174915529, 0.0, 0.932105674456;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.000357848500096, -1.43609003799e-05, 0.0650644091693, -0.00257509141326, -4.19636699414e-05, 0.000144501999664, -0.00752461776602, 0.0285340095421;
+ 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);
+}
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00965616939349, 0.0, 0.000291766839985, 0.0, 0.932105674456, 0.0, 0.0523174915529, 0.0, 3.41705796399e-05, 1.0, 0.00751193529517, 0.0, 0.00612721792429, 0.0, 0.547617329816;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.000144501999664, -4.19636699414e-05, 0.0285340095421, -0.00752461776602, -1.43609003799e-05, 0.000357848500096, -0.00257509141326, 0.0650644091693;
+ 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);
+}
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00965542888285, 0.0, 4.04460947997e-05, 0.0, 0.931897839258, 0.0, 0.00790011087396, 0.0, 4.04460947997e-05, 1.0, 0.00965542888285, 0.0, 0.00790011087396, 0.0, 0.931897839258;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.000144813214739, -1.69983168063e-05, 0.0286213566286, -0.00332018673511, -1.69983168063e-05, 0.000144813214739, -0.00332018673511, 0.0286213566286;
+ 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> MakeDrivetrainLowLowController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 0.848849954683, 0.0396601987617, 5.07410924717, 1.93309188139, 0.0396601987617, 0.848849954683, 1.93309188139, 5.07410924717;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 122.814750097, 4.68501085975, 3.50201207752, 0.435944310585, 3.50201207752, 0.435944310585, 122.814750097, 4.68501085975;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowLowPlantCoefficients());
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 0.84475614898, 0.00630513438581, 4.80790352862, 0.482569304809, 0.00630513438581, 1.23496685529, 1.52827048914, 35.0095906678;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 122.438502625, 4.65238426191, -3.6222797894, -0.114901477949, 9.82445972218, 1.21630521516, 139.573569983, 11.6848475814;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowHighPlantCoefficients());
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 1.23976956418, 0.051008821609, 35.4898554745, 3.96774253511, 0.051008821609, 0.839953440087, 1.36277593575, 4.73889175169;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 139.573569983, 11.6848475814, 9.82445972219, 1.21630521516, -3.62227978939, -0.114901477949, 122.438502625, 4.65238426191;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighLowPlantCoefficients());
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 1.23189783926, 0.00790011087396, 34.8130461883, 0.806392261077, 0.00790011087396, 1.23189783926, 0.806392261077, 34.8130461883;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 139.67425847, 11.6895445084, 2.52787999423, 0.599798020725, 2.52787999422, 0.599798020724, 139.67425847, 11.6895445084;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighHighPlantCoefficients());
+}
+
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(4);
+ plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowLowPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowHighPlantCoefficients());
+ plants[2] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighLowPlantCoefficients());
+ plants[3] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighHighPlantCoefficients());
+ return StateFeedbackPlant<4, 2, 2>(plants);
+}
+
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
+ ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(4);
+ controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowLowController());
+ controllers[1] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowHighController());
+ controllers[2] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighLowController());
+ controllers[3] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighHighController());
+ return StateFeedbackLoop<4, 2, 2>(controllers);
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
new file mode 100644
index 0000000..f5d5896
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
@@ -0,0 +1,32 @@
+#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController();
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController();
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController();
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController();
+
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
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..89e41c6
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -0,0 +1,294 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/network/team_number.h"
+#include "aos/common/queue_testutils.h"
+#include "aos/common/controls/polytope.h"
+#include "aos/common/controls/control_loop_test.h"
+
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.h"
+#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
+#include "frc971/queues/other_sensors.q.h"
+
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+
+class Environment : public ::testing::Environment {
+ public:
+ virtual ~Environment() {}
+ // how to set up the environment.
+ virtual void SetUp() {
+ aos::controls::HPolytope<0>::Init();
+ }
+};
+::testing::Environment* const holder_env =
+ ::testing::AddGlobalTestEnvironment(new Environment);
+
+class TeamNumberEnvironment : public ::testing::Environment {
+ public:
+ // Override this to define how to set up the environment.
+ virtual void SetUp() { aos::network::OverrideTeamNumber(971); }
+};
+
+::testing::Environment* const team_number_env =
+ ::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment);
+
+// Class which simulates the drivetrain and sends out queue messages containing
+// the position.
+class DrivetrainSimulation {
+ public:
+ // Constructs a motor simulation.
+ // TODO(aschuh) Do we want to test the clutch one too?
+ 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_->mutable_X(0, 0) = 0.0;
+ drivetrain_plant_->mutable_X(1, 0) = 0.0;
+ drivetrain_plant_->mutable_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_->mutable_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 ::aos::testing::ControlLoopTest {
+ protected:
+ // 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_() {
+ ::frc971::sensors::gyro_reading.Clear();
+ }
+
+ 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() {
+ ::frc971::sensors::gyro_reading.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();
+ SimulateTimestep(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) {
+ SimulateTimestep(false);
+ } else {
+ SimulateTimestep(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();
+ SimulateTimestep(true);
+ }
+ VerifyNearGoal();
+}
+
+::aos::controls::HPolytope<2> MakeBox(double x1_min, double x1_max,
+ double x2_min, double x2_max) {
+ Eigen::Matrix<double, 4, 2> box_H;
+ box_H << /*[[*/ 1.0, 0.0 /*]*/,
+ /*[*/-1.0, 0.0 /*]*/,
+ /*[*/ 0.0, 1.0 /*]*/,
+ /*[*/ 0.0,-1.0 /*]]*/;
+ Eigen::Matrix<double, 4, 1> box_k;
+ box_k << /*[[*/ x1_max /*]*/,
+ /*[*/-x1_min /*]*/,
+ /*[*/ x2_max /*]*/,
+ /*[*/-x2_min /*]]*/;
+ ::aos::controls::HPolytope<2> t_poly(box_H, box_k);
+ return t_poly;
+}
+
+class CoerceGoalTest : public ::testing::Test {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+// WHOOOHH!
+TEST_F(CoerceGoalTest, Inside) {
+ ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+
+ Eigen::Matrix<double, 1, 2> K;
+ K << /*[[*/ 1, -1 /*]]*/;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << /*[[*/ 1.5, 1.5 /*]]*/;
+
+ Eigen::Matrix<double, 2, 1> output =
+ ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+
+ EXPECT_EQ(R(0, 0), output(0, 0));
+ EXPECT_EQ(R(1, 0), output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, Outside_Inside_Intersect) {
+ ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1, -1;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << 5, 5;
+
+ Eigen::Matrix<double, 2, 1> output =
+ ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+
+ EXPECT_EQ(2.0, output(0, 0));
+ EXPECT_EQ(2.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, Outside_Inside_no_Intersect) {
+ ::aos::controls::HPolytope<2> box = MakeBox(3, 4, 1, 2);
+
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1, -1;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << 5, 5;
+
+ Eigen::Matrix<double, 2, 1> output =
+ ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+
+ EXPECT_EQ(3.0, output(0, 0));
+ EXPECT_EQ(2.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, Middle_Of_Edge) {
+ ::aos::controls::HPolytope<2> box = MakeBox(0, 4, 1, 2);
+
+ Eigen::Matrix<double, 1, 2> K;
+ K << -1, 1;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << 5, 5;
+
+ Eigen::Matrix<double, 2, 1> output =
+ ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+
+ EXPECT_EQ(2.0, output(0, 0));
+ EXPECT_EQ(2.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, PerpendicularLine) {
+ ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1, 1;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << 5, 5;
+
+ Eigen::Matrix<double, 2, 1> output =
+ ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+
+ EXPECT_EQ(1.0, output(0, 0));
+ EXPECT_EQ(1.0, output(1, 0));
+}
+
+} // 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..f06acc0
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,11 @@
+#include "bot3/control_loops/drivetrain/drivetrain.h"
+
+#include "aos/linux_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/polydrivetrain_cim_plant.cc b/bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc
new file mode 100644
index 0000000..1287483
--- /dev/null
+++ b/bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<1, 1, 1> MakeCIMPlantCoefficients() {
+ Eigen::Matrix<double, 1, 1> A;
+ A << 0.614537580221;
+ Eigen::Matrix<double, 1, 1> B;
+ B << 15.9657598852;
+ Eigen::Matrix<double, 1, 1> C;
+ C << 1;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<1, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<1, 1, 1> MakeCIMController() {
+ Eigen::Matrix<double, 1, 1> L;
+ L << 0.604537580221;
+ Eigen::Matrix<double, 1, 1> K;
+ K << 0.0378646293422;
+ return StateFeedbackController<1, 1, 1>(L, K, MakeCIMPlantCoefficients());
+}
+
+StateFeedbackPlant<1, 1, 1> MakeCIMPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<1, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<1, 1, 1>(MakeCIMPlantCoefficients());
+ return StateFeedbackPlant<1, 1, 1>(plants);
+}
+
+StateFeedbackLoop<1, 1, 1> MakeCIMLoop() {
+ ::std::vector<StateFeedbackController<1, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<1, 1, 1>(MakeCIMController());
+ return StateFeedbackLoop<1, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h b/bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h
new file mode 100644
index 0000000..12b2c59
--- /dev/null
+++ b/bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<1, 1, 1> MakeCIMPlantCoefficients();
+
+StateFeedbackController<1, 1, 1> MakeCIMController();
+
+StateFeedbackPlant<1, 1, 1> MakeCIMPlant();
+
+StateFeedbackLoop<1, 1, 1> MakeCIMLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
diff --git a/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
new file mode 100644
index 0000000..df05328
--- /dev/null
+++ b/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -0,0 +1,125 @@
+#include "bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.548849954683, 0.0396601987617, 0.0396601987617, 0.548849954683;
+ Eigen::Matrix<double, 2, 2> B;
+ B << 0.0648871256127, -0.0057041694345, -0.0057041694345, 0.0648871256127;
+ Eigen::Matrix<double, 2, 2> C;
+ C << 1.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.547617329816, 0.00612721792429, 0.0523174915529, 0.932105674456;
+ Eigen::Matrix<double, 2, 2> B;
+ B << 0.0650644091693, -0.00257509141326, -0.00752461776602, 0.0285340095421;
+ Eigen::Matrix<double, 2, 2> C;
+ C << 1.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.932105674456, 0.0523174915529, 0.00612721792429, 0.547617329816;
+ Eigen::Matrix<double, 2, 2> B;
+ B << 0.0285340095421, -0.00752461776602, -0.00257509141326, 0.0650644091693;
+ Eigen::Matrix<double, 2, 2> C;
+ C << 1.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.931897839258, 0.00790011087396, 0.00790011087396, 0.931897839258;
+ Eigen::Matrix<double, 2, 2> B;
+ B << 0.0286213566286, -0.00332018673511, -0.00332018673511, 0.0286213566286;
+ Eigen::Matrix<double, 2, 2> C;
+ C << 1.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
+ Eigen::Matrix<double, 2, 2> L;
+ L << 0.528849954683, 0.0396601987617, 0.0396601987617, 0.528849954683;
+ Eigen::Matrix<double, 2, 2> K;
+ K << -0.740281917714, 0.546140778145, 0.546140778145, -0.740281917714;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
+}
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
+ Eigen::Matrix<double, 2, 2> L;
+ L << 0.524323257635, 0.0520813668112, 0.0520813668112, 0.915399746637;
+ Eigen::Matrix<double, 2, 2> K;
+ K << -0.740249343264, 0.560664230143, 1.6383045686, 11.786792809;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
+}
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
+ Eigen::Matrix<double, 2, 2> L;
+ L << 0.915985613609, 0.00645546174383, 0.00645546174383, 0.523737390662;
+ Eigen::Matrix<double, 2, 2> K;
+ K << 11.786792809, 1.6383045686, 0.560664230143, -0.740249343264;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
+}
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
+ Eigen::Matrix<double, 2, 2> L;
+ L << 0.911897839258, 0.00790011087396, 0.00790011087396, 0.911897839258;
+ Eigen::Matrix<double, 2, 2> K;
+ K << 11.7867933868, 1.64333460977, 1.64333460977, 11.7867933868;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients());
+ plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients());
+ plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients());
+ return StateFeedbackPlant<2, 2, 2>(plants);
+}
+
+StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop() {
+ ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
+ controllers[0] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController());
+ controllers[1] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController());
+ controllers[2] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController());
+ controllers[3] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController());
+ return StateFeedbackLoop<2, 2, 2>(controllers);
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h b/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
new file mode 100644
index 0000000..191d1aa
--- /dev/null
+++ b/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
@@ -0,0 +1,32 @@
+#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController();
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController();
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController();
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController();
+
+StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant();
+
+StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop();
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/bot3/control_loops/python/drivetrain.py b/bot3/control_loops/python/drivetrain.py
new file mode 100755
index 0000000..65faf2a
--- /dev/null
+++ b/bot3/control_loops/python/drivetrain.py
@@ -0,0 +1,239 @@
+#!/usr/bin/python
+
+import control_loop
+import controls
+import numpy
+import sys
+from matplotlib import pylab
+
+
+class CIM(control_loop.ControlLoop):
+ def __init__(self):
+ super(CIM, self).__init__("CIM")
+ # Stall Torque in N m
+ self.stall_torque = 2.42
+ # Stall Current in Amps
+ self.stall_current = 133
+ # Free Speed in RPM
+ self.free_speed = 4650.0
+ # Free Current in Amps
+ self.free_current = 2.7
+ # Moment of inertia of the CIM in kg m^2
+ self.J = 0.0001
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[-self.Kt / self.Kv / (self.J * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[self.Kt / (self.J * self.R)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([0.01])
+ self.PlaceObserverPoles([0.01])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
+
+class Drivetrain(control_loop.ControlLoop):
+ def __init__(self, name="Drivetrain", left_low=True, right_low=True):
+ super(Drivetrain, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 2.42
+ # Stall Current in Amps
+ self.stall_current = 133
+ # Free Speed in RPM. Used number from last year.
+ self.free_speed = 4650.0
+ # Free Current in Amps
+ self.free_current = 2.7
+ # Moment of inertia of the drivetrain in kg m^2
+ # Just borrowed from last year.
+ self.J = 4.5
+ # Mass of the robot, in kg.
+ self.m = 60
+ # Radius of the robot, in meters (from last year).
+ self.rb = 0.617998644 / 2.0
+ # Radius of the wheels, in meters.
+ self.r = .04445
+ # Resistance of the motor, divided by the number of motors.
+ self.R = 12.0 / self.stall_current / 4
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratios
+ self.G_low = 14.0 / 60.0 * 17.0 / 50.0
+ self.G_high = 30.0 / 44.0 * 17.0 / 50.0
+ if left_low:
+ self.Gl = self.G_low
+ else:
+ self.Gl = self.G_high
+ if right_low:
+ self.Gr = self.G_low
+ else:
+ self.Gr = self.G_high
+ # Control loop time step
+ self.dt = 0.01
+
+ # These describe the way that a given side of a robot will be influenced
+ # by the other side. Units of 1 / kg.
+ self.msp = 1.0 / self.m + self.rb * self.rb / self.J
+ self.msn = 1.0 / self.m - self.rb * self.rb / self.J
+ # The calculations which we will need for A and B.
+ self.tcl = -self.Kt / self.Kv / (self.Gl * self.Gl * self.R * self.r * self.r)
+ self.tcr = -self.Kt / self.Kv / (self.Gr * self.Gr * self.R * self.r * self.r)
+ self.mpl = self.Kt / (self.Gl * self.R * self.r)
+ self.mpr = self.Kt / (self.Gr * self.R * self.r)
+
+ # State feedback matrices
+ # X will be of the format
+ # [[positionl], [velocityl], [positionr], velocityr]]
+ self.A_continuous = numpy.matrix(
+ [[0, 1, 0, 0],
+ [0, self.msp * self.tcl, 0, self.msn * self.tcr],
+ [0, 0, 0, 1],
+ [0, self.msn * self.tcl, 0, self.msp * self.tcr]])
+ self.B_continuous = numpy.matrix(
+ [[0, 0],
+ [self.msp * self.mpl, self.msn * self.mpr],
+ [0, 0],
+ [self.msn * self.mpl, self.msp * self.mpr]])
+ self.C = numpy.matrix([[1, 0, 0, 0],
+ [0, 0, 1, 0]])
+ self.D = numpy.matrix([[0, 0],
+ [0, 0]])
+
+ #print "THE NUMBER I WANT" + str(numpy.linalg.inv(self.A_continuous) * -self.B_continuous * numpy.matrix([[12.0], [12.0]]))
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ # Poles from last year.
+ self.hp = 0.65
+ self.lp = 0.83
+ self.PlaceControllerPoles([self.hp, self.lp, self.hp, self.lp])
+ print self.K
+ q_pos = 0.07
+ q_vel = 1.0
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ print self.A
+ print self.B
+ print self.K
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+ self.hlp = 0.3
+ self.llp = 0.4
+ self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
+
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ self.InitializeState()
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ drivetrain = Drivetrain()
+ simulated_left = []
+ simulated_right = []
+ for _ in xrange(100):
+ drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
+ simulated_left.append(drivetrain.X[0, 0])
+ simulated_right.append(drivetrain.X[2, 0])
+
+ #pylab.plot(range(100), simulated_left)
+ #pylab.plot(range(100), simulated_right)
+ #pylab.show()
+
+ # Simulate forwards motion.
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ #pylab.plot(range(100), close_loop_left)
+ #pylab.plot(range(100), close_loop_right)
+ #pylab.show()
+
+ # Try turning in place
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ #pylab.plot(range(100), close_loop_left)
+ #pylab.plot(range(100), close_loop_right)
+ #pylab.show()
+
+ # Try turning just one side.
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ #pylab.plot(range(100), close_loop_left)
+ #pylab.plot(range(100), close_loop_right)
+ #pylab.show()
+
+ # Write the generated constants out to a file.
+ print "Output one"
+ drivetrain_low_low = Drivetrain(name="DrivetrainLowLow", left_low=True, right_low=True)
+ drivetrain_low_high = Drivetrain(name="DrivetrainLowHigh", left_low=True, right_low=False)
+ drivetrain_high_low = Drivetrain(name="DrivetrainHighLow", left_low=False, right_low=True)
+ drivetrain_high_high = Drivetrain(name="DrivetrainHighHigh", left_low=False, right_low=False)
+
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "Drivetrain", [drivetrain_low_low, drivetrain_low_high,
+ drivetrain_high_low, drivetrain_high_high],
+ namespaces = ["bot3", "control_loops"])
+ if argv[1][-3:] == '.cc':
+ dog_loop_writer.Write(argv[2], argv[1])
+ else:
+ dog_loop_writer.Write(argv[1], argv[2])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/bot3/control_loops/python/polydrivetrain.py b/bot3/control_loops/python/polydrivetrain.py
new file mode 100755
index 0000000..49d390b
--- /dev/null
+++ b/bot3/control_loops/python/polydrivetrain.py
@@ -0,0 +1,501 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+import polytope
+import drivetrain
+import control_loop
+import controls
+from matplotlib import pylab
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+def CoerceGoal(region, K, w, R):
+ """Intersects a line with a region, and finds the closest point to R.
+
+ Finds a point that is closest to R inside the region, and on the line
+ defined by K X = w. If it is not possible to find a point on the line,
+ finds a point that is inside the region and closest to the line. This
+ function assumes that
+
+ Args:
+ region: HPolytope, the valid goal region.
+ K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
+ w: float, the offset in the equation above.
+ R: numpy.matrix (2 x 1), the point to be closest to.
+
+ Returns:
+ numpy.matrix (2 x 1), the point.
+ """
+ return DoCoerceGoal(region, K, w, R)[0]
+
+def DoCoerceGoal(region, K, w, R):
+ if region.IsInside(R):
+ return (R, True)
+
+ perpendicular_vector = K.T / numpy.linalg.norm(K)
+ parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
+ [-perpendicular_vector[0, 0]]])
+
+ # We want to impose the constraint K * X = w on the polytope H * X <= k.
+ # We do this by breaking X up into parallel and perpendicular components to
+ # the half plane. This gives us the following equation.
+ #
+ # parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
+ #
+ # Then, substitute this into the polytope.
+ #
+ # H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
+ #
+ # Substitute K * X = w
+ #
+ # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
+ #
+ # Move all the knowns to the right side.
+ #
+ # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
+ #
+ # Let t = parallel.T \dot X, the component parallel to the surface.
+ #
+ # H * parallel * t <= k - H * perpendicular * w
+ #
+ # This is a polytope which we can solve, and use to figure out the range of X
+ # that we care about!
+
+ t_poly = polytope.HPolytope(
+ region.H * parallel_vector,
+ region.k - region.H * perpendicular_vector * w)
+
+ vertices = t_poly.Vertices()
+
+ if vertices.shape[0]:
+ # The region exists!
+ # Find the closest vertex
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in vertices:
+ point = parallel_vector * vertex + perpendicular_vector * w
+ length = numpy.linalg.norm(R - point)
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
+
+ return (closest_point, True)
+ else:
+ # Find the vertex of the space that is closest to the line.
+ region_vertices = region.Vertices()
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in region_vertices:
+ point = vertex.T
+ length = numpy.abs((perpendicular_vector.T * point)[0, 0])
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
+
+ return (closest_point, False)
+
+
+class VelocityDrivetrainModel(control_loop.ControlLoop):
+ def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel"):
+ super(VelocityDrivetrainModel, self).__init__(name)
+ self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
+ right_low=right_low)
+ self.dt = 0.01
+ self.A_continuous = numpy.matrix(
+ [[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
+ [self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
+
+ self.B_continuous = numpy.matrix(
+ [[self._drivetrain.B_continuous[1, 0], self._drivetrain.B_continuous[1, 1]],
+ [self._drivetrain.B_continuous[3, 0], self._drivetrain.B_continuous[3, 1]]])
+ self.C = numpy.matrix(numpy.eye(2));
+ self.D = numpy.matrix(numpy.zeros((2, 2)));
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ # FF * X = U (steady state)
+ self.FF = self.B.I * (numpy.eye(2) - self.A)
+
+ self.PlaceControllerPoles([0.6, 0.6])
+ self.PlaceObserverPoles([0.02, 0.02])
+
+ self.G_high = self._drivetrain.G_high
+ self.G_low = self._drivetrain.G_low
+ self.R = self._drivetrain.R
+ self.r = self._drivetrain.r
+ self.Kv = self._drivetrain.Kv
+ self.Kt = self._drivetrain.Kt
+
+ self.U_max = self._drivetrain.U_max
+ self.U_min = self._drivetrain.U_min
+
+
+class VelocityDrivetrain(object):
+ HIGH = 'high'
+ LOW = 'low'
+ SHIFTING_UP = 'up'
+ SHIFTING_DOWN = 'down'
+
+ def __init__(self):
+ self.drivetrain_low_low = VelocityDrivetrainModel(
+ left_low=True, right_low=True, name='VelocityDrivetrainLowLow')
+ self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name='VelocityDrivetrainLowHigh')
+ self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = 'VelocityDrivetrainHighLow')
+ self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = 'VelocityDrivetrainHighHigh')
+
+ # X is [lvel, rvel]
+ self.X = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ self.U_poly = polytope.HPolytope(
+ numpy.matrix([[1, 0],
+ [-1, 0],
+ [0, 1],
+ [0, -1]]),
+ numpy.matrix([[12],
+ [12],
+ [12],
+ [12]]))
+
+ self.U_max = numpy.matrix(
+ [[12.0],
+ [12.0]])
+ self.U_min = numpy.matrix(
+ [[-12.0000000000],
+ [-12.0000000000]])
+
+ self.dt = 0.01
+
+ self.R = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ # ttrust is the comprimise between having full throttle negative inertia,
+ # and having no throttle negative inertia. A value of 0 is full throttle
+ # inertia. A value of 1 is no throttle negative inertia.
+ self.ttrust = 1.0
+
+ self.left_gear = VelocityDrivetrain.LOW
+ self.right_gear = VelocityDrivetrain.LOW
+ self.left_shifter_position = 0.0
+ self.right_shifter_position = 0.0
+ self.left_cim = drivetrain.CIM()
+ self.right_cim = drivetrain.CIM()
+
+ def IsInGear(self, gear):
+ return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
+
+ def MotorRPM(self, shifter_position, velocity):
+ if shifter_position > 0.5:
+ return (velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
+ else:
+ return (velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
+
+ def CurrentDrivetrain(self):
+ if self.left_shifter_position > 0.5:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_high_high
+ else:
+ return self.drivetrain_high_low
+ else:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_low_high
+ else:
+ return self.drivetrain_low_low
+
+ def SimShifter(self, gear, shifter_position):
+ if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
+ shifter_position = min(shifter_position + 0.5, 1.0)
+ else:
+ shifter_position = max(shifter_position - 0.5, 0.0)
+
+ if shifter_position == 1.0:
+ gear = VelocityDrivetrain.HIGH
+ elif shifter_position == 0.0:
+ gear = VelocityDrivetrain.LOW
+
+ return gear, shifter_position
+
+ def ComputeGear(self, wheel_velocity, should_print=False, current_gear=False, gear_name=None):
+ high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
+ low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
+ #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
+ high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+ low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+ high_power = high_torque * high_omega
+ low_power = low_torque * low_omega
+ #if should_print:
+ # print gear_name, "High omega", high_omega, "Low omega", low_omega
+ # print gear_name, "High torque", high_torque, "Low torque", low_torque
+ # print gear_name, "High power", high_power, "Low power", low_power
+
+ # Shift algorithm improvements.
+ # TODO(aschuh):
+ # It takes time to shift. Shifting down for 1 cycle doesn't make sense
+ # because you will end up slower than without shifting. Figure out how
+ # to include that info.
+ # If the driver is still in high gear, but isn't asking for the extra power
+ # from low gear, don't shift until he asks for it.
+ goal_gear_is_high = high_power > low_power
+ #goal_gear_is_high = True
+
+ if not self.IsInGear(current_gear):
+ print gear_name, 'Not in gear.'
+ return current_gear
+ else:
+ is_high = current_gear is VelocityDrivetrain.HIGH
+ if is_high != goal_gear_is_high:
+ if goal_gear_is_high:
+ print gear_name, 'Shifting up.'
+ return VelocityDrivetrain.SHIFTING_UP
+ else:
+ print gear_name, 'Shifting down.'
+ return VelocityDrivetrain.SHIFTING_DOWN
+ else:
+ return current_gear
+
+ def FilterVelocity(self, throttle):
+ # Invert the plant to figure out how the velocity filter would have to work
+ # out in order to filter out the forwards negative inertia.
+ # This math assumes that the left and right power and velocity are equal.
+
+ # The throttle filter should filter such that the motor in the highest gear
+ # should be controlling the time constant.
+ # Do this by finding the index of FF that has the lowest value, and computing
+ # the sums using that index.
+ FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+ min_FF_sum_index = numpy.argmin(FF_sum)
+ min_FF_sum = FF_sum[min_FF_sum_index, 0]
+ min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
+ # Compute the FF sum for high gear.
+ high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
+
+ # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
+ # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
+ # - self.K[0, :].sum() * x_avg
+
+ # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
+ # (self.K[0, :].sum() + self.FF[0, :].sum())
+
+ # U = (K + FF) * R - K * X
+ # (K + FF) ^-1 * (U + K * X) = R
+
+ # Scale throttle by min_FF_sum / high_min_FF_sum. This will make low gear
+ # have the same velocity goal as high gear, and so that the robot will hold
+ # the same speed for the same throttle for all gears.
+ adjusted_ff_voltage = numpy.clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
+ return ((adjusted_ff_voltage + self.ttrust * min_K_sum * (self.X[0, 0] + self.X[1, 0]) / 2.0)
+ / (self.ttrust * min_K_sum + min_FF_sum))
+
+ def Update(self, throttle, steering):
+ # Shift into the gear which sends the most power to the floor.
+ # This is the same as sending the most torque down to the floor at the
+ # wheel.
+
+ self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
+ current_gear=self.left_gear,
+ gear_name="left")
+ self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
+ current_gear=self.right_gear,
+ gear_name="right")
+ if self.IsInGear(self.left_gear):
+ self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ # Filter the throttle to provide a nicer response.
+ fvel = self.FilterVelocity(throttle)
+
+ # Constant radius means that angualar_velocity / linear_velocity = constant.
+ # Compute the left and right velocities.
+ left_velocity = fvel - steering * numpy.abs(fvel)
+ right_velocity = fvel + steering * numpy.abs(fvel)
+
+ # Write this constraint in the form of K * R = w
+ # angular velocity / linear velocity = constant
+ # (left - right) / (left + right) = constant
+ # left - right = constant * left + constant * right
+
+ # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
+ # (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
+ # constant
+ # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
+ # (-steering * sign(fvel)) = constant
+ # (-steering * sign(fvel)) * (left + right) = left - right
+ # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
+
+ equality_k = numpy.matrix(
+ [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
+ equality_w = 0.0
+
+ self.R[0, 0] = left_velocity
+ self.R[1, 0] = right_velocity
+
+ # Construct a constraint on R by manipulating the constraint on U
+ # Start out with H * U <= k
+ # U = FF * R + K * (R - X)
+ # H * (FF * R + K * R - K * X) <= k
+ # H * (FF + K) * R <= k + H * K * X
+ R_poly = polytope.HPolytope(
+ self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
+ self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
+
+ # Limit R back inside the box.
+ self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+
+ FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
+ self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
+ else:
+ print 'Not all in gear'
+ if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
+ # TODO(austin): Use battery volts here.
+ R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ self.U_ideal[0, 0] = numpy.clip(
+ self.left_cim.K * (R_left - self.left_cim.X) + R_left / self.left_cim.Kv,
+ self.left_cim.U_min, self.left_cim.U_max)
+ self.left_cim.Update(self.U_ideal[0, 0])
+
+ R_right = self.MotorRPM(self.right_shifter_position, self.X[1, 0])
+ self.U_ideal[1, 0] = numpy.clip(
+ self.right_cim.K * (R_right - self.right_cim.X) + R_right / self.right_cim.Kv,
+ self.right_cim.U_min, self.right_cim.U_max)
+ self.right_cim.Update(self.U_ideal[1, 0])
+ else:
+ assert False
+
+ self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+
+ # TODO(austin): Model the robot as not accelerating when you shift...
+ # This hack only works when you shift at the same time.
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+
+ self.left_gear, self.left_shifter_position = self.SimShifter(
+ self.left_gear, self.left_shifter_position)
+ self.right_gear, self.right_shifter_position = self.SimShifter(
+ self.right_gear, self.right_shifter_position)
+
+ print "U is", self.U[0, 0], self.U[1, 0]
+ print "Left shifter", self.left_gear, self.left_shifter_position, "Right shifter", self.right_gear, self.right_shifter_position
+
+
+def main(argv):
+ vdrivetrain = VelocityDrivetrain()
+
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name"
+ else:
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high],
+ namespaces = ["bot3", "control_loops"])
+
+ if argv[1][-3:] == '.cc':
+ dog_loop_writer.Write(argv[2], argv[1])
+ else:
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ cim_writer = control_loop.ControlLoopWriter(
+ "CIM", [drivetrain.CIM()])
+
+ if argv[3][-3:] == '.cc':
+ cim_writer.Write(argv[4], argv[3])
+ else:
+ cim_writer.Write(argv[3], argv[4])
+ return
+
+ vl_plot = []
+ vr_plot = []
+ ul_plot = []
+ ur_plot = []
+ radius_plot = []
+ t_plot = []
+ left_gear_plot = []
+ right_gear_plot = []
+ vdrivetrain.left_shifter_position = 0.0
+ vdrivetrain.right_shifter_position = 0.0
+ vdrivetrain.left_gear = VelocityDrivetrain.LOW
+ vdrivetrain.right_gear = VelocityDrivetrain.LOW
+
+ print "K is", vdrivetrain.CurrentDrivetrain().K
+
+ if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
+ print "Left is high"
+ else:
+ print "Left is low"
+ if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
+ print "Right is high"
+ else:
+ print "Right is low"
+
+ for t in numpy.arange(0, 4.0, vdrivetrain.dt):
+ if t < 1.0:
+ vdrivetrain.Update(throttle=1.00, steering=0.0)
+ elif t < 1.2:
+ vdrivetrain.Update(throttle=1.00, steering=0.0)
+ else:
+ vdrivetrain.Update(throttle=1.00, steering=0.0)
+ t_plot.append(t)
+ vl_plot.append(vdrivetrain.X[0, 0])
+ vr_plot.append(vdrivetrain.X[1, 0])
+ ul_plot.append(vdrivetrain.U[0, 0])
+ ur_plot.append(vdrivetrain.U[1, 0])
+ left_gear_plot.append((vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+ right_gear_plot.append((vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+
+ fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
+ turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
+ if abs(fwd_velocity) < 0.0000001:
+ radius_plot.append(turn_velocity)
+ else:
+ radius_plot.append(turn_velocity / fwd_velocity)
+
+ cim_velocity_plot = []
+ cim_voltage_plot = []
+ cim_time = []
+ cim = drivetrain.CIM()
+ R = numpy.matrix([[300]])
+ for t in numpy.arange(0, 0.5, cim.dt):
+ U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
+ cim.Update(U)
+ cim_velocity_plot.append(cim.X[0, 0])
+ cim_voltage_plot.append(U[0, 0] * 10)
+ cim_time.append(t)
+ #pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
+ #pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
+ #pylab.legend()
+ #pylab.show()
+
+ # TODO(austin):
+ # Shifting compensation.
+
+ # Tighten the turn.
+ # Closed loop drive.
+
+ pylab.plot(t_plot, vl_plot, label='left velocity')
+ pylab.plot(t_plot, vr_plot, label='right velocity')
+ pylab.plot(t_plot, ul_plot, label='left voltage')
+ pylab.plot(t_plot, ur_plot, label='right voltage')
+ pylab.plot(t_plot, radius_plot, label='radius')
+ pylab.plot(t_plot, left_gear_plot, label='left gear high')
+ pylab.plot(t_plot, right_gear_plot, label='right gear high')
+ pylab.legend()
+ pylab.show()
+ return 0
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/bot3/control_loops/python/polydrivetrain_test.py b/bot3/control_loops/python/polydrivetrain_test.py
new file mode 100755
index 0000000..434cdca
--- /dev/null
+++ b/bot3/control_loops/python/polydrivetrain_test.py
@@ -0,0 +1,82 @@
+#!/usr/bin/python
+
+import polydrivetrain
+import numpy
+from numpy.testing import *
+import polytope
+import unittest
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+class TestVelocityDrivetrain(unittest.TestCase):
+ def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+ H = numpy.matrix([[1, 0],
+ [-1, 0],
+ [0, 1],
+ [0, -1]])
+ K = numpy.matrix([[x1_max],
+ [-x1_min],
+ [x2_max],
+ [-x2_min]])
+ return polytope.HPolytope(H, K)
+
+ def test_coerce_inside(self):
+ """Tests coercion when the point is inside the box."""
+ box = self.MakeBox(1, 2, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
+ numpy.matrix([[1.5], [1.5]])),
+ numpy.matrix([[1.5], [1.5]]))
+
+ def test_coerce_outside_intersect(self):
+ """Tests coercion when the line intersects the box."""
+ box = self.MakeBox(1, 2, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
+
+ def test_coerce_outside_no_intersect(self):
+ """Tests coercion when the line does not intersect the box."""
+ box = self.MakeBox(3, 4, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[3.0], [2.0]]))
+
+ def test_coerce_middle_of_edge(self):
+ """Tests coercion when the line intersects the middle of an edge."""
+ box = self.MakeBox(0, 4, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[-1, 1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
+
+ def test_coerce_perpendicular_line(self):
+ """Tests coercion when the line does not intersect and is in quadrant 2."""
+ box = self.MakeBox(1, 2, 1, 2)
+
+ # x1 = -x2
+ K = numpy.matrix([[1, 1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[1.0], [1.0]]))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/bot3/control_loops/update_drivetrain.sh b/bot3/control_loops/update_drivetrain.sh
new file mode 100755
index 0000000..91ff838
--- /dev/null
+++ b/bot3/control_loops/update_drivetrain.sh
@@ -0,0 +1,9 @@
+#!/bin/bash
+#
+# Updates the drivetrain controllers for the third robot.
+
+cd $(dirname $0)
+
+PYTHONPATH=$PYTHONPATH:../../frc971/control_loops/python \
+./python/drivetrain.py drivetrain/drivetrain_dog_motor_plant.h \
+ drivetrain/drivetrain_dog_motor_plant.cc
diff --git a/bot3/control_loops/update_polydrivetrain.sh b/bot3/control_loops/update_polydrivetrain.sh
new file mode 100755
index 0000000..d8e2d7a
--- /dev/null
+++ b/bot3/control_loops/update_polydrivetrain.sh
@@ -0,0 +1,11 @@
+#!/bin/bash
+#
+# Updates the polydrivetrain controllers and CIM models for the third robot.
+
+cd $(dirname $0)
+
+PYTHONPATH=$PYTHONPATH:../../frc971/control_loops/python \
+./python/polydrivetrain.py drivetrain/polydrivetrain_dog_motor_plant.h \
+ drivetrain/polydrivetrain_dog_motor_plant.cc \
+ drivetrain/polydrivetrain_cim_plant.h \
+ drivetrain/polydrivetrain_cim_plant.cc
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
new file mode 100644
index 0000000..20cb7d3
--- /dev/null
+++ b/bot3/input/input.gyp
@@ -0,0 +1,41 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'joystick_reader',
+ 'type': 'executable',
+ 'sources': [
+ 'joystick_reader.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/prime/input/input.gyp:joystick_input',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:log_interval',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/autonomous/autonomous.gyp:auto_queue',
+ ],
+ },
+ {
+ 'target_name': 'sensor_receiver',
+ 'type': 'executable',
+ 'sources': [
+ 'sensor_receiver.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_constants',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/queues/queues.gyp:queues',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:wrapping_counter',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:sensor_reader',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/controls/controls.gyp:output_check',
+ ],
+ },
+ ],
+}
diff --git a/bot3/input/joystick_reader.cc b/bot3/input/joystick_reader.cc
new file mode 100644
index 0000000..9156509
--- /dev/null
+++ b/bot3/input/joystick_reader.cc
@@ -0,0 +1,613 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/prime/input/joystick_input.h"
+#include "aos/common/input/driver_station_data.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/common/time.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/constants.h"
+#include "frc971/queues/other_sensors.q.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/actions/shoot_action.q.h"
+#include "frc971/actions/action_client.h"
+#include "frc971/actions/catch_action.q.h"
+#include "frc971/actions/shoot_action.h"
+
+using ::frc971::control_loops::drivetrain;
+using ::frc971::sensors::gyro_reading;
+
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::ControlBit;
+
+#define OLD_DS 0
+
+namespace frc971 {
+namespace input {
+namespace joysticks {
+
+const ButtonLocation kDriveControlLoopEnable1(1, 7),
+ kDriveControlLoopEnable2(1, 11);
+const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
+const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kQuickTurn(1, 5);
+
+const ButtonLocation kCatch(3, 10);
+
+#if OLD_DS
+const ButtonLocation kFire(3, 11);
+const ButtonLocation kUnload(1, 4);
+const ButtonLocation kReload(1, 2);
+
+const ButtonLocation kRollersOut(3, 12);
+const ButtonLocation kRollersIn(3, 7);
+
+const ButtonLocation kTuck(3, 9);
+const ButtonLocation kIntakePosition(3, 8);
+const ButtonLocation kIntakeOpenPosition(3, 10);
+const ButtonLocation kVerticalTuck(3, 1);
+const JoystickAxis kFlipRobot(3, 3);
+
+const ButtonLocation kLongShot(3, 5);
+const ButtonLocation kCloseShot(3, 2);
+const ButtonLocation kFenderShot(3, 3);
+const ButtonLocation kTrussShot(2, 11);
+const ButtonLocation kHumanPlayerShot(3, 2);
+#else
+const ButtonLocation kFire(3, 9);
+const ButtonLocation kUnload(1, 4);
+const ButtonLocation kReload(1, 2);
+
+const ButtonLocation kRollersOut(3, 8);
+const ButtonLocation kRollersIn(3, 3);
+
+const ButtonLocation kTuck(3, 4);
+const ButtonLocation kIntakePosition(3, 5);
+const ButtonLocation kIntakeOpenPosition(3, 11);
+const ButtonLocation kVerticalTuck(2, 6);
+const JoystickAxis kFlipRobot(3, 3);
+
+const ButtonLocation kLongShot(3, 7);
+const ButtonLocation kCloseShot(3, 6);
+const ButtonLocation kFenderShot(3, 2);
+const ButtonLocation kTrussShot(2, 11);
+const ButtonLocation kHumanPlayerShot(3, 1);
+#endif
+
+const ButtonLocation kUserLeft(2, 7);
+const ButtonLocation kUserRight(2, 10);
+
+const JoystickAxis kAdjustClawGoal(3, 2);
+const JoystickAxis kAdjustClawSeparation(3, 1);
+
+struct ClawGoal {
+ double angle;
+ double separation;
+};
+
+struct ShotGoal {
+ ClawGoal claw;
+ double shot_power;
+ double velocity_compensation;
+ double intake_power;
+};
+
+const double kIntakePower = 4.0;
+// In case we have to quickly adjust it.
+const double kGrabSeparation = 0;
+const double kShootSeparation = 0.11 + kGrabSeparation;
+
+const ClawGoal kTuckGoal = {-2.273474, -0.749484};
+const ClawGoal kVerticalTuckGoal = {0, kGrabSeparation};
+const ClawGoal kIntakeGoal = {-2.24, kGrabSeparation};
+const ClawGoal kIntakeOpenGoal = {-2.0, 1.1};
+
+// TODO(austin): Tune these by hand...
+const ClawGoal kFlippedTuckGoal = {2.733474, -0.75};
+const ClawGoal kFlippedIntakeGoal = {2.0, kGrabSeparation};
+const ClawGoal kFlippedIntakeOpenGoal = {0.95, 1.0};
+
+// 34" between near edge of colored line and rear edge of bumper.
+// Only works running?
+const ShotGoal kLongShotGoal = {
+ {-1.08, kShootSeparation}, 145, 0.04, kIntakePower};
+// old 34" {-1.06, kShootSeparation}, 140, 0.04, kIntakePower};
+const ShotGoal kFlippedLongShotGoal = {
+ {0.96, kShootSeparation}, 145, 0.09, kIntakePower};
+// old 34" {0.96, kShootSeparation}, 140, 0.09, kIntakePower};
+
+// 78" between near edge of colored line and rear edge of bumper.
+const ShotGoal kCloseShotGoal = {
+ {-0.95, kShootSeparation}, 105, 0.2, kIntakePower};
+// 3/4" plunger {-0.90, kShootSeparation}, 105, 0.2, kIntakePower};
+const ShotGoal kFlippedMediumShotGoal = {
+ {0.865, kShootSeparation}, 120, 0.2, kIntakePower};
+// 3/4" plunger {0.80, kShootSeparation}, 105, 0.2, kIntakePower};
+
+// Shot from the fender.
+const ShotGoal kFenderShotGoal = {
+ {-0.68, kShootSeparation}, 115.0, 0.0, kIntakePower};
+const ShotGoal kFlippedShortShotGoal = {
+ {0.63, kShootSeparation}, 115.0, 0.0, kIntakePower};
+
+const ShotGoal kHumanShotGoal = {
+ {-0.90, kShootSeparation}, 140, 0.04, kIntakePower};
+const ShotGoal kFlippedHumanShotGoal = {
+ {0.90, kShootSeparation}, 140, 0, kIntakePower};
+const ShotGoal kTrussShotGoal = {
+ {-0.68, kShootSeparation}, 88.0, 0.4, kIntakePower};
+const ShotGoal kFlippedTrussShotGoal = {
+ {0.68, kShootSeparation}, 92.0, 0.4, kIntakePower};
+
+const ShotGoal kFlippedDemoShotGoal = {
+ {1.0, kShootSeparation}, 65.0, 0.0, kIntakePower};
+const ShotGoal kDemoShotGoal = {
+ {-1.0, kShootSeparation}, 50.0, 0.0, kIntakePower};
+
+const ClawGoal k254PassGoal = {-1.95, kGrabSeparation};
+const ClawGoal kFlipped254PassGoal = {1.96, kGrabSeparation};
+
+// Makes a new ShootAction action.
+::std::unique_ptr<TypedAction< ::frc971::actions::CatchActionGroup>>
+MakeCatchAction() {
+ return ::std::unique_ptr<TypedAction< ::frc971::actions::CatchActionGroup>>(
+ new TypedAction< ::frc971::actions::CatchActionGroup>(
+ &::frc971::actions::catch_action));
+}
+
+// A queue which queues Actions and cancels them.
+class ActionQueue {
+ public:
+ // Queues up an action for sending.
+ void QueueAction(::std::unique_ptr<Action> action) {
+ if (current_action_) {
+ LOG(INFO, "Queueing action, canceling prior\n");
+ current_action_->Cancel();
+ next_action_ = ::std::move(action);
+ } else {
+ LOG(INFO, "Queueing action\n");
+ current_action_ = ::std::move(action);
+ current_action_->Start();
+ }
+ }
+
+ // Cancels the current action, and runs the next one when the current one has
+ // finished.
+ void CancelCurrentAction() {
+ LOG(INFO, "Canceling current action\n");
+ if (current_action_) {
+ current_action_->Cancel();
+ }
+ }
+
+ // Cancels all running actions.
+ void CancelAllActions() {
+ LOG(DEBUG, "Cancelling all actions\n");
+ if (current_action_) {
+ current_action_->Cancel();
+ }
+ next_action_.reset();
+ }
+
+ // Runs the next action when the current one is finished running.
+ void Tick() {
+ if (current_action_) {
+ if (!current_action_->Running()) {
+ LOG(INFO, "Action is done.\n");
+ current_action_ = ::std::move(next_action_);
+ if (current_action_) {
+ LOG(INFO, "Running next action\n");
+ current_action_->Start();
+ }
+ }
+ }
+ }
+
+ // Returns true if any action is running or could be running.
+ // For a one cycle faster response, call Tick before running this.
+ bool Running() { return static_cast<bool>(current_action_); }
+
+ private:
+ ::std::unique_ptr<Action> current_action_;
+ ::std::unique_ptr<Action> next_action_;
+};
+
+
+class Reader : public ::aos::input::JoystickInput {
+ public:
+ Reader()
+ : is_high_gear_(false),
+ shot_power_(80.0),
+ goal_angle_(0.0),
+ separation_angle_(kGrabSeparation),
+ velocity_compensation_(0.0),
+ intake_power_(0.0),
+ was_running_(false) {}
+
+ virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
+ if (data.GetControlBit(ControlBit::kAutonomous)) {
+ if (data.PosEdge(ControlBit::kEnabled)){
+ LOG(INFO, "Starting auto mode\n");
+ ::frc971::autonomous::autonomous.MakeWithBuilder()
+ .run_auto(true)
+ .Send();
+ } else if (data.NegEdge(ControlBit::kEnabled)) {
+ LOG(INFO, "Stopping auto mode\n");
+ ::frc971::autonomous::autonomous.MakeWithBuilder()
+ .run_auto(false)
+ .Send();
+ } else if (!data.GetControlBit(ControlBit::kEnabled)) {
+ {
+ auto goal = drivetrain.goal.MakeMessage();
+ goal->Zero();
+ goal->control_loop_driving = false;
+ goal->left_goal = goal->right_goal = 0;
+ goal->left_velocity_goal = goal->right_velocity_goal = 0;
+ if (!goal.Send()) {
+ LOG(WARNING, "sending 0 drivetrain goal failed\n");
+ }
+ }
+ {
+ // TODO(brians): Make sure this doesn't make it unbrake and not move
+ // or something weird.
+ auto goal = control_loops::shooter_queue_group.goal.MakeMessage();
+ goal->Zero();
+ if (!goal.Send()) {
+ LOG(WARNING, "sending 0 shooter goal failed\n");
+ }
+ }
+ }
+ } else {
+ HandleTeleop(data);
+ }
+ }
+
+ void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
+ bool is_control_loop_driving = false;
+ double left_goal = 0.0;
+ double right_goal = 0.0;
+ const double wheel = -data.GetAxis(kSteeringWheel);
+ const double throttle = -data.GetAxis(kDriveThrottle);
+ const double kThrottleGain = 1.0 / 2.5;
+ if (false && (data.IsPressed(kDriveControlLoopEnable1) ||
+ data.IsPressed(kDriveControlLoopEnable2))) {
+ // TODO(austin): Static sucks!
+ static double distance = 0.0;
+ static double angle = 0.0;
+ static double filtered_goal_distance = 0.0;
+ if (data.PosEdge(kDriveControlLoopEnable1) ||
+ data.PosEdge(kDriveControlLoopEnable2)) {
+ if (drivetrain.position.FetchLatest() && gyro_reading.FetchLatest()) {
+ distance = (drivetrain.position->left_encoder +
+ drivetrain.position->right_encoder) /
+ 2.0 -
+ throttle * kThrottleGain / 2.0;
+ angle = gyro_reading->angle;
+ filtered_goal_distance = distance;
+ }
+ }
+ is_control_loop_driving = true;
+
+ // const double gyro_angle = Gyro.View().angle;
+ const double goal_theta = angle - wheel * 0.27;
+ const double goal_distance = distance + throttle * kThrottleGain;
+ const double robot_width = 22.0 / 100.0 * 2.54;
+ const double kMaxVelocity = 0.6;
+ if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance += kMaxVelocity * 0.02;
+ } else if (goal_distance <
+ -kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance -= kMaxVelocity * 0.02;
+ } else {
+ filtered_goal_distance = goal_distance;
+ }
+ left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
+ right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
+ is_high_gear_ = false;
+
+ LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
+ }
+ if (!drivetrain.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .highgear(is_high_gear_)
+ .quickturn(data.IsPressed(kQuickTurn))
+ .control_loop_driving(is_control_loop_driving)
+ .left_goal(left_goal)
+ .right_goal(right_goal)
+ .left_velocity_goal(0)
+ .right_velocity_goal(0)
+ .Send()) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+ if (data.PosEdge(kShiftHigh)) {
+ is_high_gear_ = false;
+ }
+ if (data.PosEdge(kShiftLow)) {
+ is_high_gear_ = true;
+ }
+ }
+
+ void SetGoal(ClawGoal goal) {
+ goal_angle_ = goal.angle;
+ separation_angle_ = goal.separation;
+ moving_for_shot_ = false;
+ velocity_compensation_ = 0.0;
+ intake_power_ = 0.0;
+ }
+
+ void SetGoal(ShotGoal goal) {
+ goal_angle_ = goal.claw.angle;
+ shot_separation_angle_ = goal.claw.separation;
+ separation_angle_ = kGrabSeparation;
+ moving_for_shot_ = true;
+ shot_power_ = goal.shot_power;
+ velocity_compensation_ = goal.velocity_compensation;
+ intake_power_ = goal.intake_power;
+ }
+
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ HandleDrivetrain(data);
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+ if (data.IsPressed(kRollersIn) || data.IsPressed(kRollersOut)) {
+ intake_power_ = 0.0;
+ separation_angle_ = kGrabSeparation;
+ moving_for_shot_ = false;
+ }
+
+ static const double kAdjustClawGoalDeadband = 0.08;
+ double claw_goal_adjust = data.GetAxis(kAdjustClawGoal);
+ if (OLD_DS || ::std::abs(claw_goal_adjust) < kAdjustClawGoalDeadband) {
+ claw_goal_adjust = 0;
+ } else {
+ claw_goal_adjust = (claw_goal_adjust -
+ ((claw_goal_adjust < 0) ? -kAdjustClawGoalDeadband
+ : kAdjustClawGoalDeadband)) *
+ 0.035;
+ }
+ double claw_separation_adjust = data.GetAxis(kAdjustClawSeparation);
+ if (OLD_DS ||
+ ::std::abs(claw_separation_adjust) < kAdjustClawGoalDeadband) {
+ claw_separation_adjust = 0;
+ } else {
+ claw_separation_adjust =
+ (claw_separation_adjust -
+ ((claw_separation_adjust < 0) ? -kAdjustClawGoalDeadband
+ : kAdjustClawGoalDeadband)) *
+ -0.035;
+ }
+
+#if OLD_DS
+ if (data.IsPressed(kFenderShot)) {
+#else
+ if (data.GetAxis(kFlipRobot) > 0.9) {
+#endif
+ claw_goal_adjust += claw_separation_adjust;
+ claw_goal_adjust *= -1;
+
+ if (data.IsPressed(kIntakeOpenPosition)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedIntakeOpenGoal);
+ } else if (data.IsPressed(kIntakePosition)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedIntakeGoal);
+ } else if (data.IsPressed(kVerticalTuck)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kVerticalTuckGoal);
+ } else if (data.IsPressed(kTuck)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedTuckGoal);
+ } else if (data.PosEdge(kLongShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedLongShotGoal);
+ } else if (data.PosEdge(kCloseShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedMediumShotGoal);
+ } else if (data.PosEdge(kFenderShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedShortShotGoal);
+ } else if (data.PosEdge(kHumanPlayerShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedHumanShotGoal);
+ } else if (data.PosEdge(kUserLeft)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlipped254PassGoal);
+ } else if (data.PosEdge(kUserRight)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedDemoShotGoal);
+ } else if (data.PosEdge(kTrussShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedTrussShotGoal);
+ }
+ } else {
+ if (data.IsPressed(kIntakeOpenPosition)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kIntakeOpenGoal);
+ } else if (data.IsPressed(kIntakePosition)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kIntakeGoal);
+ } else if (data.IsPressed(kVerticalTuck)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kVerticalTuckGoal);
+ } else if (data.IsPressed(kTuck)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kTuckGoal);
+ } else if (data.PosEdge(kLongShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kLongShotGoal);
+ } else if (data.PosEdge(kCloseShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kCloseShotGoal);
+ } else if (data.PosEdge(kFenderShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFenderShotGoal);
+ } else if (data.PosEdge(kHumanPlayerShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kHumanShotGoal);
+ } else if (data.PosEdge(kUserLeft)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(k254PassGoal);
+ } else if (data.PosEdge(kUserRight)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kDemoShotGoal);
+ } else if (data.PosEdge(kTrussShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kTrussShotGoal);
+ }
+ }
+
+ /*
+ if (data.PosEdge(kCatch)) {
+ auto catch_action = MakeCatchAction();
+ catch_action->GetGoal()->catch_angle = goal_angle_;
+ action_queue_.QueueAction(::std::move(catch_action));
+ }
+ */
+
+ if (data.PosEdge(kFire)) {
+ action_queue_.QueueAction(actions::MakeShootAction());
+ } else if (data.NegEdge(kFire)) {
+ action_queue_.CancelCurrentAction();
+ }
+
+ action_queue_.Tick();
+ if (data.IsPressed(kUnload) || data.IsPressed(kReload)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ intake_power_ = 0.0;
+ velocity_compensation_ = 0.0;
+ }
+
+ // Send out the claw and shooter goals if no actions are running.
+ if (!action_queue_.Running()) {
+ goal_angle_ += claw_goal_adjust;
+ separation_angle_ += claw_separation_adjust;
+
+ // If the action just ended, turn the intake off and stop velocity
+ // compensating.
+ if (was_running_) {
+ intake_power_ = 0.0;
+ velocity_compensation_ = 0.0;
+ }
+
+ control_loops::drivetrain.status.FetchLatest();
+ double goal_angle = goal_angle_;
+ if (control_loops::drivetrain.status.get()) {
+ goal_angle +=
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed);
+ } else {
+ LOG_INTERVAL(no_drivetrain_status_);
+ }
+
+ if (moving_for_shot_) {
+ auto &claw_status = control_loops::claw_queue_group.status;
+ claw_status.FetchLatest();
+ if (claw_status.get()) {
+ if (::std::abs(claw_status->bottom - goal_angle) < 0.2) {
+ moving_for_shot_ = false;
+ separation_angle_ = shot_separation_angle_;
+ }
+ }
+ }
+
+ double separation_angle = separation_angle_;
+
+ if (data.IsPressed(kCatch)) {
+ const double kCatchSeparation = 1.0;
+ goal_angle -= kCatchSeparation / 2.0;
+ separation_angle = kCatchSeparation;
+ }
+
+ bool intaking =
+ data.IsPressed(kRollersIn) || data.IsPressed(kIntakePosition) ||
+ data.IsPressed(kIntakeOpenPosition) || data.IsPressed(kCatch);
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(goal_angle)
+ .separation_angle(separation_angle)
+ .intake(intaking ? 12.0
+ : (data.IsPressed(kRollersOut) ? -12.0
+ : intake_power_))
+ .centering(intaking ? 12.0 : 0.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(shot_power_)
+ .shot_requested(data.IsPressed(kFire))
+ .unload_requested(data.IsPressed(kUnload))
+ .load_requested(data.IsPressed(kReload))
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ }
+ }
+ was_running_ = action_queue_.Running();
+ }
+
+ double SpeedToAngleOffset(double speed) {
+ const frc971::constants::Values &values = frc971::constants::GetValues();
+ // scale speed to a [0.0-1.0] on something close to the max
+ // TODO(austin): Change the scale factor for different shots.
+ return (speed / values.drivetrain_max_speed) * velocity_compensation_;
+ }
+
+ private:
+ bool is_high_gear_;
+ double shot_power_;
+ double goal_angle_;
+ double separation_angle_, shot_separation_angle_;
+ double velocity_compensation_;
+ double intake_power_;
+ bool was_running_;
+ bool moving_for_shot_ = false;
+
+ ActionQueue action_queue_;
+
+ ::aos::util::SimpleLogInterval no_drivetrain_status_ =
+ ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
+ "no drivetrain status");
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace frc971
+
+int main() {
+ ::aos::Init();
+ ::frc971::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}
diff --git a/bot3/input/sensor_receiver.cc b/bot3/input/sensor_receiver.cc
new file mode 100644
index 0000000..6aec89d
--- /dev/null
+++ b/bot3/input/sensor_receiver.cc
@@ -0,0 +1,160 @@
+#include <inttypes.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/wrapping_counter.h"
+#include "aos/common/time.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/controls/output_check.q.h"
+
+#include "bbb/sensor_reader.h"
+
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
+#include "bot3/queues/to_log.q.h"
+#include "frc971/queues/other_sensors.q.h"
+#include "frc971/shifter_hall_effect.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+using ::bot3::control_loops::drivetrain;
+using ::frc971::sensors::gyro_reading;
+using ::aos::util::WrappingCounter;
+
+namespace bot3 {
+namespace {
+
+// TODO (danielp): Find out the real ratios here.
+double drivetrain_translate(int32_t in) {
+ return static_cast<double>(in)
+ / (256.0 /*cpr*/ * 4.0 /*quad*/)
+ * (18.0 / 50.0 /*output stage*/) * (56.0 / 30.0 /*encoder gears*/)
+ // * constants::GetValues().drivetrain_encoder_ratio
+ * (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+}
+
+static const double kVcc = 5.15;
+
+// Translates values from the ADC into voltage.
+double adc_translate(uint16_t in) {
+ if (false) {
+ // This is the simple theoretical math.
+ static const uint16_t kMaximumValue = 0x3FF;
+ static const double kR1 = 5, kR2 = 6.65;
+ const double raw =
+ (kVcc * static_cast<double>(in) / static_cast<double>(kMaximumValue));
+ return (raw * (kR1 + kR2) - (kVcc / 2) * kR2) / kR1;
+ } else {
+ // This is from a linear regression calculated with some actual data points.
+ static const double kM = 0.012133, kB = -3.6813;
+ return static_cast<double>(in) * kM + kB;
+ }
+}
+
+double battery_translate(uint16_t in_high, uint16_t in_low) {
+ const double high = adc_translate(in_high), low = adc_translate(in_low);
+ static const double kDividerBig = 5.55, kDividerSmall = 2.66;
+ return (high - low) * (kDividerBig + kDividerSmall) / kDividerSmall +
+ kDividerBig / kDividerSmall * kVcc;
+}
+
+double gyro_translate(int64_t in) {
+ return in / 16.0 / 1000.0 / (180.0 / M_PI);
+}
+
+double hall_translate(const ::frc971::constants::ShifterHallEffect &k, uint16_t in_low,
+ uint16_t in_high) {
+ const double low_ratio =
+ 0.5 * (in_low - static_cast<double>(k.low_gear_low)) /
+ static_cast<double>(k.low_gear_middle - k.low_gear_low);
+ const double high_ratio =
+ 0.5 + 0.5 * (in_high - static_cast<double>(k.high_gear_middle)) /
+ static_cast<double>(k.high_gear_high - k.high_gear_middle);
+
+ // Return low when we are below 1/2, and high when we are above 1/2.
+ if (low_ratio + high_ratio < 1.0) {
+ return low_ratio;
+ } else {
+ return high_ratio;
+ }
+}
+
+void PacketReceived(const ::bbb::DataStruct *data,
+ const ::aos::time::Time &cape_timestamp) {
+ ::aos::time::TimeFreezer time_freezer;
+
+ ::frc971::logging_structs::CapeReading reading_to_log(
+ cape_timestamp, static_cast<uint16_t>(sizeof(*data)),
+ data->main.low_left_drive_hall, data->main.high_left_drive_hall,
+ data->main.low_right_drive_hall, data->main.high_right_drive_hall);
+ LOG_STRUCT(DEBUG, "cape reading", reading_to_log);
+ bool bad_gyro;
+ // TODO(brians): Switch to LogInterval for these things.
+ if (data->uninitialized_gyro) {
+ LOG(DEBUG, "uninitialized gyro\n");
+ bad_gyro = true;
+ } else if (data->zeroing_gyro) {
+ LOG(DEBUG, "zeroing gyro\n");
+ bad_gyro = true;
+ } else if (data->bad_gyro) {
+ LOG(ERROR, "bad gyro\n");
+ bad_gyro = true;
+ } else if (data->old_gyro_reading) {
+ LOG(WARNING, "old/bad gyro reading\n");
+ bad_gyro = true;
+ } else {
+ bad_gyro = false;
+ }
+
+ if (!bad_gyro) {
+ gyro_reading.MakeWithBuilder()
+ .angle(gyro_translate(data->gyro_angle))
+ .Send();
+ }
+
+ if (data->analog_errors != 0) {
+ LOG(WARNING, "%" PRIu8 " analog errors\n", data->analog_errors);
+ }
+
+ if (data->main.output_check_pulse_length != 0) {
+ auto message = ::aos::controls::output_check_received.MakeMessage();
+ // TODO(brians): Fix this math to match what the cRIO actually does.
+ // It's close but not quite right.
+ message->pulse_length =
+ static_cast<double>(data->main.output_check_pulse_length) / 10000.0;
+ if (message->pulse_length > 2.7) {
+ LOG(WARNING, "insane PWM pulse length %fms\n", message->pulse_length);
+ } else {
+ message->pwm_value = (message->pulse_length - 0.5) / 2.0 * 255.0 + 0.5;
+ LOG_STRUCT(DEBUG, "received", *message);
+ message.Send();
+ }
+ }
+
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(drivetrain_translate(data->main.right_drive))
+ .left_encoder(-drivetrain_translate(data->main.left_drive))
+ .left_shifter_position(hall_translate(control_loops::kBot3LeftDriveShifter,
+ data->main.low_left_drive_hall,
+ data->main.high_left_drive_hall))
+ .right_shifter_position(hall_translate(control_loops::kBot3RightDriveShifter,
+ data->main.low_right_drive_hall,
+ data->main.high_right_drive_hall))
+ .battery_voltage(battery_translate(data->main.battery_voltage_high,
+ data->main.battery_voltage_low))
+ .Send();
+}
+
+} // namespace
+} // namespace bot3
+
+int main() {
+ ::aos::Init(::bbb::SensorReader::kRelativePriority);
+ ::bbb::SensorReader reader("comp");
+ while (true) {
+ ::bot3::PacketReceived(reader.ReadPacket(), reader.GetCapeTimestamp());
+ }
+ ::aos::Cleanup();
+}
diff --git a/bot3/output/motor_writer.cc b/bot3/output/motor_writer.cc
new file mode 100644
index 0000000..c390a9e
--- /dev/null
+++ b/bot3/output/motor_writer.cc
@@ -0,0 +1,126 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "aos/prime/output/motor_output.h"
+#include "aos/common/logging/logging.h"
+#include "aos/linux_code/init.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/common/time.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/controls/output_check.q.h"
+
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/queues/rollers.q.h"
+
+using ::aos::util::SimpleLogInterval;
+
+namespace bot3 {
+namespace output {
+
+class MotorWriter : public ::aos::MotorOutput {
+ // Maximum age of an output packet before the motors get zeroed instead.
+ static const int kOutputMaxAgeMS = 20;
+ static constexpr ::aos::time::Time kOldLogInterval =
+ ::aos::time::Time::InSeconds(0.5);
+
+ double Cap(double value, double max) {
+ if (value > max) return max;
+ if (value < -max) return -max;
+ return value;
+ }
+
+ virtual void RunIteration() {
+ values_.digital_module = 0;
+ values_.pressure_switch_channel = 1;
+ values_.compressor_channel = 1;
+ values_.solenoid_module = 0;
+
+ if (true) {
+ static auto &drivetrain = ::bot3::control_loops::drivetrain.output;
+ drivetrain.FetchLatest();
+ if (drivetrain.IsNewerThanMS(kOutputMaxAgeMS)) {
+ LOG_STRUCT(DEBUG, "will output", *drivetrain);
+ SetPWMOutput(3, drivetrain->right_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(6, -drivetrain->left_voltage / 12.0, kTalonBounds);
+ SetSolenoid(7, drivetrain->left_high);
+ SetSolenoid(8, drivetrain->right_high);
+ } else {
+ DisablePWMOutput(3);
+ DisablePWMOutput(8);
+ LOG_INTERVAL(drivetrain_old_);
+ }
+ drivetrain_old_.Print();
+ }
+
+ {
+ // This isn't actually a queue group...
+ static auto &rollers = ::bot3::rollers;
+ rollers.FetchLatest();
+ if (rollers.IsNewerThanMS(kOutputMaxAgeMS)) {
+ LOG_STRUCT(DEBUG, "will output", *rollers);
+ // TODO (danielp): Figure out what the actual PWM outputs will be here.
+ // There are two motors for each of these.
+ SetPWMOutput(1, rollers->front_intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(5, -rollers->front_intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(2, rollers->back_intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(7, -rollers->back_intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(4, rollers->low_goal_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(8, -rollers->low_goal_voltage / 12.0, kTalonBounds);
+
+ // We have two pistons for each intake.
+ SetSolenoid(5, rollers->front_extended);
+ SetSolenoid(6, rollers->front_extended);
+ SetSolenoid(3, rollers->back_extended);
+ SetSolenoid(4, rollers->back_extended);
+ } else {
+ DisablePWMOutput(1);
+ DisablePWMOutput(5);
+ DisablePWMOutput(2);
+ DisablePWMOutput(7);
+ DisablePWMOutput(4);
+ DisablePWMOutput(8);
+
+ // Retract intakes.
+ SetSolenoid(5, false);
+ SetSolenoid(6, false);
+ SetSolenoid(3, false);
+ SetSolenoid(4, false);
+
+ LOG_INTERVAL(rollers_old_);
+ }
+ rollers_old_.Print();
+ }
+
+ {
+ auto message = ::aos::controls::output_check_sent.MakeMessage();
+ ++output_check_;
+ if (output_check_ == 0) output_check_ = 1;
+ SetRawPWMOutput(10, output_check_);
+ message->pwm_value = output_check_;
+ message->pulse_length =
+ static_cast<double>(message->pwm_value) / 255.0 * 2.0 + 0.5;
+ LOG_STRUCT(DEBUG, "sending", *message);
+ message.Send();
+ }
+ }
+
+ SimpleLogInterval drivetrain_old_ =
+ SimpleLogInterval(kOldLogInterval, WARNING, "drivetrain too old");
+ SimpleLogInterval rollers_old_ =
+ SimpleLogInterval(kOldLogInterval, WARNING, "rollers too old");
+
+ uint8_t output_check_ = 0;
+};
+
+constexpr ::aos::time::Time MotorWriter::kOldLogInterval;
+
+} // namespace output
+} // namespace bot3
+
+int main() {
+ ::aos::Init();
+ ::bot3::output::MotorWriter writer;
+ writer.Run();
+ ::aos::Cleanup();
+}
diff --git a/bot3/output/output.gyp b/bot3/output/output.gyp
new file mode 100644
index 0000000..e3243af
--- /dev/null
+++ b/bot3/output/output.gyp
@@ -0,0 +1,23 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'motor_writer',
+ 'type': 'executable',
+ 'sources': [
+ 'motor_writer.cc'
+ ],
+ 'dependencies': [
+ '<(AOS)/prime/output/output.gyp:motor_output',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/bot3/queues/queues.gyp:queues',
+ '<(AOS)/common/util/util.gyp:log_interval',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/controls/controls.gyp:output_check',
+ ],
+ },
+ ],
+}
diff --git a/bot3/prime/build.sh b/bot3/prime/build.sh
new file mode 100755
index 0000000..6ae1c81
--- /dev/null
+++ b/bot3/prime/build.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+cd $(dirname $0)
+
+exec ../../aos/build/build.py $0 bot3_prime prime.gyp "$@"
diff --git a/bot3/prime/prime.gyp b/bot3/prime/prime.gyp
new file mode 100644
index 0000000..5e5a00b
--- /dev/null
+++ b/bot3/prime/prime.gyp
@@ -0,0 +1,50 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'All',
+ 'type': 'none',
+ 'dependencies': [
+ '<(AOS)/build/aos_all.gyp:Prime',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:all_tests',
+
+ '../control_loops/drivetrain/drivetrain.gyp:drivetrain',
+ '../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
+ '../autonomous/autonomous.gyp:auto',
+ #'../input/input.gyp:joystick_reader',
+ '../output/output.gyp:motor_writer',
+ '../input/input.gyp:sensor_receiver',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:uart_reader_main',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:test_sensor_receiver',
+ '<(DEPTH)/bbb_cape/src/flasher/flasher.gyp:stm32_flasher',
+ '<(AOS)/prime/input/input.gyp:joystick_proxy',
+ ],
+ 'variables': {
+ 'cape_src': '<(DEPTH)/bbb_cape/src/cape',
+ 'cape_hex': '<(cape_src)/.obj/main_comp.hex',
+ },
+ 'actions': [
+ {
+ 'action_name': 'make_cape',
+ 'inputs': [
+ '<!@(find <(cape_src) -name ".*" -prune -o -type f -print)',
+ '<(cape_src)/Makefile',
+ ],
+ 'outputs': [
+ '<(cape_hex)',
+ ],
+ 'action': ['make', '-C', '<(cape_src)'],
+ 'message': 'Building cape code',
+ },
+ ],
+ 'copies': [
+ {
+ 'destination': '<(rsync_dir)',
+ 'files': [
+ '<(cape_hex)',
+ 'start_list.txt',
+ ],
+ },
+ ],
+ },
+ ],
+}
diff --git a/bot3/prime/start_list.txt b/bot3/prime/start_list.txt
new file mode 100644
index 0000000..fcaf76d
--- /dev/null
+++ b/bot3/prime/start_list.txt
@@ -0,0 +1,7 @@
+binary_log_writer
+motor_writer
+joystick_reader
+drivetrain
+auto
+sensor_receiver
+joystick_proxy
diff --git a/bot3/queues/queues.gyp b/bot3/queues/queues.gyp
new file mode 100644
index 0000000..65ffa45
--- /dev/null
+++ b/bot3/queues/queues.gyp
@@ -0,0 +1,19 @@
+{
+ 'variables': {
+ 'queue_files': [
+ 'rollers.q',
+ 'to_log.q',
+ ]
+ },
+ 'targets': [
+ {
+ 'target_name': 'queues',
+ 'type': 'static_library',
+ 'sources': ['<@(queue_files)'],
+ 'variables': {
+ 'header_path': 'bot3/queues',
+ },
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ ],
+}
diff --git a/bot3/queues/rollers.q b/bot3/queues/rollers.q
new file mode 100644
index 0000000..1b706d0
--- /dev/null
+++ b/bot3/queues/rollers.q
@@ -0,0 +1,15 @@
+package bot3;
+
+message Rollers {
+ // Positive voltage = intaking, Negative = spitting.
+ double front_intake_voltage;
+ double back_intake_voltage;
+ // Voltage for the low goal rollers.
+ // Positive voltage = ball towards back, Negative = ball towards front.
+ double low_goal_voltage;
+
+ // Whether the front and back intake pistons are extended.
+ bool front_extended;
+ bool back_extended;
+};
+queue Rollers rollers;
diff --git a/bot3/queues/to_log.q b/bot3/queues/to_log.q
new file mode 100644
index 0000000..aa28082
--- /dev/null
+++ b/bot3/queues/to_log.q
@@ -0,0 +1,11 @@
+package frc971.logging_structs;
+
+struct CapeReading {
+ Time time;
+ uint16_t struct_size;
+
+ uint16_t left_low;
+ uint16_t left_high;
+ uint16_t right_low;
+ uint16_t right_high;
+};
diff --git a/frc971/autonomous/auto_main.cc b/frc971/autonomous/auto_main.cc
index 3126a22..9a70682 100644
--- a/frc971/autonomous/auto_main.cc
+++ b/frc971/autonomous/auto_main.cc
@@ -1,4 +1,4 @@
-#include "stdio.h"
+#include <stdio.h>
#include "aos/common/time.h"
#include "aos/linux_code/init.h"
@@ -27,7 +27,6 @@
::aos::time::Time start_time = ::aos::time::Time::Now();
::frc971::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());
diff --git a/frc971/constants.h b/frc971/constants.h
index 173efad..2dcd150 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -4,6 +4,7 @@
#include <stdint.h>
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/shifter_hall_effect.h"
namespace frc971 {
namespace constants {
@@ -11,17 +12,6 @@
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
-// Contains the voltages for an analog hall effect sensor on a shifter.
-struct ShifterHallEffect {
- // The numbers to use for scaling raw voltages to 0-1.
- // Low is near 0.0, high is near 1.0
- double low_gear_middle, low_gear_low;
- double high_gear_high, high_gear_middle;
-
- // The numbers for when the dog is clear of each gear.
- double clear_low, clear_high;
-};
-
// This structure contains current values for all of the things that change.
struct Values {
// This is useful for representing the 2 sides of a hall effect sensor etc.
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 64d6264..e85351e 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -18,6 +18,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/other_sensors.q.h"
#include "frc971/constants.h"
+#include "frc971/shifter_hall_effect.h"
using frc971::sensors::gyro_reading;
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 8e82be3..61accc0 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -15,6 +15,7 @@
#include "frc971/queues/to_log.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/shifter_hall_effect.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
diff --git a/frc971/output/wpilib_interface.cc b/frc971/output/wpilib_interface.cc
index 0dc3170..6c45324 100644
--- a/frc971/output/wpilib_interface.cc
+++ b/frc971/output/wpilib_interface.cc
@@ -24,6 +24,7 @@
#include "frc971/constants.h"
#include "frc971/queues/other_sensors.q.h"
#include "frc971/queues/to_log.q.h"
+#include "frc971/shifter_hall_effect.h"
#include <WPILib.h>
diff --git a/frc971/shifter_hall_effect.h b/frc971/shifter_hall_effect.h
new file mode 100644
index 0000000..aaa5ebd
--- /dev/null
+++ b/frc971/shifter_hall_effect.h
@@ -0,0 +1,21 @@
+#ifndef FRC971_SHIFTER_HALL_EFFECT_H_
+#define FRC971_SHIFTER_HALL_EFFECT_H_
+
+namespace frc971 {
+namespace constants {
+
+// Contains the voltages for an analog hall effect sensor on a shifter.
+struct ShifterHallEffect {
+ // The numbers to use for scaling raw voltages to 0-1.
+ // Low is near 0.0, high is near 1.0
+ double low_gear_middle, low_gear_low;
+ double high_gear_high, high_gear_middle;
+
+ // The numbers for when the dog is clear of each gear.
+ double clear_low, clear_high;
+};
+
+} // constants
+} // frc971
+
+#endif
diff --git a/output/.gitignore b/output/.gitignore
index 62a7a9b..b617a67 100644
--- a/output/.gitignore
+++ b/output/.gitignore
@@ -12,3 +12,5 @@
/arm-[^-]*-debug-[^-]*/
/amd64-[^-]*-[^-]*/
/amd64-[^-]*-debug-[^-]*/
+/bot3-[^-]*-[^-]*-[^-]*/
+/bot3-[^-]*-[^-]*-debug-[^-]*/