Pulled polydrivetrain out into a separate file.
Change-Id: I315e25dddce79b1db5316634a3934b5e6738fa12
diff --git a/y2014/control_loops/drivetrain/BUILD b/y2014/control_loops/drivetrain/BUILD
index 21d3051..77631af 100644
--- a/y2014/control_loops/drivetrain/BUILD
+++ b/y2014/control_loops/drivetrain/BUILD
@@ -78,6 +78,28 @@
)
cc_library(
+ name = 'polydrivetrain',
+ srcs = [
+ 'polydrivetrain.cc',
+ ],
+ hdrs = [
+ 'polydrivetrain.h',
+ ],
+ deps = [
+ ':drivetrain_queue',
+ '//y2014:constants',
+ '//aos/common/controls:polytope',
+ '//aos/common:math',
+ '//aos/common/messages:robot_state',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:coerce_goal',
+ '//aos/common/util:log_interval',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/logging:matrix_logging',
+ ],
+)
+
+cc_library(
name = 'drivetrain_lib',
srcs = [
'drivetrain.cc',
@@ -87,9 +109,9 @@
],
deps = [
':drivetrain_queue',
+ ':polydrivetrain',
'//aos/common/controls:control_loop',
'//y2014:constants',
- '//aos/common/controls:polytope',
'//aos/common:math',
'//frc971/control_loops:state_feedback_loop',
'//frc971/control_loops:coerce_goal',
diff --git a/y2014/control_loops/drivetrain/drivetrain.cc b/y2014/control_loops/drivetrain/drivetrain.cc
index b6dffa0..9a3aff1 100644
--- a/y2014/control_loops/drivetrain/drivetrain.cc
+++ b/y2014/control_loops/drivetrain/drivetrain.cc
@@ -17,6 +17,7 @@
#include "frc971/control_loops/coerce_goal.h"
#include "y2014/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/shifter_hall_effect.h"
@@ -239,452 +240,6 @@
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.0;
- // 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.0;
- // 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>(
- constants::GetValues().make_v_drivetrain_loop())),
- ttrust_(1.1),
- wheel_(0.0),
- throttle_(0.0),
- quickturn_(false),
- stale_count_(0),
- position_time_delta_(kDt),
- 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 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 / constants::GetValues().high_gear_ratio / kWheelRadius;
- } else {
- return velocity / constants::GetValues().low_gear_ratio / kWheelRadius;
- }
- }
-
- Gear ComputeGear(const 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 auto &values = constants::GetValues();
- 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(values.left_drive, current_left_velocity, left_gear_);
- Gear right_requested =
- ComputeGear(values.right_drive, 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 =
- constants::GetValues().clutch_transmission ? HIGH : SHIFTING_UP;
- const Gear shift_down =
- constants::GetValues().clutch_transmission ? LOW : 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 DrivetrainQueue::Position *position) {
- const auto &values = constants::GetValues();
- if (position == NULL) {
- ++stale_count_;
- } else {
- last_position_ = position_;
- position_ = *position;
- position_time_delta_ = (stale_count_ + 1) * kDt;
- stale_count_ = 0;
- }
-
-#if HAVE_SHIFTERS
- if (position) {
- GearLogging gear_logging;
- // Switch to the correct controller.
- const double left_middle_shifter_position =
- (values.left_drive.clear_high + values.left_drive.clear_low) / 2.0;
- const double right_middle_shifter_position =
- (values.right_drive.clear_high + values.right_drive.clear_low) / 2.0;
-
- if (position->left_shifter_position < left_middle_shifter_position ||
- left_gear_ == LOW) {
- 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 ||
- right_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 > values.left_drive.clear_high && left_gear_ == SHIFTING_UP) {
- left_gear_ = HIGH;
- }
- if (position->left_shifter_position < values.left_drive.clear_low && left_gear_ == SHIFTING_DOWN) {
- left_gear_ = LOW;
- }
- if (position->right_shifter_position > values.right_drive.clear_high && right_gear_ == SHIFTING_UP) {
- right_gear_ = HIGH;
- }
- if (position->right_shifter_position < values.right_drive.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);
- }
-#else
- (void) values;
-#endif
- }
-
- 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() {
- const auto &values = constants::GetValues();
- // TODO(austin): Observer for the current velocity instead of difference
- // calculations.
- ++counter_;
-#if HAVE_SHIFTERS
- 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(values.left_drive, position_.left_shifter_position,
- current_left_velocity);
- const double right_motor_speed =
- MotorSpeed(values.right_drive, 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);
- }
-#else
- (void) values;
-#endif
-
-#if HAVE_SHIFTERS
- if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
-#else
- {
-#endif
- // 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();
-#if HAVE_SHIFTERS
- } 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 / ::aos::robot_state->voltage_battery;
-#endif
- }
- }
-
- void SendMotors(DrivetrainQueue::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_;
- DrivetrainQueue::Position last_position_;
- DrivetrainQueue::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 DrivetrainQueue::Goal *goal,
const DrivetrainQueue::Position *position,
diff --git a/y2014/control_loops/drivetrain/polydrivetrain.cc b/y2014/control_loops/drivetrain/polydrivetrain.cc
new file mode 100644
index 0000000..c97be6f
--- /dev/null
+++ b/y2014/control_loops/drivetrain/polydrivetrain.cc
@@ -0,0 +1,419 @@
+#include "y2014/control_loops/drivetrain/polydrivetrain.h"
+
+#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 "aos/common/messages/robot_state.q.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
+#include "y2014/constants.h"
+#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+
+#define HAVE_SHIFTERS 1
+
+namespace frc971 {
+namespace control_loops {
+
+using ::y2014::control_loops::drivetrain::kDt;
+
+PolyDrivetrain::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>(
+ constants::GetValues().make_v_drivetrain_loop())),
+ ttrust_(1.1),
+ wheel_(0.0),
+ throttle_(0.0),
+ quickturn_(false),
+ stale_count_(0),
+ position_time_delta_(kDt),
+ left_gear_(LOW),
+ right_gear_(LOW),
+ counter_(0) {
+ last_position_.Zero();
+ position_.Zero();
+}
+
+double PolyDrivetrain::MotorSpeed(
+ const 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 / constants::GetValues().high_gear_ratio / kWheelRadius;
+ } else {
+ return velocity / constants::GetValues().low_gear_ratio / kWheelRadius;
+ }
+}
+
+PolyDrivetrain::Gear PolyDrivetrain::ComputeGear(
+ const 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 PolyDrivetrain::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 auto &values = constants::GetValues();
+ 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(values.left_drive, current_left_velocity, left_gear_);
+ Gear right_requested =
+ ComputeGear(values.right_drive, 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 =
+ constants::GetValues().clutch_transmission ? HIGH : SHIFTING_UP;
+ const Gear shift_down =
+ constants::GetValues().clutch_transmission ? LOW : 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 PolyDrivetrain::SetPosition(const DrivetrainQueue::Position *position) {
+ const auto &values = constants::GetValues();
+ if (position == NULL) {
+ ++stale_count_;
+ } else {
+ last_position_ = position_;
+ position_ = *position;
+ position_time_delta_ = (stale_count_ + 1) * kDt;
+ stale_count_ = 0;
+ }
+
+#if HAVE_SHIFTERS
+ if (position) {
+ GearLogging gear_logging;
+ // Switch to the correct controller.
+ const double left_middle_shifter_position =
+ (values.left_drive.clear_high + values.left_drive.clear_low) / 2.0;
+ const double right_middle_shifter_position =
+ (values.right_drive.clear_high + values.right_drive.clear_low) / 2.0;
+
+ if (position->left_shifter_position < left_middle_shifter_position ||
+ left_gear_ == LOW) {
+ 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 ||
+ right_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);
+ }
+ }
+
+ if (position->left_shifter_position > values.left_drive.clear_high &&
+ left_gear_ == SHIFTING_UP) {
+ left_gear_ = HIGH;
+ }
+ if (position->left_shifter_position < values.left_drive.clear_low &&
+ left_gear_ == SHIFTING_DOWN) {
+ left_gear_ = LOW;
+ }
+ if (position->right_shifter_position > values.right_drive.clear_high &&
+ right_gear_ == SHIFTING_UP) {
+ right_gear_ = HIGH;
+ }
+ if (position->right_shifter_position < values.right_drive.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);
+ }
+#else
+ (void)values;
+#endif
+}
+
+double PolyDrivetrain::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 PolyDrivetrain::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 PolyDrivetrain::Update() {
+ const auto &values = constants::GetValues();
+ // TODO(austin): Observer for the current velocity instead of difference
+ // calculations.
+ ++counter_;
+#if HAVE_SHIFTERS
+ 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(values.left_drive, position_.left_shifter_position,
+ current_left_velocity);
+ const double right_motor_speed =
+ MotorSpeed(values.right_drive, 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);
+ }
+#else
+ (void)values;
+#endif
+
+#if HAVE_SHIFTERS
+ if (IsInGear(left_gear_) && IsInGear(right_gear_))
+#endif
+ {
+ // 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();
+#if HAVE_SHIFTERS
+ } 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 / ::aos::robot_state->voltage_battery;
+#endif
+ }
+}
+
+void PolyDrivetrain::SendMotors(DrivetrainQueue::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;
+ }
+}
+
+constexpr double PolyDrivetrain::kStallTorque;
+constexpr double PolyDrivetrain::kStallCurrent;
+constexpr double PolyDrivetrain::kFreeSpeed;
+constexpr double PolyDrivetrain::kFreeCurrent;
+constexpr double PolyDrivetrain::kWheelRadius;
+constexpr double PolyDrivetrain::kR;
+constexpr double PolyDrivetrain::Kv;
+constexpr double PolyDrivetrain::Kt;
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2014/control_loops/drivetrain/polydrivetrain.h b/y2014/control_loops/drivetrain/polydrivetrain.h
new file mode 100644
index 0000000..82ebd24
--- /dev/null
+++ b/y2014/control_loops/drivetrain/polydrivetrain.h
@@ -0,0 +1,81 @@
+#ifndef Y2014_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_H_
+#define Y2014_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_H_
+
+#include "aos/common/controls/polytope.h"
+
+#include "y2014/constants.h"
+#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+class PolyDrivetrain {
+ public:
+ enum Gear { HIGH, LOW, SHIFTING_UP, SHIFTING_DOWN };
+ // Stall Torque in N m
+ static constexpr double kStallTorque =
+ ::y2014::control_loops::drivetrain::kStallTorque;
+ // Stall Current in Amps
+ static constexpr double kStallCurrent = ::y2014::control_loops::drivetrain::kStallCurrent;
+ // Free Speed in RPM. Used number from last year.
+ static constexpr double kFreeSpeed =
+ ::y2014::control_loops::drivetrain::kFreeSpeedRPM;
+ // Free Current in Amps
+ static constexpr double kFreeCurrent =
+ ::y2014::control_loops::drivetrain::kFreeCurrent;
+ static constexpr double kWheelRadius =
+ ::y2014::control_loops::drivetrain::kWheelRadius;
+ // Resistance of the motor, divided by the number of motors.
+ static constexpr double kR = ::y2014::control_loops::drivetrain::kR;
+ // Motor velocity constant
+ static constexpr double Kv = ::y2014::control_loops::drivetrain::kV;
+
+ // Torque constant
+ static constexpr double Kt = ::y2014::control_loops::drivetrain::kT;
+
+ PolyDrivetrain();
+
+ static bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
+
+ static double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
+ double shifter_position, double velocity);
+
+ Gear ComputeGear(const constants::ShifterHallEffect &hall_effect,
+ double velocity, Gear current);
+
+ void SetGoal(double wheel, double throttle, bool quickturn, bool highgear);
+
+ void SetPosition(const DrivetrainQueue::Position *position);
+
+ double FilterVelocity(double throttle);
+
+ double MaxVelocity();
+
+ void Update();
+
+ void SendMotors(DrivetrainQueue::Output *output);
+
+ 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_;
+ DrivetrainQueue::Position last_position_;
+ DrivetrainQueue::Position position_;
+ int counter_;
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2014_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_H_
diff --git a/y2014/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
index 32016b1..a9003aa 100755
--- a/y2014/control_loops/python/drivetrain.py
+++ b/y2014/control_loops/python/drivetrain.py
@@ -21,10 +21,10 @@
# 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
+ self.resistance = 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))
+ (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Control loop time step
@@ -32,9 +32,9 @@
# State feedback matrices
self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.R)]])
+ [[-self.Kt / self.Kv / (self.J * self.resistance)]])
self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.R)]])
+ [[self.Kt / (self.J * self.resistance)]])
self.C = numpy.matrix([[1]])
self.D = numpy.matrix([[0]])
@@ -53,14 +53,16 @@
class Drivetrain(control_loop.ControlLoop):
def __init__(self, name="Drivetrain", left_low=True, right_low=True):
super(Drivetrain, self).__init__(name)
+ # Number of motors per side
+ self.num_motors = 2
# Stall Torque in N m
- self.stall_torque = 2.42
+ self.stall_torque = 2.42 * self.num_motors
# Stall Current in Amps
- self.stall_current = 133.0
+ self.stall_current = 133.0 * self.num_motors
# Free Speed in RPM. Used number from last year.
self.free_speed = 4650.0
# Free Current in Amps
- self.free_current = 2.7
+ self.free_current = 2.7 * self.num_motors
# Moment of inertia of the drivetrain in kg m^2
# Just borrowed from last year.
self.J = 4.5
@@ -71,10 +73,10 @@
# 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 / 2
+ self.resistance = 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))
+ (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratios
@@ -97,10 +99,10 @@
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)
+ self.tcl = -self.Kt / self.Kv / (self.Gl * self.Gl * self.resistance * self.r * self.r)
+ self.tcr = -self.Kt / self.Kv / (self.Gr * self.Gr * self.resistance * self.r * self.r)
+ self.mpl = self.Kt / (self.Gl * self.resistance * self.r)
+ self.mpr = self.Kt / (self.Gr * self.resistance * self.r)
# State feedback matrices
# X will be of the format
@@ -298,6 +300,29 @@
namespaces = namespaces)
dog_loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
drivetrain_low_low.dt))
+ dog_loop_writer.AddConstant(control_loop.Constant("kStallTorque", "%f",
+ drivetrain_low_low.stall_torque))
+ dog_loop_writer.AddConstant(control_loop.Constant("kStallCurrent", "%f",
+ drivetrain_low_low.stall_current))
+ dog_loop_writer.AddConstant(control_loop.Constant("kFreeSpeedRPM", "%f",
+ drivetrain_low_low.free_speed))
+ dog_loop_writer.AddConstant(control_loop.Constant("kFreeCurrent", "%f",
+ drivetrain_low_low.free_current))
+ dog_loop_writer.AddConstant(control_loop.Constant("kJ", "%f",
+ drivetrain_low_low.J))
+ dog_loop_writer.AddConstant(control_loop.Constant("kMass", "%f",
+ drivetrain_low_low.m))
+ dog_loop_writer.AddConstant(control_loop.Constant("kRobotRadius", "%f",
+ drivetrain_low_low.rb))
+ dog_loop_writer.AddConstant(control_loop.Constant("kWheelRadius", "%f",
+ drivetrain_low_low.r))
+ dog_loop_writer.AddConstant(control_loop.Constant("kR", "%f",
+ drivetrain_low_low.resistance))
+ dog_loop_writer.AddConstant(control_loop.Constant("kV", "%f",
+ drivetrain_low_low.Kv))
+ dog_loop_writer.AddConstant(control_loop.Constant("kT", "%f",
+ drivetrain_low_low.Kt))
+
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
else: