Fixed claw angle conversion bug and scaled the hall effect on the drivetrain propperly.
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 233a3ea..f1d2f93 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -31,10 +31,10 @@
double bottom_angle;
// How much higher the top claw is.
double separation_angle;
- // top claw intake roller
+ // top claw intake roller
double intake;
- // bottom claw tusk centering
- double centering;
+ // bottom claw tusk centering
+ double centering;
};
message Position {
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index 196b62c..76cbeca 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeClawPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.0, 0.00740659366663, 0.0, 0.0, 1.0, 0.0, 0.00740659366663, 0.0, 0.0, 0.530576083967, 0.0, 0.0, 0.0, 0.0, 0.530576083967;
+ A << 1.0, 0.0, 0.00767587925947, 0.0, 0.0, 1.0, 0.0, 0.00767587925947, 0.0, 0.0, 0.574320358283, 0.0, 0.0, 0.0, 0.0, 0.574320358283;
Eigen::Matrix<double, 4, 2> B;
- B << 0.00101390984157, 0.0, 0.0, 0.00101390984157, 0.183524472124, 0.0, 0.0, 0.183524472124;
+ B << 0.000908630807869, 0.0, 0.0, 0.000908630807869, 0.166422350613, 0.0, 0.0, 0.166422350613;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 1, 1, 0, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackController<4, 2, 2> MakeClawController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.43057608397, -4.48948312405e-16, -1.43057608397, 1.43057608397, 31.1907717473, -9.79345171104e-15, -31.1907717473, 31.1907717473;
+ L << 1.47432035828, 4.62747953155e-16, -1.47432035828, 1.47432035828, 35.823366785, 1.12408806964e-14, -35.823366785, 35.823366785;
Eigen::Matrix<double, 2, 4> K;
- K << 110.395400642, 0.0, 2.50425726274, 0.0, 0.0, 170.435941688, 0.0, 2.89797614353;
+ K << 78.988151683, 0.0, 1.65649264651, 0.0, 0.0, 109.921478378, 0.0, 2.09683545663;
return StateFeedbackController<4, 2, 2>(L, K, MakeClawPlantCoefficients());
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index e7aae01..93e23cc 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -200,18 +200,24 @@
}
static bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
- static double MotorSpeed(double shifter_position, double velocity) {
+ static double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
+ double shifter_position, double velocity) {
// TODO(austin): G_high, G_low and kWheelRadius
- if (shifter_position > 0.57) {
+ 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(double velocity, Gear current) {
- const double low_omega = MotorSpeed(0, ::std::abs(velocity));
- const double high_omega = MotorSpeed(1.0, ::std::abs(velocity));
+ 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);
@@ -246,6 +252,7 @@
// 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_;
@@ -253,8 +260,10 @@
(position_.right_encoder - last_position_.right_encoder) /
position_time_delta_;
- Gear left_requested = ComputeGear(current_left_velocity, left_gear_);
- Gear right_requested = ComputeGear(current_right_velocity, right_gear_);
+ 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 {
@@ -298,6 +307,7 @@
}
}
void SetPosition(const Drivetrain::Position *position) {
+ const auto &values = constants::GetValues();
if (position == NULL) {
++stale_count_;
} else {
@@ -310,9 +320,14 @@
if (position) {
GearLogging gear_logging;
// Switch to the correct controller.
- // TODO(austin): Un-hard code 0.57
- if (position->left_shifter_position < 0.57) {
- if (position->right_shifter_position < 0.57 || right_gear_ == LOW) {
+ 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) {
+ 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);
@@ -322,7 +337,8 @@
loop_->set_controller_index(gear_logging.controller_index = 1);
}
} else {
- if (position->right_shifter_position < 0.57 || left_gear_ == LOW) {
+ 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);
@@ -334,16 +350,16 @@
}
// TODO(austin): Constants.
- if (position->left_shifter_position > 0.9 && left_gear_ == SHIFTING_UP) {
+ if (position->left_shifter_position > values.left_drive.clear_high && left_gear_ == SHIFTING_UP) {
left_gear_ = HIGH;
}
- if (position->left_shifter_position < 0.1 && left_gear_ == SHIFTING_DOWN) {
+ if (position->left_shifter_position < values.left_drive.clear_low && left_gear_ == SHIFTING_DOWN) {
left_gear_ = LOW;
}
- if (position->right_shifter_position > 0.9 && right_gear_ == SHIFTING_UP) {
+ if (position->right_shifter_position > values.right_drive.clear_high && right_gear_ == SHIFTING_UP) {
right_gear_ = HIGH;
}
- if (position->right_shifter_position < 0.1 && right_gear_ == SHIFTING_DOWN) {
+ if (position->right_shifter_position < values.right_drive.clear_low && right_gear_ == SHIFTING_DOWN) {
right_gear_ = LOW;
}
@@ -401,6 +417,7 @@
}
void Update() {
+ const auto &values = constants::GetValues();
// TODO(austin): Observer for the current velocity instead of difference
// calculations.
++counter_;
@@ -411,9 +428,11 @@
(position_.right_encoder - last_position_.right_encoder) /
position_time_delta_;
const double left_motor_speed =
- MotorSpeed(position_.left_shifter_position, current_left_velocity);
+ MotorSpeed(values.left_drive, position_.left_shifter_position,
+ current_left_velocity);
const double right_motor_speed =
- MotorSpeed(position_.right_shifter_position, current_right_velocity);
+ MotorSpeed(values.right_drive, position_.right_shifter_position,
+ current_right_velocity);
{
CIMLogging logging;
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index 73039db..420f13a 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -19,7 +19,7 @@
self.free_current = 2.7
# Moment of inertia of the claw in kg m^2
# approzimately 0.76 (without ball) in CAD
- self.J = 0.70
+ self.J = 0.80
# Resistance of the motor
self.R = 12.0 / self.stall_current + 0.024 + .003
# Motor velocity constant
@@ -62,13 +62,13 @@
#controlability = controls.ctrb(self.A, self.B);
#print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)
- self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.06 ** 2.0)), 0.0, 0.0],
- [0.0, 0.0, 0.10, 0.0],
- [0.0, 0.0, 0.0, 0.1]])
+ self.Q = numpy.matrix([[(1.0 / (0.12 ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (0.08 ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, 0.050, 0.0],
+ [0.0, 0.0, 0.0, 0.07]])
- self.R = numpy.matrix([[(1.0 / (20.0 ** 2.0)), 0.0],
- [0.0, (1.0 / (20.0 ** 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 "K unaugmented"
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 0ef0df5..ed6bc22 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -324,7 +324,6 @@
//::std::cout << "Measurement error is " << Y_ - C() * X_hat;
//X_hat = A() * X_hat + B() * U;
if (new_y_) {
- LOG(INFO, "Got Y. R is (%f, %f, %f)\n", R(0, 0), R(1, 0), R(2, 0));
X_hat = (A() - L() * C()) * X_hat + L() * Y_ + B() * U;
new_y_ = false;
} else {