Add stuff we have so far to wpilib_interface.
Change-Id: If880956affacf585d12f4df799f3c6426cf677f1
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index 7cd67e3..e43c245 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -84,18 +84,16 @@
}
void Intake::CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal) {
- const auto &values = constants::GetValues();
-
// Limit the goal to min/max allowable angles.
- if ((*goal)(0, 0) >= values.intake.limits.upper) {
+ if ((*goal)(0, 0) >= constants::Values::kIntakeRange.upper) {
LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
- values.intake.limits.upper);
- (*goal)(0, 0) = values.intake.limits.upper;
+ constants::Values::kIntakeRange.upper);
+ (*goal)(0, 0) = constants::Values::kIntakeRange.upper;
}
- if ((*goal)(0, 0) <= values.intake.limits.lower) {
+ if ((*goal)(0, 0) <= constants::Values::kIntakeRange.lower) {
LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
- values.intake.limits.lower);
- (*goal)(0, 0) = values.intake.limits.lower;
+ constants::Values::kIntakeRange.lower);
+ (*goal)(0, 0) = constants::Values::kIntakeRange.lower;
}
}
@@ -133,13 +131,12 @@
}
bool Intake::CheckHardLimits() {
- const auto &values = constants::GetValues();
// Returns whether hard limits have been exceeded.
- if (angle() >= values.intake.limits.upper_hard ||
- angle() <= values.intake.limits.lower_hard) {
+ if (angle() >= constants::Values::kIntakeRange.upper_hard ||
+ angle() <= constants::Values::kIntakeRange.lower_hard) {
LOG(ERROR, "Intake at %f out of bounds [%f, %f], ESTOPing\n", angle(),
- values.intake.limits.lower_hard, values.intake.limits.upper_hard);
+ constants::Values::kIntakeRange.lower_hard, constants::Values::kIntakeRange.upper_hard);
return true;
}
@@ -255,30 +252,29 @@
void Arm::CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal) {
// Limit the goals to min/max allowable angles.
- const auto &values = constants::GetValues();
- if ((*goal)(0, 0) >= values.shoulder.limits.upper) {
+ if ((*goal)(0, 0) >= constants::Values::kShoulderRange.upper) {
LOG(WARNING, "Shoulder goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
- values.shoulder.limits.upper);
- (*goal)(0, 0) = values.shoulder.limits.upper;
+ constants::Values::kShoulderRange.upper);
+ (*goal)(0, 0) = constants::Values::kShoulderRange.upper;
}
- if ((*goal)(0, 0) <= values.shoulder.limits.lower) {
+ if ((*goal)(0, 0) <= constants::Values::kShoulderRange.lower) {
LOG(WARNING, "Shoulder goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
- values.shoulder.limits.lower);
- (*goal)(0, 0) = values.shoulder.limits.lower;
+ constants::Values::kShoulderRange.lower);
+ (*goal)(0, 0) = constants::Values::kShoulderRange.lower;
}
const double wrist_goal_angle_ungrounded = (*goal)(2, 0) - (*goal)(0, 0);
- if (wrist_goal_angle_ungrounded >= values.wrist.limits.upper) {
+ if (wrist_goal_angle_ungrounded >= constants::Values::kWristRange.upper) {
LOG(WARNING, "Wrist goal %s above limit, %f > %f\n", name,
- wrist_goal_angle_ungrounded, values.wrist.limits.upper);
- (*goal)(2, 0) = values.wrist.limits.upper + (*goal)(0, 0);
+ wrist_goal_angle_ungrounded, constants::Values::kWristRange.upper);
+ (*goal)(2, 0) = constants::Values::kWristRange.upper + (*goal)(0, 0);
}
- if (wrist_goal_angle_ungrounded <= values.wrist.limits.lower) {
+ if (wrist_goal_angle_ungrounded <= constants::Values::kWristRange.lower) {
LOG(WARNING, "Wrist goal %s below limit, %f < %f\n", name,
- wrist_goal_angle_ungrounded, values.wrist.limits.lower);
- (*goal)(2, 0) = values.wrist.limits.lower + (*goal)(0, 0);
+ wrist_goal_angle_ungrounded, constants::Values::kWristRange.lower);
+ (*goal)(2, 0) = constants::Values::kWristRange.lower + (*goal)(0, 0);
}
}
@@ -313,20 +309,19 @@
}
bool Arm::CheckHardLimits() {
- const auto &values = constants::GetValues();
- if (shoulder_angle() >= values.shoulder.limits.upper_hard ||
- shoulder_angle() <= values.shoulder.limits.lower_hard) {
+ if (shoulder_angle() >= constants::Values::kShoulderRange.upper_hard ||
+ shoulder_angle() <= constants::Values::kShoulderRange.lower_hard) {
LOG(ERROR, "Shoulder at %f out of bounds [%f, %f], ESTOPing\n",
- shoulder_angle(), values.shoulder.limits.lower_hard,
- values.shoulder.limits.upper_hard);
+ shoulder_angle(), constants::Values::kShoulderRange.lower_hard,
+ constants::Values::kShoulderRange.upper_hard);
return true;
}
- if (wrist_angle() - shoulder_angle() >= values.wrist.limits.upper_hard ||
- wrist_angle() - shoulder_angle() <= values.wrist.limits.lower_hard) {
+ if (wrist_angle() - shoulder_angle() >= constants::Values::kWristRange.upper_hard ||
+ wrist_angle() - shoulder_angle() <= constants::Values::kWristRange.lower_hard) {
LOG(ERROR, "Wrist at %f out of bounds [%f, %f], ESTOPing\n",
- wrist_angle() - shoulder_angle(), values.wrist.limits.lower_hard,
- values.wrist.limits.upper_hard);
+ wrist_angle() - shoulder_angle(), constants::Values::kWristRange.lower_hard,
+ constants::Values::kWristRange.upper_hard);
return true;
}
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 0db3112..fa227bd 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -14,6 +14,7 @@
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/control_loops/superstructure/intake_plant.h"
#include "y2016/control_loops/superstructure/arm_plant.h"
+
#include "y2016/constants.h"
using ::aos::time::Time;
@@ -80,9 +81,11 @@
SuperstructureSimulation()
: intake_plant_(new IntakePlant(MakeIntakePlant())),
plant_arm_(new ArmPlant(MakeArmPlant())),
- pot_encoder_intake_(INTAKE_ENCODER_INDEX_DIFFERENCE),
- pot_encoder_shoulder_(SHOULDER_ENCODER_INDEX_DIFFERENCE),
- pot_encoder_wrist_(WRIST_ENCODER_INDEX_DIFFERENCE),
+ pot_encoder_intake_(
+ constants::Values::kIntakeEncoderIndexDifference),
+ pot_encoder_shoulder_(
+ constants::Values::kShoulderEncoderIndexDifference),
+ pot_encoder_wrist_(constants::Values::kWristEncoderIndexDifference),
superstructure_queue_(".y2016.control_loops.superstructure", 0x0,
".y2016.control_loops.superstructure.goal",
".y2016.control_loops.superstructure.status",
@@ -164,14 +167,12 @@
pot_encoder_wrist_.MoveTo(angle_wrist);
// Validate that everything is within range.
- EXPECT_GE(angle_intake, constants::GetValues().intake.limits.lower_hard);
- EXPECT_LE(angle_intake, constants::GetValues().intake.limits.upper_hard);
- EXPECT_GE(angle_shoulder,
- constants::GetValues().shoulder.limits.lower_hard);
- EXPECT_LE(angle_shoulder,
- constants::GetValues().shoulder.limits.upper_hard);
- EXPECT_GE(angle_wrist, constants::GetValues().wrist.limits.lower_hard);
- EXPECT_LE(angle_wrist, constants::GetValues().wrist.limits.upper_hard);
+ EXPECT_GE(angle_intake, constants::Values::kIntakeRange.lower_hard);
+ EXPECT_LE(angle_intake, constants::Values::kIntakeRange.upper_hard);
+ EXPECT_GE(angle_shoulder, constants::Values::kShoulderRange.lower_hard);
+ EXPECT_LE(angle_shoulder, constants::Values::kShoulderRange.upper_hard);
+ EXPECT_GE(angle_wrist, constants::Values::kWristRange.lower_hard);
+ EXPECT_LE(angle_wrist, constants::Values::kWristRange.upper_hard);
}
private:
@@ -301,12 +302,11 @@
// Check that we are near our soft limit.
superstructure_queue_.status.FetchLatest();
- const auto &values = constants::GetValues();
- EXPECT_NEAR(values.intake.limits.upper,
+ EXPECT_NEAR(constants::Values::kIntakeRange.upper,
superstructure_queue_.status->intake.angle, 0.001);
- EXPECT_NEAR(values.shoulder.limits.upper,
+ EXPECT_NEAR(constants::Values::kShoulderRange.upper,
superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(values.wrist.limits.upper + values.shoulder.limits.upper,
+ EXPECT_NEAR(constants::Values::kWristRange.upper + constants::Values::kShoulderRange.upper,
superstructure_queue_.status->wrist.angle, 0.001);
// Set some ridiculous goals to test lower limits.
@@ -326,11 +326,11 @@
// Check that we are near our soft limit.
superstructure_queue_.status.FetchLatest();
- EXPECT_NEAR(values.intake.limits.lower,
+ EXPECT_NEAR(constants::Values::kIntakeRange.lower,
superstructure_queue_.status->intake.angle, 0.001);
- EXPECT_NEAR(values.shoulder.limits.lower,
+ EXPECT_NEAR(constants::Values::kShoulderRange.lower,
superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(values.wrist.limits.lower + values.shoulder.limits.lower,
+ EXPECT_NEAR(constants::Values::kWristRange.lower + constants::Values::kShoulderRange.lower,
superstructure_queue_.status->wrist.angle, 0.001);
}
@@ -363,11 +363,11 @@
// Tests that starting at the lower hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, LowerHardstopStartup) {
superstructure_plant_.InitializeIntakePosition(
- constants::GetValues().intake.limits.lower);
+ constants::Values::kIntakeRange.lower);
superstructure_plant_.InitializeShoulderPosition(
- constants::GetValues().shoulder.limits.lower);
+ constants::Values::kShoulderRange.lower);
superstructure_plant_.InitializeWristPosition(
- constants::GetValues().wrist.limits.lower);
+ constants::Values::kWristRange.lower);
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(0.0)
@@ -382,11 +382,11 @@
// Tests that starting at the upper hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, UpperHardstopStartup) {
superstructure_plant_.InitializeIntakePosition(
- constants::GetValues().intake.limits.upper);
+ constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(
- constants::GetValues().shoulder.limits.upper);
+ constants::Values::kShoulderRange.upper);
superstructure_plant_.InitializeWristPosition(
- constants::GetValues().wrist.limits.upper);
+ constants::Values::kWristRange.upper);
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(0.0)
@@ -401,11 +401,11 @@
// Tests that resetting WPILib results in a rezero.
TEST_F(SuperstructureTest, ResetTest) {
superstructure_plant_.InitializeIntakePosition(
- constants::GetValues().intake.limits.upper);
+ constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(
- constants::GetValues().shoulder.limits.upper);
+ constants::Values::kShoulderRange.upper);
superstructure_plant_.InitializeWristPosition(
- constants::GetValues().wrist.limits.upper);
+ constants::Values::kWristRange.upper);
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.3)
.angle_shoulder(0.3)
@@ -446,17 +446,17 @@
// Tests that the integrators works.
TEST_F(SuperstructureTest, IntegratorTest) {
superstructure_plant_.InitializeIntakePosition(
- constants::GetValues().intake.limits.lower);
+ constants::Values::kIntakeRange.lower);
superstructure_plant_.InitializeShoulderPosition(
- constants::GetValues().shoulder.limits.lower);
+ constants::Values::kShoulderRange.lower);
superstructure_plant_.InitializeWristPosition(
- constants::GetValues().wrist.limits.lower);
+ constants::Values::kWristRange.lower);
superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
- .Send();
+ .angle_intake(0.0)
+ .angle_shoulder(0.0)
+ .angle_wrist(0.0)
+ .Send();
RunForTime(Time::InSeconds(8));