got the new drive code working
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index c0c5983..24e7dd8 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -99,14 +99,14 @@
double left, double right, double gyro, bool control_loop_driving) {
// Decay the offset quickly because this gyro is great.
_offset = (0.25) * (right - left - gyro * width) / 2.0 + 0.75 * _offset;
- const double angle_error = (_right_goal - _left_goal) / width - (_raw_right - _offset - _raw_left - _offset) / width;
+ //const double angle_error = (_right_goal - _left_goal) / width - (_raw_right - _offset - _raw_left - _offset) / width;
// TODO(aschuh): Add in the gyro.
_integral_offset = 0.0;
_offset = 0.0;
_gyro = gyro;
_control_loop_driving = control_loop_driving;
SetRawPosition(left, right);
- LOG(DEBUG, "Left %f->%f Right %f->%f Gyro %f aerror %f ioff %f\n", left + _offset, _left_goal, right - _offset, _right_goal, gyro, angle_error, _integral_offset);
+ //LOG(DEBUG, "Left %f->%f Right %f->%f Gyro %f aerror %f ioff %f\n", left + _offset, _left_goal, right - _offset, _right_goal, gyro, angle_error, _integral_offset);
}
void Update(bool update_observer, bool stop_motors) {
@@ -193,7 +193,8 @@
stale_count_(0),
position_time_delta_(0.01),
left_gear_(LOW),
- right_gear_(LOW) {
+ right_gear_(LOW),
+ counter_(0) {
last_position_.Zero();
position_.Zero();
@@ -202,7 +203,7 @@
static double MotorSpeed(double shifter_position, double velocity) {
// TODO(austin): G_high, G_low and kWheelRadius
- if (shifter_position > 0.5) {
+ if (shifter_position > 0.57) {
return velocity / G_high / kWheelRadius;
} else {
return velocity / G_low / kWheelRadius;
@@ -210,7 +211,7 @@
}
void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
- const double kWheelNonLinearity = 0.4;
+ 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);
@@ -252,20 +253,52 @@
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) {
+ // TODO(austin): Un-hard code 0.57
+ if (position->left_shifter_position < 0.57) {
+ if (position->right_shifter_position < 0.57) {
+ LOG(DEBUG, "Loop Left low, Right low\n");
loop_->set_controller_index(0);
} else {
+ LOG(DEBUG, "Loop Left low, Right high\n");
loop_->set_controller_index(1);
}
} else {
- if (position->right_shifter_position < 0.5) {
+ if (position->right_shifter_position < 0.57) {
+ LOG(DEBUG, "Loop Left high, Right low\n");
loop_->set_controller_index(2);
} else {
+ LOG(DEBUG, "Loop Left high, Right high\n");
loop_->set_controller_index(3);
}
}
+ switch (left_gear_) {
+ case LOW:
+ LOG(DEBUG, "Left is in low\n");
+ break;
+ case HIGH:
+ LOG(DEBUG, "Left is in high\n");
+ break;
+ case SHIFTING_UP:
+ LOG(DEBUG, "Left is shifting up\n");
+ break;
+ case SHIFTING_DOWN:
+ LOG(DEBUG, "Left is shifting down\n");
+ break;
+ }
+ switch (right_gear_) {
+ case LOW:
+ LOG(DEBUG, "Right is in low\n");
+ break;
+ case HIGH:
+ LOG(DEBUG, "Right is in high\n");
+ break;
+ case SHIFTING_UP:
+ LOG(DEBUG, "Right is shifting up\n");
+ break;
+ case SHIFTING_DOWN:
+ LOG(DEBUG, "Right is shifting down\n");
+ break;
+ }
// TODO(austin): Constants.
if (position->left_shifter_position > 0.9 && left_gear_ == SHIFTING_UP) {
left_gear_ = HIGH;
@@ -332,11 +365,12 @@
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) * 100.0 /
+ (position_.left_encoder - last_position_.left_encoder) /
position_time_delta_;
const double current_right_velocity =
- (position_.right_encoder - last_position_.right_encoder) * 100.0 /
+ (position_.right_encoder - last_position_.right_encoder) /
position_time_delta_;
const double left_motor_speed =
MotorSpeed(position_.left_shifter_position, current_left_velocity);
@@ -346,10 +380,16 @@
// 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;
+ LOG(DEBUG, "Setting left CIM to %f at robot speed %f\n", left_motor_speed,
+ current_left_velocity);
}
if (IsInGear(right_gear_)) {
- right_cim_->X_hat(1, 0) = right_motor_speed;
+ right_cim_->X_hat(0, 0) = right_motor_speed;
+ LOG(DEBUG, "Setting right CIM to %f at robot speed %f\n",
+ right_motor_speed, current_right_velocity);
}
+ LOG(DEBUG, "robot speed l=%f r=%f\n", current_left_velocity,
+ current_right_velocity);
if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
// FF * X = U (steady state)
@@ -401,36 +441,40 @@
for (int i = 0; i < 2; i++) {
loop_->U[i] = ::aos::Clip(U_ideal[i], -12, 12);
}
+
+ // TODO(austin): Model this better.
+ // TODO(austin): Feed back?
+ loop_->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;
R_left(0, 0) = left_motor_speed;
+ const double wiggle = (static_cast<double>((counter_ % 4) / 2) - 0.5) * 3.5;
- // 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);
+ loop_->U(0, 0) =
+ ::aos::Clip((R_left / Kv)(0, 0) + wiggle, -position_.battery_voltage,
+ position_.battery_voltage);
+ right_cim_->X_hat = right_cim_->A() * right_cim_->X_hat +
+ right_cim_->B() * loop_->U(0, 0);
::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);
- }
-
- 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;
+ loop_->U(1, 0) =
+ ::aos::Clip((R_right / Kv)(0, 0) + wiggle, -position_.battery_voltage,
+ position_.battery_voltage);
+ right_cim_->X_hat = right_cim_->A() * right_cim_->X_hat +
+ right_cim_->B() * loop_->U(1, 0);
+ loop_->U *= 12.0 / position_.battery_voltage;
}
}
void SendMotors(Drivetrain::Output *output) {
LOG(DEBUG, "left pwm: %f right pwm: %f wheel: %f throttle: %f\n",
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 (output != NULL) {
+ output->left_voltage = loop_->U(0, 0);
+ output->right_voltage = loop_->U(1, 0);
+ }
// Go in high gear if anything wants to be in high gear.
// TODO(austin): Seperate these.
if (left_gear_ == HIGH || left_gear_ == SHIFTING_UP ||
@@ -458,6 +502,7 @@
Gear right_gear_;
Drivetrain::Position last_position_;
Drivetrain::Position position_;
+ int counter_;
};
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 0f128ad..1dcc947 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -22,6 +22,7 @@
double right_encoder;
double left_shifter_position;
double right_shifter_position;
+ double battery_voltage;
};
message Output {
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
index 1cd17ea..4b1a1ba 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.899177606502, 0.000686937184856, 0.000686937184856, 0.899177606502;
+ A << 0.8793262727, 0.0205382709865, 0.0205382709865, 0.8793262727;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0186835844877, -0.000127297602107, -0.000127297602107, 0.0186835844877;
+ B << 0.0223622719242, -0.00380598503859, -0.00380598503859, 0.0223622719242;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.89917740051, 0.000149762601927, 0.000716637450627, 0.978035563679;
+ A << 0.879138263008, 0.00451814985125, 0.0216200529991, 0.97348145244;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0186836226605, -6.07092175404e-05, -0.00013280141337, 0.0089037164526;
+ B << 0.0223971123485, -0.00183152094495, -0.00400645206708, 0.0107498150538;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.978035563679, 0.000716637450627, 0.000149762601927, 0.89917740051;
+ A << 0.97348145244, 0.0216200529991, 0.00451814985125, 0.879138263008;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0089037164526, -0.00013280141337, -6.07092175404e-05, 0.0186836226605;
+ B << 0.0107498150538, -0.00400645206708, -0.00183152094495, 0.0223971123485;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.978035518136, 0.000156145735499, 0.000156145735499, 0.978035518136;
+ A << 0.973439382316, 0.00475228155545, 0.00475228155545, 0.973439382316;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0089037349145, -6.32967463335e-05, -6.32967463335e-05, 0.0089037349145;
+ B << 0.0107668690064, -0.00192643083821, -0.00192643083821, 0.0107668690064;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,33 +73,33 @@
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.879177606502, 0.000686937184856, 0.000686937184856, 0.879177606502;
+ L << 0.8593262727, 0.0205382709865, 0.0205382709865, 0.8593262727;
Eigen::Matrix<double, 2, 2> K;
- K << 16.0138530269, 0.145874699657, 0.145874699657, 16.0138530269;
+ K << 13.0245570096, 3.13517071687, 3.13517071687, 13.0245570096;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.879178111554, 0.000716636558747, 0.000716636558747, 0.958034852635;
+ L << 0.859606856614, 0.0216072038404, 0.0216072038404, 0.953012858834;
Eigen::Matrix<double, 2, 2> K;
- K << 16.0138530273, 0.145983330189, 0.319338534789, 42.460353773;
+ K << 13.0245575441, 3.13849145977, 6.86545006825, 35.9127730204;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.958040379369, 0.000149803514919, 0.000149803514919, 0.87917258482;
+ L << 0.956890244313, 0.00522697730081, 0.00522697730081, 0.855729471134;
Eigen::Matrix<double, 2, 2> K;
- K << 42.460353773, 0.319338534789, 0.145983330189, 16.0138530273;
+ K << 35.9127730204, 6.86545006825, 3.13849145977, 13.0245575441;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.958035518136, 0.000156145735499, 0.000156145735499, 0.958035518136;
+ L << 0.953439382316, 0.00475228155545, 0.00475228155545, 0.953439382316;
Eigen::Matrix<double, 2, 2> K;
- K << 42.460353773, 0.319388212341, 0.319388212341, 42.460353773;
+ K << 35.9127730463, 6.86696893904, 6.86696893904, 35.9127730463;
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 27bebbc..c6a3f05 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -62,7 +62,7 @@
self.free_current = 2.7
# Moment of inertia of the drivetrain in kg m^2
# Just borrowed from last year.
- self.J = 6.4
+ self.J = 4.5
# Mass of the robot, in kg.
self.m = 68
# Radius of the robot, in meters (from last year).
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index da9d414..f28cf42 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -209,9 +209,9 @@
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)
+ shifter_position = min(shifter_position + 0.5, 1.0)
else:
- shifter_position = max(shifter_position - 0.1, 0.0)
+ shifter_position = max(shifter_position - 0.5, 0.0)
if shifter_position == 1.0:
gear = VelocityDrivetrain.HIGH
@@ -441,11 +441,11 @@
for t in numpy.arange(0, 4.0, vdrivetrain.dt):
if t < 1.0:
- vdrivetrain.Update(throttle=0.60, steering=0.0)
+ vdrivetrain.Update(throttle=1.00, steering=0.0)
elif t < 1.2:
- vdrivetrain.Update(throttle=0.60, steering=0.0)
+ vdrivetrain.Update(throttle=1.00, steering=0.0)
else:
- vdrivetrain.Update(throttle=0.60, steering=0.0)
+ 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])