Merge branch 'claw' into ben_shooter
Conflicts:
frc971/control_loops/claw/claw.cc
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 4fea843..dd47975 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -34,12 +34,12 @@
const ShifterHallEffect kCompLeftDriveShifter{0.83, 2.32, 1.2, 1.0};
const ShifterHallEffect kCompRightDriveShifter{0.865, 2.375, 1.2, 1.0};
-const ShifterHallEffect kPracticeLeftDriveShifter{2.082283, 0.834433, 0.60,
+const ShifterHallEffect kPracticeLeftDriveShifter{5, 0, 0.60,
0.47};
-const ShifterHallEffect kPracticeRightDriveShifter{2.070124, 0.838993, 0.62,
+const ShifterHallEffect kPracticeRightDriveShifter{5, 0, 0.62,
0.55};
const double shooter_zeroing_off_speed = 0.0;
-const double shooter_zeroing_speed = 0.1;
+const double shooter_zeroing_speed = 0.05;
const Values *DoGetValues() {
uint16_t team = ::aos::network::GetTeamNumber();
@@ -69,8 +69,8 @@
1.57,
0,
0,
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, 0, 0}, {1.0, 1.1, 0, 0}, {2.0, 2.1, 0, 0}},
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, 0, 0}, {1.0, 1.1, 0, 0}, {2.0, 2.1, 0, 0}},
+ {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, -0.1, 0.05}, {1.0, 1.1, 1.0, 1.1}, {2.0, 2.1, 2.0, 2.1}},
+ {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, -0.1, 0.05}, {1.0, 1.1, 1.0, 1.1}, {2.0, 2.1, 2.0, 2.1}},
0.01, // claw_unimportant_epsilon
0.9, // start_fine_tune_pos
4.0,
@@ -121,31 +121,25 @@
control_loops::MakeDogDrivetrainLoop,
// ShooterLimits
// TODO(ben): make these real numbers
- {-0.000446, 0.300038, -0.001, 0.304354,
- 0.014436,
- {-2, 0.001786, 0.001786, -2}, {-2, -0.000446, -2, 0.026938}, {0.006096, 0.026416, 0, 0},
+ {-0.001042, 0.294084, -0.001935, 0.303460, 0.0138401,
+ {-0.002, 0.000446, -0.002, 0.000446},
+ {-0.002, 0.009078, -0.002, 0.009078},
+ {0.003869, 0.026194, 0.003869, 0.026194},
shooter_zeroing_off_speed,
shooter_zeroing_speed
},
- {0.5,
- 0.2,
- 0.1,
- -0.446558,
- 0.90675,
- -0.39110,
- 0.843349,
-#if 0
- separations (top, bottom)
- hard min position:-0.253845, position:-0.001136,
- soft min position:-0.244528, position:-0.047269,
- soft max position:0.526326, position:-0.510872,
- hard max position:0.517917, position:-0.582685,
-#endif
- {-1.62102, 1.039699, -1.606248, 0.989702, {-1.65, -1.546252, -1.65, -1.548752}, {-0.13249, -0.02113, -0.134763, -0.021589}, {0.934024, 1.05, 0.92970, 1.05}},
- {-1.420352, 1.348313, -1.161281, 1.264001, {-1.45, -1.283771, -1.45, -1.28468}, {-0.332476, -0.214984, -0.334294, -0.217029}, {1.248547, 1.37, 1.245366, 1.37}},
- 0.01, // claw_unimportant_epsilon
- 0.9, // start_fine_tune_pos
- 4.0,
+ {0.400000,
+ 0.200000,
+ 0.000000,
+ -0.762218,
+ 0.912207,
+ -0.362218,
+ 0.512207,
+ {-1.682379, 1.043334, -1.282379, 0.643334, {-1.7, -1.544662, -1.7, -1.547616}, {-0.130218, -0.019771, -0.132036, -0.018862}, {0.935842, 1.1, 0.932660, 1.1}},
+ {-1.225821, 1.553752, -0.825821, 1.153752, {-1.3, -1.088331, -1.3, -1.088331}, {-0.134536, -0.018408, -0.136127, -0.019771}, {1.447396, 1.6, 1.443987, 1.6}},
+ 0.020000, // claw_unimportant_epsilon
+ -0.200000, // start_fine_tune_pos
+ 4.000000,
}
};
break;
diff --git a/frc971/constants.h b/frc971/constants.h
index 3416a39..f696e8b 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -11,8 +11,8 @@
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
-const uint16_t kCompTeamNumber = 971;
-const uint16_t kPracticeTeamNumber = 8971;
+const uint16_t kCompTeamNumber = 8971;
+const uint16_t kPracticeTeamNumber = 971;
// Contains the voltages for an analog hall effect sensor on a shifter.
struct ShifterHallEffect {
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 19e9a18..00dca6f 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -45,9 +45,18 @@
namespace frc971 {
namespace control_loops {
+static const double kZeroingVoltage = 4.0;
+static const double kMaxVoltage = 12.0;
+
+ClawLimitedLoop::ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> loop)
+ : StateFeedbackLoop<4, 2, 2>(loop),
+ uncapped_average_voltage_(0.0),
+ is_zeroing_(true) {}
+
void ClawLimitedLoop::CapU() {
uncapped_average_voltage_ = U(0, 0) + U(1, 0) / 2.0;
if (is_zeroing_) {
+ LOG(DEBUG, "zeroing\n");
const frc971::constants::Values &values = constants::GetValues();
if (uncapped_average_voltage_ > values.claw.max_zeroing_voltage) {
const double difference =
@@ -63,13 +72,108 @@
double max_value =
::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0) + U(0, 0)));
- if (max_value > 12.0) {
+ const double k_max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
+ if (max_value > k_max_voltage) {
LOG(DEBUG, "Capping U because max is %f\n", max_value);
- U = U * 12.0 / max_value;
+ U = U * k_max_voltage / max_value;
LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
}
}
+ZeroedStateFeedbackLoop::ZeroedStateFeedbackLoop(const char *name,
+ ClawMotor *motor)
+ : offset_(0.0),
+ name_(name),
+ motor_(motor),
+ zeroing_state_(UNKNOWN_POSITION),
+ posedge_value_(0.0),
+ negedge_value_(0.0),
+ encoder_(0.0),
+ last_encoder_(0.0) {}
+
+void ZeroedStateFeedbackLoop::SetPositionValues(const HalfClawPosition &claw) {
+ front_.Update(claw.front);
+ calibration_.Update(claw.calibration);
+ back_.Update(claw.back);
+
+ bool any_sensor_triggered = any_triggered();
+ if (any_sensor_triggered && any_triggered_last_) {
+ // We are still on the hall effect and nothing has changed.
+ min_hall_effect_on_angle_ =
+ ::std::min(min_hall_effect_on_angle_, claw.position);
+ max_hall_effect_on_angle_ =
+ ::std::max(max_hall_effect_on_angle_, claw.position);
+ } else if (!any_sensor_triggered && !any_triggered_last_) {
+ // We are still off the hall effect and nothing has changed.
+ min_hall_effect_off_angle_ =
+ ::std::min(min_hall_effect_off_angle_, claw.position);
+ max_hall_effect_off_angle_ =
+ ::std::max(max_hall_effect_off_angle_, claw.position);
+ } else if (any_sensor_triggered && !any_triggered_last_) {
+ // Saw a posedge on the hall effect. Reset the limits.
+ min_hall_effect_on_angle_ = ::std::min(claw.posedge_value, claw.position);
+ max_hall_effect_on_angle_ = ::std::max(claw.posedge_value, claw.position);
+ } else if (!any_sensor_triggered && any_triggered_last_) {
+ // Saw a negedge on the hall effect. Reset the limits.
+ min_hall_effect_off_angle_ = ::std::min(claw.negedge_value, claw.position);
+ max_hall_effect_off_angle_ = ::std::max(claw.negedge_value, claw.position);
+ }
+
+ posedge_value_ = claw.posedge_value;
+ negedge_value_ = claw.negedge_value;
+ last_encoder_ = encoder_;
+ if (front().value() || calibration().value() || back().value()) {
+ last_on_encoder_ = encoder_;
+ } else {
+ last_off_encoder_ = encoder_;
+ }
+ encoder_ = claw.position;
+ any_triggered_last_ = any_sensor_triggered;
+}
+
+void ZeroedStateFeedbackLoop::Reset(const HalfClawPosition &claw) {
+ set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
+
+ front_.Reset();
+ calibration_.Reset();
+ back_.Reset();
+ // 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;
+ max_hall_effect_on_angle_ = claw.position;
+ min_hall_effect_off_angle_ = claw.position;
+ max_hall_effect_off_angle_ = claw.position;
+ any_triggered_last_ = any_triggered();
+}
+
+bool TopZeroedStateFeedbackLoop::SetCalibrationOnEdge(
+ const constants::Values::Claws::Claw &claw_values,
+ JointZeroingState zeroing_state) {
+ double edge_encoder;
+ double edge_angle;
+ if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
+ LOG(INFO, "Calibration edge edge should be %f.\n", edge_angle);
+ SetCalibration(edge_encoder, edge_angle);
+ set_zeroing_state(zeroing_state);
+ return true;
+ }
+ return false;
+}
+
+bool BottomZeroedStateFeedbackLoop::SetCalibrationOnEdge(
+ const constants::Values::Claws::Claw &claw_values,
+ JointZeroingState zeroing_state) {
+ double edge_encoder;
+ double edge_angle;
+ if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
+ LOG(INFO, "Calibration edge.\n");
+ SetCalibration(edge_encoder, edge_angle);
+ set_zeroing_state(zeroing_state);
+ return true;
+ }
+ return false;
+}
+
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
: aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
has_top_claw_goal_(false),
@@ -90,46 +194,59 @@
const constants::Values::Claws::AnglePair &angles, double *edge_encoder,
double *edge_angle, const HallEffectTracker &sensor,
const char *hall_effect_name) {
+ bool found_edge = false;
+
if (sensor.posedge_count_changed()) {
- if (min_current_hall_effect_edge_ == max_curent_hall_effect_edge_) {
+ if (min_hall_effect_off_angle_ == max_hall_effect_off_angle_) {
// we oddly got two of the same edge.
*edge_angle = last_edge_value_;
- return true;
- } else if (posedge_value_ <
- (min_current_hall_effect_edge_ + max_current_hall_effect_edge_) / 2) {
- *edge_angle = angles.upper_angle;
- LOG(INFO, "%s Posedge upper of %s -> %f\n", name_, hall_effect_name,
- *edge_angle);
+ found_edge = true;
} else {
- *edge_angle = angles.lower_angle;
- LOG(INFO, "%s Posedge lower of %s -> %f\n", name_, hall_effect_name,
- *edge_angle);
+ const double average_last_encoder =
+ (min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
+ if (posedge_value_ < average_last_encoder) {
+ *edge_angle = angles.upper_decreasing_angle;
+ LOG(INFO, "%s Posedge upper of %s -> %f posedge: %f avg_encoder: %f\n",
+ name_, hall_effect_name, *edge_angle, posedge_value_,
+ average_last_encoder);
+ } else {
+ *edge_angle = angles.lower_angle;
+ LOG(INFO, "%s Posedge lower of %s -> %f posedge: %f avg_encoder: %f\n",
+ name_, hall_effect_name, *edge_angle, posedge_value_,
+ average_last_encoder);
+ }
}
*edge_encoder = posedge_value_;
- last_edge_value_ = posedge_value_;
- return true;
+ found_edge = true;
}
if (sensor.negedge_count_changed()) {
- // we oddly got two of the same edge.
- if (min_current_hall_effect_edge_ == max_curent_hall_effect_edge_) {
+ if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
*edge_angle = last_edge_value_;
- return true;
- } else if (negedge_value_ > (min_current_hall_effect_edge_ +
- max_current_hall_effect_edge_) / 2) {
- *edge_angle = angles.upper_angle;
- LOG(INFO, "%s Negedge upper of %s -> %f\n", name_, hall_effect_name,
- *edge_angle);
+ found_edge = true;
} else {
- *edge_angle = angles.lower_angle;
- LOG(INFO, "%s Negedge lower of %s -> %f\n", name_, hall_effect_name,
- *edge_angle);
+ const double average_last_encoder =
+ (min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
+ if (negedge_value_ > average_last_encoder) {
+ *edge_angle = angles.upper_angle;
+ LOG(INFO, "%s Negedge upper of %s -> %f negedge: %f avg_encoder: %f\n",
+ name_, hall_effect_name, *edge_angle, negedge_value_,
+ average_last_encoder);
+ } else {
+ *edge_angle = angles.lower_decreasing_angle;
+ LOG(INFO, "%s Negedge lower of %s -> %f negedge: %f avg_encoder: %f\n",
+ name_, hall_effect_name, *edge_angle, negedge_value_,
+ average_last_encoder);
+ }
+ *edge_encoder = negedge_value_;
}
- *edge_encoder = negedge_value_;
- last_edge_value_ = posedge_value_;
- return true;
+ found_edge = true;
}
- return false;
+ if (found_edge) {
+ last_edge_value_ = *edge_angle;
+ }
+
+ return found_edge;
}
bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
@@ -270,14 +387,8 @@
}
if (reset()) {
- bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
- top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
- // close up the min and max edge positions as they are no longer valid and
- // will be expanded in future iterations
- top_claw_.min_current_hall_effect_edge_ =
- top_claw_.max_current_hall_effect_edge_ = position->top.position;
- bottom_claw_.min_current_hall_effect_edge_ =
- bottom_claw_.max_current_hall_effect_edge_ = position->bottom.position;
+ top_claw_.Reset(position->top);
+ bottom_claw_.Reset(position->bottom);
}
if (::aos::robot_state.get() == nullptr) {
@@ -528,7 +639,8 @@
LOG(DEBUG, "Goal is %f (bottom) %f, separation is %f\n", claw_.R(0, 0),
claw_.R(1, 0), separation);
- // Only cap power when one of the halves of the claw is unknown.
+ // Only cap power when one of the halves of the claw is moving slowly and
+ // could wind up.
claw_.set_is_zeroing(mode_ == UNKNOWN_LOCATION || mode_ == FINE_TUNE_TOP ||
mode_ == FINE_TUNE_BOTTOM);
claw_.Update(output == nullptr);
@@ -573,6 +685,18 @@
if (output) {
output->top_claw_voltage = claw_.U(1, 0) + claw_.U(0, 0);
output->bottom_claw_voltage = claw_.U(0, 0);
+
+ if (output->top_claw_voltage > kMaxVoltage) {
+ output->top_claw_voltage = kMaxVoltage;
+ } else if (output->top_claw_voltage < -kMaxVoltage) {
+ output->top_claw_voltage = -kMaxVoltage;
+ }
+
+ if (output->bottom_claw_voltage > kMaxVoltage) {
+ output->bottom_claw_voltage = kMaxVoltage;
+ } else if (output->bottom_claw_voltage < -kMaxVoltage) {
+ output->bottom_claw_voltage = -kMaxVoltage;
+ }
}
status->done = false;
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 50076a7..dc8e6ba 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -23,10 +23,7 @@
class ClawLimitedLoop : public StateFeedbackLoop<4, 2, 2> {
public:
- ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> loop)
- : StateFeedbackLoop<4, 2, 2>(loop),
- uncapped_average_voltage_(0.0),
- is_zeroing_(true) {}
+ ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> loop);
virtual void CapU();
void set_is_zeroing(bool is_zeroing) { is_zeroing_ = is_zeroing; }
@@ -49,15 +46,7 @@
// controller.
class ZeroedStateFeedbackLoop {
public:
- ZeroedStateFeedbackLoop(const char *name, ClawMotor *motor)
- : offset_(0.0),
- name_(name),
- motor_(motor),
- zeroing_state_(UNKNOWN_POSITION),
- posedge_value_(0.0),
- negedge_value_(0.0),
- encoder_(0.0),
- last_encoder_(0.0) {}
+ ZeroedStateFeedbackLoop(const char *name, ClawMotor *motor);
const static int kZeroingMaxVoltage = 5;
@@ -78,26 +67,12 @@
}
JointZeroingState zeroing_state() const { return zeroing_state_; }
- void SetPositionValues(const HalfClawPosition &claw) {
- front_.Update(claw.front);
- calibration_.Update(claw.calibration);
- back_.Update(claw.back);
+ void SetPositionValues(const HalfClawPosition &claw);
- if (any_hall_effect_changed()) {
- // if the hall effect has changed we are new edge
- // so we zero out the interval this new edge has covered
- min_current_hall_effect_edge_ = claw.position;
- max_current_hall_effect_edge_ = claw.position;
- } else if (claw.position > max_current_hall_effect_edge_) {
- max_current_hall_effect_edge_ = claw.position;
- } else if (claw.position < min_current_hall_effect_edge_) {
- min_current_hall_effect_edge_ = claw.position;
- }
+ void Reset(const HalfClawPosition &claw);
- posedge_value_ = claw.posedge_value;
- negedge_value_ = claw.negedge_value;
- last_encoder_ = encoder_;
- encoder_ = claw.position;
+ bool ready() {
+ return front_.ready() && calibration_.ready() && back_.ready();
}
double absolute_position() const { return encoder() + offset(); }
@@ -114,6 +89,9 @@
bool front_or_back_triggered() const {
return front().value() || back().value();
}
+ bool any_triggered() const {
+ return calibration().value() || front().value() || back().value();
+ }
double encoder() const { return encoder_; }
double last_encoder() const { return last_encoder_; }
@@ -140,10 +118,15 @@
double posedge_value_;
double negedge_value_;
double last_edge_value_;
- double min_current_hall_effect_edge_;
- double max_current_hall_effect_edge_;
+ double min_hall_effect_on_angle_;
+ double max_hall_effect_on_angle_;
+ double min_hall_effect_off_angle_;
+ double max_hall_effect_off_angle_;
double encoder_;
double last_encoder_;
+ double last_on_encoder_;
+ double last_off_encoder_;
+ bool any_triggered_last_;
private:
// Does the edges of 1 sensor for GetPositionOfEdge.
@@ -162,17 +145,7 @@
void SetCalibration(double edge_encoder, double edge_angle);
bool SetCalibrationOnEdge(const constants::Values::Claws::Claw &claw_values,
- JointZeroingState zeroing_state) {
- double edge_encoder;
- double edge_angle;
- if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
- LOG(INFO, "Calibration edge.\n");
- SetCalibration(edge_encoder, edge_angle);
- set_zeroing_state(zeroing_state);
- return true;
- }
- return false;
- }
+ JointZeroingState zeroing_state);
};
class BottomZeroedStateFeedbackLoop : public ZeroedStateFeedbackLoop {
@@ -184,17 +157,7 @@
void SetCalibration(double edge_encoder, double edge_angle);
bool SetCalibrationOnEdge(const constants::Values::Claws::Claw &claw_values,
- JointZeroingState zeroing_state) {
- double edge_encoder;
- double edge_angle;
- if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
- LOG(INFO, "Calibration edge.\n");
- SetCalibration(edge_encoder, edge_angle);
- set_zeroing_state(zeroing_state);
- return true;
- }
- return false;
- }
+ JointZeroingState zeroing_state);
};
class ClawMotor
diff --git a/frc971/control_loops/claw/claw_calibration.cc b/frc971/control_loops/claw/claw_calibration.cc
index 0167c08..608c72d 100644
--- a/frc971/control_loops/claw/claw_calibration.cc
+++ b/frc971/control_loops/claw/claw_calibration.cc
@@ -8,69 +8,144 @@
typedef constants::Values::Claws Claws;
-bool DoGetPositionOfEdge(
- const double start_position,
- const control_loops::HalfClawPosition &last_claw_position,
- const control_loops::HalfClawPosition &claw_position,
- const HallEffectStruct &last_hall_effect,
- const HallEffectStruct &hall_effect,
- Claws::AnglePair *limits) {
+class Sensor {
+ public:
+ Sensor(const double start_position,
+ const HallEffectStruct &initial_hall_effect)
+ : start_position_(start_position),
+ last_hall_effect_(initial_hall_effect),
+ last_posedge_count_(initial_hall_effect.posedge_count),
+ last_negedge_count_(initial_hall_effect.negedge_count) {
+ last_on_min_position_ = start_position;
+ last_on_max_position_ = start_position;
+ last_off_min_position_ = start_position;
+ last_off_max_position_ = start_position;
+ }
- if (hall_effect.posedge_count != last_hall_effect.posedge_count) {
- if (claw_position.posedge_value < last_claw_position.position) {
- limits->upper_angle = claw_position.posedge_value - start_position;
- } else {
- limits->lower_decreasing_angle =
- claw_position.posedge_value - start_position;
+ bool DoGetPositionOfEdge(
+ const control_loops::HalfClawPosition &claw_position,
+ const HallEffectStruct &hall_effect, Claws::AnglePair *limits) {
+ bool print = false;
+
+ if (hall_effect.posedge_count != last_posedge_count_) {
+ const double avg_off_position = (last_off_min_position_ + last_off_max_position_) / 2.0;
+ if (claw_position.posedge_value < avg_off_position) {
+ printf("Posedge upper current %f posedge %f avg_off %f [%f, %f]\n",
+ claw_position.position, claw_position.posedge_value,
+ avg_off_position, last_off_min_position_,
+ last_off_max_position_);
+ limits->upper_decreasing_angle = claw_position.posedge_value - start_position_;
+ } else {
+ printf("Posedge lower current %f posedge %f avg_off %f [%f, %f]\n",
+ claw_position.position, claw_position.posedge_value,
+ avg_off_position, last_off_min_position_,
+ last_off_max_position_);
+ limits->lower_angle =
+ claw_position.posedge_value - start_position_;
+ }
+ print = true;
}
- return true;
- }
- if (hall_effect.negedge_count != last_hall_effect.negedge_count) {
- if (claw_position.negedge_value > last_claw_position.position) {
- limits->upper_decreasing_angle =
- claw_position.negedge_value - start_position;
- } else {
- limits->lower_angle = claw_position.negedge_value - start_position;
+ if (hall_effect.negedge_count != last_negedge_count_) {
+ const double avg_on_position = (last_on_min_position_ + last_on_max_position_) / 2.0;
+ if (claw_position.negedge_value > avg_on_position) {
+ printf("Negedge upper current %f negedge %f last_on %f [%f, %f]\n",
+ claw_position.position, claw_position.negedge_value,
+ avg_on_position, last_on_min_position_,
+ last_on_max_position_);
+ limits->upper_angle =
+ claw_position.negedge_value - start_position_;
+ } else {
+ printf("Negedge lower current %f negedge %f last_on %f [%f, %f]\n",
+ claw_position.position, claw_position.negedge_value,
+ avg_on_position, last_on_min_position_,
+ last_on_max_position_);
+ limits->lower_decreasing_angle = claw_position.negedge_value - start_position_;
+ }
+ print = true;
}
- return true;
+
+ if (hall_effect.current) {
+ if (!last_hall_effect_.current) {
+ last_on_min_position_ = last_on_max_position_ = claw_position.position;
+ } else {
+ last_on_min_position_ =
+ ::std::min(claw_position.position, last_on_min_position_);
+ last_on_max_position_ =
+ ::std::max(claw_position.position, last_on_max_position_);
+ }
+ } else {
+ if (last_hall_effect_.current) {
+ last_off_min_position_ = last_off_max_position_ = claw_position.position;
+ } else {
+ last_off_min_position_ =
+ ::std::min(claw_position.position, last_off_min_position_);
+ last_off_max_position_ =
+ ::std::max(claw_position.position, last_off_max_position_);
+ }
+ }
+
+ last_hall_effect_ = hall_effect;
+ last_posedge_count_ = hall_effect.posedge_count;
+ last_negedge_count_ = hall_effect.negedge_count;
+
+ return print;
}
- return false;
-}
+ private:
+ const double start_position_;
+ HallEffectStruct last_hall_effect_;
+ int32_t last_posedge_count_;
+ int32_t last_negedge_count_;
+ double last_on_min_position_;
+ double last_off_min_position_;
+ double last_on_max_position_;
+ double last_off_max_position_;
+};
-bool GetPositionOfEdge(
- const double start_position,
- const control_loops::HalfClawPosition &last_claw_position,
- const control_loops::HalfClawPosition &claw_position, Claws::Claw *claw) {
+class ClawSensors {
+ public:
+ ClawSensors(const double start_position,
+ const control_loops::HalfClawPosition &initial_claw_position)
+ : start_position_(start_position),
+ front_(start_position, initial_claw_position.front),
+ calibration_(start_position, initial_claw_position.calibration),
+ back_(start_position, initial_claw_position.back) {}
- if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
- last_claw_position.front, claw_position.front,
- &claw->front)) {
- return true;
- }
- if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
- last_claw_position.calibration,
- claw_position.calibration, &claw->calibration)) {
- return true;
- }
- if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
- last_claw_position.back, claw_position.back,
- &claw->back)) {
- return true;
+ bool GetPositionOfEdge(const control_loops::HalfClawPosition &claw_position,
+ Claws::Claw *claw) {
+
+ bool print = false;
+ if (front_.DoGetPositionOfEdge(claw_position,
+ claw_position.front, &claw->front)) {
+ print = true;
+ } else if (calibration_.DoGetPositionOfEdge(claw_position,
+ claw_position.calibration,
+ &claw->calibration)) {
+ print = true;
+ } else if (back_.DoGetPositionOfEdge(claw_position,
+ claw_position.back, &claw->back)) {
+ print = true;
+ }
+
+ double position = claw_position.position - start_position_;
+
+ if (position > claw->upper_limit) {
+ claw->upper_hard_limit = claw->upper_limit = position;
+ print = true;
+ }
+ if (position < claw->lower_limit) {
+ claw->lower_hard_limit = claw->lower_limit = position;
+ print = true;
+ }
+ return print;
}
- double position = claw_position.position - start_position;
-
- if (position > claw->upper_limit) {
- claw->upper_hard_limit = claw->upper_limit = position;
- return true;
- }
- if (position < claw->lower_limit) {
- claw->lower_hard_limit = claw->lower_limit = position;
- return true;
- }
- return false;
-}
+ private:
+ const double start_position_;
+ Sensor front_;
+ Sensor calibration_;
+ Sensor back_;
+};
int Main() {
while (!control_loops::claw_queue_group.position.FetchNextBlocking());
@@ -80,6 +155,11 @@
const double bottom_start_position =
control_loops::claw_queue_group.position->bottom.position;
+ ClawSensors top(top_start_position,
+ control_loops::claw_queue_group.position->top);
+ ClawSensors bottom(bottom_start_position,
+ control_loops::claw_queue_group.position->bottom);
+
Claws limits;
limits.claw_zeroing_off_speed = 0.5;
@@ -138,17 +218,16 @@
while (true) {
if (control_loops::claw_queue_group.position.FetchNextBlocking()) {
bool print = false;
- if (GetPositionOfEdge(top_start_position, last_position.top,
- control_loops::claw_queue_group.position->top,
- &limits.upper_claw)) {
+ if (top.GetPositionOfEdge(control_loops::claw_queue_group.position->top,
+ &limits.upper_claw)) {
print = true;
- LOG(DEBUG, "Got an edge on the upper claw\n");
+ printf("Got an edge on the upper claw\n");
}
- if (GetPositionOfEdge(bottom_start_position, last_position.bottom,
- control_loops::claw_queue_group.position->bottom,
- &limits.lower_claw)) {
+ if (bottom.GetPositionOfEdge(
+ control_loops::claw_queue_group.position->bottom,
+ &limits.lower_claw)) {
print = true;
- LOG(DEBUG, "Got an edge on the lower claw\n");
+ printf("Got an edge on the lower claw\n");
}
const double top_position =
control_loops::claw_queue_group.position->top.position -
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 3fa9079..99194bd 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -29,7 +29,7 @@
class TeamNumberEnvironment : public ::testing::Environment {
public:
// Override this to define how to set up the environment.
- virtual void SetUp() { aos::network::OverrideTeamNumber(971); }
+ virtual void SetUp() { aos::network::OverrideTeamNumber(1); }
};
::testing::Environment* const team_number_env =
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index 5d6598e..196b62c 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeClawPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.0, 0.00904786878843, 0.0, 0.0, 1.0, 0.0, 0.00904786878843, 0.0, 0.0, 0.815818233346, 0.0, 0.0, 0.0, 0.0, 0.815818233346;
+ A << 1.0, 0.0, 0.00740659366663, 0.0, 0.0, 1.0, 0.0, 0.00740659366663, 0.0, 0.0, 0.530576083967, 0.0, 0.0, 0.0, 0.0, 0.530576083967;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000326582411818, 0.0, 0.0, 0.000326582411818, 0.0631746179893, 0.0, 0.0, 0.0631746179893;
+ B << 0.00101390984157, 0.0, 0.0, 0.00101390984157, 0.183524472124, 0.0, 0.0, 0.183524472124;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 1, 1, 0, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackController<4, 2, 2> MakeClawController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.71581823335, 5.38760974287e-16, -1.71581823335, 1.71581823335, 64.8264890043, 2.03572300346e-14, -64.8264890043, 64.8264890043;
+ L << 1.43057608397, -4.48948312405e-16, -1.43057608397, 1.43057608397, 31.1907717473, -9.79345171104e-15, -31.1907717473, 31.1907717473;
Eigen::Matrix<double, 2, 4> K;
- K << 146.100132128, 0.0, 6.7282816813, 0.0, 0.0, 275.346049928, 0.0, 12.0408756114;
+ K << 110.395400642, 0.0, 2.50425726274, 0.0, 0.0, 170.435941688, 0.0, 2.89797614353;
return StateFeedbackController<4, 2, 2>(L, K, MakeClawPlantCoefficients());
}
diff --git a/frc971/control_loops/hall_effect_tracker.h b/frc971/control_loops/hall_effect_tracker.h
index 79d14af..7e6617c 100644
--- a/frc971/control_loops/hall_effect_tracker.h
+++ b/frc971/control_loops/hall_effect_tracker.h
@@ -7,6 +7,7 @@
namespace frc971 {
+// TODO(brians): Have a Reset() for when the cape resets.
class HallEffectTracker {
public:
int32_t get_posedges() const { return posedges_.count(); }
@@ -26,23 +27,38 @@
negedges_.update(position.negedge_count);
}
+ void Reset() {
+ posedges_.Reset();
+ negedges_.Reset();
+ }
+
+ 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;
}
- bool count_changed() const {
- return previous_count_ != count_;
- }
+ void Reset() { first_ = true; }
+
+ bool count_changed() const { return !first_ && 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;
};
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index 09718e5..73039db 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -14,12 +14,12 @@
# Stall Current in Amps
self.stall_current = 133
# Free Speed in RPM, pulled from drivetrain
- self.free_speed = 4650.0
+ self.free_speed = 5500.0
# Free Current in Amps
self.free_current = 2.7
# Moment of inertia of the claw in kg m^2
# approzimately 0.76 (without ball) in CAD
- self.J = 0.76
+ self.J = 0.70
# Resistance of the motor
self.R = 12.0 / self.stall_current + 0.024 + .003
# Motor velocity constant
@@ -63,9 +63,9 @@
#print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)
self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.03 ** 2.0)), 0.0, 0.0],
- [0.0, 0.0, 0.2, 0.0],
- [0.0, 0.0, 0.0, 2.00]])
+ [0.0, (1.0 / (0.06 ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, 0.10, 0.0],
+ [0.0, 0.0, 0.0, 0.1]])
self.R = numpy.matrix([[(1.0 / (20.0 ** 2.0)), 0.0],
[0.0, (1.0 / (20.0 ** 2.0))]])
@@ -73,6 +73,8 @@
print "K unaugmented"
print self.K
+ print "Placed controller poles"
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
self.rpl = .05
self.ipl = 0.008
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index c2b9dbb..89f682a 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -20,7 +20,7 @@
# This rough estimate should about include the effect of the masses
# of the gears. If this number is too low, the eigen values of self.A
# will start to become extremely small.
- self.J = 20
+ self.J = 200
# Resistance of the motor, divided by the number of motors.
self.R = 12.0 / self.stall_current / 2.0
# Motor velocity constant
@@ -110,7 +110,7 @@
self.C = numpy.matrix([[1.0, 0.0, 0.0]])
self.D = numpy.matrix([[0.0]])
- self.PlaceControllerPoles([0.55, 0.45, 0.80])
+ self.PlaceControllerPoles([0.50, 0.35, 0.80])
print "K"
print self.K
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 8f435fb..4789e1d 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -174,7 +174,7 @@
shooter_.set_controller_index(1);
} else {
// Otherwise use the controller with the spring.
- shooter_.set_controller_index(0);
+ shooter_.set_controller_index(1);
}
if (shooter_.controller_index() != last_controller_index) {
shooter_.RecalculatePowerGoal();
@@ -216,16 +216,24 @@
break;
case STATE_REQUEST_LOAD:
if (position) {
- if (position->plunger && position->latch) {
- // The plunger is back and we are latched, get ready to shoot.
- state_ = STATE_PREPARE_SHOT;
- latch_piston_ = true;
- } else if (position->pusher_distal.current) {
+ if (position->pusher_distal.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.
state_ = STATE_LOAD_BACKTRACK;
- latch_piston_ = false;
+
+ // The plunger is already back and latched. Don't release it.
+ if (position->plunger && position->latch) {
+ latch_piston_ = true;
+ } else {
+ latch_piston_ = false;
+ }
+ } else if (position->plunger && position->latch) {
+ // The plunger is back and we are latched. We most likely got here
+ // from Initialize, in which case we want to 'load' again anyways to
+ // zero.
+ Load();
+ latch_piston_ = true;
} else {
// Off the sensor, start loading.
Load();
@@ -248,7 +256,7 @@
values.shooter.zeroing_speed);
}
cap_goal = true;
- shooter_.set_max_voltage(12.0);
+ shooter_.set_max_voltage(4.0);
if (position) {
if (!position->pusher_distal.current) {
@@ -289,17 +297,19 @@
// Latch if the plunger is far enough back to trigger the hall effect.
// This happens when the distal sensor is triggered.
- latch_piston_ = position->pusher_distal.current;
+ latch_piston_ = position->pusher_distal.current || position->plunger;
- // Check if we are latched and back.
- if (position->plunger && position->latch) {
+ // Check if we are latched and back. Make sure the plunger is all the
+ // way back as well.
+ if (position->plunger && position->latch &&
+ position->pusher_distal.current) {
state_ = STATE_PREPARE_SHOT;
} else if (position->plunger &&
::std::abs(shooter_.absolute_position() -
shooter_.goal_position()) < 0.001) {
// We are at the goal, but not latched.
state_ = STATE_LOADING_PROBLEM;
- loading_problem_end_time_ = Time::Now() + Time::InSeconds(0.5);
+ loading_problem_end_time_ = Time::Now() + kLoadProblemEndTimeout;
}
}
if (load_timeout_ < Time::Now()) {
@@ -354,7 +364,7 @@
// We are there, set the brake and move on.
latch_piston_ = true;
brake_piston_ = true;
- shooter_brake_set_time_ = Time::Now() + Time::InSeconds(0.05);
+ shooter_brake_set_time_ = Time::Now() + kShooterBrakeSetTime;
state_ = STATE_READY;
} else {
latch_piston_ = true;
@@ -384,8 +394,7 @@
if (goal->shot_requested && !disabled) {
LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
- prepare_fire_end_time_ =
- Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
+ prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
apply_some_voltage = true;
state_ = STATE_PREPARE_FIRE;
}
@@ -408,14 +417,13 @@
shooter_loop_disable = true;
if (disabled) {
// If we are disabled, reset the backlash bias timer.
- prepare_fire_end_time_ =
- Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
+ 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() + Time::InSeconds(1);
+ shot_end_time_ = Time::Now() + kShotEndTimeout;
state_ = STATE_FIRE;
latch_piston_ = false;
} else {
@@ -432,7 +440,7 @@
if (position->plunger) {
// If disabled and the plunger is still back there, reset the
// timeout.
- shot_end_time_ = Time::Now() + Time::InSeconds(1);
+ shot_end_time_ = Time::Now() + kShotEndTimeout;
}
}
}
@@ -463,7 +471,14 @@
// If it is latched and the plunger is back, move the pusher back to catch
// the plunger.
- if (position->plunger && position->latch) {
+ bool all_back;
+ if (position) {
+ all_back = position->plunger && position->latch;
+ } else {
+ all_back = last_position_.plunger && last_position_.latch;
+ }
+
+ if (all_back) {
// Pull back to 0, 0.
shooter_.SetGoalPosition(0.0, 0.0);
if (shooter_.absolute_position() < 0.005) {
@@ -478,7 +493,7 @@
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
latch_piston_ = false;
state_ = STATE_UNLOAD_MOVE;
- unload_timeout_ = Time::Now() + Time::InSeconds(2.0);
+ unload_timeout_ = Time::Now() + kUnloadTimeout;
}
if (Time::Now() > unload_timeout_) {
@@ -491,11 +506,11 @@
break;
case STATE_UNLOAD_MOVE: {
if (disabled) {
- unload_timeout_ = Time::Now() + Time::InSeconds(2.0);
+ unload_timeout_ = Time::Now() + kUnloadTimeout;
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
}
cap_goal = true;
- shooter_.set_max_voltage(6.0);
+ shooter_.set_max_voltage(5.0);
// Slowly move back until we hit the upper limit.
// If we were at the limit last cycle, we are done unloading.
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 322a556..195894b 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -107,6 +107,13 @@
bool capped_goal_;
};
+const Time kUnloadTimeout = Time::InSeconds(10);
+const Time kLoadTimeout = Time::InSeconds(10);
+const Time kLoadProblemEndTimeout = Time::InSeconds(0.5);
+const Time kShooterBrakeSetTime = Time::InSeconds(0.05);
+const Time kShotEndTimeout = Time::InSeconds(1.0);
+const Time kPrepareFireEndTime = Time::InMS(40);
+
class ShooterMotor
: public aos::control_loops::ControlLoop<control_loops::ShooterGroup> {
public:
@@ -150,12 +157,12 @@
// Enter state STATE_UNLOAD
void Unload() {
state_ = STATE_UNLOAD;
- unload_timeout_ = Time::Now() + Time::InSeconds(1);
+ unload_timeout_ = Time::Now() + kUnloadTimeout;
}
// Enter state STATE_LOAD
void Load() {
state_ = STATE_LOAD;
- load_timeout_ = Time::Now() + Time::InSeconds(1);
+ load_timeout_ = Time::Now() + kLoadTimeout;
}
control_loops::ShooterGroup::Position last_position_;
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 74cb0ec..8aa4c27 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -606,6 +606,9 @@
// 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.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
index 50642b9..546387d 100755
--- a/frc971/control_loops/shooter/shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/shooter_motor_plant.cc
@@ -9,7 +9,7 @@
StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients() {
Eigen::Matrix<double, 3, 3> A;
- A << 0.997145287595, 0.00115072867987, 0.000356210952805, -0.322204030364, -0.000199174994385, 0.0402046120149, 0.0, 0.0, 1.0;
+ A << 0.999391114909, 0.00811316740387, 7.59766686183e-05, -0.113584343654, 0.64780421498, 0.0141730519709, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 3, 1> B;
B << 0.0, 0.0, 1.0;
Eigen::Matrix<double, 1, 3> C;
@@ -25,7 +25,7 @@
StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients() {
Eigen::Matrix<double, 3, 3> A;
- A << 1.0, 0.00115359397892, 0.000356613321821, 0.0, 0.000172163011452, 0.0403047209622, 0.0, 0.0, 1.0;
+ A << 1.0, 0.00811505488455, 7.59852687598e-05, 0.0, 0.648331305446, 0.0141763492481, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 3, 1> B;
B << 0.0, 0.0, 1.0;
Eigen::Matrix<double, 1, 3> C;
@@ -41,17 +41,17 @@
StateFeedbackController<3, 1, 1> MakeSprungShooterController() {
Eigen::Matrix<double, 3, 1> L;
- L << 0.996946112601, 10.71141318, 224.213599484;
+ L << 1.64719532989, 57.0572680832, 636.74290365;
Eigen::Matrix<double, 1, 3> K;
- K << 121.388812879, 5.06126911425, 0.196946112601;
+ K << 450.571849185, 11.8404918938, 0.997195329889;
return StateFeedbackController<3, 1, 1>(L, K, MakeSprungShooterPlantCoefficients());
}
StateFeedbackController<3, 1, 1> MakeShooterController() {
Eigen::Matrix<double, 3, 1> L;
- L << 1.00017216301, 11.0141047888, 223.935057347;
+ L << 1.64833130545, 57.2417604572, 636.668851906;
Eigen::Matrix<double, 1, 3> K;
- K << 122.81439697, 5.05065025388, 0.200172163011;
+ K << 349.173113146, 8.65077618169, 0.848331305446;
return StateFeedbackController<3, 1, 1>(L, K, MakeShooterPlantCoefficients());
}
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
index 4a0e6f2..42b166f 100755
--- a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<2, 1, 1> MakeRawSprungShooterPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.997145287595, 0.00115072867987, -0.322204030364, -0.000199174994385;
+ A << 0.999391114909, 0.00811316740387, -0.113584343654, 0.64780421498;
Eigen::Matrix<double, 2, 1> B;
- B << 0.000356210952805, 0.0402046120149;
+ B << 7.59766686183e-05, 0.0141730519709;
Eigen::Matrix<double, 1, 2> C;
C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00115359397892, 0.0, 0.000172163011452;
+ A << 1.0, 0.00811505488455, 0.0, 0.648331305446;
Eigen::Matrix<double, 2, 1> B;
- B << 0.000356613321821, 0.0403047209622;
+ B << 7.59852687598e-05, 0.0141763492481;
Eigen::Matrix<double, 1, 2> C;
C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
@@ -41,17 +41,17 @@
StateFeedbackController<2, 1, 1> MakeRawSprungShooterController() {
Eigen::Matrix<double, 2, 1> L;
- L << 0.896946112601, 1.86767549049;
+ L << 1.54719532989, 43.9345489758;
Eigen::Matrix<double, 1, 2> K;
- K << 743.451871215, -4.17563006819;
+ K << 2126.06977433, 41.3223370936;
return StateFeedbackController<2, 1, 1>(L, K, MakeRawSprungShooterPlantCoefficients());
}
StateFeedbackController<2, 1, 1> MakeRawShooterController() {
Eigen::Matrix<double, 2, 1> L;
- L << 0.900172163011, 2.15224193635;
+ L << 1.54833130545, 44.1155797675;
Eigen::Matrix<double, 1, 2> K;
- K << 750.532425926, -4.15528738406;
+ K << 2133.83569145, 41.3499425476;
return StateFeedbackController<2, 1, 1>(L, K, MakeRawShooterPlantCoefficients());
}
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index 1150304..d751a43 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -5,6 +5,7 @@
#include "aos/linux_code/init.h"
#include "aos/prime/input/joystick_input.h"
+#include "aos/common/input/driver_station_data.h"
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -29,10 +30,14 @@
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
+const ButtonLocation kClawClosed(3, 7);
+const ButtonLocation kClawOpen(3, 9);
+const ButtonLocation kFire(3, 11);
+const ButtonLocation kUnload(3, 12);
class Reader : public ::aos::input::JoystickInput {
public:
- Reader() {}
+ Reader() : closed_(true) {}
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
static bool is_high_gear = false;
@@ -105,22 +110,34 @@
if (data.PosEdge(kShiftLow)) {
is_high_gear = true;
}
+ if (data.PosEdge(kClawClosed)) {
+ closed_ = true;
+ }
+ if (data.PosEdge(kClawOpen)) {
+ closed_ = false;
+ }
+ double separation_angle = closed_ ? 0.0 : 0.5;
+ double goal_angle = closed_ ? 0.0 : -0.2;
if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
- .bottom_angle(0)
- .separation_angle(0)
- .intake(false).Send()) {
+ .bottom_angle(goal_angle)
+ .separation_angle(separation_angle)
+ .intake(false)
+ .Send()) {
LOG(WARNING, "sending claw goal failed\n");
}
if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
- .shot_power(0)
- .shot_requested(false)
- .unload_requested(true)
- .Send()) {
+ .shot_power(0.25)
+ .shot_requested(data.IsPressed(kFire))
+ .unload_requested(data.IsPressed(kUnload))
+ .Send()) {
LOG(WARNING, "sending shooter goal failed\n");
}
}
}
+
+ private:
+ bool closed_;
};
} // namespace joysticks
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 84ccc70..8c4172d 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -49,13 +49,13 @@
// Translates values from the ADC into voltage.
// TODO(brian): Fix this.
double adc_translate(uint16_t in) {
- static const double kVRefN = 0;
- static const double kVRefP = 3.3;
- static const int kMaximumValue = 0xFFF;
- static const double kDividerGnd = 31.6, kDividerOther = 28;
- return (kVRefN +
- (static_cast<double>(in) / static_cast<double>(kMaximumValue) *
- (kVRefP - kVRefN))) * (kDividerGnd + kDividerOther) / kDividerGnd;
+ static const double kVcc = 5;
+ static const double kR1 = 5, kR2 = 6.65;
+ static const uint16_t kMaximumValue = 0x3FF;
+ return (kVcc *
+ (static_cast<double>(in) / static_cast<double>(kMaximumValue)) -
+ kVcc * kR1) /
+ kR2;
}
double gyro_translate(int64_t in) {
diff --git a/frc971/output/motor_writer.cc b/frc971/output/motor_writer.cc
index 22a120e..e17d6bc 100644
--- a/frc971/output/motor_writer.cc
+++ b/frc971/output/motor_writer.cc
@@ -28,7 +28,7 @@
virtual void RunIteration() {
values_.digital_module = 0;
- values_.pressure_switch_channel = 14;
+ values_.pressure_switch_channel = 1;
values_.compressor_channel = 1;
values_.solenoid_module = 0;
@@ -37,7 +37,7 @@
if (drivetrain.output.IsNewerThanMS(kOutputMaxAgeMS)) {
LOG_STRUCT(DEBUG, "will output", *drivetrain.output.get());
SetPWMOutput(3, drivetrain.output->right_voltage / 12.0, kTalonBounds);
- SetPWMOutput(8, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(6, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
SetSolenoid(7, drivetrain.output->left_high);
SetSolenoid(8, drivetrain.output->right_high);
} else {
@@ -53,7 +53,7 @@
shooter.FetchLatest();
if (shooter.IsNewerThanMS(kOutputMaxAgeMS)) {
LOG_STRUCT(DEBUG, "will output", *shooter.get());
- SetPWMOutput(9, shooter->voltage / 12.0, kTalonBounds);
+ SetPWMOutput(7, shooter->voltage / 12.0, kTalonBounds);
SetSolenoid(6, !shooter->latch_piston);
SetSolenoid(5, !shooter->brake_piston);
} else {
@@ -68,8 +68,8 @@
claw.FetchLatest();
if (claw.IsNewerThanMS(kOutputMaxAgeMS)) {
LOG_STRUCT(DEBUG, "will output", *claw.get());
- SetPWMOutput(6, claw->intake_voltage / 12.0, kTalonBounds);
- SetPWMOutput(7, claw->intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(9, claw->intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(8, claw->intake_voltage / 12.0, kTalonBounds);
SetPWMOutput(1, -claw->bottom_claw_voltage / 12.0, kTalonBounds);
SetPWMOutput(2, claw->top_claw_voltage / 12.0, kTalonBounds);
SetPWMOutput(5, claw->tusk_voltage / 12.0, kTalonBounds); // left