Adjust constants to whatever is in CAD.
Change-Id: Ib1d5cb8edd400eff4cbd3b3b0176ccb5203130fe
diff --git a/y2016/constants.h b/y2016/constants.h
index ba16902..ed86d0c 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -37,34 +37,40 @@
// The ratio from the encoder shaft to the drivetrain wheels.
static constexpr double kDrivetrainEncoderRatio =
- (18.0 / 50.0) /*output reduction*/ * (56.0 / 30.0);
+ (18.0 / 36.0) /*output reduction*/ * (44.0 / 30.0);
// The gear ratios from motor shafts to the drivetrain wheels for high and low
// gear.
- static constexpr double kLowGearRatio = 18.0 / 60.0 * 18.0 / 50.0;
- static constexpr double kHighGearRatio = 28.0 / 50.0 * 18.0 / 50.0;
+ static constexpr double kLowGearRatio = 18.0 / 60.0 * 18.0 / 36.0;
+ static constexpr double kHighGearRatio = 28.0 / 50.0 * 18.0 / 36.0;
- static constexpr double kTurnWidth = 25.0 / 100.0 * 2.54; // Robot width.
+ static constexpr double kTurnWidth = 0.601; // Robot width.
// Ratios for our subsystems.
static constexpr double kShooterEncoderRatio = 1.0;
- static constexpr double kIntakeEncoderRatio = 16.0 / 58.0 * 18.0 / 72.0 * 14.0 / 64.0;
- static constexpr double kShoulderEncoderRatio = 16.0 / 58.0 * 18.0 / 72.0 * 14.0 / 64.0;
- static constexpr double kWristEncoderRatio = 16.0 / 48.0 * 18.0 / 64.0 * 14.0 / 54.0;
+ static constexpr double kIntakeEncoderRatio =
+ 16.0 / 48.0 * 18.0 / 72.0 * 14.0 / 64.0;
+ static constexpr double kShoulderEncoderRatio =
+ 16.0 / 58.0 * 18.0 / 72.0 * 14.0 / 64.0;
+ static constexpr double kWristEncoderRatio =
+ 16.0 / 48.0 * 18.0 / 64.0 * 14.0 / 54.0;
- static constexpr double kIntakePotRatio = 16.0 / 58.0;
+ static constexpr double kIntakePotRatio = 16.0 / 48.0;
static constexpr double kShoulderPotRatio = 16.0 / 58.0;
static constexpr double kWristPotRatio = 16.0 / 48.0;
// Difference in radians between index pulses.
- static constexpr double kIntakeEncoderIndexDifference = 2.0 * M_PI * kIntakeEncoderRatio;
- static constexpr double kShoulderEncoderIndexDifference = 2.0 * M_PI * kShoulderEncoderRatio;
- static constexpr double kWristEncoderIndexDifference = 2.0 * M_PI * kWristEncoderRatio;
+ static constexpr double kIntakeEncoderIndexDifference =
+ 2.0 * M_PI * kIntakeEncoderRatio;
+ static constexpr double kShoulderEncoderIndexDifference =
+ 2.0 * M_PI * kShoulderEncoderRatio;
+ static constexpr double kWristEncoderIndexDifference =
+ 2.0 * M_PI * kWristEncoderRatio;
// Subsystem motion ranges, in whatever units that their respective queues say
// the use.
- static constexpr Range kIntakeRange{-0.4, 2.0, -0.3, 1.9};
- static constexpr Range kShoulderRange{-0.2, 2.0, -0.1, 1.9};
+ static constexpr Range kIntakeRange{-0.270, 2.0, -0.200, 1.9};
+ static constexpr Range kShoulderRange{-0.050, 2.0, 0.000, 1.9};
static constexpr Range kWristRange{-2.0, 2.0, -1.9, 1.0};
// ///// Dynamic constants. /////
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index fa227bd..1bd4eca 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -81,8 +81,7 @@
SuperstructureSimulation()
: intake_plant_(new IntakePlant(MakeIntakePlant())),
plant_arm_(new ArmPlant(MakeArmPlant())),
- pot_encoder_intake_(
- constants::Values::kIntakeEncoderIndexDifference),
+ pot_encoder_intake_(constants::Values::kIntakeEncoderIndexDifference),
pot_encoder_shoulder_(
constants::Values::kShoulderEncoderIndexDifference),
pot_encoder_wrist_(constants::Values::kWristEncoderIndexDifference),
@@ -245,9 +244,9 @@
// Tests that the superstructure does nothing when the goal is zero.
TEST_F(SuperstructureTest, DoesNothing) {
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
+ .angle_intake(constants::Values::kIntakeRange.lower)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(constants::Values::kWristRange.lower)
.max_angular_velocity_intake(20)
.max_angular_velocity_shoulder(20)
.max_angular_velocity_wrist(20)
@@ -306,7 +305,8 @@
superstructure_queue_.status->intake.angle, 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(constants::Values::kWristRange.upper + constants::Values::kShoulderRange.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.
@@ -330,16 +330,17 @@
superstructure_queue_.status->intake.angle, 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.lower,
superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(constants::Values::kWristRange.lower + constants::Values::kShoulderRange.lower,
+ EXPECT_NEAR(constants::Values::kWristRange.lower +
+ constants::Values::kShoulderRange.lower,
superstructure_queue_.status->wrist.angle, 0.001);
}
// Tests that the loop zeroes when run for a while.
TEST_F(SuperstructureTest, ZeroTest) {
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
+ .angle_intake(constants::Values::kIntakeRange.lower)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(constants::Values::kWristRange.lower)
.max_angular_velocity_intake(20)
.max_angular_acceleration_intake(20)
.max_angular_velocity_shoulder(20)
@@ -369,9 +370,9 @@
superstructure_plant_.InitializeWristPosition(
constants::Values::kWristRange.lower);
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
+ .angle_intake(constants::Values::kIntakeRange.lower)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(constants::Values::kWristRange.lower)
.Send());
// We have to wait for it to put the elevator in a safe position as well.
RunForTime(Time::InSeconds(5));
@@ -388,9 +389,9 @@
superstructure_plant_.InitializeWristPosition(
constants::Values::kWristRange.upper);
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
+ .angle_intake(constants::Values::kIntakeRange.lower)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(constants::Values::kWristRange.lower)
.Send());
// We have to wait for it to put the elevator in a safe position as well.
RunForTime(Time::InSeconds(5));
@@ -406,11 +407,12 @@
constants::Values::kShoulderRange.upper);
superstructure_plant_.InitializeWristPosition(
constants::Values::kWristRange.upper);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.3)
- .angle_shoulder(0.3)
- .angle_wrist(0.3)
- .Send());
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.lower + 0.3)
+ .angle_shoulder(constants::Values::kShoulderRange.lower + 0.3)
+ .angle_wrist(constants::Values::kWristRange.lower + 0.3)
+ .Send());
RunForTime(Time::InSeconds(5));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
@@ -425,11 +427,12 @@
// Tests that the internal goals don't change while disabled.
TEST_F(SuperstructureTest, DisabledGoalTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.03)
- .angle_shoulder(0.03)
- .angle_wrist(0.03)
- .Send());
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.lower + 0.03)
+ .angle_shoulder(constants::Values::kShoulderRange.lower + 0.03)
+ .angle_wrist(constants::Values::kWristRange.lower + 0.03)
+ .Send());
RunForTime(Time::InMS(100), false);
EXPECT_EQ(0.0, superstructure_.intake_.goal(0, 0));