Renamed constants to be in a claw structure.
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 7089f5a..87b90dc 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -53,15 +53,16 @@
true,
control_loops::MakeVClutchDrivetrainLoop,
control_loops::MakeClutchDrivetrainLoop,
- 0.5,
- 0.1,
- 0.1,
- 0.0,
- 1.57,
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
- 0.01, // claw_unimportant_epsilon
- 0.9, // start_fine_tune_pos
+ {0.5,
+ 0.1,
+ 0.1,
+ 0.0,
+ 1.57,
+ {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
+ {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
+ 0.01, // claw_unimportant_epsilon
+ 0.9, // start_fine_tune_pos
+ }
};
break;
case kPracticeTeamNumber:
@@ -74,15 +75,16 @@
false,
control_loops::MakeVDogDrivetrainLoop,
control_loops::MakeDogDrivetrainLoop,
- 0.5,
- 0.2,
- 0.1,
- 0.0,
- 1.57,
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
- 0.01, // claw_unimportant_epsilon
- 0.9, // start_fine_tune_pos
+ {0.5,
+ 0.2,
+ 0.1,
+ 0.0,
+ 1.57,
+ {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
+ {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
+ 0.01, // claw_unimportant_epsilon
+ 0.9, // start_fine_tune_pos
+ }
};
break;
default:
diff --git a/frc971/constants.h b/frc971/constants.h
index 1a4f39a..94061cc 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -40,36 +40,38 @@
::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
- double claw_zeroing_off_speed;
- double claw_zeroing_speed;
- double claw_zeroing_separation;
+ struct Claws {
+ double claw_zeroing_off_speed;
+ double claw_zeroing_speed;
+ double claw_zeroing_separation;
- // claw seperation that would be considered a collision
- double claw_min_seperation;
- double claw_max_seperation;
+ // claw seperation that would be considered a collision
+ double claw_min_seperation;
+ double claw_max_seperation;
- // Three hall effects are known as front, calib and back
- struct AnglePair {
- double lower_angle;
- double upper_angle;
+ // Three hall effects are known as front, calib and back
+ struct AnglePair {
+ double lower_angle;
+ double upper_angle;
+ };
+
+ struct Claw {
+ double lower_hard_limit;
+ double upper_hard_limit;
+ double lower_limit;
+ double upper_limit;
+ AnglePair front;
+ AnglePair calibration;
+ AnglePair back;
+ };
+
+ Claw upper_claw;
+ Claw lower_claw;
+
+ double claw_unimportant_epsilon;
+ double start_fine_tune_pos;
};
-
- struct Claw {
- double lower_hard_limit;
- double upper_hard_limit;
- double lower_limit;
- double upper_limit;
- AnglePair front;
- AnglePair calibration;
- AnglePair back;
- };
-
-
- Claw upper_claw;
- Claw lower_claw;
-
- double claw_unimportant_epsilon;
- double start_fine_tune_pos;
+ Claws claw;
};
// Creates (once) a Values instance and returns a reference to it.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 49b1886..667d2a4 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -69,7 +69,7 @@
const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
- const constants::Values::Claw &claw_values, double *edge_encoder,
+ const constants::Values::Claws::Claw &claw_values, double *edge_encoder,
double *edge_angle) {
// TODO(austin): Validate that the hall effect edge makes sense.
@@ -284,25 +284,25 @@
LOG(DEBUG, "Calibrating the bottom of the claw\n");
if (!doing_calibration_fine_tune_) {
if (::std::abs(bottom_absolute_position() -
- values.start_fine_tune_pos) <
- values.claw_unimportant_epsilon) {
+ values.claw.start_fine_tune_pos) <
+ values.claw.claw_unimportant_epsilon) {
doing_calibration_fine_tune_ = true;
- bottom_claw_goal_ += values.claw_zeroing_speed * dt;
+ bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
LOG(DEBUG, "Ready to fine tune the bottom\n");
} else {
// send bottom to zeroing start
- bottom_claw_goal_ = values.start_fine_tune_pos;
+ bottom_claw_goal_ = values.claw.start_fine_tune_pos;
LOG(DEBUG, "Going to the start position for the bottom\n");
}
} else {
- bottom_claw_goal_ += values.claw_zeroing_speed * dt;
+ bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
if (top_claw_.front_hall_effect() || top_claw_.back_hall_effect() ||
bottom_claw_.front_hall_effect() ||
bottom_claw_.back_hall_effect()) {
// We shouldn't hit a limit, but if we do, go back to the zeroing
// point and try again.
doing_calibration_fine_tune_ = false;
- bottom_claw_goal_ = values.start_fine_tune_pos;
+ bottom_claw_goal_ = values.claw.start_fine_tune_pos;
LOG(DEBUG, "Found a limit, starting over.\n");
}
@@ -312,14 +312,14 @@
// do calibration
bottom_claw_.SetCalibration(
position->bottom.posedge_value,
- values.lower_claw.calibration.lower_angle);
+ 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.start_fine_tune_pos;
+ bottom_claw_goal_ = values.claw.start_fine_tune_pos;
}
} else {
LOG(DEBUG, "Fine tuning\n");
@@ -327,48 +327,50 @@
}
// now set the top claw to track
- top_claw_goal_ = bottom_claw_goal_ + values.claw_zeroing_separation;
+ top_claw_goal_ = bottom_claw_goal_ + values.claw.claw_zeroing_separation;
} else {
// bottom claw must be calibrated, start on the top
if (!doing_calibration_fine_tune_) {
- if (::std::abs(top_absolute_position() - values.start_fine_tune_pos) <
- values.claw_unimportant_epsilon) {
+ if (::std::abs(top_absolute_position() -
+ values.claw.start_fine_tune_pos) <
+ values.claw.claw_unimportant_epsilon) {
doing_calibration_fine_tune_ = true;
- top_claw_goal_ += values.claw_zeroing_speed * dt;
+ top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
LOG(DEBUG, "Ready to fine tune the top\n");
} else {
// send top to zeroing start
- top_claw_goal_ = values.start_fine_tune_pos;
+ top_claw_goal_ = values.claw.start_fine_tune_pos;
LOG(DEBUG, "Going to the start position for the top\n");
}
} else {
- top_claw_goal_ += values.claw_zeroing_speed * dt;
+ top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
if (top_claw_.front_hall_effect() || top_claw_.back_hall_effect() ||
bottom_claw_.front_hall_effect() ||
bottom_claw_.back_hall_effect()) {
// this should not happen, but now we know it won't
doing_calibration_fine_tune_ = false;
- top_claw_goal_ = values.start_fine_tune_pos;
+ top_claw_goal_ = values.claw.start_fine_tune_pos;
LOG(DEBUG, "Found a limit, starting over.\n");
}
if (top_claw_.calibration_hall_effect()) {
if (top_claw_.calibration_hall_effect_posedge_count_changed() &&
position) {
// do calibration
- top_claw_.SetCalibration(position->top.posedge_value,
- values.upper_claw.calibration.lower_angle);
+ top_claw_.SetCalibration(
+ position->top.posedge_value,
+ values.claw.upper_claw.calibration.lower_angle);
top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
// calinrated 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.start_fine_tune_pos;
+ top_claw_goal_ = values.claw.start_fine_tune_pos;
}
}
}
// now set the bottom claw to track
- bottom_claw_goal_ = top_claw_goal_ - values.claw_zeroing_separation;
+ bottom_claw_goal_ = top_claw_goal_ - values.claw.claw_zeroing_separation;
}
mode = FINE_TUNE;
} else {
@@ -400,8 +402,8 @@
if (enabled) {
// Time to slowly move back up to find any position to narrow down the
// zero.
- top_claw_goal_ += values.claw_zeroing_off_speed * dt;
- bottom_claw_goal_ += values.claw_zeroing_off_speed * dt;
+ top_claw_goal_ += values.claw.claw_zeroing_off_speed * dt;
+ bottom_claw_goal_ += values.claw.claw_zeroing_off_speed * dt;
// TODO(austin): Goal velocity too!
LOG(DEBUG, "Bottom is known.\n");
}
@@ -409,8 +411,8 @@
// We don't know where either claw is. Slowly start moving down to find
// any hall effect.
if (enabled) {
- top_claw_goal_ -= values.claw_zeroing_off_speed * dt;
- bottom_claw_goal_ -= values.claw_zeroing_off_speed * dt;
+ top_claw_goal_ -= values.claw.claw_zeroing_off_speed * dt;
+ bottom_claw_goal_ -= values.claw.claw_zeroing_off_speed * dt;
// TODO(austin): Goal velocity too!
LOG(DEBUG, "Both are unknown.\n");
}
@@ -418,14 +420,14 @@
if (enabled) {
top_claw_.SetCalibrationOnEdge(
- values.upper_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ values.claw.upper_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
bottom_claw_.SetCalibrationOnEdge(
- values.lower_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ values.claw.lower_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
} else {
top_claw_.SetCalibrationOnEdge(
- values.upper_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ values.claw.upper_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
bottom_claw_.SetCalibrationOnEdge(
- values.lower_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ values.claw.lower_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
}
mode = UNKNOWN_LOCATION;
}
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 8157b67..bef130b 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -143,7 +143,7 @@
// Returns true if an edge was detected in the last cycle and then sets the
// edge_position to the absolute position of the edge.
- bool GetPositionOfEdge(const constants::Values::Claw &claw,
+ bool GetPositionOfEdge(const constants::Values::Claws::Claw &claw,
double *edge_encoder, double *edge_angle);
#undef COUNT_SETTER_GETTER
@@ -186,7 +186,7 @@
// encoder value.
void SetCalibration(double edge_encoder, double edge_angle);
- bool SetCalibrationOnEdge(const constants::Values::Claw &claw_values,
+ bool SetCalibrationOnEdge(const constants::Values::Claws::Claw &claw_values,
JointZeroingState zeroing_state) {
double edge_encoder;
double edge_angle;
@@ -208,7 +208,7 @@
// encoder value.
void SetCalibration(double edge_encoder, double edge_angle);
- bool SetCalibrationOnEdge(const constants::Values::Claw &claw_values,
+ bool SetCalibrationOnEdge(const constants::Values::Claws::Claw &claw_values,
JointZeroingState zeroing_state) {
double edge_encoder;
double edge_angle;
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index dbf4ae3..c6c7e89 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -83,7 +83,7 @@
}
// Makes sure pos is inside range (inclusive)
- bool CheckRange(double pos, struct constants::Values::AnglePair pair) {
+ bool CheckRange(double pos, struct constants::Values::Claws::AnglePair pair) {
return (pos >= pair.lower_angle && pos <= pair.upper_angle);
}
@@ -102,18 +102,18 @@
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
position->top.front_hall_effect =
- CheckRange(pos[TOP_CLAW], values.upper_claw.front);
+ CheckRange(pos[TOP_CLAW], values.claw.upper_claw.front);
position->top.calibration_hall_effect =
- CheckRange(pos[TOP_CLAW], values.upper_claw.calibration);
+ CheckRange(pos[TOP_CLAW], values.claw.upper_claw.calibration);
position->top.back_hall_effect =
- CheckRange(pos[TOP_CLAW], values.upper_claw.back);
+ CheckRange(pos[TOP_CLAW], values.claw.upper_claw.back);
position->bottom.front_hall_effect =
- CheckRange(pos[BOTTOM_CLAW], values.lower_claw.front);
+ CheckRange(pos[BOTTOM_CLAW], values.claw.lower_claw.front);
position->bottom.calibration_hall_effect =
- CheckRange(pos[BOTTOM_CLAW], values.lower_claw.calibration);
+ CheckRange(pos[BOTTOM_CLAW], values.claw.lower_claw.calibration);
position->bottom.back_hall_effect =
- CheckRange(pos[BOTTOM_CLAW], values.lower_claw.back);
+ CheckRange(pos[BOTTOM_CLAW], values.claw.lower_claw.back);
}
// Sends out the position queue messages.
@@ -141,28 +141,28 @@
!last_position_.top.front_hall_effect) {
++position->top.front_hall_effect_posedge_count;
- if (last_top_position < values.upper_claw.front.lower_angle) {
+ if (last_top_position < values.claw.upper_claw.front.lower_angle) {
LOG(DEBUG, "Top: Positive lower edge front hall effect\n");
- position->top.posedge_value =
- values.upper_claw.front.lower_angle - initial_position_[TOP_CLAW];
+ position->top.posedge_value = values.claw.upper_claw.front.lower_angle -
+ initial_position_[TOP_CLAW];
} else {
LOG(DEBUG, "Top: Positive upper edge front hall effect\n");
- position->top.posedge_value =
- values.upper_claw.front.upper_angle - initial_position_[TOP_CLAW];
+ position->top.posedge_value = values.claw.upper_claw.front.upper_angle -
+ initial_position_[TOP_CLAW];
}
}
if (!position->top.front_hall_effect &&
last_position_.top.front_hall_effect) {
++position->top.front_hall_effect_negedge_count;
- if (top_position < values.upper_claw.front.lower_angle) {
+ if (top_position < values.claw.upper_claw.front.lower_angle) {
LOG(DEBUG, "Top: Negative lower edge front hall effect\n");
- position->top.negedge_value =
- values.upper_claw.front.lower_angle - initial_position_[TOP_CLAW];
+ position->top.negedge_value = values.claw.upper_claw.front.lower_angle -
+ initial_position_[TOP_CLAW];
} else {
LOG(DEBUG, "Top: Negative upper edge front hall effect\n");
- position->top.negedge_value =
- values.upper_claw.front.upper_angle - initial_position_[TOP_CLAW];
+ position->top.negedge_value = values.claw.upper_claw.front.upper_angle -
+ initial_position_[TOP_CLAW];
}
}
@@ -171,29 +171,32 @@
!last_position_.top.calibration_hall_effect) {
++position->top.calibration_hall_effect_posedge_count;
- if (last_top_position < values.upper_claw.calibration.lower_angle) {
+ if (last_top_position < values.claw.upper_claw.calibration.lower_angle) {
LOG(DEBUG, "Top: Positive lower edge calibration hall effect\n");
position->top.posedge_value =
- values.upper_claw.calibration.lower_angle -
+ values.claw.upper_claw.calibration.lower_angle -
initial_position_[TOP_CLAW];
} else {
LOG(DEBUG, "Top: Positive upper edge calibration hall effect\n");
position->top.posedge_value =
- values.upper_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
+ values.claw.upper_claw.calibration.upper_angle -
+ initial_position_[TOP_CLAW];
}
}
if (!position->top.calibration_hall_effect &&
last_position_.top.calibration_hall_effect) {
++position->top.calibration_hall_effect_negedge_count;
- if (top_position < values.upper_claw.calibration.lower_angle) {
+ if (top_position < values.claw.upper_claw.calibration.lower_angle) {
LOG(DEBUG, "Top: Negative lower edge calibration hall effect\n");
position->top.negedge_value =
- values.upper_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
+ values.claw.upper_claw.calibration.lower_angle -
+ initial_position_[TOP_CLAW];
} else {
LOG(DEBUG, "Top: Negative upper edge calibration hall effect\n");
position->top.negedge_value =
- values.upper_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
+ values.claw.upper_claw.calibration.upper_angle -
+ initial_position_[TOP_CLAW];
}
}
@@ -202,28 +205,28 @@
!last_position_.top.back_hall_effect) {
++position->top.back_hall_effect_posedge_count;
- if (last_top_position < values.upper_claw.back.lower_angle) {
+ if (last_top_position < values.claw.upper_claw.back.lower_angle) {
LOG(DEBUG, "Top: Positive lower edge back hall effect\n");
- position->top.posedge_value =
- values.upper_claw.back.lower_angle - initial_position_[TOP_CLAW];
+ position->top.posedge_value = values.claw.upper_claw.back.lower_angle -
+ initial_position_[TOP_CLAW];
} else {
LOG(DEBUG, "Top: Positive upper edge back hall effect\n");
- position->top.posedge_value =
- values.upper_claw.back.upper_angle - initial_position_[TOP_CLAW];
+ position->top.posedge_value = values.claw.upper_claw.back.upper_angle -
+ initial_position_[TOP_CLAW];
}
}
if (!position->top.back_hall_effect &&
last_position_.top.back_hall_effect) {
++position->top.back_hall_effect_negedge_count;
- if (top_position < values.upper_claw.back.lower_angle) {
+ if (top_position < values.claw.upper_claw.back.lower_angle) {
LOG(DEBUG, "Top: Negative upper edge back hall effect\n");
- position->top.negedge_value =
- values.upper_claw.back.lower_angle - initial_position_[TOP_CLAW];
+ position->top.negedge_value = values.claw.upper_claw.back.lower_angle -
+ initial_position_[TOP_CLAW];
} else {
LOG(DEBUG, "Top: Negative lower edge back hall effect\n");
- position->top.negedge_value =
- values.upper_claw.back.upper_angle - initial_position_[TOP_CLAW];
+ position->top.negedge_value = values.claw.upper_claw.back.upper_angle -
+ initial_position_[TOP_CLAW];
}
}
@@ -233,28 +236,32 @@
!last_position_.bottom.front_hall_effect) {
++position->bottom.front_hall_effect_posedge_count;
- if (last_bottom_position < values.lower_claw.front.lower_angle) {
+ if (last_bottom_position < values.claw.lower_claw.front.lower_angle) {
LOG(DEBUG, "Bottom: Positive lower edge front hall effect\n");
position->bottom.posedge_value =
- values.lower_claw.front.lower_angle - initial_position_[BOTTOM_CLAW];
+ values.claw.lower_claw.front.lower_angle -
+ initial_position_[BOTTOM_CLAW];
} else {
LOG(DEBUG, "Bottom: Positive upper edge front hall effect\n");
position->bottom.posedge_value =
- values.lower_claw.front.upper_angle - initial_position_[BOTTOM_CLAW];
+ values.claw.lower_claw.front.upper_angle -
+ initial_position_[BOTTOM_CLAW];
}
}
if (!position->bottom.front_hall_effect &&
last_position_.bottom.front_hall_effect) {
++position->bottom.front_hall_effect_negedge_count;
- if (bottom_position < values.lower_claw.front.lower_angle) {
+ if (bottom_position < values.claw.lower_claw.front.lower_angle) {
LOG(DEBUG, "Bottom: Negative lower edge front hall effect\n");
position->bottom.negedge_value =
- values.lower_claw.front.lower_angle - initial_position_[BOTTOM_CLAW];
+ values.claw.lower_claw.front.lower_angle -
+ initial_position_[BOTTOM_CLAW];
} else {
LOG(DEBUG, "Bottom: Negative upper edge front hall effect\n");
position->bottom.negedge_value =
- values.lower_claw.front.upper_angle - initial_position_[BOTTOM_CLAW];
+ values.claw.lower_claw.front.upper_angle -
+ initial_position_[BOTTOM_CLAW];
}
}
@@ -263,15 +270,16 @@
!last_position_.bottom.calibration_hall_effect) {
++position->bottom.calibration_hall_effect_posedge_count;
- if (last_bottom_position < values.lower_claw.calibration.lower_angle) {
+ if (last_bottom_position <
+ values.claw.lower_claw.calibration.lower_angle) {
LOG(DEBUG, "Bottom: Positive lower edge calibration hall effect\n");
position->bottom.posedge_value =
- values.lower_claw.calibration.lower_angle -
+ values.claw.lower_claw.calibration.lower_angle -
initial_position_[BOTTOM_CLAW];
} else {
LOG(DEBUG, "Bottom: Positive upper edge calibration hall effect\n");
position->bottom.posedge_value =
- values.lower_claw.calibration.upper_angle -
+ values.claw.lower_claw.calibration.upper_angle -
initial_position_[BOTTOM_CLAW];
}
}
@@ -279,15 +287,15 @@
last_position_.bottom.calibration_hall_effect) {
++position->bottom.calibration_hall_effect_negedge_count;
- if (bottom_position < values.lower_claw.calibration.lower_angle) {
+ if (bottom_position < values.claw.lower_claw.calibration.lower_angle) {
LOG(DEBUG, "Bottom: Negative lower edge calibration hall effect\n");
position->bottom.negedge_value =
- values.lower_claw.calibration.lower_angle -
+ values.claw.lower_claw.calibration.lower_angle -
initial_position_[BOTTOM_CLAW];
} else {
LOG(DEBUG, "Bottom: Negative upper edge calibration hall effect\n");
position->bottom.negedge_value =
- values.lower_claw.calibration.upper_angle -
+ values.claw.lower_claw.calibration.upper_angle -
initial_position_[BOTTOM_CLAW];
}
}
@@ -297,28 +305,32 @@
!last_position_.bottom.back_hall_effect) {
++position->bottom.back_hall_effect_posedge_count;
- if (last_bottom_position < values.lower_claw.back.lower_angle) {
+ if (last_bottom_position < values.claw.lower_claw.back.lower_angle) {
LOG(DEBUG, "Bottom: Positive lower edge back hall effect\n");
position->bottom.posedge_value =
- values.lower_claw.back.lower_angle - initial_position_[BOTTOM_CLAW];
+ values.claw.lower_claw.back.lower_angle -
+ initial_position_[BOTTOM_CLAW];
} else {
LOG(DEBUG, "Bottom: Positive upper edge back hall effect\n");
position->bottom.posedge_value =
- values.lower_claw.back.upper_angle - initial_position_[BOTTOM_CLAW];
+ values.claw.lower_claw.back.upper_angle -
+ initial_position_[BOTTOM_CLAW];
}
}
if (!position->bottom.back_hall_effect &&
last_position_.bottom.back_hall_effect) {
++position->bottom.back_hall_effect_negedge_count;
- if (bottom_position < values.lower_claw.back.lower_angle) {
+ if (bottom_position < values.claw.lower_claw.back.lower_angle) {
LOG(DEBUG, "Bottom: Negative lower edge back hall effect\n");
position->bottom.negedge_value =
- values.lower_claw.back.lower_angle - initial_position_[BOTTOM_CLAW];
+ values.claw.lower_claw.back.lower_angle -
+ initial_position_[BOTTOM_CLAW];
} else {
LOG(DEBUG, "Bottom: Negative upper edge back hall effect\n");
position->bottom.negedge_value =
- values.lower_claw.back.upper_angle - initial_position_[BOTTOM_CLAW];
+ values.claw.lower_claw.back.upper_angle -
+ initial_position_[BOTTOM_CLAW];
}
}
@@ -339,16 +351,16 @@
claw_plant_->Update();
// Check that the claw is within the limits.
- EXPECT_GE(v.upper_claw.upper_hard_limit, claw_plant_->Y(0, 0));
- EXPECT_LE(v.upper_claw.lower_hard_limit, claw_plant_->Y(0, 0));
+ EXPECT_GE(v.claw.upper_claw.upper_limit, claw_plant_->Y(0, 0));
+ EXPECT_LE(v.claw.upper_claw.lower_hard_limit, claw_plant_->Y(0, 0));
- EXPECT_GE(v.lower_claw.upper_hard_limit, claw_plant_->Y(1, 0));
- EXPECT_LE(v.lower_claw.lower_hard_limit, claw_plant_->Y(1, 0));
+ EXPECT_GE(v.claw.lower_claw.upper_hard_limit, claw_plant_->Y(1, 0));
+ EXPECT_LE(v.claw.lower_claw.lower_hard_limit, claw_plant_->Y(1, 0));
EXPECT_LE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
- v.claw_max_seperation);
+ v.claw.claw_max_seperation);
EXPECT_GE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
- v.claw_min_seperation);
+ v.claw.claw_min_seperation);
}
// The whole claw.
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> claw_plant_;
@@ -391,7 +403,7 @@
".frc971.control_loops.claw_queue_group.status"),
claw_motor_(&claw_queue_group),
claw_motor_plant_(0.4, 0.2),
- min_seperation_(constants::GetValues().claw_min_seperation) {
+ min_seperation_(constants::GetValues().claw.claw_min_seperation) {
// Flush the robot state queue so we can use clean shared memory for this
// test.
::aos::robot_state.Clear();