Added motor speed matching code into the shifter code.
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 3e24dc2..c0c5983 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -13,6 +13,7 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_motor_plant.h"
+#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/queues/Piston.q.h"
@@ -139,6 +140,40 @@
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 = .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;
+ // Gear ratios
+ static constexpr double G_low = 16.0 / 60.0 * 19.0 / 50.0;
+ static constexpr double G_high = 28.0 / 48.0 * 19.0 / 50.0;
+
PolyDrivetrain()
: U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
/*[*/ -1, 0 /*]*/,
@@ -148,15 +183,32 @@
/*[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
/*[*/ 12 /*]]*/).finished()),
- loop_(new StateFeedbackLoop<2, 2, 2>(MakeVDrivetrainLoop())) {
+ loop_(new StateFeedbackLoop<2, 2, 2>(MakeVDrivetrainLoop())),
+ left_cim_(new StateFeedbackLoop<1, 1, 1>(MakeCIMLoop())),
+ right_cim_(new StateFeedbackLoop<1, 1, 1>(MakeCIMLoop())),
+ 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) {
- ttrust_ = 1.1;
-
- wheel_ = 0.0;
- throttle_ = 0.0;
- quickturn_ = false;
- highgear_ = true;
+ last_position_.Zero();
+ position_.Zero();
}
+ static bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
+
+ static double MotorSpeed(double shifter_position, double velocity) {
+ // TODO(austin): G_high, G_low and kWheelRadius
+ if (shifter_position > 0.5) {
+ return velocity / G_high / kWheelRadius;
+ } else {
+ return velocity / G_low / kWheelRadius;
+ }
+ }
+
void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
const double kWheelNonLinearity = 0.4;
// Apply a sin function that's scaled to make it feel better.
@@ -165,13 +217,71 @@
wheel_ = sin(angular_range * wheel_) / sin(angular_range);
throttle_ = throttle;
quickturn_ = quickturn;
- highgear_ = highgear;
- if (highgear_) {
- loop_->set_controller_index(3);
- } else {
- loop_->set_controller_index(0);
+
+ // TODO(austin): Fix the upshift logic to include states.
+ const Gear requested_gear = highgear ? HIGH : LOW;
+
+ if (left_gear_ != requested_gear) {
+ if (IsInGear(left_gear_)) {
+ if (requested_gear == HIGH) {
+ left_gear_ = SHIFTING_UP;
+ } else {
+ left_gear_ = SHIFTING_DOWN;
+ }
+ }
+ }
+ if (right_gear_ != requested_gear) {
+ if (IsInGear(right_gear_)) {
+ if (requested_gear == HIGH) {
+ right_gear_ = SHIFTING_UP;
+ } else {
+ 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) {
+ // Switch to the correct controller.
+ // TODO(austin): Un-hard code 0.5
+ if (position->left_shifter_position < 0.5) {
+ if (position->right_shifter_position < 0.5) {
+ loop_->set_controller_index(0);
+ } else {
+ loop_->set_controller_index(1);
+ }
+ } else {
+ if (position->right_shifter_position < 0.5) {
+ loop_->set_controller_index(2);
+ } else {
+ loop_->set_controller_index(3);
+ }
+ }
+ // TODO(austin): Constants.
+ if (position->left_shifter_position > 0.9 && left_gear_ == SHIFTING_UP) {
+ left_gear_ = HIGH;
+ }
+ if (position->left_shifter_position < 0.1 && left_gear_ == SHIFTING_DOWN) {
+ left_gear_ = LOW;
+ }
+ if (position->right_shifter_position > 0.9 && right_gear_ == SHIFTING_UP) {
+ right_gear_ = HIGH;
+ }
+ if (position->right_shifter_position < 0.1 && right_gear_ == SHIFTING_DOWN) {
+ right_gear_ = LOW;
+ }
+ }
+ }
+
double FilterVelocity(double throttle) {
const Eigen::Matrix<double, 2, 2> FF =
loop_->B().inverse() *
@@ -220,57 +330,100 @@
}
void Update() {
- // FF * X = U (steady state)
- const Eigen::Matrix<double, 2, 2> FF =
- loop_->B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+ // TODO(austin): Observer for the current velocity instead of difference
+ // calculations.
+ const double current_left_velocity =
+ (position_.left_encoder - last_position_.left_encoder) * 100.0 /
+ position_time_delta_;
+ const double current_right_velocity =
+ (position_.right_encoder - last_position_.right_encoder) * 100.0 /
+ position_time_delta_;
+ const double left_motor_speed =
+ MotorSpeed(position_.left_shifter_position, current_left_velocity);
+ const double right_motor_speed =
+ MotorSpeed(position_.right_shifter_position, current_right_velocity);
- // 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_);
+ // Reset the CIM model to the current conditions to be ready for when we shift.
+ if (IsInGear(left_gear_)) {
+ left_cim_->X_hat(0, 0) = left_motor_speed;
+ }
+ if (IsInGear(right_gear_)) {
+ right_cim_->X_hat(1, 0) = right_motor_speed;
+ }
- const double sign_svel = wheel_ * ((fvel > 0.0) ? 1.0 : -1.0);
- double steering_velocity;
- if (quickturn_) {
- steering_velocity = wheel_ * MaxVelocity();
+ 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_->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_->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_->U[i] = ::aos::Clip(U_ideal[i], -12, 12);
+ }
} else {
- steering_velocity = ::std::abs(fvel) * wheel_;
- }
- const double left_velocity = fvel - steering_velocity;
- const double right_velocity = fvel + steering_velocity;
+ // Any motor is not in gear. Speed match.
+ ::Eigen::Matrix<double, 1, 1> R_left;
+ R_left(0, 0) = left_motor_speed;
- // Integrate velocity to get the position.
- // This position is used to get integral control.
- loop_->R << left_velocity, right_velocity;
+ // TODO(austin): Use battery volts here at some point.
+ loop_->U(0, 0) = ::aos::Clip(
+ (left_cim_->K() * (R_left - left_cim_->X_hat) + R_left / Kv)(0, 0), -12, 12);
+ right_cim_->X_hat = right_cim_->A() * right_cim_->X_hat + right_cim_->B() * loop_->U(0, 0);
- 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_->R =
- CoerceGoal(R_poly, equality_k, equality_w, loop_->R);
+ ::Eigen::Matrix<double, 1, 1> R_right;
+ R_right(0, 0) = right_motor_speed;
+ loop_->U(1, 0) = ::aos::Clip(
+ (right_cim_->K() * (R_right - right_cim_->X_hat) + R_right / Kv)(0, 0), -12,
+ 12);
+ right_cim_->X_hat = right_cim_->A() * right_cim_->X_hat + right_cim_->B() * loop_->U(1, 0);
}
- 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_->U[i] = ::aos::Clip(U_ideal[i], -12, 12);
+ if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
+ // TODO(austin): Model this better.
+ // TODO(austin): Feed back?
+ loop_->X_hat = loop_->A() * loop_->X_hat + loop_->B() * loop_->U;
}
-
- // TODO(austin): Feed back?
- loop_->X_hat = loop_->A() * loop_->X_hat + loop_->B() * loop_->U;
}
void SendMotors(Drivetrain::Output *output) {
@@ -278,7 +431,10 @@
loop_->U(0, 0), loop_->U(1, 0), wheel_, throttle_);
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
- if (highgear_) {
+ // Go in high gear if anything wants to be in high gear.
+ // TODO(austin): Seperate these.
+ if (left_gear_ == HIGH || left_gear_ == SHIFTING_UP ||
+ right_gear_ == HIGH || right_gear_ == SHIFTING_UP) {
shifters.MakeWithBuilder().set(false).Send();
} else {
shifters.MakeWithBuilder().set(true).Send();
@@ -289,12 +445,19 @@
const ::aos::controls::HPolytope<2> U_Poly_;
::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;
+ ::std::unique_ptr<StateFeedbackLoop<1, 1, 1>> left_cim_;
+ ::std::unique_ptr<StateFeedbackLoop<1, 1, 1>> right_cim_;
- double ttrust_;
+ const double ttrust_;
double wheel_;
double throttle_;
bool quickturn_;
- bool highgear_;
+ int stale_count_;
+ double position_time_delta_;
+ Gear left_gear_;
+ Gear right_gear_;
+ Drivetrain::Position last_position_;
+ Drivetrain::Position position_;
};
@@ -318,7 +481,7 @@
_left_pwm = 0.0;
_right_pwm = 0.0;
}
- void Update(void) {
+ void Update() {
double overPower;
float sensitivity = 1.7;
float angular_power;
@@ -455,7 +618,7 @@
static PolyDrivetrain dt_openloop;
bool bad_pos = false;
- if (position == NULL) {
+ if (position == nullptr) {
LOG(WARNING, "no position\n");
bad_pos = true;
}
@@ -481,6 +644,7 @@
dt_closedloop.SetRawPosition(left_encoder, right_encoder);
}
}
+ dt_openloop.SetPosition(position);
dt_closedloop.Update(position, output == NULL);
dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
dt_openloop.Update();
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index 89a2321..e925de3 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -24,6 +24,7 @@
'drivetrain.cc',
'drivetrain_motor_plant.cc',
'polydrivetrain_motor_plant.cc',
+ 'polydrivetrain_cim_plant.cc',
],
'dependencies': [
'drivetrain_loop',
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 2085eb8..0f128ad 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -20,6 +20,8 @@
message Position {
double left_encoder;
double right_encoder;
+ double left_shifter_position;
+ double right_shifter_position;
};
message Output {
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_cim_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_cim_plant.cc
new file mode 100644
index 0000000..1287483
--- /dev/null
+++ b/frc971/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/frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h
new file mode 100644
index 0000000..12b2c59
--- /dev/null
+++ b/frc971/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/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
index d07f22b..1cd17ea 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
@@ -75,7 +75,7 @@
Eigen::Matrix<double, 2, 2> L;
L << 0.879177606502, 0.000686937184856, 0.000686937184856, 0.879177606502;
Eigen::Matrix<double, 2, 2> K;
- K << 21.3663935118, 0.182343374572, 0.182343374572, 21.3663935118;
+ K << 16.0138530269, 0.145874699657, 0.145874699657, 16.0138530269;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
@@ -83,7 +83,7 @@
Eigen::Matrix<double, 2, 2> L;
L << 0.879178111554, 0.000716636558747, 0.000716636558747, 0.958034852635;
Eigen::Matrix<double, 2, 2> K;
- K << 21.3663935124, 0.182479162737, 0.399173168486, 53.6921632348;
+ K << 16.0138530273, 0.145983330189, 0.319338534789, 42.460353773;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
@@ -91,7 +91,7 @@
Eigen::Matrix<double, 2, 2> L;
L << 0.958040379369, 0.000149803514919, 0.000149803514919, 0.87917258482;
Eigen::Matrix<double, 2, 2> K;
- K << 53.6921632348, 0.399173168486, 0.182479162737, 21.3663935124;
+ K << 42.460353773, 0.319338534789, 0.145983330189, 16.0138530273;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
@@ -99,7 +99,7 @@
Eigen::Matrix<double, 2, 2> L;
L << 0.958035518136, 0.000156145735499, 0.000156145735499, 0.958035518136;
Eigen::Matrix<double, 2, 2> K;
- K << 53.6921632349, 0.399235265426, 0.399235265426, 53.6921632349;
+ K << 42.460353773, 0.319388212341, 0.319388212341, 42.460353773;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index fe20228..27bebbc 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -41,6 +41,7 @@
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]])
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 0251408..da9d414 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -117,7 +117,7 @@
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.PlaceControllerPoles([0.5, 0.5])
+ self.PlaceControllerPoles([0.6, 0.6])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high
@@ -132,8 +132,14 @@
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_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')
@@ -171,42 +177,65 @@
# inertia. A value of 1 is no throttle negative inertia.
self.ttrust = 1.0
- self.left_high = False
- self.right_high = False
+ 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_high:
- if self.right_high:
+ 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_high:
+ 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.1, 1.0)
+ else:
+ shifter_position = max(shifter_position - 0.1, 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"
+ #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
- if (high_power > low_power) != current_gear:
- if high_power > low_power:
- print gear_name, "Shifting to high"
- else:
- print gear_name, "Shifting to low"
+ #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):
@@ -215,7 +244,23 @@
# 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.
- return high_power > low_power
+ 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
@@ -255,65 +300,99 @@
# This is the same as sending the most torque down to the floor at the
# wheel.
- self.left_high = self.ComputeGear(self.X[0, 0], should_print=True, current_gear=self.left_high, gear_name="left")
- self.right_high = self.ComputeGear(self.X[1, 0], should_print=True, current_gear=self.right_high, gear_name="right")
+ 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])
- FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
- # Filter the throttle to provide a nicer response.
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ # Filter the throttle to provide a nicer response.
+ fvel = self.FilterVelocity(throttle)
- # TODO(austin): fn
- 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)
- # 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
- # 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
- # (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
- 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
- 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)
- # 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)
- # 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])
- FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
- self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
+ 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)
- self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+
+ # 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) != 3:
+ if len(argv) != 5:
print "Expected .h file name and .cc file name"
else:
loop_writer = control_loop.ControlLoopWriter(
@@ -326,8 +405,15 @@
loop_writer.Write(argv[2], argv[1])
else:
loop_writer.Write(argv[1], argv[2])
- return
+ 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 = []
@@ -337,38 +423,40 @@
t_plot = []
left_gear_plot = []
right_gear_plot = []
- vdrivetrain.left_high = True
- vdrivetrain.right_high = True
+ 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_high:
+ if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
print "Left is high"
else:
print "Left is low"
- if vdrivetrain.right_high:
+ if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
print "Right is high"
else:
print "Right is low"
- for t in numpy.arange(0, 2.0, vdrivetrain.dt):
+ for t in numpy.arange(0, 4.0, vdrivetrain.dt):
if t < 1.0:
- vdrivetrain.Update(throttle=0.60, steering=0.3)
- elif t < 1.5:
- vdrivetrain.Update(throttle=0.60, steering=-0.3)
+ vdrivetrain.Update(throttle=0.60, steering=0.0)
+ elif t < 1.2:
+ vdrivetrain.Update(throttle=0.60, steering=0.0)
else:
- vdrivetrain.Update(throttle=0.0, steering=0.3)
+ vdrivetrain.Update(throttle=0.60, 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_high * 2.0 - 10.0)
- right_gear_plot.append(vdrivetrain.right_high * 2.0 - 10.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 fwd_velocity < 0.0000001:
+ if abs(fwd_velocity) < 0.0000001:
radius_plot.append(turn_velocity)
else:
radius_plot.append(turn_velocity / fwd_velocity)
@@ -384,11 +472,16 @@
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()
+ #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')
diff --git a/frc971/control_loops/update_polydrivetrain.sh b/frc971/control_loops/update_polydrivetrain.sh
index 71e5d09..cd0d71e 100755
--- a/frc971/control_loops/update_polydrivetrain.sh
+++ b/frc971/control_loops/update_polydrivetrain.sh
@@ -1,6 +1,8 @@
#!/bin/bash
#
-# Updates the drivetrain controller.
+# Updates the polydrivetrain controller and CIM models.
./python/polydrivetrain.py drivetrain/polydrivetrain_motor_plant.h \
- drivetrain/polydrivetrain_motor_plant.cc
+ drivetrain/polydrivetrain_motor_plant.cc \
+ drivetrain/polydrivetrain_cim_plant.h \
+ drivetrain/polydrivetrain_cim_plant.cc