Reorganize directory structure for third robot.
I added a "bot3" directory, so that we can have all the third
robot specific stuff live there.
Most of these changes were just copying files from frc971. Many
of the files have no modifications except changed paths in the
include directives, changed paths in gyp files, and changed
namespace names. Some of the code is new, however, for instance,
I modified motor_writer to control the motors and pistons on
the third robot, as well as added a new "rollers" queue to
transfer data pertaining to these.
I also updated the build system so that builds for the normal
robot code and the third robot code would in no way conflict
with each other. Finally, I figured out how I want to handle
the constants, although I haven't put actual values for
that in yet.
I cleaned up the python code, adding in the small changes to
that which Comran wanted.
Change-Id: I04e17dc8b17a980338b718a78e348ea647ec060b
diff --git a/bot3/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