Merged in the stuff most recently on the robot.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index b804399..58b2ca0 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -4,6 +4,7 @@
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
@@ -34,7 +35,8 @@
// correct side of the zero and go zero.
// Valid region plan.
-// Difference between the arms has a range, and the values of each arm has a range.
+// Difference between the arms has a range, and the values of each arm has a
+// range.
// If a claw runs up against a static limit, don't let the goal change outside
// the limit.
// If a claw runs up against a movable limit, move both claws outwards to get
@@ -170,9 +172,9 @@
void ZeroedStateFeedbackLoop::Reset(const HalfClawPosition &claw) {
set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
- front_.Reset();
- calibration_.Reset();
- back_.Reset();
+ front_.Reset(claw.front);
+ calibration_.Reset(claw.calibration);
+ back_.Reset(claw.back);
// close up the min and max edge positions as they are no longer valid and
// will be expanded in future iterations
min_hall_effect_on_angle_ = claw.position;
@@ -211,7 +213,8 @@
}
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
- : aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
+ : aos::control_loops::ControlLoop<control_loops::ClawGroup, true, true,
+ false>(my_claw),
has_top_claw_goal_(false),
top_claw_goal_(0.0),
top_claw_(this),
@@ -226,17 +229,54 @@
const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
+bool ZeroedStateFeedbackLoop::SawFilteredPosedge(
+ const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB) {
+ if (posedge_filter_ == nullptr && this_sensor.posedge_count_changed() &&
+ !sensorA.posedge_count_changed() && !sensorB.posedge_count_changed() &&
+ this_sensor.value() && !this_sensor.last_value()) {
+ posedge_filter_ = &this_sensor;
+ } else if (posedge_filter_ == &this_sensor &&
+ !this_sensor.posedge_count_changed() &&
+ !sensorA.posedge_count_changed() &&
+ !sensorB.posedge_count_changed() && this_sensor.value()) {
+ posedge_filter_ = nullptr;
+ return true;
+ } else if (posedge_filter_ == &this_sensor) {
+ posedge_filter_ = nullptr;
+ }
+ return false;
+}
+
+bool ZeroedStateFeedbackLoop::SawFilteredNegedge(
+ const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB) {
+ if (negedge_filter_ == nullptr && this_sensor.negedge_count_changed() &&
+ !sensorA.negedge_count_changed() && !sensorB.negedge_count_changed() &&
+ !this_sensor.value() && this_sensor.last_value()) {
+ negedge_filter_ = &this_sensor;
+ } else if (negedge_filter_ == &this_sensor &&
+ !this_sensor.negedge_count_changed() &&
+ !sensorA.negedge_count_changed() &&
+ !sensorB.negedge_count_changed() && !this_sensor.value()) {
+ negedge_filter_ = nullptr;
+ return true;
+ } else if (negedge_filter_ == &this_sensor) {
+ negedge_filter_ = nullptr;
+ }
+ return false;
+}
+
bool ZeroedStateFeedbackLoop::DoGetPositionOfEdge(
const constants::Values::Claws::AnglePair &angles, double *edge_encoder,
- double *edge_angle, const HallEffectTracker &sensor,
+ double *edge_angle, const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA, const HallEffectTracker &sensorB,
const char *hall_effect_name) {
bool found_edge = false;
- if (sensor.posedge_count_changed()) {
+ if (SawFilteredPosedge(this_sensor, sensorA, sensorB)) {
if (min_hall_effect_off_angle_ == max_hall_effect_off_angle_) {
- // we oddly got two of the same edge.
- *edge_angle = last_edge_value_;
- found_edge = true;
+ LOG(WARNING, "%s: Uncertain which side, rejecting posedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
@@ -251,14 +291,14 @@
name_, hall_effect_name, *edge_angle, posedge_value_,
average_last_encoder);
}
- }
- *edge_encoder = posedge_value_;
- found_edge = true;
- }
- if (sensor.negedge_count_changed()) {
- if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
- *edge_angle = last_edge_value_;
+ *edge_encoder = posedge_value_;
found_edge = true;
+ }
+ }
+
+ if (SawFilteredNegedge(this_sensor, sensorA, sensorB)) {
+ if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
+ LOG(WARNING, "%s: Uncertain which side, rejecting negedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
@@ -274,12 +314,8 @@
average_last_encoder);
}
*edge_encoder = negedge_value_;
+ found_edge = true;
}
- found_edge = true;
- }
-
- if (found_edge) {
- last_edge_value_ = *edge_angle;
}
return found_edge;
@@ -288,23 +324,16 @@
bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
const constants::Values::Claws::Claw &claw_values, double *edge_encoder,
double *edge_angle) {
- // TODO(austin): Validate that the hall effect edge makes sense.
- // We must now be on the side of the edge that we expect to be, and the
- // encoder must have been on either side of the edge before and after.
-
- // TODO(austin): Compute the last off range min and max and compare the edge
- // value to the middle of the range. This will be quite a bit more reliable.
-
- if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle,
- front_, "front")) {
+ if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle, front_,
+ calibration_, back_, "front")) {
return true;
}
if (DoGetPositionOfEdge(claw_values.calibration, edge_encoder, edge_angle,
- calibration_, "calibration")) {
+ calibration_, front_, back_, "calibration")) {
return true;
}
- if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle,
- back_, "back")) {
+ if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle, back_,
+ calibration_, front_, "back")) {
return true;
}
return false;
@@ -358,14 +387,12 @@
const double separation = *top_goal - *bottom_goal;
if (separation > values.claw.claw_max_separation) {
- LOG(DEBUG, "Greater than\n");
const double dsep = (separation - values.claw.claw_max_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
LOG(DEBUG, "Goals now bottom: %f, top: %f\n", *bottom_goal, *top_goal);
}
if (separation < values.claw.claw_min_separation) {
- LOG(DEBUG, "Less than\n");
const double dsep = (separation - values.claw.claw_min_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
@@ -396,7 +423,8 @@
return (
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- (::aos::robot_state->autonomous &&
+ (((::aos::robot_state.get() == NULL) ? true
+ : ::aos::robot_state->autonomous) &&
((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -411,7 +439,7 @@
void ClawMotor::RunIteration(const control_loops::ClawGroup::Goal *goal,
const control_loops::ClawGroup::Position *position,
control_loops::ClawGroup::Output *output,
- ::aos::control_loops::Status *status) {
+ control_loops::ClawGroup::Status *status) {
constexpr double dt = 0.01;
// Disable the motors now so that all early returns will return with the
@@ -420,6 +448,15 @@
output->top_claw_voltage = 0;
output->bottom_claw_voltage = 0;
output->intake_voltage = 0;
+ output->tusk_voltage = 0;
+ }
+
+ if (goal) {
+ if (::std::isnan(goal->bottom_angle) ||
+ ::std::isnan(goal->separation_angle) || ::std::isnan(goal->intake) ||
+ ::std::isnan(goal->centering)) {
+ return;
+ }
}
if (reset()) {
@@ -427,10 +464,6 @@
bottom_claw_.Reset(position->bottom);
}
- if (::aos::robot_state.get() == nullptr) {
- return;
- }
-
const frc971::constants::Values &values = constants::GetValues();
if (position) {
@@ -454,25 +487,33 @@
initial_separation_ =
top_claw_.absolute_position() - bottom_claw_.absolute_position();
}
- LOG(DEBUG, "Claw position is (top: %f bottom: %f\n",
- top_claw_.absolute_position(), bottom_claw_.absolute_position());
+ LOG_STRUCT(DEBUG, "absolute position",
+ ClawPositionToLog(top_claw_.absolute_position(),
+ bottom_claw_.absolute_position()));
}
- const bool autonomous = ::aos::robot_state->autonomous;
- const bool enabled = ::aos::robot_state->enabled;
+ bool autonomous, enabled;
+ if (::aos::robot_state.get() == nullptr) {
+ autonomous = true;
+ enabled = false;
+ } else {
+ autonomous = ::aos::robot_state->autonomous;
+ enabled = ::aos::robot_state->enabled;
+ }
double bottom_claw_velocity_ = 0.0;
double top_claw_velocity_ = 0.0;
- if ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
- bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- (autonomous &&
- ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
- top_claw_.zeroing_state() ==
- ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
- (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
- bottom_claw_.zeroing_state() ==
- ZeroedStateFeedbackLoop::DISABLED_CALIBRATION)))) {
+ if (goal != NULL &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
+ bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
+ (autonomous &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))))) {
// Ready to use the claw.
// Limit the goals here.
bottom_claw_goal_ = goal->bottom_angle;
@@ -483,9 +524,9 @@
mode_ = READY;
} else if (top_claw_.zeroing_state() !=
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
bottom_claw_.zeroing_state() !=
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
// Time to fine tune the zero.
// Limit the goals here.
if (!enabled) {
@@ -527,23 +568,22 @@
mode_ = PREP_FINE_TUNE_BOTTOM;
}
- if (bottom_claw_.calibration().value()) {
- if (bottom_claw_.calibration().posedge_count_changed() &&
- position) {
- // do calibration
- bottom_claw_.SetCalibration(
- position->bottom.posedge_value,
- values.claw.lower_claw.calibration.lower_angle);
- bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
- // calibrated so we are done fine tuning bottom
- doing_calibration_fine_tune_ = false;
- LOG(DEBUG, "Calibrated the bottom correctly!\n");
- } else {
- doing_calibration_fine_tune_ = false;
- bottom_claw_goal_ = values.claw.start_fine_tune_pos;
- top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
- mode_ = PREP_FINE_TUNE_BOTTOM;
- }
+ if (position && bottom_claw_.SawFilteredPosedge(
+ bottom_claw_.calibration(), bottom_claw_.front(),
+ bottom_claw_.back())) {
+ // do calibration
+ bottom_claw_.SetCalibration(
+ position->bottom.posedge_value,
+ values.claw.lower_claw.calibration.lower_angle);
+ bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calibrated so we are done fine tuning bottom
+ doing_calibration_fine_tune_ = false;
+ LOG(DEBUG, "Calibrated the bottom correctly!\n");
+ } else if (bottom_claw_.calibration().last_value()) {
+ doing_calibration_fine_tune_ = false;
+ bottom_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
+ mode_ = PREP_FINE_TUNE_BOTTOM;
} else {
LOG(DEBUG, "Fine tuning\n");
}
@@ -583,23 +623,23 @@
LOG(DEBUG, "Found a limit, starting over.\n");
mode_ = PREP_FINE_TUNE_TOP;
}
- if (top_claw_.calibration().value()) {
- if (top_claw_.calibration().posedge_count_changed() &&
- position) {
- // do calibration
- top_claw_.SetCalibration(
- position->top.posedge_value,
- values.claw.upper_claw.calibration.lower_angle);
- top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
- // calibrated so we are done fine tuning top
- doing_calibration_fine_tune_ = false;
- LOG(DEBUG, "Calibrated the top correctly!\n");
- } else {
- doing_calibration_fine_tune_ = false;
- top_claw_goal_ = values.claw.start_fine_tune_pos;
- top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
- mode_ = PREP_FINE_TUNE_TOP;
- }
+
+ if (position &&
+ top_claw_.SawFilteredPosedge(top_claw_.calibration(),
+ top_claw_.front(), top_claw_.back())) {
+ // do calibration
+ top_claw_.SetCalibration(
+ position->top.posedge_value,
+ values.claw.upper_claw.calibration.lower_angle);
+ top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calibrated so we are done fine tuning top
+ doing_calibration_fine_tune_ = false;
+ LOG(DEBUG, "Calibrated the top correctly!\n");
+ } else if (top_claw_.calibration().last_value()) {
+ doing_calibration_fine_tune_ = false;
+ top_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
+ mode_ = PREP_FINE_TUNE_TOP;
}
}
// now set the bottom claw to track
@@ -609,8 +649,8 @@
doing_calibration_fine_tune_ = false;
if (!was_enabled_ && enabled) {
if (position) {
- top_claw_goal_ = position->top.position;
- bottom_claw_goal_ = position->bottom.position;
+ top_claw_goal_ = position->top.position + top_claw_.offset();
+ bottom_claw_goal_ = position->bottom.position + bottom_claw_.offset();
initial_separation_ =
position->top.position - position->bottom.position;
} else {
@@ -644,18 +684,26 @@
}
}
- if (enabled) {
- top_claw_.SetCalibrationOnEdge(
- values.claw.upper_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
- bottom_claw_.SetCalibrationOnEdge(
- values.claw.lower_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
- } else {
- // TODO(austin): Only calibrate on the predetermined edge.
- // We might be able to just ignore this since the backlash is soooo low. :)
- top_claw_.SetCalibrationOnEdge(
- values.claw.upper_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
- bottom_claw_.SetCalibrationOnEdge(
- values.claw.lower_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ if (position) {
+ if (enabled) {
+ top_claw_.SetCalibrationOnEdge(
+ values.claw.upper_claw,
+ ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.claw.lower_claw,
+ ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ } else {
+ // TODO(austin): Only calibrate on the predetermined edge.
+ // We might be able to just ignore this since the backlash is soooo
+ // low.
+ // :)
+ top_claw_.SetCalibrationOnEdge(
+ values.claw.upper_claw,
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.claw.lower_claw,
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ }
}
mode_ = UNKNOWN_LOCATION;
}
@@ -672,8 +720,8 @@
if (position != nullptr) {
separation = position->top.position - position->bottom.position;
}
- LOG(DEBUG, "Goal is %f (bottom) %f, separation is %f\n", claw_.R(0, 0),
- claw_.R(1, 0), separation);
+ LOG_STRUCT(DEBUG, "actual goal",
+ ClawGoalToLog(claw_.R(0, 0), claw_.R(1, 0), separation));
// Only cap power when one of the halves of the claw is moving slowly and
// could wind up.
@@ -709,8 +757,9 @@
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
U = claw_.K() * (R - claw_.X_hat);
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
- LOG(DEBUG, "Uncapped is %f, max is %f, difference is %f\n",
+ LOG(DEBUG, "Moving the goal by %f to prevent windup."
+ " Uncapped is %f, max is %f, difference is %f\n",
+ dx,
claw_.uncapped_average_voltage(), values.claw.max_zeroing_voltage,
(claw_.uncapped_average_voltage() -
values.claw.max_zeroing_voltage));
@@ -735,8 +784,19 @@
}
if (output) {
+ if (goal) {
+ //setup the intake
+ output->intake_voltage =
+ (goal->intake > 12.0) ? 12 : (goal->intake < -12.0) ? -12.0
+ : goal->intake;
+ output->tusk_voltage = goal->centering;
+ output->tusk_voltage =
+ (goal->centering > 12.0) ? 12 : (goal->centering < -12.0)
+ ? -12.0
+ : goal->centering;
+ }
output->top_claw_voltage = claw_.U(1, 0);
- output->bottom_claw_voltage = claw_.U(0, 0);
+ output->bottom_claw_voltage = claw_.U(0, 0);
if (output->top_claw_voltage > kMaxVoltage) {
output->top_claw_voltage = kMaxVoltage;
@@ -750,10 +810,41 @@
output->bottom_claw_voltage = -kMaxVoltage;
}
}
- status->done = false;
- was_enabled_ = ::aos::robot_state->enabled;
+ status->bottom = bottom_absolute_position();
+ status->separation = top_absolute_position() - bottom_absolute_position();
+ status->bottom_velocity = claw_.X_hat(2, 0);
+ status->separation_velocity = claw_.X_hat(3, 0);
+
+ if (goal) {
+ bool bottom_done =
+ ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.020;
+ bool bottom_velocity_done = ::std::abs(status->bottom_velocity) < 0.2;
+ bool separation_done =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.020;
+ bool separation_done_with_ball =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.06;
+ status->done = is_ready() && separation_done && bottom_done && bottom_velocity_done;
+ status->done_with_ball =
+ is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
+ } else {
+ status->done = status->done_with_ball = false;
+ }
+
+ status->zeroed = is_ready();
+ status->zeroed_for_auto =
+ (top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+
+ was_enabled_ = enabled;
}
} // namespace control_loops
} // namespace frc971
+
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index f71bf6b..bdce229 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -31,6 +31,7 @@
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/aos/build/externals.gyp:libcdd',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'claw_loop',
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 48650b4..dab1dca 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -75,10 +75,6 @@
void Reset(const HalfClawPosition &claw);
- bool ready() {
- return front_.ready() && calibration_.ready() && back_.ready();
- }
-
double absolute_position() const { return encoder() + offset(); }
const HallEffectTracker &front() const { return front_; }
@@ -107,6 +103,14 @@
bool GetPositionOfEdge(const constants::Values::Claws::Claw &claw,
double *edge_encoder, double *edge_angle);
+ bool SawFilteredPosedge(const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB);
+
+ bool SawFilteredNegedge(const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB);
+
#undef COUNT_SETTER_GETTER
protected:
@@ -121,7 +125,6 @@
JointZeroingState zeroing_state_;
double posedge_value_;
double negedge_value_;
- double last_edge_value_;
double min_hall_effect_on_angle_;
double max_hall_effect_on_angle_;
double min_hall_effect_off_angle_;
@@ -132,11 +135,16 @@
double last_off_encoder_;
bool any_triggered_last_;
+ const HallEffectTracker* posedge_filter_ = nullptr;
+ const HallEffectTracker* negedge_filter_ = nullptr;
+
private:
// Does the edges of 1 sensor for GetPositionOfEdge.
bool DoGetPositionOfEdge(const constants::Values::Claws::AnglePair &angles,
double *edge_encoder, double *edge_angle,
const HallEffectTracker &sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB,
const char *hall_effect_name);
};
@@ -164,8 +172,8 @@
JointZeroingState zeroing_state);
};
-class ClawMotor
- : public aos::control_loops::ControlLoop<control_loops::ClawGroup> {
+class ClawMotor : public aos::control_loops::ControlLoop<
+ control_loops::ClawGroup, true, true, false> {
public:
explicit ClawMotor(control_loops::ClawGroup *my_claw =
&control_loops::claw_queue_group);
@@ -201,7 +209,7 @@
virtual void RunIteration(const control_loops::ClawGroup::Goal *goal,
const control_loops::ClawGroup::Position *position,
control_loops::ClawGroup::Output *output,
- ::aos::control_loops::Status *status);
+ control_loops::ClawGroup::Status *status);
double top_absolute_position() const {
return claw_.X_hat(1, 0) + claw_.X_hat(0, 0);
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 333f83c..116a182 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -31,7 +31,10 @@
double bottom_angle;
// How much higher the top claw is.
double separation_angle;
- bool intake;
+ // top claw intake roller
+ double intake;
+ // bottom claw tusk centering
+ double centering;
};
message Position {
@@ -48,10 +51,38 @@
double tusk_voltage;
};
+ message Status {
+ // True if zeroed enough for the current period (autonomous or teleop).
+ bool zeroed;
+ // True if zeroed as much as we will force during autonomous.
+ bool zeroed_for_auto;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ bool done;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ // seperation allowance much wider as a ball may be included
+ bool done_with_ball;
+ // Dump the values of the state matrix.
+ double bottom;
+ double bottom_velocity;
+ double separation;
+ double separation_velocity;
+ };
+
queue Goal goal;
queue Position position;
queue Output output;
- queue aos.control_loops.Status status;
+ queue Status status;
};
queue_group ClawGroup claw_queue_group;
+
+struct ClawPositionToLog {
+ double top;
+ double bottom;
+};
+
+struct ClawGoalToLog {
+ double bottom_pos;
+ double bottom_vel;
+ double separation;
+};
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index e35cd7f..8704ad0 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -231,6 +231,7 @@
v.claw.claw_max_separation);
EXPECT_GE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
v.claw.claw_min_separation);
+ ::aos::time::Time::IncrementMockTime(::aos::time::Time::InMS(10.0));
}
// The whole claw.
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> claw_plant_;
@@ -283,6 +284,7 @@
.reader_pid(254)
.cape_resets(5)
.Send();
+ ::aos::time::Time::EnableMockTime(::aos::time::Time::InSeconds(0.0));
}
void SendDSPacket(bool enabled) {
@@ -309,9 +311,23 @@
virtual ~ClawTest() {
::aos::robot_state.Clear();
::bbb::sensor_generation.Clear();
+ ::aos::time::Time::DisableMockTime();
}
};
+TEST_F(ClawTest, HandlesNAN) {
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(::std::nan(""))
+ .separation_angle(::std::nan(""))
+ .Send();
+ for (int i = 0; i < 500; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+}
+
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ClawTest, ZerosCorrectly) {
claw_queue_group.goal.MakeWithBuilder()
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index babbb04..106491d 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -25,9 +25,9 @@
StateFeedbackController<4, 2, 2> MakeClawController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.42518438347, 4.71027737605e-16, -1.42518438347, 1.04485564063, 30.6346010502, 1.00485917356e-14, -30.6346010502, 2.04727497147;
+ L << 1.48518438347, 2.30607329869e-16, -1.48518438347, 1.10485564063, 34.6171964667, 5.33831435952e-15, -34.6171964667, 3.52560374486;
Eigen::Matrix<double, 2, 4> K;
- K << 50.0, 0.0, 1.0, 0.0, 23.5668757858, 300.0, -0.88836718554, 1.1;
+ K << 104.272994613, 0.0, 1.72618753001, 0.0, 49.1477742369, 129.930293084, -0.546087759204, 0.551235956004;
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 b54949d..c8b943c 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -11,15 +11,16 @@
#include "aos/controls/polytope.h"
#include "aos/common/commonmath.h"
#include "aos/common/logging/queue_logging.h"
+#include "aos/common/logging/matrix_logging.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/other_sensors.q.h"
#include "frc971/constants.h"
-using frc971::sensors::gyro;
+using frc971::sensors::gyro_reading;
namespace frc971 {
namespace control_loops {
@@ -65,8 +66,36 @@
SetRawPosition(left, right);
}
+ void SetExternalMotors(double left_voltage, double right_voltage) {
+ loop_->U << left_voltage, right_voltage;
+ }
+
void Update(bool stop_motors) {
- loop_->Update(stop_motors);
+ if (_control_loop_driving) {
+ loop_->Update(stop_motors);
+ } else {
+ if (stop_motors) {
+ loop_->U.setZero();
+ loop_->U_uncapped.setZero();
+ }
+ loop_->UpdateObserver();
+ }
+ ::Eigen::Matrix<double, 4, 1> E = loop_->R - loop_->X_hat;
+ LOG_MATRIX(DEBUG, "E", E);
+ }
+
+ double GetEstimatedRobotSpeed() {
+ // lets just call the average of left and right velocities close enough
+ return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
+ }
+
+ double GetEstimatedLeftEncoder() {
+ // lets just call the average of left and right velocities close enough
+ return loop_->X_hat(0, 0);
+ }
+
+ double GetEstimatedRightEncoder() {
+ return loop_->X_hat(2, 0);
}
void SendMotors(Drivetrain::Output *output) {
@@ -75,10 +104,6 @@
output->right_voltage = loop_->U(1, 0);
}
}
- void PrintMotors() const {
- ::Eigen::Matrix<double, 4, 1> E = loop_->R - loop_->X_hat;
- LOG(DEBUG, "E[0, 0]: %f E[1, 0] %f E[2, 0] %f E[3, 0] %f\n", E(0, 0), E(1, 0), E(2, 0), E(3, 0));
- }
private:
::std::unique_ptr<StateFeedbackLoop<4, 2, 2>> loop_;
@@ -137,8 +162,6 @@
/*[*/ 12 /*]]*/).finished()),
loop_(new StateFeedbackLoop<2, 2, 2>(
constants::GetValues().make_v_drivetrain_loop())),
- 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),
@@ -154,18 +177,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);
@@ -200,6 +229,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_;
@@ -207,8 +237,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 {
@@ -252,6 +284,7 @@
}
}
void SetPosition(const Drivetrain::Position *position) {
+ const auto &values = constants::GetValues();
if (position == NULL) {
++stale_count_;
} else {
@@ -264,9 +297,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);
@@ -276,7 +314,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);
@@ -288,16 +327,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;
}
@@ -355,6 +394,7 @@
}
void Update() {
+ const auto &values = constants::GetValues();
// TODO(austin): Observer for the current velocity instead of difference
// calculations.
++counter_;
@@ -365,9 +405,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;
@@ -376,7 +418,6 @@
// shift.
if (IsInGear(left_gear_)) {
logging.left_in_gear = true;
- left_cim_->X_hat(0, 0) = left_motor_speed;
} else {
logging.left_in_gear = false;
}
@@ -384,7 +425,6 @@
logging.left_velocity = current_left_velocity;
if (IsInGear(right_gear_)) {
logging.right_in_gear = true;
- right_cim_->X_hat(0, 0) = right_motor_speed;
} else {
logging.right_in_gear = false;
}
@@ -451,23 +491,19 @@
} 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;
-
- 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_left(0, 0) = left_motor_speed;
R_right(0, 0) = right_motor_speed;
- 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);
+
+ const double wiggle =
+ (static_cast<double>((counter_ % 20) / 10) - 0.5) * 5.0;
+
+ loop_->U(0, 0) = ::aos::Clip(
+ (R_left / Kv)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
+ loop_->U(1, 0) = ::aos::Clip(
+ (R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
loop_->U *= 12.0 / position_.battery_voltage;
}
}
@@ -485,8 +521,6 @@
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_;
const double ttrust_;
double wheel_;
@@ -516,7 +550,7 @@
void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
const Drivetrain::Position *position,
Drivetrain::Output *output,
- Drivetrain::Status * /*status*/) {
+ Drivetrain::Status * status) {
// TODO(aschuh): These should be members of the class.
static DrivetrainMotorsSS dt_closedloop;
static PolyDrivetrain dt_openloop;
@@ -526,6 +560,7 @@
LOG_INTERVAL(no_position_);
bad_pos = true;
}
+ no_position_.Print();
double wheel = goal->steering;
double throttle = goal->throttle;
@@ -541,22 +576,43 @@
if (!bad_pos) {
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
- if (gyro.FetchLatest()) {
- LOG_STRUCT(DEBUG, "using", *gyro);
- dt_closedloop.SetPosition(left_encoder, right_encoder, gyro->angle,
- control_loop_driving);
+ if (gyro_reading.FetchLatest()) {
+ LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+ dt_closedloop.SetPosition(left_encoder, right_encoder,
+ gyro_reading->angle, control_loop_driving);
} else {
dt_closedloop.SetRawPosition(left_encoder, right_encoder);
}
}
dt_openloop.SetPosition(position);
- dt_closedloop.Update(output == NULL);
dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
dt_openloop.Update();
+
if (control_loop_driving) {
+ dt_closedloop.Update(output == NULL);
dt_closedloop.SendMotors(output);
} else {
dt_openloop.SendMotors(output);
+ if (output) {
+ dt_closedloop.SetExternalMotors(output->left_voltage,
+ output->right_voltage);
+ }
+ dt_closedloop.Update(output == NULL);
+ }
+
+ // set the output status of the controll loop state
+ if (status) {
+ bool done = false;
+ if (goal) {
+ done = ((::std::abs(goal->left_goal -
+ dt_closedloop.GetEstimatedLeftEncoder()) <
+ constants::GetValues().drivetrain_done_distance) &&
+ (::std::abs(goal->right_goal -
+ dt_closedloop.GetEstimatedRightEncoder()) <
+ constants::GetValues().drivetrain_done_distance));
+ }
+ status->is_done = done;
+ status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index 5941f63..bf73d9f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -22,9 +22,7 @@
'type': 'static_library',
'sources': [
'polydrivetrain_dog_motor_plant.cc',
- 'polydrivetrain_clutch_motor_plant.cc',
'drivetrain_dog_motor_plant.cc',
- 'drivetrain_clutch_motor_plant.cc',
],
'dependencies': [
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
@@ -50,6 +48,7 @@
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/common/util/util.gyp:log_interval',
'<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/logging/logging.gyp:matrix_logging',
],
'export_dependent_settings': [
'<(DEPTH)/aos/build/externals.gyp:libcdd',
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 443282c..50d9dbf 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -45,12 +45,13 @@
message Output {
float left_voltage;
float right_voltage;
- bool left_high;
- bool right_high;
+ bool left_high;
+ bool right_high;
};
message Status {
bool is_done;
+ double robot_speed;
};
queue Goal goal;
diff --git a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc
deleted file mode 100644
index b3aa088..0000000
--- a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeClutchDrivetrainPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00876671940282, 0.0, 0.000204905465153, 0.0, 0.764245148008, 0.0, 0.0373841350548, 0.0, 0.000204905465153, 1.0, 0.00876671940282, 0.0, 0.0373841350548, 0.0, 0.764245148008;
- Eigen::Matrix<double, 4, 2> B;
- B << 0.000157874070659, -2.62302512161e-05, 0.0301793267864, -0.00478559834045, -2.62302512161e-05, 0.000157874070659, -0.00478559834045, 0.0301793267864;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 0, 0, 0, 0, 1, 0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<4, 2, 2> MakeClutchDrivetrainController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.60424514801, 0.0373841350548, 53.4463554671, 4.58647914599, 0.0373841350548, 1.60424514801, 4.58647914599, 53.4463554671;
- Eigen::Matrix<double, 2, 4> K;
- K << 292.330461448, 10.4890095334, -85.5980253252, -0.517234397951, -58.0206391358, -1.5636023242, 153.384904309, 5.5616531565;
- return StateFeedbackController<4, 2, 2>(L, K, MakeClutchDrivetrainPlantCoefficients());
-}
-
-StateFeedbackPlant<4, 2, 2> MakeClutchDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeClutchDrivetrainPlantCoefficients());
- return StateFeedbackPlant<4, 2, 2>(plants);
-}
-
-StateFeedbackLoop<4, 2, 2> MakeClutchDrivetrainLoop() {
- ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeClutchDrivetrainController());
- return StateFeedbackLoop<4, 2, 2>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h
deleted file mode 100644
index e9444e6..0000000
--- a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeClutchDrivetrainPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeClutchDrivetrainController();
-
-StateFeedbackPlant<4, 2, 2> MakeClutchDrivetrainPlant();
-
-StateFeedbackLoop<4, 2, 2> MakeClutchDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index 7822056..231eefb 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -7,11 +7,11 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDogDrivetrainPlantCoefficients() {
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00923787174605, 0.0, 0.000131162317098, 0.0, 0.851672729447, 0.0, 0.0248457251406, 0.0, 0.000131162317098, 1.0, 0.00923787174605, 0.0, 0.0248457251406, 0.0, 0.851672729447;
+ A << 1.0, 0.00860955515291, 0.0, 0.000228184998733, 0.0, 0.735841675858, 0.0, 0.0410810558113, 0.0, 0.000228184998733, 1.0, 0.00860955515291, 0.0, 0.0410810558113, 0.0, 0.735841675858;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000126364935405, -2.17474127771e-05, 0.0245934537462, -0.00411955394149, -2.17474127771e-05, 0.000126364935405, -0.00411955394149, 0.0245934537462;
+ B << 0.000272244648044, -4.46778919705e-05, 0.0517213538779, -0.00804353916233, -4.46778919705e-05, 0.000272244648044, -0.00804353916233, 0.0517213538779;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -23,23 +23,23 @@
return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<4, 2, 2> MakeDogDrivetrainController() {
+StateFeedbackController<4, 2, 2> MakeDrivetrainController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.69167272945, 0.0248457251406, 64.4706646869, 3.2355304474, 0.0248457251406, 1.69167272945, 3.2355304474, 64.4706646869;
+ L << 1.57584167586, 0.0410810558113, 50.0130674801, 4.93325200717, 0.0410810558113, 1.57584167586, 4.93325200717, 50.0130674801;
Eigen::Matrix<double, 2, 4> K;
- K << 248.918529922, 14.4460993245, 41.6953764051, 3.43594323497, 41.6953764051, 3.43594323497, 248.918529922, 14.4460993245;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDogDrivetrainPlantCoefficients());
+ K << 128.210620632, 6.93828382074, 5.11036686771, 0.729493080206, 5.1103668677, 0.729493080206, 128.210620632, 6.93828382074;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainPlantCoefficients());
}
-StateFeedbackPlant<4, 2, 2> MakeDogDrivetrainPlant() {
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDogDrivetrainPlantCoefficients());
+ plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainPlantCoefficients());
return StateFeedbackPlant<4, 2, 2>(plants);
}
-StateFeedbackLoop<4, 2, 2> MakeDogDrivetrainLoop() {
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDogDrivetrainController());
+ controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainController());
return StateFeedbackLoop<4, 2, 2>(controllers);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
index ba3d584..3829e9a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
@@ -6,13 +6,13 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDogDrivetrainPlantCoefficients();
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients();
-StateFeedbackController<4, 2, 2> MakeDogDrivetrainController();
+StateFeedbackController<4, 2, 2> MakeDrivetrainController();
-StateFeedbackPlant<4, 2, 2> MakeDogDrivetrainPlant();
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
-StateFeedbackLoop<4, 2, 2> MakeDogDrivetrainLoop();
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 73f8525..0827fec 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -13,7 +13,7 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/other_sensors.q.h"
using ::aos::time::Time;
@@ -50,7 +50,7 @@
// TODO(aschuh) Do we want to test the clutch one too?
DrivetrainSimulation()
: drivetrain_plant_(
- new StateFeedbackPlant<4, 2, 2>(MakeDogDrivetrainPlant())),
+ new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
my_drivetrain_loop_(".frc971.control_loops.drivetrain",
0x8a8dde77, ".frc971.control_loops.drivetrain.goal",
".frc971.control_loops.drivetrain.position",
@@ -134,7 +134,7 @@
.reader_pid(254)
.cape_resets(5)
.Send();
- ::frc971::sensors::gyro.Clear();
+ ::frc971::sensors::gyro_reading.Clear();
SendDSPacket(true);
}
@@ -158,7 +158,7 @@
virtual ~DrivetrainTest() {
::aos::robot_state.Clear();
- ::frc971::sensors::gyro.Clear();
+ ::frc971::sensors::gyro_reading.Clear();
::bbb::sensor_generation.Clear();
}
};
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc
deleted file mode 100644
index 82962f0..0000000
--- a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc
+++ /dev/null
@@ -1,125 +0,0 @@
-#include "frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowLowPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.764245148008, 0.0373841350548, 0.0373841350548, 0.764245148008;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0301793267864, -0.00478559834045, -0.00478559834045, 0.0301793267864;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowHighPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.763446428918, 0.00494258902788, 0.042202491067, 0.968991856576;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0302815719967, -0.00184882243178, -0.00540240320973, 0.011598890947;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighLowPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.968991856576, 0.042202491067, 0.00494258902788, 0.763446428918;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.011598890947, -0.00540240320973, -0.00184882243178, 0.0302815719967;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighHighPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.968881997557, 0.00555499847336, 0.00555499847336, 0.968881997557;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0116399847578, -0.0020779000091, -0.0020779000091, 0.0116399847578;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowLowController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.744245148008, 0.0373841350548, 0.0373841350548, 0.744245148008;
- Eigen::Matrix<double, 2, 2> K;
- K << 5.78417881324, 2.15594244513, 2.15594244513, 5.78417881324;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainLowLowPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowHighController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.742469928763, 0.0421768815418, 0.0421768815418, 0.949968356732;
- Eigen::Matrix<double, 2, 2> K;
- K << 5.78418649682, 2.16715237139, 6.33258809821, 32.8220766317;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainLowHighPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighLowController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.954934950673, 0.00591596315544, 0.00591596315544, 0.737503334821;
- Eigen::Matrix<double, 2, 2> K;
- K << 32.8220766317, 6.33258809821, 2.16715237139, 5.78418649682;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainHighLowPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighHighController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.948881997557, 0.00555499847336, 0.00555499847336, 0.948881997557;
- Eigen::Matrix<double, 2, 2> K;
- K << 32.8220767657, 6.33643373411, 6.33643373411, 32.8220767657;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainHighHighPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 2, 2> MakeVClutchDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainHighHighPlantCoefficients());
- return StateFeedbackPlant<2, 2, 2>(plants);
-}
-
-StateFeedbackLoop<2, 2, 2> MakeVClutchDrivetrainLoop() {
- ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainHighHighController());
- return StateFeedbackLoop<2, 2, 2>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h
deleted file mode 100644
index 85c87c1..0000000
--- a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowLowPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowLowController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowHighPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowHighController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighLowPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighLowController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighHighPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighHighController();
-
-StateFeedbackPlant<2, 2, 2> MakeVClutchDrivetrainPlant();
-
-StateFeedbackLoop<2, 2, 2> MakeVClutchDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index b3d4277..b31cf85 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -7,11 +7,11 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowLowPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.851672729447, 0.0248457251406, 0.0248457251406, 0.851672729447;
+ A << 0.735841675858, 0.0410810558113, 0.0410810558113, 0.735841675858;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0245934537462, -0.00411955394149, -0.00411955394149, 0.0245934537462;
+ B << 0.0517213538779, -0.00804353916233, -0.00804353916233, 0.0517213538779;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -23,11 +23,11 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowHighPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.851389310398, 0.00553670185935, 0.0264939835067, 0.967000817219;
+ A << 0.735048848179, 0.0131811893199, 0.045929121897, 0.915703853642;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0246404461385, -0.00200815724925, -0.00439284398274, 0.0119687766843;
+ B << 0.0518765869984, -0.00481755802263, -0.00899277497558, 0.0308091755839;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -39,11 +39,11 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighLowPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.967000817219, 0.0264939835067, 0.00553670185935, 0.851389310398;
+ A << 0.915703853642, 0.045929121897, 0.0131811893199, 0.735048848179;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0119687766843, -0.00439284398274, -0.00200815724925, 0.0246404461385;
+ B << 0.0308091755839, -0.00899277497558, -0.00481755802263, 0.0518765869984;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -55,11 +55,11 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighHighPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.966936300149, 0.00589655754287, 0.00589655754287, 0.966936300149;
+ A << 0.915439806567, 0.0146814193986, 0.0146814193986, 0.915439806567;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0119921769728, -0.00213867661221, -0.00213867661221, 0.0119921769728;
+ B << 0.0309056814511, -0.00536587314624, -0.00536587314624, 0.0309056814511;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -71,53 +71,53 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowLowController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.831672729447, 0.0248457251406, 0.0248457251406, 0.831672729447;
+ L << 0.715841675858, 0.0410810558113, 0.0410810558113, 0.715841675858;
Eigen::Matrix<double, 2, 2> K;
- K << 10.7028500331, 2.80305051463, 2.80305051463, 10.7028500331;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainLowLowPlantCoefficients());
+ K << 2.81809403994, 1.23253744933, 1.23253744933, 2.81809403994;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowHighController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.831852326508, 0.0264837489415, 0.0264837489415, 0.946537801108;
+ L << 0.715885457343, 0.0459077351335, 0.0459077351335, 0.894867244478;
Eigen::Matrix<double, 2, 2> K;
- K << 10.7028511964, 2.80768406175, 6.14180888507, 31.6936764099;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainLowHighPlantCoefficients());
+ K << 2.81810038978, 1.23928174475, 2.31332592354, 10.6088017388;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighLowController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.951097545753, 0.0063707209266, 0.0063707209266, 0.827292581863;
+ L << 0.902328849033, 0.014581304798, 0.014581304798, 0.708423852788;
Eigen::Matrix<double, 2, 2> K;
- K << 31.6936764099, 6.14180888507, 2.80768406175, 10.7028511964;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainHighLowPlantCoefficients());
+ K << 10.6088017388, 2.31332592354, 1.23928174475, 2.81810038978;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighHighController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.946936300149, 0.00589655754287, 0.00589655754287, 0.946936300149;
+ L << 0.895439806567, 0.0146814193986, 0.0146814193986, 0.895439806567;
Eigen::Matrix<double, 2, 2> K;
- K << 31.6936764663, 6.14392885659, 6.14392885659, 31.6936764663;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainHighHighPlantCoefficients());
+ K << 10.6088022944, 2.31694961514, 2.31694961514, 10.6088022944;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
-StateFeedbackPlant<2, 2, 2> MakeVDogDrivetrainPlant() {
+StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant() {
::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainHighHighPlantCoefficients());
+ plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients());
+ plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients());
+ plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients());
return StateFeedbackPlant<2, 2, 2>(plants);
}
-StateFeedbackLoop<2, 2, 2> MakeVDogDrivetrainLoop() {
+StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop() {
::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainHighHighController());
+ controllers[0] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController());
+ controllers[1] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController());
+ controllers[2] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController());
+ controllers[3] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController());
return StateFeedbackLoop<2, 2, 2>(controllers);
}
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
index 613bff4..27aa4dd 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
@@ -6,25 +6,25 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowLowPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowLowController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController();
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowHighPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowHighController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController();
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighLowPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighLowController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController();
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighHighPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighHighController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController();
-StateFeedbackPlant<2, 2, 2> MakeVDogDrivetrainPlant();
+StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant();
-StateFeedbackLoop<2, 2, 2> MakeVDogDrivetrainLoop();
+StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop();
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/hall_effect_tracker.h b/frc971/control_loops/hall_effect_tracker.h
index 7e6617c..b63a34b 100644
--- a/frc971/control_loops/hall_effect_tracker.h
+++ b/frc971/control_loops/hall_effect_tracker.h
@@ -20,47 +20,42 @@
bool negedge_count_changed() const { return negedges_.count_changed(); }
bool value() const { return value_; }
+ bool last_value() const { return last_value_; }
void Update(const HallEffectStruct &position) {
+ last_value_ = value_;
value_ = position.current;
posedges_.update(position.posedge_count);
negedges_.update(position.negedge_count);
}
- void Reset() {
- posedges_.Reset();
- negedges_.Reset();
+ void Reset(const HallEffectStruct &position) {
+ posedges_.Reset(position.posedge_count);
+ negedges_.Reset(position.negedge_count);
+ value_ = position.current;
+ last_value_ = position.current;
}
- bool ready() { return posedges_.ready() && negedges_.ready(); }
-
private:
class {
public:
void update(int32_t count) {
- if (first_) {
- count_ = count;
- LOG(DEBUG, "First time through the hall effect, resetting\n");
- }
previous_count_ = count_;
count_ = count;
- first_ = false;
}
- void Reset() { first_ = true; }
+ void Reset(int32_t count) { count_ = count; }
- bool count_changed() const { return !first_ && previous_count_ != count_; }
+ bool count_changed() const { return previous_count_ != count_; }
int32_t count() const { return count_; }
- bool ready() { return !first_; }
-
private:
int32_t count_ = 0;
int32_t previous_count_ = 0;
- bool first_ = true;
} posedges_, negedges_;
bool value_ = false;
+ bool last_value_ = false;
};
} // namespace frc971
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index aacf31e..ca69a2b 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -23,6 +23,7 @@
# measured from CAD
self.J_top = 0.3
self.J_bottom = 0.9
+
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
@@ -144,8 +145,8 @@
print "eigenvalues"
print numpy.linalg.eig(F)[0]
- self.rpl = .05
- self.ipl = 0.008
+ self.rpl = .02
+ self.ipl = 0.004
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl,
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 90faf9f..a103c79 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -1,8 +1,21 @@
import controls
import numpy
+class Constant(object):
+ def __init__ (self, name, formatt, value):
+ self.name = name
+ self.formatt = formatt
+ self.value = value
+ self.formatToType = {}
+ self.formatToType['%f'] = "double";
+ self.formatToType['%d'] = "int";
+ def __str__ (self):
+ return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
+ (self.formatToType[self.formatt], self.name, self.value)
+
+
class ControlLoopWriter(object):
- def __init__(self, gain_schedule_name, loops, namespaces=None):
+ def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
"""Constructs a control loop writer.
Args:
@@ -24,6 +37,16 @@
self._namespace_end = '\n'.join(
['} // namespace %s' % name for name in reversed(self._namespaces)])
+
+ self._constant_list = []
+
+ def AddConstant(self, constant):
+ """Adds a constant to write.
+
+ Args:
+ constant: Constant, the constant to add to the header.
+ """
+ self._constant_list.append(constant)
def _TopDirectory(self):
return self._namespaces[0]
@@ -74,6 +97,10 @@
fd.write('\n')
fd.write(self._namespace_start)
+
+ for const in self._constant_list:
+ fd.write(str(const))
+
fd.write('\n\n')
for loop in self._loops:
fd.write(loop.DumpPlantHeader())
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 001fd1e..3d6e441 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -1,6 +1,7 @@
#!/usr/bin/python
import control_loop
+import controls
import numpy
import sys
from matplotlib import pylab
@@ -89,7 +90,7 @@
self.Gr = self.G_high
# Control loop time step
self.dt = 0.01
-
+
# These describe the way that a given side of a robot will be influenced
# by the other side. Units of 1 / kg.
self.msp = 1.0 / self.m + self.rb * self.rb / self.J
@@ -118,13 +119,29 @@
self.D = numpy.matrix([[0, 0],
[0, 0]])
+ #print "THE NUMBER I WANT" + str(numpy.linalg.inv(self.A_continuous) * -self.B_continuous * numpy.matrix([[12.0], [12.0]]))
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
# Poles from last year.
self.hp = 0.65
self.lp = 0.83
- self.PlaceControllerPoles([self.hp, self.hp, self.lp, self.lp])
+ self.PlaceControllerPoles([self.hp, self.lp, self.hp, self.lp])
+ print self.K
+ q_pos = 0.07
+ q_vel = 1.0
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel ** 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 self.A
+ print self.B
+ print self.K
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
self.hlp = 0.07
self.llp = 0.09
@@ -200,6 +217,7 @@
#pylab.show()
# Write the generated constants out to a file.
+ print "Output one"
drivetrain = Drivetrain()
if len(argv) != 5:
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 280db16..f2dfdbe 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -396,10 +396,10 @@
print "Expected .h file name and .cc file name"
else:
dog_loop_writer = control_loop.ControlLoopWriter(
- "VDogDrivetrain", [vdrivetrain.drivetrain_low_low,
- vdrivetrain.drivetrain_low_high,
- vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high])
+ "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index 89f682a..69f2599 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -30,6 +30,9 @@
self.Kt = self.stall_torque / self.stall_current
# Spring constant for the springs, N/m
self.Ks = 2800.0
+ # Maximum extension distance (Distance from the 0 force point on the
+ # spring to the latch position.)
+ self.max_extension = 0.32385
# Gear ratio multiplied by radius of final sprocket.
self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
@@ -235,7 +238,13 @@
sprung_shooter = SprungShooterDeltaU()
shooter = ShooterDeltaU()
- loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter, shooter])
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter,
+ shooter])
+
+ loop_writer.AddConstant(control_loop.Constant("kMaxExtension", "%f",
+ sprung_shooter.max_extension))
+ loop_writer.AddConstant(control_loop.Constant("kSpringConstant", "%f",
+ sprung_shooter.Ks))
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else:
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 4789e1d..4a3e6e4 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -5,8 +5,8 @@
#include <algorithm>
#include "aos/common/control_loop/control_loops.q.h"
-#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
#include "frc971/constants.h"
#include "frc971/control_loops/shooter/shooter_motor_plant.h"
@@ -27,17 +27,17 @@
// against last cycle's voltage.
if (X_hat(2, 0) > last_voltage_ + 4.0) {
voltage_ -= X_hat(2, 0) - (last_voltage_ + 4.0);
- LOG(INFO, "Capping due to runawway\n");
+ LOG(DEBUG, "Capping due to runaway\n");
} else if (X_hat(2, 0) < last_voltage_ - 4.0) {
voltage_ += X_hat(2, 0) - (last_voltage_ - 4.0);
- LOG(INFO, "Capping due to runawway\n");
+ LOG(DEBUG, "Capping due to runaway\n");
}
voltage_ = std::min(max_voltage_, voltage_);
voltage_ = std::max(-max_voltage_, voltage_);
U(0, 0) = voltage_ - old_voltage;
- LOG(INFO, "X_hat is %f, applied is %f\n", X_hat(2, 0), voltage_);
+ LOG_STRUCT(DEBUG, "output", ShooterVoltageToLog(X_hat(2, 0), voltage_));
last_voltage_ = voltage_;
capped_goal_ = false;
@@ -56,7 +56,7 @@
R(0, 0) -= dx;
}
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
} else if (uncapped_voltage() < -max_voltage_) {
double dx;
if (controller_index() == 0) {
@@ -69,7 +69,7 @@
R(0, 0) -= dx;
}
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
} else {
capped_goal_ = false;
}
@@ -85,13 +85,10 @@
void ZeroedStateFeedbackLoop::SetCalibration(double encoder_val,
double known_position) {
- LOG(INFO, "Setting calibration such that %f -> %f\n", encoder_val,
- known_position);
- LOG(INFO, "Position was %f\n", absolute_position());
+ double old_position = absolute_position();
double previous_offset = offset_;
offset_ = known_position - encoder_val;
double doffset = offset_ - previous_offset;
- LOG(INFO, "Changing offset from %f to %f\n", previous_offset, offset_);
X_hat(0, 0) += doffset;
// Offset our measurements because the offset is baked into them.
Y_(0, 0) += doffset;
@@ -100,7 +97,10 @@
if (controller_index() == 0) {
R(2, 0) += -A(1, 0) / A(1, 2) * (doffset);
}
- LOG(INFO, "Validation: position is %f\n", absolute_position());
+ LOG_STRUCT(
+ DEBUG, "sensor edge (fake?)",
+ ShooterChangeCalibration(encoder_val, known_position, old_position,
+ absolute_position(), previous_offset, offset_));
}
ShooterMotor::ShooterMotor(control_loops::ShooterGroup *my_shooter)
@@ -111,18 +111,48 @@
load_timeout_(0, 0),
shooter_brake_set_time_(0, 0),
unload_timeout_(0, 0),
- prepare_fire_end_time_(0, 0),
shot_end_time_(0, 0),
- cycles_not_moved_(0) {}
+ cycles_not_moved_(0),
+ shot_count_(0),
+ zeroed_(false),
+ distal_posedge_validation_cycles_left_(0),
+ proximal_posedge_validation_cycles_left_(0),
+ last_distal_current_(true),
+ last_proximal_current_(true) {}
double ShooterMotor::PowerToPosition(double power) {
- // LOG(WARNING, "power to position not correctly implemented\n");
const frc971::constants::Values &values = constants::GetValues();
- double new_pos = ::std::min(::std::max(power, values.shooter.lower_limit),
+ double maxpower = 0.5 * kSpringConstant *
+ (kMaxExtension * kMaxExtension -
+ (kMaxExtension - values.shooter.upper_limit) *
+ (kMaxExtension - values.shooter.upper_limit));
+ if (power < 0) {
+ LOG_STRUCT(WARNING, "negative power", PowerAdjustment(power, 0));
+ power = 0;
+ } else if (power > maxpower) {
+ LOG_STRUCT(WARNING, "power too high", PowerAdjustment(power, maxpower));
+ power = maxpower;
+ }
+
+ double mp = kMaxExtension * kMaxExtension - (power + power) / kSpringConstant;
+ double new_pos = 0.10;
+ if (mp < 0) {
+ LOG(ERROR,
+ "Power calculation has negative number before square root (%f).\n", mp);
+ } else {
+ new_pos = kMaxExtension - ::std::sqrt(mp);
+ }
+
+ new_pos = ::std::min(::std::max(new_pos, values.shooter.lower_limit),
values.shooter.upper_limit);
return new_pos;
}
+double ShooterMotor::PositionToPower(double position) {
+ double power = kSpringConstant * position * (kMaxExtension - position / 2.0);
+ return power;
+}
+
// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
const control_loops::ShooterGroup::Goal *goal,
@@ -131,15 +161,23 @@
control_loops::ShooterGroup::Status *status) {
constexpr double dt = 0.01;
+ if (::std::isnan(goal->shot_power)) {
+ state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because got a shot power of NAN.\n");
+ }
+
// we must always have these or we have issues.
if (goal == NULL || status == NULL) {
if (output) output->voltage = 0;
LOG(ERROR, "Thought I would just check for null and die.\n");
return;
}
+ status->ready = false;
if (reset()) {
state_ = STATE_INITIALIZE;
+ last_distal_current_ = position->pusher_distal.current;
+ last_proximal_current_ = position->pusher_proximal.current;
}
if (position) {
shooter_.CorrectPosition(position->position);
@@ -154,11 +192,8 @@
// Don't even let the control loops run.
bool shooter_loop_disable = false;
- // Adds voltage to take up slack in gears before shot.
- bool apply_some_voltage = false;
-
-
- const bool disabled = !::aos::robot_state->enabled;
+ const bool disabled =
+ !::aos::robot_state.get() || !::aos::robot_state->enabled;
// If true, move the goal if we saturate.
bool cap_goal = false;
@@ -174,7 +209,7 @@
shooter_.set_controller_index(1);
} else {
// Otherwise use the controller with the spring.
- shooter_.set_controller_index(1);
+ shooter_.set_controller_index(0);
}
if (shooter_.controller_index() != last_controller_index) {
shooter_.RecalculatePowerGoal();
@@ -200,13 +235,18 @@
values.shooter.upper_limit);
}
- state_ = STATE_REQUEST_LOAD;
-
// Go to the current position.
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
// If the plunger is all the way back, we want to be latched.
latch_piston_ = position->plunger;
brake_piston_ = false;
+ if (position->latch == latch_piston_) {
+ state_ = STATE_REQUEST_LOAD;
+ } else {
+ shooter_loop_disable = true;
+ LOG(DEBUG,
+ "Not moving on until the latch has moved to avoid a crash\n");
+ }
} else {
// If we can't start yet because we don't know where we are, set the
// latch and brake to their defaults.
@@ -216,7 +256,9 @@
break;
case STATE_REQUEST_LOAD:
if (position) {
- if (position->pusher_distal.current) {
+ zeroed_ = false;
+ if (position->pusher_distal.current ||
+ position->pusher_proximal.current) {
// We started on the sensor, back up until we are found.
// If the plunger is all the way back and not latched, it won't be
// there for long.
@@ -259,12 +301,13 @@
shooter_.set_max_voltage(4.0);
if (position) {
- if (!position->pusher_distal.current) {
+ if (!position->pusher_distal.current &&
+ !position->pusher_proximal.current) {
Load();
}
+ latch_piston_ = position->plunger;
}
- latch_piston_ = false;
brake_piston_ = false;
break;
case STATE_LOAD:
@@ -281,18 +324,48 @@
shooter_.SetGoalPosition(0.0, 0.0);
if (position) {
+ // TODO(austin): Validate that this is the right edge.
// If we see a posedge on any of the hall effects,
if (position->pusher_proximal.posedge_count !=
- last_proximal_posedge_count_) {
- LOG(DEBUG, "Setting calibration using proximal sensor\n");
- shooter_.SetCalibration(position->pusher_proximal.posedge_value,
- values.shooter.pusher_proximal.upper_angle);
+ last_proximal_posedge_count_ &&
+ !last_proximal_current_) {
+ proximal_posedge_validation_cycles_left_ = 2;
}
+ if (proximal_posedge_validation_cycles_left_ > 0) {
+ if (position->pusher_proximal.current) {
+ --proximal_posedge_validation_cycles_left_;
+ if (proximal_posedge_validation_cycles_left_ == 0) {
+ shooter_.SetCalibration(
+ position->pusher_proximal.posedge_value,
+ values.shooter.pusher_proximal.upper_angle);
+
+ LOG(DEBUG, "Setting calibration using proximal sensor\n");
+ zeroed_ = true;
+ }
+ } else {
+ proximal_posedge_validation_cycles_left_ = 0;
+ }
+ }
+
if (position->pusher_distal.posedge_count !=
- last_distal_posedge_count_) {
- LOG(DEBUG, "Setting calibration using distal sensor\n");
- shooter_.SetCalibration(position->pusher_distal.posedge_value,
- values.shooter.pusher_distal.upper_angle);
+ last_distal_posedge_count_ &&
+ !last_distal_current_) {
+ distal_posedge_validation_cycles_left_ = 2;
+ }
+ if (distal_posedge_validation_cycles_left_ > 0) {
+ if (position->pusher_distal.current) {
+ --distal_posedge_validation_cycles_left_;
+ if (distal_posedge_validation_cycles_left_ == 0) {
+ shooter_.SetCalibration(
+ position->pusher_distal.posedge_value,
+ values.shooter.pusher_distal.upper_angle);
+
+ LOG(DEBUG, "Setting calibration using distal sensor\n");
+ zeroed_ = true;
+ }
+ } else {
+ distal_posedge_validation_cycles_left_ = 0;
+ }
}
// Latch if the plunger is far enough back to trigger the hall effect.
@@ -303,7 +376,11 @@
// way back as well.
if (position->plunger && position->latch &&
position->pusher_distal.current) {
- state_ = STATE_PREPARE_SHOT;
+ if (!zeroed_) {
+ state_ = STATE_REQUEST_LOAD;
+ } else {
+ state_ = STATE_PREPARE_SHOT;
+ }
} else if (position->plunger &&
::std::abs(shooter_.absolute_position() -
shooter_.goal_position()) < 0.001) {
@@ -317,6 +394,7 @@
if (!position->pusher_distal.current ||
!position->pusher_proximal.current) {
state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because took too long to load.\n");
}
}
} else if (goal->unload_requested) {
@@ -326,7 +404,8 @@
break;
case STATE_LOADING_PROBLEM:
if (disabled) {
- Load();
+ state_ = STATE_REQUEST_LOAD;
+ break;
}
// We got to the goal, but the latch hasn't registered as down. It might
// be stuck, or on it's way but not there yet.
@@ -378,15 +457,8 @@
LOG(DEBUG, "In ready\n");
// Wait until the brake is set, and a shot is requested or the shot power
// is changed.
- if (::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) > 0.002) {
- // TODO(austin): Add a state to release the brake.
-
- // TODO(austin): Do we want to set the brake here or after shooting?
- // Depends on air usage.
- LOG(DEBUG, "Preparing shot again.\n");
- state_ = STATE_PREPARE_SHOT;
- } else if (Time::Now() > shooter_brake_set_time_) {
+ if (Time::Now() > shooter_brake_set_time_) {
+ status->ready = true;
// We have waited long enough for the brake to set, turn the shooter
// control loop off.
shooter_loop_disable = true;
@@ -394,13 +466,23 @@
if (goal->shot_requested && !disabled) {
LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
- prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
- apply_some_voltage = true;
- state_ = STATE_PREPARE_FIRE;
+ shot_end_time_ = Time::Now() + kShotEndTimeout;
+ firing_starting_position_ = shooter_.absolute_position();
+ state_ = STATE_FIRE;
}
- } else {
- LOG(DEBUG, "Nothing %d %d\n", goal->shot_requested, !disabled);
}
+ if (state_ == STATE_READY &&
+ ::std::abs(shooter_.absolute_position() -
+ PowerToPosition(goal->shot_power)) > 0.002) {
+ // TODO(austin): Add a state to release the brake.
+
+ // TODO(austin): Do we want to set the brake here or after shooting?
+ // Depends on air usage.
+ status->ready = false;
+ LOG(DEBUG, "Preparing shot again.\n");
+ state_ = STATE_PREPARE_SHOT;
+ }
+
shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
latch_piston_ = true;
@@ -411,29 +493,6 @@
}
break;
- case STATE_PREPARE_FIRE:
- // Apply a bit of voltage to bias the gears for a little bit of time, and
- // then fire.
- shooter_loop_disable = true;
- if (disabled) {
- // If we are disabled, reset the backlash bias timer.
- prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
- break;
- }
- if (Time::Now() > prepare_fire_end_time_) {
- cycles_not_moved_ = 0;
- firing_starting_position_ = shooter_.absolute_position();
- shot_end_time_ = Time::Now() + kShotEndTimeout;
- state_ = STATE_FIRE;
- latch_piston_ = false;
- } else {
- apply_some_voltage = true;
- latch_piston_ = true;
- }
-
- brake_piston_ = true;
- break;
-
case STATE_FIRE:
if (disabled) {
if (position) {
@@ -461,6 +520,7 @@
cycles_not_moved_ > 3) ||
Time::Now() > shot_end_time_) {
state_ = STATE_REQUEST_LOAD;
+ ++shot_count_;
}
latch_piston_ = false;
brake_piston_ = true;
@@ -500,6 +560,7 @@
// We have been stuck trying to unload for way too long, give up and
// turn everything off.
state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because took too long to unload.\n");
}
brake_piston_ = false;
@@ -510,7 +571,7 @@
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
}
cap_goal = true;
- shooter_.set_max_voltage(5.0);
+ shooter_.set_max_voltage(6.0);
// Slowly move back until we hit the upper limit.
// If we were at the limit last cycle, we are done unloading.
@@ -526,8 +587,8 @@
shooter_.SetGoalPosition(
::std::min(
values.shooter.upper_limit,
- shooter_.goal_position() + values.shooter.zeroing_speed * dt),
- values.shooter.zeroing_speed);
+ shooter_.goal_position() + values.shooter.unload_speed * dt),
+ values.shooter.unload_speed);
}
latch_piston_ = false;
@@ -537,7 +598,7 @@
if (goal->load_requested) {
state_ = STATE_REQUEST_LOAD;
}
- // If we are ready to load again,
+ // If we are ready to load again,
shooter_loop_disable = true;
latch_piston_ = false;
@@ -545,6 +606,7 @@
break;
case STATE_ESTOP:
+ LOG(WARNING, "estopped\n");
// Totally lost, go to a safe state.
shooter_loop_disable = true;
latch_piston_ = true;
@@ -552,13 +614,10 @@
break;
}
- if (apply_some_voltage) {
- shooter_.Update(true);
- shooter_.ZeroPower();
- if (output) output->voltage = 2.0;
- } else if (!shooter_loop_disable) {
- LOG(DEBUG, "Running the loop, goal is %f, position is %f\n",
- shooter_.goal_position(), shooter_.absolute_position());
+ if (!shooter_loop_disable) {
+ LOG_STRUCT(DEBUG, "running the loop",
+ ShooterStatusToLog(shooter_.goal_position(),
+ shooter_.absolute_position()));
if (!cap_goal) {
shooter_.set_max_voltage(12.0);
}
@@ -573,25 +632,38 @@
if (output) output->voltage = 0.0;
}
+ status->hard_stop_power = PositionToPower(shooter_.absolute_position());
+
if (output) {
output->latch_piston = latch_piston_;
output->brake_piston = brake_piston_;
}
- status->done = ::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) < 0.004;
+ if (position) {
+ LOG_STRUCT(DEBUG, "internal state",
+ ShooterStateToLog(
+ shooter_.absolute_position(), shooter_.absolute_velocity(),
+ state_, position->latch, position->pusher_proximal.current,
+ position->pusher_distal.current, position->plunger,
+ brake_piston_, latch_piston_));
- if (position) {
last_position_ = *position;
- LOG(DEBUG,
- "pos > absolute: %f velocity: %f state= %d\n",
- shooter_.absolute_position(), shooter_.absolute_velocity(),
- state_);
- }
- if (position) {
+
last_distal_posedge_count_ = position->pusher_distal.posedge_count;
last_proximal_posedge_count_ = position->pusher_proximal.posedge_count;
+ last_distal_current_ = position->pusher_distal.current;
+ last_proximal_current_ = position->pusher_proximal.current;
}
+
+ status->shots = shot_count_;
+}
+
+void ShooterMotor::ZeroOutputs() {
+ queue_group()->output.MakeWithBuilder()
+ .voltage(0)
+ .latch_piston(latch_piston_)
+ .brake_piston(brake_piston_)
+ .Send();
}
} // namespace control_loops
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index efcc0e7..10e0f4e 100755
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -30,6 +30,7 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'shooter_loop',
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 195894b..1ab224b 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -66,7 +66,6 @@
void CorrectPosition(double position) {
Eigen::Matrix<double, 1, 1> Y;
Y << position + offset_ - kPositionOffset;
- LOG(DEBUG, "Setting position to %f\n", position);
Correct(Y);
}
@@ -97,7 +96,7 @@
private:
// The offset between what is '0' (0 rate on the spring) and the 0 (all the
// way cocked).
- constexpr static double kPositionOffset = 0.305054 + 0.0254;
+ constexpr static double kPositionOffset = kMaxExtension;
// The accumulated voltage to apply to the motor.
double voltage_;
double last_voltage_;
@@ -108,10 +107,11 @@
};
const Time kUnloadTimeout = Time::InSeconds(10);
-const Time kLoadTimeout = Time::InSeconds(10);
-const Time kLoadProblemEndTimeout = Time::InSeconds(0.5);
+const Time kLoadTimeout = Time::InSeconds(2);
+const Time kLoadProblemEndTimeout = Time::InSeconds(1.0);
const Time kShooterBrakeSetTime = Time::InSeconds(0.05);
-const Time kShotEndTimeout = Time::InSeconds(1.0);
+// Time to wait after releasing the latch piston before winching back again.
+const Time kShotEndTimeout = Time::InSeconds(0.2);
const Time kPrepareFireEndTime = Time::InMS(40);
class ShooterMotor
@@ -124,6 +124,7 @@
bool capped_goal() const { return shooter_.capped_goal(); }
double PowerToPosition(double power);
+ double PositionToPower(double position);
typedef enum {
STATE_INITIALIZE = 0,
@@ -133,7 +134,6 @@
STATE_LOADING_PROBLEM = 4,
STATE_PREPARE_SHOT = 5,
STATE_READY = 6,
- STATE_PREPARE_FIRE = 7,
STATE_FIRE = 8,
STATE_UNLOAD = 9,
STATE_UNLOAD_MOVE = 10,
@@ -150,6 +150,9 @@
ShooterGroup::Output *output, ShooterGroup::Status *status);
private:
+ // We have to override this to keep the pistons in the correct positions.
+ virtual void ZeroOutputs();
+
// Friend the test classes for acces to the internal state.
friend class testing::ShooterTest_UnloadWindupPositive_Test;
friend class testing::ShooterTest_UnloadWindupNegative_Test;
@@ -180,14 +183,10 @@
// wait for brake to set
Time shooter_brake_set_time_;
-
+
// The timeout for unloading.
Time unload_timeout_;
- // we are attempting to take up some of the backlash
- // in the gears before the plunger hits
- Time prepare_fire_end_time_;
-
// time that shot must have completed
Time shot_end_time_;
@@ -201,6 +200,12 @@
bool brake_piston_;
int32_t last_distal_posedge_count_;
int32_t last_proximal_posedge_count_;
+ uint32_t shot_count_;
+ bool zeroed_;
+ int distal_posedge_validation_cycles_left_;
+ int proximal_posedge_validation_cycles_left_;
+ bool last_distal_current_;
+ bool last_proximal_current_;
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index 6310320..2a42172 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -41,10 +41,14 @@
// Whether it's ready to shoot right now.
bool ready;
// Whether the plunger is in and out of the way of grabbing a ball.
+ // TODO(ben): Populate these!
bool cocked;
// How many times we've shot.
int32_t shots;
bool done;
+ // What we think the current position of the hard stop on the shooter is, in
+ // shot power (Joules).
+ double hard_stop_power;
};
queue Goal goal;
@@ -54,3 +58,43 @@
};
queue_group ShooterGroup shooter_queue_group;
+
+struct ShooterStateToLog {
+ double absolute_position;
+ double absolute_velocity;
+ uint32_t state;
+ bool latch_sensor;
+ bool proximal;
+ bool distal;
+ bool plunger;
+ bool brake;
+ bool latch_piston;
+};
+
+struct ShooterVoltageToLog {
+ double X_hat;
+ double applied;
+};
+
+struct ShooterMovingGoal {
+ double dx;
+};
+
+struct ShooterChangeCalibration {
+ double encoder;
+ double real_position;
+ double old_position;
+ double new_position;
+ double old_offset;
+ double new_offset;
+};
+
+struct ShooterStatusToLog {
+ double goal;
+ double position;
+};
+
+struct PowerAdjustment {
+ double requested_power;
+ double actual_power;
+};
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 8aa4c27..669e147 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -50,7 +50,7 @@
// The difference between the position with 0 at the back, and the position
// with 0 measured where the spring has 0 force.
- constexpr static double kPositionOffset = 0.305054 + 0.0254;
+ constexpr static double kPositionOffset = kMaxExtension;
void Reinitialize(double initial_position) {
LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
@@ -83,7 +83,8 @@
// (encoder, hall effect).
void SetPhysicalSensors(control_loops::ShooterGroup::Position *position) {
const frc971::constants::Values &values = constants::GetValues();
- position->position = GetPosition();
+
+ position->position = GetPosition();
LOG(DEBUG, "Physical shooter at {%f}\n", GetAbsolutePosition());
@@ -134,12 +135,27 @@
}
}
- // Sends out the position queue messages.
void SendPositionMessage() {
+ // the first bool is false
+ SendPositionMessage(false, false, false, false);
+ }
+
+ // Sends out the position queue messages.
+ // if the first bool is false then this is
+ // just the default state, otherwise will force
+ // it into a state using the passed values
+ void SendPositionMessage(bool use_passed, bool plunger_in,
+ bool latch_in, bool brake_in) {
const frc971::constants::Values &values = constants::GetValues();
::aos::ScopedMessagePtr<control_loops::ShooterGroup::Position> position =
shooter_queue_group_.position.MakeMessage();
+ if (use_passed) {
+ plunger_latched_ = latch_in && plunger_in;
+ latch_piston_state_ = plunger_latched_;
+ brake_piston_state_ = brake_in;
+ }
+
SetPhysicalSensors(position.get());
position->latch = latch_piston_state_;
@@ -170,10 +186,6 @@
latch_piston_state_ && latch_delay_count_ >= 0) {
ASSERT_EQ(0, latch_delay_count_) << ": The test doesn't support that.";
latch_delay_count_ = -6;
- if (GetAbsolutePosition() > 0.01) {
- EXPECT_GE(last_voltage_, 1)
- << ": Must preload the gearbox when firing.";
- }
}
if (shooter_queue_group_.output->brake_piston && !brake_piston_state_ &&
@@ -206,6 +218,7 @@
shooter_plant_->D() * shooter_plant_->U;
} else {
shooter_plant_->U << last_voltage_;
+ //shooter_plant_->U << shooter_queue_group_.output->voltage;
shooter_plant_->Update();
}
LOG(DEBUG, "Plant index is %d\n", shooter_plant_->plant_index());
@@ -272,6 +285,7 @@
};
class ShooterTest : public ::testing::Test {
+
protected:
::aos::common::testing::GlobalCoreInstance my_core;
@@ -333,21 +347,39 @@
};
TEST_F(ShooterTest, PowerConversion) {
- // test a couple of values return the right thing
- EXPECT_EQ(0.021, shooter_motor_.PowerToPosition(0.021));
- EXPECT_EQ(0.175, shooter_motor_.PowerToPosition(0.175));
-
const frc971::constants::Values &values = constants::GetValues();
+ // test a couple of values return the right thing
+ EXPECT_NEAR(0.254001, shooter_motor_.PowerToPosition(140.0), 0.00001);
+ EXPECT_NEAR(0.00058, shooter_motor_.PowerToPosition(0.53), 0.00001);
+ EXPECT_NEAR(0.095251238129837101, shooter_motor_.PowerToPosition(73.67),
+ 0.00001);
+
// value too large should get max
- EXPECT_EQ(values.shooter.upper_limit,
- shooter_motor_.PowerToPosition(505050.99));
+ EXPECT_NEAR(values.shooter.upper_limit,
+ shooter_motor_.PowerToPosition(505050.99), 0.00001);
// negative values should zero
- EXPECT_EQ(values.shooter.lower_limit, shooter_motor_.PowerToPosition(-123.4));
+ EXPECT_NEAR(0, shooter_motor_.PowerToPosition(-123.4), 0.00001);
+}
+
+// Test that PowerToPosition and PositionToPower are inverses of each other.
+// Note that PowerToPosition will cap position whereas PositionToPower will not
+// cap power.
+TEST_F(ShooterTest, InversePowerConversion) {
+ // Test a few values.
+ double power = 140.0;
+ double position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
+ power = .53;
+ position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
+ power = 71.971;
+ position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, GoesToValue) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 200; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -356,13 +388,15 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, Fire) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.021).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 120; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -370,21 +404,21 @@
SendDSPacket(true);
}
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
+ shooter_queue_group_.goal.MakeWithBuilder()
+ .shot_power(35.0)
+ .shot_requested(true)
+ .Send();
- bool hit_preparefire = false;
bool hit_fire = false;
for (int i = 0; i < 400; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
- if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
- hit_preparefire = true;
- }
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
shooter_queue_group_.goal.MakeWithBuilder()
+ .shot_power(17.0)
.shot_requested(false)
.Send();
}
@@ -393,15 +427,16 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- EXPECT_TRUE(hit_preparefire);
EXPECT_TRUE(hit_fire);
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, FireLong) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -411,16 +446,12 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
- bool hit_preparefire = false;
bool hit_fire = false;
for (int i = 0; i < 400; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
- if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
- hit_preparefire = true;
- }
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
shooter_queue_group_.goal.MakeWithBuilder()
@@ -432,16 +463,30 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power), pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- EXPECT_TRUE(hit_preparefire);
EXPECT_TRUE(hit_fire);
}
+// Verifies that it doesn't try to go out too far if you give it a ridicilous
+// power.
+TEST_F(ShooterTest, LoadTooFar) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(500.0).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_LT(
+ shooter_motor_plant_.GetAbsolutePosition(),
+ constants::GetValuesForTeam(971).shooter.upper_limit);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, MoveGoal) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -449,7 +494,7 @@
SendDSPacket(true);
}
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.11).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(14.0).Send();
for (int i = 0; i < 100; ++i) {
shooter_motor_plant_.SendPositionMessage();
@@ -459,14 +504,16 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, Unload) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -476,7 +523,9 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_group_.goal.MakeWithBuilder().unload_requested(true).Send();
- for (int i = 0; i < 400; ++i) {
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -484,13 +533,13 @@
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
- shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ shooter_motor_plant_.GetAbsolutePosition(), 0.015);
EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupNegative) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.20).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -502,7 +551,9 @@
int kicked_delay = 20;
int capped_goal_count = 0;
- for (int i = 0; i < 400; ++i) {
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
@@ -512,15 +563,15 @@
shooter_motor_.shooter_.R(0, 0) -= 100;
}
}
- if (shooter_motor_.capped_goal()) {
- ++capped_goal_count;
- }
+ if (shooter_motor_.capped_goal() && kicked_delay < 0) {
+ ++capped_goal_count;
+ }
shooter_motor_plant_.Simulate();
SendDSPacket(true);
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
- shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ shooter_motor_plant_.GetAbsolutePosition(), 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
EXPECT_LE(1, capped_goal_count);
EXPECT_GE(3, capped_goal_count);
@@ -528,7 +579,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupPositive) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.20).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -540,7 +591,9 @@
int kicked_delay = 20;
int capped_goal_count = 0;
- for (int i = 0; i < 400; ++i) {
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
@@ -550,15 +603,15 @@
shooter_motor_.shooter_.R(0, 0) += 0.1;
}
}
- if (shooter_motor_.capped_goal()) {
- ++capped_goal_count;
- }
+ if (shooter_motor_.capped_goal() && kicked_delay < 0) {
+ ++capped_goal_count;
+ }
shooter_motor_plant_.Simulate();
SendDSPacket(true);
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
- shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ shooter_motor_plant_.GetAbsolutePosition(), 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
EXPECT_LE(1, capped_goal_count);
EXPECT_GE(3, capped_goal_count);
@@ -571,7 +624,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, StartsOnDistal) {
Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 200; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -580,7 +633,9 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -589,7 +644,7 @@
TEST_F(ShooterTest, StartsOnProximal) {
Reinitialize(
HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 300; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -598,17 +653,59 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
+class ShooterZeroingTest : public ShooterTest,
+ public ::testing::WithParamInterface<
+ ::std::tr1::tuple<bool, bool, bool, double>> {};
+
+TEST_P(ShooterZeroingTest, AllDisparateStartingZero) {
+ bool latch = ::std::tr1::get<0>(GetParam());
+ bool brake = ::std::tr1::get<1>(GetParam());
+ bool plunger_back = ::std::tr1::get<2>(GetParam());
+ double start_pos = ::std::tr1::get<3>(GetParam());
+ // flag to initialize test
+ //printf("@@@@ l= %d b= %d p= %d s= %.3f\n",
+ // latch, brake, plunger_back, start_pos);
+ bool initialized = false;
+ Reinitialize(start_pos);
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(120.0).Send();
+ for (int i = 0; i < 200; ++i) {
+ shooter_motor_plant_.SendPositionMessage(!initialized, plunger_back, latch, brake);
+ initialized = true;
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ // EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
+ ASSERT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
+
+INSTANTIATE_TEST_CASE_P(
+ ShooterZeroingTest, ShooterZeroingTest,
+ ::testing::Combine(
+ ::testing::Bool(), ::testing::Bool(), ::testing::Bool(),
+ ::testing::Values(
+ 0.05, constants::GetValuesForTeam(971).shooter.upper_limit - 0.05,
+ HallEffectMiddle(constants::GetValuesForTeam(971)
+ .shooter.pusher_proximal),
+ HallEffectMiddle(constants::GetValuesForTeam(971)
+ .shooter.pusher_distal),
+ constants::GetValuesForTeam(971).shooter.latch_max_safe_position -
+ 0.001)));
+
// TODO(austin): Slip the encoder somewhere.
// TODO(austin): Test all the timeouts...
-// TODO(austin): Test starting latched and with the plunger back.
-// TODO(austin): Verify that we zeroed if we started latched and all the way back.
-
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
old mode 100755
new mode 100644
index 968fd04..606395a
--- a/frc971/control_loops/shooter/shooter_motor_plant.h
+++ b/frc971/control_loops/shooter/shooter_motor_plant.h
@@ -5,6 +5,10 @@
namespace frc971 {
namespace control_loops {
+static constexpr double kMaxExtension = 0.323850;
+
+static constexpr double kSpringConstant = 2800.000000;
+
StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients();
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h
old mode 100755
new mode 100644
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 0ef0df5..f289e81 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -323,14 +323,17 @@
//::std::cout << "Predict xhat before " << X_hat;
//::std::cout << "Measurement error is " << Y_ - C() * X_hat;
//X_hat = A() * X_hat + B() * U;
+ //::std::cout << "Predict xhat after " << X_hat;
+ UpdateObserver();
+ }
+
+ void UpdateObserver() {
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 {
X_hat = A() * X_hat + B() * U;
}
- //::std::cout << "Predict xhat after " << X_hat;
}
// Sets the current controller to be index and verifies that it isn't out of