Renamed wrist constants.
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index 796ff62..5c0411a 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -12,43 +12,44 @@
namespace {
-const double kCompHorizontalHallEffectStartAngle = 72 * M_PI / 180.0;
-const double kPracticeHorizontalHallEffectStartAngle = 72 * M_PI / 180.0;
+const double kCompWristHallEffectStartAngle = 72 * M_PI / 180.0;
+const double kPracticeWristHallEffectStartAngle = 72 * M_PI / 180.0;
-const double kCompHorizontalHallEffectStopAngle = 100 * M_PI / 180.0;
-const double kPracticeHorizontalHallEffectStopAngle = 100 * M_PI / 180.0;
+const double kCompWristHallEffectStopAngle = 100 * M_PI / 180.0;
+const double kPracticeWristHallEffectStopAngle = 100 * M_PI / 180.0;
-const double kPracticeHorizontalUpperPhysicalLimit = 95 * M_PI / 180.0;
-const double kCompHorizontalUpperPhysicalLimit = 95 * M_PI / 180.0;
+const double kPracticeWristUpperPhysicalLimit = 95 * M_PI / 180.0;
+const double kCompWristUpperPhysicalLimit = 95 * M_PI / 180.0;
-const double kPracticeHorizontalLowerPhysicalLimit = -37.5 * M_PI / 180.0;
-const double kCompHorizontalLowerPhysicalLimit = -37.5 * M_PI / 180.0;
+const double kPracticeWristLowerPhysicalLimit = -37.5 * M_PI / 180.0;
+const double kCompWristLowerPhysicalLimit = -37.5 * M_PI / 180.0;
-const double kPracticeHorizontalUpperLimit = 93 * M_PI / 180.0;
-const double kCompHorizontalUpperLimit = 93 * M_PI / 180.0;
+const double kPracticeWristUpperLimit = 93 * M_PI / 180.0;
+const double kCompWristUpperLimit = 93 * M_PI / 180.0;
-const double kPracticeHorizontalLowerLimit = -36 * M_PI / 180.0;
-const double kCompHorizontalLowerLimit = -36 * M_PI / 180.0;
+const double kPracticeWristLowerLimit = -36 * M_PI / 180.0;
+const double kCompWristLowerLimit = -36 * M_PI / 180.0;
-const double kHorizontalZeroingSpeed = 1.0;
+const double kWristZeroingSpeed = 1.0;
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
struct Values {
// Wrist hall effect positive and negative edges.
- double horizontal_hall_effect_start_angle;
- double horizontal_hall_effect_stop_angle;
+ double wrist_hall_effect_start_angle;
+ double wrist_hall_effect_stop_angle;
// Upper and lower extreme limits of travel for the wrist.
- double horizontal_upper_limit;
- double horizontal_lower_limit;
+ double wrist_upper_limit;
+ double wrist_lower_limit;
+
// Physical limits. These are here for testing.
- double horizontal_upper_physical_limit;
- double horizontal_lower_physical_limit;
+ double wrist_upper_physical_limit;
+ double wrist_lower_physical_limit;
// Zeroing speed.
- double horizontal_zeroing_speed;
+ double wrist_zeroing_speed;
// what camera_center returns
int camera_center;
@@ -65,23 +66,23 @@
::aos::robot_state->team_id);
switch (::aos::robot_state->team_id) {
case kCompTeamNumber:
- values = new Values{kCompHorizontalHallEffectStartAngle,
- kCompHorizontalHallEffectStopAngle,
- kCompHorizontalUpperLimit,
- kCompHorizontalLowerLimit,
- kCompHorizontalUpperPhysicalLimit,
- kCompHorizontalLowerPhysicalLimit,
- kHorizontalZeroingSpeed,
+ values = new Values{kCompWristHallEffectStartAngle,
+ kCompWristHallEffectStopAngle,
+ kCompWristUpperLimit,
+ kCompWristLowerLimit,
+ kCompWristUpperPhysicalLimit,
+ kCompWristLowerPhysicalLimit,
+ kWristZeroingSpeed,
kCompCameraCenter};
break;
case kPracticeTeamNumber:
- values = new Values{kPracticeHorizontalHallEffectStartAngle,
- kPracticeHorizontalHallEffectStopAngle,
- kPracticeHorizontalUpperLimit,
- kPracticeHorizontalLowerLimit,
- kPracticeHorizontalUpperPhysicalLimit,
- kPracticeHorizontalLowerPhysicalLimit,
- kHorizontalZeroingSpeed,
+ values = new Values{kPracticeWristHallEffectStartAngle,
+ kPracticeWristHallEffectStopAngle,
+ kPracticeWristUpperLimit,
+ kPracticeWristLowerLimit,
+ kPracticeWristUpperPhysicalLimit,
+ kPracticeWristLowerPhysicalLimit,
+ kWristZeroingSpeed,
kPracticeCameraCenter};
break;
default:
@@ -95,50 +96,50 @@
} // namespace
-bool horizontal_hall_effect_start_angle(double *angle) {
+bool wrist_hall_effect_start_angle(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->horizontal_hall_effect_start_angle;
+ *angle = values->wrist_hall_effect_start_angle;
return true;
}
-bool horizontal_hall_effect_stop_angle(double *angle) {
+bool wrist_hall_effect_stop_angle(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->horizontal_hall_effect_stop_angle;
+ *angle = values->wrist_hall_effect_stop_angle;
return true;
}
-bool horizontal_upper_limit(double *angle) {
+bool wrist_upper_limit(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->horizontal_upper_limit;
+ *angle = values->wrist_upper_limit;
return true;
}
-bool horizontal_lower_limit(double *angle) {
+bool wrist_lower_limit(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->horizontal_lower_limit;
+ *angle = values->wrist_lower_limit;
return true;
}
-bool horizontal_upper_physical_limit(double *angle) {
+bool wrist_upper_physical_limit(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->horizontal_upper_physical_limit;
+ *angle = values->wrist_upper_physical_limit;
return true;
}
-bool horizontal_lower_physical_limit(double *angle) {
+bool wrist_lower_physical_limit(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->horizontal_lower_physical_limit;
+ *angle = values->wrist_lower_physical_limit;
return true;
}
-bool horizontal_zeroing_speed(double *speed) {
+bool wrist_zeroing_speed(double *speed) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *speed = values->horizontal_zeroing_speed;
+ *speed = values->wrist_zeroing_speed;
return true;
}
diff --git a/frc971/constants.h b/frc971/constants.h
index 9ac4152..187ab6a 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -14,17 +14,17 @@
const uint16_t kPracticeTeamNumber = 5971;
// Sets *angle to how many radians from horizontal to the location of interest.
-bool horizontal_hall_effect_start_angle(double *angle);
-bool horizontal_hall_effect_stop_angle(double *angle);
+bool wrist_hall_effect_start_angle(double *angle);
+bool wrist_hall_effect_stop_angle(double *angle);
// These are the soft stops for up and down.
-bool horizontal_lower_limit(double *angle);
-bool horizontal_upper_limit(double *angle);
+bool wrist_lower_limit(double *angle);
+bool wrist_upper_limit(double *angle);
// These are the hard stops. Don't use these for anything but testing.
-bool horizontal_lower_physical_limit(double *angle);
-bool horizontal_upper_physical_limit(double *angle);
+bool wrist_lower_physical_limit(double *angle);
+bool wrist_upper_physical_limit(double *angle);
// Returns the speed to move the wrist at when zeroing in rad/sec
-bool horizontal_zeroing_speed(double *speed);
+bool wrist_zeroing_speed(double *speed);
// Sets *center to how many pixels off center the vertical line
// on the camera view is.
diff --git a/frc971/control_loops/wrist.cc b/frc971/control_loops/wrist.cc
index de62580..5bdce25 100644
--- a/frc971/control_loops/wrist.cc
+++ b/frc971/control_loops/wrist.cc
@@ -25,22 +25,22 @@
}
bool WristMotor::FetchConstants() {
- if (!constants::horizontal_lower_limit(&horizontal_lower_limit_)) {
- LOG(ERROR, "Failed to fetch the horizontal lower limit constant.\n");
+ if (!constants::wrist_lower_limit(&wrist_lower_limit_)) {
+ LOG(ERROR, "Failed to fetch the wrist lower limit constant.\n");
return false;
}
- if (!constants::horizontal_upper_limit(&horizontal_upper_limit_)) {
- LOG(ERROR, "Failed to fetch the horizontal upper limit constant.\n");
+ if (!constants::wrist_upper_limit(&wrist_upper_limit_)) {
+ LOG(ERROR, "Failed to fetch the wrist upper limit constant.\n");
return false;
}
- if (!constants::horizontal_hall_effect_start_angle(
- &horizontal_hall_effect_start_angle_)) {
- LOG(ERROR, "Failed to fetch the horizontal start angle constant.\n");
+ if (!constants::wrist_hall_effect_start_angle(
+ &wrist_hall_effect_start_angle_)) {
+ LOG(ERROR, "Failed to fetch the wrist start angle constant.\n");
return false;
}
- if (!constants::horizontal_zeroing_speed(
- &horizontal_zeroing_speed_)) {
- LOG(ERROR, "Failed to fetch the horizontal zeroing speed constant.\n");
+ if (!constants::wrist_zeroing_speed(
+ &wrist_zeroing_speed_)) {
+ LOG(ERROR, "Failed to fetch the wrist zeroing speed constant.\n");
return false;
}
@@ -48,18 +48,18 @@
}
double WristMotor::ClipGoal(double goal) const {
- return std::min(horizontal_upper_limit_,
- std::max(horizontal_lower_limit_, goal));
+ return std::min(wrist_upper_limit_,
+ std::max(wrist_lower_limit_, goal));
}
const double kMaxZeroingVoltage = 5.0;
void WristMotor::WristStateFeedbackLoop::CapU() {
if (wrist_motor_->state_ == READY) {
- if (Y(0, 0) >= wrist_motor_->horizontal_upper_limit_) {
+ if (Y(0, 0) >= wrist_motor_->wrist_upper_limit_) {
U(0, 0) = std::min(0.0, U(0, 0));
}
- if (Y(0, 0) <= wrist_motor_->horizontal_lower_limit_) {
+ if (Y(0, 0) <= wrist_motor_->wrist_lower_limit_) {
U(0, 0) = std::max(0.0, U(0, 0));
}
}
@@ -104,7 +104,7 @@
double absolute_position;
if (position) {
absolute_position =
- position->pos + horizontal_hall_effect_start_angle_;
+ position->pos + wrist_hall_effect_start_angle_;
if (state_ == READY) {
absolute_position -= zero_offset_;
}
@@ -146,8 +146,8 @@
state_ = ZEROING;
} else {
// Slowly creep off the sensor.
- zeroing_position_ -= horizontal_zeroing_speed_ / 100;
- loop_->R << zeroing_position_, -horizontal_zeroing_speed_;
+ zeroing_position_ -= wrist_zeroing_speed_ / 100;
+ loop_->R << zeroing_position_, -wrist_zeroing_speed_;
break;
}
case ZEROING:
@@ -176,8 +176,8 @@
}
} else {
// Slowly creep towards the sensor.
- zeroing_position_ += horizontal_zeroing_speed_ / 100;
- loop_->R << zeroing_position_, horizontal_zeroing_speed_;
+ zeroing_position_ += wrist_zeroing_speed_ / 100;
+ loop_->R << zeroing_position_, wrist_zeroing_speed_;
}
break;
diff --git a/frc971/control_loops/wrist.h b/frc971/control_loops/wrist.h
index 81c3bcb..99d9920 100644
--- a/frc971/control_loops/wrist.h
+++ b/frc971/control_loops/wrist.h
@@ -91,10 +91,10 @@
double last_off_position_;
// Local cache of the wrist geometry constants.
- double horizontal_lower_limit_;
- double horizontal_upper_limit_;
- double horizontal_hall_effect_start_angle_;
- double horizontal_zeroing_speed_;
+ double wrist_lower_limit_;
+ double wrist_upper_limit_;
+ double wrist_hall_effect_start_angle_;
+ double wrist_zeroing_speed_;
DISALLOW_COPY_AND_ASSIGN(WristMotor);
};
diff --git a/frc971/control_loops/wrist_lib_test.cc b/frc971/control_loops/wrist_lib_test.cc
index 36474ae..2e329af 100644
--- a/frc971/control_loops/wrist_lib_test.cc
+++ b/frc971/control_loops/wrist_lib_test.cc
@@ -57,12 +57,12 @@
void SendPositionMessage() {
const double angle = GetPosition();
- double horizontal_hall_effect_start_angle;
- ASSERT_TRUE(constants::horizontal_hall_effect_start_angle(
- &horizontal_hall_effect_start_angle));
- double horizontal_hall_effect_stop_angle;
- ASSERT_TRUE(constants::horizontal_hall_effect_stop_angle(
- &horizontal_hall_effect_stop_angle));
+ double wrist_hall_effect_start_angle;
+ ASSERT_TRUE(constants::wrist_hall_effect_start_angle(
+ &wrist_hall_effect_start_angle));
+ double wrist_hall_effect_stop_angle;
+ ASSERT_TRUE(constants::wrist_hall_effect_stop_angle(
+ &wrist_hall_effect_stop_angle));
::aos::ScopedMessagePtr<control_loops::WristLoop::Position> position =
my_wrist_loop_.position.MakeMessage();
@@ -71,8 +71,8 @@
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
double abs_position = GetAbsolutePosition();
- if (abs_position >= horizontal_hall_effect_start_angle &&
- abs_position <= horizontal_hall_effect_stop_angle) {
+ if (abs_position >= wrist_hall_effect_start_angle &&
+ abs_position <= wrist_hall_effect_stop_angle) {
position->hall_effect = true;
} else {
position->hall_effect = false;
@@ -80,11 +80,11 @@
// Only set calibration if it changed last cycle. Calibration starts out
// with a value of 0.
- if ((last_position_ < horizontal_hall_effect_start_angle ||
- last_position_ > horizontal_hall_effect_stop_angle) &&
+ if ((last_position_ < wrist_hall_effect_start_angle ||
+ last_position_ > wrist_hall_effect_stop_angle) &&
position->hall_effect) {
calibration_value_ =
- horizontal_hall_effect_start_angle - initial_position_;
+ wrist_hall_effect_start_angle - initial_position_;
}
position->calibration = calibration_value_;
@@ -99,15 +99,17 @@
wrist_plant_->Update();
// Assert that we are in the right physical range.
- double horizontal_upper_physical_limit;
- ASSERT_TRUE(constants::horizontal_upper_physical_limit(
- &horizontal_upper_physical_limit));
- double horizontal_lower_physical_limit;
- ASSERT_TRUE(constants::horizontal_lower_physical_limit(
- &horizontal_lower_physical_limit));
+ double wrist_upper_physical_limit;
+ ASSERT_TRUE(constants::wrist_upper_physical_limit(
+ &wrist_upper_physical_limit));
+ double wrist_lower_physical_limit;
+ ASSERT_TRUE(constants::wrist_lower_physical_limit(
+ &wrist_lower_physical_limit));
- EXPECT_GE(horizontal_upper_physical_limit, wrist_plant_->Y(0, 0));
- EXPECT_LE(horizontal_lower_physical_limit, wrist_plant_->Y(0, 0));
+ EXPECT_GE(wrist_upper_physical_limit,
+ wrist_plant_->Y(0, 0));
+ EXPECT_LE(wrist_lower_physical_limit,
+ wrist_plant_->Y(0, 0));
}
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;