Fixed collision detection.
Tests all pass, added gain scheduling to arm, etc.
Change-Id: I4de5427b816961ead5b23c7d9287ff40dcc10f46
diff --git a/y2016/control_loops/python/arm.py b/y2016/control_loops/python/arm.py
index 70740d5..75f0c59 100755
--- a/y2016/control_loops/python/arm.py
+++ b/y2016/control_loops/python/arm.py
@@ -379,8 +379,12 @@
scenario_plotter = ScenarioPlotter()
- arm = Arm()
- arm_controller = IntegralArm(name='AcceleratingIntegralArm', J=12)
+ J_accelerating = 12
+ J_decelerating = 5
+
+ arm = Arm(name='AcceleratingArm', J=J_accelerating)
+ arm_integral_controller = IntegralArm(
+ name='AcceleratingIntegralArm', J=J_accelerating)
arm_observer = IntegralArm()
# Test moving the shoulder with constant separation.
@@ -397,23 +401,27 @@
scenario_plotter.run_test(arm=arm,
end_goal=R,
iterations=300,
- controller=arm_controller,
+ controller=arm_integral_controller,
observer=arm_observer)
if len(argv) != 5:
glog.fatal('Expected .h file name and .cc file name for the wrist and integral wrist.')
else:
namespaces = ['y2016', 'control_loops', 'superstructure']
- loop_writer = control_loop.ControlLoopWriter('Arm', [arm],
- namespaces=namespaces)
+ decelerating_arm = Arm(name='DeceleratingArm', J=J_decelerating)
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Arm', [arm, decelerating_arm], namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
- decelerating_arm_controller = IntegralArm(name='DeceleratingIntegralArm', J=5)
+ decelerating_integral_arm_controller = IntegralArm(
+ name='DeceleratingIntegralArm', J=J_decelerating)
+
integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralArm', [arm_controller, decelerating_arm_controller],
+ 'IntegralArm',
+ [arm_integral_controller, decelerating_integral_arm_controller],
namespaces=namespaces)
integral_loop_writer.AddConstant(control_loop.Constant("kV_shoulder", "%f",
- arm_controller.shoulder_Kv))
+ arm_integral_controller.shoulder_Kv))
integral_loop_writer.Write(argv[3], argv[4])
if FLAGS.plot:
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 925faab..1f1bbdf 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -14,7 +14,6 @@
namespace superstructure {
namespace {
-constexpr double kLandingShoulderDownVoltage = -2.0;
// The maximum voltage the intake roller will be allowed to use.
constexpr float kMaxIntakeTopVoltage = 12.0;
constexpr float kMaxIntakeBottomVoltage = 12.0;
@@ -33,6 +32,9 @@
void CollisionAvoidance::UpdateGoal(double shoulder_angle_goal,
double wrist_angle_goal,
double intake_angle_goal) {
+ const double original_shoulder_angle_goal = shoulder_angle_goal;
+ const double original_intake_angle_goal = intake_angle_goal;
+
double shoulder_angle = arm_->shoulder_angle();
double wrist_angle = arm_->wrist_angle();
double intake_angle = intake_->angle();
@@ -48,33 +50,39 @@
// If the shoulder is below a certain angle or we want to move it below
// that angle, then the shooter has to stay level to the ground. Otherwise,
// it will crash into the frame.
- if (shoulder_angle < kMinShoulderAngleForHorizontalShooter ||
- shoulder_angle_goal < kMinShoulderAngleForHorizontalShooter) {
+ if (shoulder_angle < kMinShoulderAngleForIntakeUpInterference ||
+ original_shoulder_angle_goal < kMinShoulderAngleForIntakeUpInterference) {
wrist_angle_goal = 0.0;
// Make sure that we don't move the shoulder below a certain angle until
// the wrist is level with the ground.
+ if (::std::abs(wrist_angle) > kMaxWristAngleForMovingByIntake) {
+ shoulder_angle_goal =
+ ::std::max(original_shoulder_angle_goal,
+ kMinShoulderAngleForIntakeUpInterference + kSafetyMargin);
+ }
if (::std::abs(wrist_angle) > kMaxWristAngleForSafeArmStowing) {
shoulder_angle_goal =
- ::std::max(shoulder_angle_goal,
+ ::std::max(original_shoulder_angle_goal,
kMinShoulderAngleForHorizontalShooter + kSafetyMargin);
}
}
// Is the arm where it could interfere with the intake right now?
bool shoulder_is_in_danger =
- (shoulder_angle < kMinShoulderAngleForIntakeInterference &&
+ (shoulder_angle < kMinShoulderAngleForIntakeUpInterference &&
shoulder_angle > kMaxShoulderAngleUntilSafeIntakeStowing);
// Is the arm moving into collision zone from above?
bool shoulder_moving_into_danger_from_above =
- (shoulder_angle >= kMinShoulderAngleForIntakeInterference &&
- shoulder_angle_goal <= kMinShoulderAngleForIntakeInterference);
+ (shoulder_angle >= kMinShoulderAngleForIntakeUpInterference &&
+ original_shoulder_angle_goal <=
+ kMinShoulderAngleForIntakeUpInterference);
// Is the arm moving into collision zone from below?
bool shoulder_moving_into_danger_from_below =
(shoulder_angle <= kMaxShoulderAngleUntilSafeIntakeStowing &&
- shoulder_angle_goal >= kMaxShoulderAngleUntilSafeIntakeStowing);
+ original_shoulder_angle_goal >= kMaxShoulderAngleUntilSafeIntakeStowing);
// Avoid colliding the arm with the intake.
if (shoulder_is_in_danger || shoulder_moving_into_danger_from_above ||
@@ -82,7 +90,7 @@
// If the arm could collide with the intake, we make sure to move the
// intake out of the way. The arm has priority.
intake_angle_goal =
- ::std::min(intake_angle_goal,
+ ::std::min(original_intake_angle_goal,
kMaxIntakeAngleBeforeArmInterference - kSafetyMargin);
// Don't let the shoulder move into the collision area until the intake is
@@ -97,13 +105,13 @@
// The shoulder is closer to being above the collision area. Move it up
// there.
shoulder_angle_goal =
- ::std::max(shoulder_angle_goal,
+ ::std::max(original_shoulder_angle_goal,
kMinShoulderAngleForIntakeInterference + kSafetyMargin);
} else {
// The shoulder is closer to being below the collision zone (i.e. in
// stowing/intake position), keep it there for now.
shoulder_angle_goal =
- ::std::min(shoulder_angle_goal,
+ ::std::min(original_shoulder_angle_goal,
kMaxShoulderAngleUntilSafeIntakeStowing - kSafetyMargin);
}
}
@@ -126,13 +134,31 @@
if (shoulder_angle >=
CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing &&
shoulder_angle <=
- CollisionAvoidance::kMinShoulderAngleForIntakeInterference &&
- intake_angle > CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference) {
- LOG(DEBUG, "Collided: Intake %f > %f, and shoulder %f < %f < %f.\n", intake_angle,
- CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference,
- CollisionAvoidance::kMinShoulderAngleForIntakeInterference,
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference &&
+ intake_angle >
+ CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference) {
+ LOG(DEBUG, "Collided: Intake %f > %f, and shoulder %f < %f < %f.\n",
+ intake_angle, CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference,
+ CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing,
shoulder_angle,
- CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing);
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference);
+ return true;
+ }
+
+ if (shoulder_angle >=
+ CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing &&
+ shoulder_angle <=
+ CollisionAvoidance::kMinShoulderAngleForIntakeInterference &&
+ intake_angle < CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference &&
+ intake_angle > Superstructure::kIntakeLowerClear &&
+ ::std::abs(wrist_angle) >
+ CollisionAvoidance::kMaxWristAngleForMovingByIntake) {
+ LOG(DEBUG, "Collided: Intake %f < %f < %f, and shoulder %f < %f < %f.\n",
+ Superstructure::kIntakeLowerClear, intake_angle,
+ CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference,
+ CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing,
+ shoulder_angle,
+ CollisionAvoidance::kMinShoulderAngleForIntakeInterference);
return true;
}
@@ -551,7 +577,8 @@
// TODO(austin): Do I want to push negative power into the belly pan at this
// point? Maybe just put the goal slightly below the bellypan and call that
// good enough.
- if (arm_.goal(0, 0) <= kShoulderTransitionToLanded + 1e-4) {
+ if (arm_.goal(0, 0) <= kShoulderTransitionToLanded + 1e-4 ||
+ arm_.X_hat(0, 0) <= kShoulderTransitionToLanded + 1e-4) {
arm_.set_shoulder_asymetric_limits(kLandingShoulderDownVoltage,
max_voltage);
}
@@ -648,6 +675,7 @@
constexpr double Superstructure::kZeroingVoltage;
constexpr double Superstructure::kOperatingVoltage;
+constexpr double Superstructure::kLandingShoulderDownVoltage;
constexpr double Superstructure::kShoulderMiddleAngle;
constexpr double Superstructure::kLooseTolerance;
constexpr double Superstructure::kIntakeUpperClear;
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 083b5b8..74a059d 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -61,9 +61,6 @@
double wrist_angle,
double intake_angle);
- // TODO(phil): Verify that these numbers actually make sense. Someone needs
- // to verify these either on the CAD or the real robot.
-
// The shoulder angle (in radians) below which the shooter must be in a
// stowing position. In other words the wrist must be at angle zero if the
// shoulder is below this angle.
@@ -71,7 +68,12 @@
// The shoulder angle (in radians) below which the arm and the shooter have
// the potential to interfere with the intake.
- static constexpr double kMinShoulderAngleForIntakeInterference = 1.3;
+ static constexpr double kMinShoulderAngleForIntakeUpInterference = 1.3;
+
+ // The shoulder angle (in radians) below which the shooter should be closer to
+ // level to fix the inverted case.
+ // TODO(austin): Verify
+ static constexpr double kMinShoulderAngleForIntakeInterference = 1.1;
// The intake angle (in radians) above which the intake can interfere (i.e.
// collide) with the arm and/or shooter.
@@ -84,10 +86,14 @@
// also be placed into the belly pan.
static constexpr double kMaxWristAngleForSafeArmStowing = 0.05;
+ // The maximum absolute angle in radians that the wrist can be from horizontal
+ // while it is near the intake.
+ static constexpr double kMaxWristAngleForMovingByIntake = 0.20;
+
// The shoulder angle (in radians) below which the intake can safely move
// into the collision zone. This is necessary when the robot wants to fold up
// completely (i.e. stow the arm, shooter, and intake).
- static constexpr double kMaxShoulderAngleUntilSafeIntakeStowing = 0.2;
+ static constexpr double kMaxShoulderAngleUntilSafeIntakeStowing = 0.19;
private:
Intake *intake_;
@@ -103,6 +109,7 @@
static constexpr double kZeroingVoltage = 5.0;
static constexpr double kOperatingVoltage = 12.0;
+ static constexpr double kLandingShoulderDownVoltage = -2.0;
// This is the angle above which we will do a HIGH_ARM_ZERO, and below which
// we will do a LOW_ARM_ZERO.
@@ -115,7 +122,8 @@
// This is the angle such that the intake will clear the arm when the shooter
// is level.
- static constexpr double kIntakeUpperClear = 1.2;
+ static constexpr double kIntakeUpperClear =
+ CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference;
// This is the angle such that the intake will clear the arm when the shooter
// is at almost any position.
static constexpr double kIntakeLowerClear = 0.4;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 2c2bf5a..d278eb6 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -185,12 +185,28 @@
}
if (arm_plant_->X(0, 0) <=
Superstructure::kShoulderTransitionToLanded + 1e-4) {
- CHECK_GE(superstructure_queue_.output->voltage_shoulder, -2.00001);
+ CHECK_GE(superstructure_queue_.output->voltage_shoulder,
+ Superstructure::kLandingShoulderDownVoltage - 0.00001);
}
// Use the plant to generate the next physical state given the voltages to
// the motors.
intake_plant_->Update();
+
+ {
+ const double bemf_voltage = arm_plant_->X(1, 0) / kV_shoulder;
+ bool is_accelerating = false;
+ if (bemf_voltage > 0) {
+ is_accelerating = arm_plant_->U(0, 0) > bemf_voltage;
+ } else {
+ is_accelerating = arm_plant_->U(0, 0) < bemf_voltage;
+ }
+ if (is_accelerating) {
+ arm_plant_->set_plant_index(0);
+ } else {
+ arm_plant_->set_plant_index(1);
+ }
+ }
arm_plant_->Update();
const double angle_intake = intake_plant_->Y(0, 0);
@@ -391,7 +407,7 @@
// Set a reasonable goal.
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(M_PI / 4.0)
- .angle_shoulder(M_PI / 4.0)
+ .angle_shoulder(1.4)
.angle_wrist(M_PI / 4.0)
.max_angular_velocity_intake(20)
.max_angular_acceleration_intake(20)
@@ -402,7 +418,7 @@
.Send());
// Give it a lot of time to get there.
- RunForTime(Time::InSeconds(5));
+ RunForTime(Time::InSeconds(8));
VerifyNearGoal();
}
@@ -783,7 +799,7 @@
RunForTime(Time::InSeconds(1), false);
EXPECT_EQ(Superstructure::SLOW_RUNNING, superstructure_.state());
- RunForTime(Time::InSeconds(1), true);
+ RunForTime(Time::InSeconds(2), true);
VerifyNearGoal();
}
@@ -822,7 +838,7 @@
.max_angular_acceleration_wrist(20)
.Send());
- RunForTime(Time::InSeconds(4));
+ RunForTime(Time::InSeconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
@@ -842,10 +858,10 @@
// TODO(austin): The profile isn't feasible, so when we try to track it, we
// have trouble going from the acceleration step to the constant velocity
// step. We end up under and then overshooting.
- set_peak_intake_acceleration(1.05);
- set_peak_shoulder_acceleration(1.05);
- set_peak_wrist_acceleration(1.05);
- RunForTime(Time::InSeconds(4));
+ set_peak_intake_acceleration(1.10);
+ set_peak_shoulder_acceleration(1.20);
+ set_peak_wrist_acceleration(1.10);
+ RunForTime(Time::InSeconds(6));
VerifyNearGoal();
}
@@ -864,7 +880,7 @@
.max_angular_acceleration_wrist(20)
.Send());
- RunForTime(Time::InSeconds(4));
+ RunForTime(Time::InSeconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
@@ -881,9 +897,9 @@
.max_angular_acceleration_wrist(1)
.Send());
- set_peak_intake_acceleration(1.05);
- set_peak_shoulder_acceleration(1.05);
- set_peak_wrist_acceleration(1.05);
+ set_peak_intake_acceleration(1.20);
+ set_peak_shoulder_acceleration(1.20);
+ set_peak_wrist_acceleration(1.20);
RunForTime(Time::InSeconds(4));
VerifyNearGoal();
@@ -891,34 +907,40 @@
// Tests that the loop respects wrist acceleration limits while moving.
TEST_F(SuperstructureTest, WristAccelerationLimitTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
+ 0.1)
+ .angle_wrist(0.0)
+ .max_angular_velocity_intake(20)
+ .max_angular_acceleration_intake(20)
+ .max_angular_velocity_shoulder(20)
+ .max_angular_acceleration_shoulder(20)
+ .max_angular_velocity_wrist(20)
+ .max_angular_acceleration_wrist(20)
+ .Send());
- RunForTime(Time::InSeconds(4));
+ RunForTime(Time::InSeconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.0)
- .angle_wrist(0.5)
- .max_angular_velocity_intake(1)
- .max_angular_acceleration_intake(1)
- .max_angular_velocity_shoulder(1)
- .max_angular_acceleration_shoulder(1)
- .max_angular_velocity_wrist(1)
- .max_angular_acceleration_wrist(1)
- .Send());
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
+ 0.1)
+ .angle_wrist(0.5)
+ .max_angular_velocity_intake(1)
+ .max_angular_acceleration_intake(1)
+ .max_angular_velocity_shoulder(1)
+ .max_angular_acceleration_shoulder(1)
+ .max_angular_velocity_wrist(1)
+ .max_angular_acceleration_wrist(1)
+ .Send());
set_peak_intake_acceleration(1.05);
set_peak_shoulder_acceleration(1.05);
@@ -931,34 +953,38 @@
// Tests that the loop respects intake handles saturation while accelerating
// correctly.
TEST_F(SuperstructureTest, SaturatedIntakeProfileTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
+ .angle_wrist(0.0)
+ .max_angular_velocity_intake(20)
+ .max_angular_acceleration_intake(20)
+ .max_angular_velocity_shoulder(20)
+ .max_angular_acceleration_shoulder(20)
+ .max_angular_velocity_wrist(20)
+ .max_angular_acceleration_wrist(20)
+ .Send());
- RunForTime(Time::InSeconds(4));
+ RunForTime(Time::InSeconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.5)
- .angle_shoulder(1.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(4.5)
- .max_angular_acceleration_intake(800)
- .max_angular_velocity_shoulder(1)
- .max_angular_acceleration_shoulder(100)
- .max_angular_velocity_wrist(1)
- .max_angular_acceleration_wrist(100)
- .Send());
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.5)
+ .angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
+ .angle_wrist(0.0)
+ .max_angular_velocity_intake(4.5)
+ .max_angular_acceleration_intake(800)
+ .max_angular_velocity_shoulder(1)
+ .max_angular_acceleration_shoulder(100)
+ .max_angular_velocity_wrist(1)
+ .max_angular_acceleration_wrist(100)
+ .Send());
set_peak_intake_velocity(4.65);
set_peak_shoulder_velocity(1.00);
@@ -983,7 +1009,7 @@
.max_angular_acceleration_wrist(20)
.Send());
- RunForTime(Time::InSeconds(4));
+ RunForTime(Time::InSeconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
@@ -995,7 +1021,7 @@
.max_angular_velocity_intake(1.0)
.max_angular_acceleration_intake(1.0)
.max_angular_velocity_shoulder(5.0)
- .max_angular_acceleration_shoulder(80)
+ .max_angular_acceleration_shoulder(20)
.max_angular_velocity_wrist(1)
.max_angular_acceleration_wrist(100)
.Send());
@@ -1011,34 +1037,40 @@
// Tests that the loop respects wrist handles saturation while accelerating
// correctly.
TEST_F(SuperstructureTest, SaturatedWristProfileTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
+ 0.1)
+ .angle_wrist(0.0)
+ .max_angular_velocity_intake(20)
+ .max_angular_acceleration_intake(20)
+ .max_angular_velocity_shoulder(20)
+ .max_angular_acceleration_shoulder(20)
+ .max_angular_velocity_wrist(20)
+ .max_angular_acceleration_wrist(20)
+ .Send());
- RunForTime(Time::InSeconds(4));
+ RunForTime(Time::InSeconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.0)
- .angle_wrist(1.9)
- .max_angular_velocity_intake(1.0)
- .max_angular_acceleration_intake(1.0)
- .max_angular_velocity_shoulder(1.0)
- .max_angular_acceleration_shoulder(1.0)
- .max_angular_velocity_wrist(10.0)
- .max_angular_acceleration_wrist(160.0)
- .Send());
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
+ 0.1)
+ .angle_wrist(1.3)
+ .max_angular_velocity_intake(1.0)
+ .max_angular_acceleration_intake(1.0)
+ .max_angular_velocity_shoulder(1.0)
+ .max_angular_acceleration_shoulder(1.0)
+ .max_angular_velocity_wrist(10.0)
+ .max_angular_acceleration_wrist(160.0)
+ .Send());
set_peak_intake_velocity(1.0);
set_peak_shoulder_velocity(1.0);
@@ -1086,7 +1118,19 @@
// The arm should have reached its goal.
EXPECT_NEAR(M_PI / 4.0, superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(M_PI / 2.0, superstructure_queue_.status->wrist.angle, 0.001);
+
+ // The wrist should be forced into a stowing position.
+ EXPECT_NEAR(0, superstructure_queue_.status->wrist.angle, 0.001);
+
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.upper) // stowed
+ .angle_shoulder(M_PI / 2.0) // in the collision area
+ .angle_wrist(M_PI) // forward
+ .Send());
+
+ RunForTime(Time::InSeconds(3));
+ VerifyNearGoal();
}
// Make sure that the shooter holds itself level when the arm comes down
@@ -1102,7 +1146,7 @@
.angle_wrist(M_PI) // intentionally asking for forward
.Send());
- RunForTime(Time::InSeconds(10));
+ RunForTime(Time::InSeconds(15));
superstructure_queue_.status.FetchLatest();
ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
@@ -1202,11 +1246,14 @@
// If we are near the bottom of the range, we won't have enough power to
// compensate for the offset. This means that we fail if we get to the goal.
- superstructure_plant_.set_power_error(0.0, 3.0, 0.0);
+ superstructure_plant_.set_power_error(
+ 0.0, -Superstructure::kLandingShoulderDownVoltage, 0.0);
RunForTime(Time::InSeconds(2));
- superstructure_plant_.set_power_error(0.0, 6.0, 0.0);
+ superstructure_plant_.set_power_error(
+ 0.0, -2.0 * Superstructure::kLandingShoulderDownVoltage, 0.0);
RunForTime(Time::InSeconds(2));
- EXPECT_LE(0.0, superstructure_queue_.goal->angle_shoulder);
+ EXPECT_LE(constants::Values::kShoulderRange.lower,
+ superstructure_queue_.goal->angle_shoulder);
}
// Make sure that we land slowly.