Fixed claw angle conversion bug and scaled the hall effect on the drivetrain propperly.
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 2c766a0..1e5e1d0 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -34,10 +34,8 @@
const ShifterHallEffect kCompLeftDriveShifter{0.83, 2.32, 1.2, 1.0};
const ShifterHallEffect kCompRightDriveShifter{0.865, 2.375, 1.2, 1.0};
-const ShifterHallEffect kPracticeLeftDriveShifter{5, 0, 0.60,
- 0.47};
-const ShifterHallEffect kPracticeRightDriveShifter{5, 0, 0.62,
- 0.55};
+const ShifterHallEffect kPracticeRightDriveShifter{2.575, 3.16, 0.98, 0.40};
+const ShifterHallEffect kPracticeLeftDriveShifter{2.57, 3.15, 0.98, 0.4};
const double shooter_zeroing_speed = 0.05;
const double shooter_unload_speed = 0.08;
@@ -126,17 +124,17 @@
shooter_zeroing_speed,
shooter_unload_speed
},
- {0.400000,
- 0.200000,
- 0.000000,
- -0.762218,
- 0.912207,
- -0.362218,
- 0.512207,
- {-1.682379, 1.043334, -1.282379, 0.643334, {-1.7, -1.544662, -1.7, -1.547616}, {-0.130218, -0.019771, -0.132036, -0.018862}, {0.935842, 1.1, 0.932660, 1.1}},
- {-1.225821, 1.553752, -0.825821, 1.153752, {-1.3, -1.088331, -1.3, -1.088331}, {-0.134536, -0.018408, -0.136127, -0.019771}, {1.447396, 1.6, 1.443987, 1.6}},
- 0.020000, // claw_unimportant_epsilon
- -0.200000, // start_fine_tune_pos
+ {0.400000 * 2.0,
+ 0.200000 * 2.0,
+ 0.000000 * 2.0,
+ -0.762218 * 2.0,
+ 0.912207 * 2.0,
+ -0.849484,
+ 1.42308,
+ {-1.682379 * 2.0, 1.043334 * 2.0, -3.166136, 0.643334 * 2.0, {-1.7 * 2.0, -1.544662 * 2.0, -1.7 * 2.0, -1.547616 * 2.0}, {-0.130218 * 2.0, -0.019771 * 2.0, -0.132036 * 2.0, -0.018862 * 2.0}, {0.935842 * 2.0, 1.1 * 2.0, 0.932660 * 2.0, 1.1 * 2.0}},
+ {-1.225821 * 2.0, 1.553752 * 2.0, -2.273474, 1.153752 * 2.0, {-1.3 * 2.0, -1.088331 * 2.0, -1.3 * 2.0, -1.088331 * 2.0}, {-0.134536 * 2.0, -0.018408 * 2.0, -0.136127 * 2.0, -0.019771 * 2.0}, {1.447396 * 2.0, 1.6 * 2.0, 1.443987 * 2.0, 1.6 * 2.0}},
+ 0.020000 * 2.0, // claw_unimportant_epsilon
+ -0.200000 * 2.0, // start_fine_tune_pos
4.000000,
}
};
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 {
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 8c4172d..f375f20 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -50,12 +50,9 @@
// TODO(brian): Fix this.
double adc_translate(uint16_t in) {
static const double kVcc = 5;
- static const double kR1 = 5, kR2 = 6.65;
+ //static const double kR1 = 5, kR2 = 6.65;
static const uint16_t kMaximumValue = 0x3FF;
- return (kVcc *
- (static_cast<double>(in) / static_cast<double>(kMaximumValue)) -
- kVcc * kR1) /
- kR2;
+ return (kVcc * static_cast<double>(in) / static_cast<double>(kMaximumValue));
}
double gyro_translate(int64_t in) {
@@ -77,7 +74,8 @@
/ (256.0 /*cpr*/ * 4.0 /*quad*/)
/ (18.0 / 48.0 /*encoder gears*/)
/ (12.0 / 60.0 /*chain reduction*/)
- * (M_PI / 180.0);
+ * (M_PI / 180.0)
+ * 2.0 /*TODO(austin): Debug this, encoders read too little*/;
}
double shooter_translate(int32_t in) {