Allow the wrist to move when the shoulder isn't all the way up.
This lets us make one of the fender shots.
Change-Id: I4a0033f09978f9edf9166620739c587c0647b7ee
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 17653ad..f633d7b 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -77,6 +77,7 @@
':superstructure_plants',
'//aos/common/controls:control_loop',
'//aos/common/util:trapezoid_profile',
+ '//aos/common:math',
'//frc971/control_loops:state_feedback_loop',
'//frc971/control_loops:simple_capped_state_feedback_loop',
'//frc971/zeroing',
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index b82d2d9..e85657c 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -1,6 +1,7 @@
#include "y2016/control_loops/superstructure/superstructure.h"
#include "y2016/control_loops/superstructure/superstructure_controls.h"
+#include "aos/common/commonmath.h"
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
@@ -34,6 +35,7 @@
double intake_angle_goal) {
const double original_shoulder_angle_goal = shoulder_angle_goal;
const double original_intake_angle_goal = intake_angle_goal;
+ const double original_wrist_angle_goal = wrist_angle_goal;
double shoulder_angle = arm_->shoulder_angle();
double wrist_angle = arm_->wrist_angle();
@@ -50,20 +52,40 @@
// 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 (intake_angle < kMaxIntakeAngleBeforeArmInterference + kSafetyMargin) {
+ if (shoulder_angle < kMinShoulderAngleForHorizontalShooter ||
+ original_shoulder_angle_goal < kMinShoulderAngleForHorizontalShooter) {
+ wrist_angle_goal = 0.0;
+ } else if (shoulder_angle < kMinShoulderAngleForIntakeInterference ||
+ original_shoulder_angle_goal <
+ kMinShoulderAngleForIntakeInterference) {
+ wrist_angle_goal =
+ aos::Clip(original_wrist_angle_goal,
+ kMinWristAngleForMovingByIntake + kSafetyMargin,
+ kMaxWristAngleForMovingByIntake - kSafetyMargin);
+ }
+ } else {
+ if (shoulder_angle < kMinShoulderAngleForIntakeUpInterference ||
+ original_shoulder_angle_goal <
+ kMinShoulderAngleForIntakeUpInterference) {
+ wrist_angle_goal = 0.0;
+ }
+ }
+
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 (intake_angle < kMaxIntakeAngleBeforeArmInterference + kSafetyMargin) {
- if (::std::abs(wrist_angle) > kMaxWristAngleForMovingByIntake) {
+ if (wrist_angle > kMaxWristAngleForMovingByIntake ||
+ wrist_angle < kMinWristAngleForMovingByIntake) {
shoulder_angle_goal =
::std::max(original_shoulder_angle_goal,
kMinShoulderAngleForIntakeInterference + kSafetyMargin);
}
} else {
- if (::std::abs(wrist_angle) > kMaxWristAngleForMovingByIntake) {
+ if (wrist_angle > kMaxWristAngleForMovingByIntake ||
+ wrist_angle < kMinWristAngleForMovingByIntake) {
shoulder_angle_goal =
::std::max(original_shoulder_angle_goal,
kMinShoulderAngleForIntakeUpInterference + kSafetyMargin);
@@ -112,9 +134,16 @@
if (shoulder_angle >= kHalfwayPointBetweenSafeZones) {
// The shoulder is closer to being above the collision area. Move it up
// there.
- shoulder_angle_goal =
- ::std::max(original_shoulder_angle_goal,
- kMinShoulderAngleForIntakeInterference + kSafetyMargin);
+ if (intake_angle <
+ kMaxIntakeAngleBeforeArmInterference + kSafetyMargin) {
+ shoulder_angle_goal = ::std::max(
+ original_shoulder_angle_goal,
+ kMinShoulderAngleForIntakeInterference + kSafetyMargin);
+ } else {
+ shoulder_angle_goal = ::std::max(
+ original_shoulder_angle_goal,
+ kMinShoulderAngleForIntakeUpInterference + kSafetyMargin);
+ }
} else {
// The shoulder is closer to being below the collision zone (i.e. in
// stowing/intake position), keep it there for now.
@@ -159,14 +188,18 @@
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",
+ (wrist_angle > CollisionAvoidance::kMaxWristAngleForMovingByIntake ||
+ wrist_angle < CollisionAvoidance::kMinWristAngleForMovingByIntake)) {
+ LOG(DEBUG,
+ "Collided: Intake %f < %f < %f, shoulder %f < %f < %f, and %f < %f < "
+ "%f.\n",
Superstructure::kIntakeLowerClear, intake_angle,
CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference,
CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing,
shoulder_angle,
- CollisionAvoidance::kMinShoulderAngleForIntakeInterference);
+ CollisionAvoidance::kMinShoulderAngleForIntakeInterference,
+ CollisionAvoidance::kMinWristAngleForMovingByIntake, wrist_angle,
+ CollisionAvoidance::kMaxWristAngleForMovingByIntake);
return true;
}
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index fd00fd1..7dad0ec 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -77,7 +77,7 @@
// The intake angle (in radians) above which the intake can interfere (i.e.
// collide) with the arm and/or shooter.
- static constexpr double kMaxIntakeAngleBeforeArmInterference = 1.12;
+ static constexpr double kMaxIntakeAngleBeforeArmInterference = 1.05;
// The maximum absolute angle (in radians) that the wrist must be below in
// order for the shouler to be allowed to move below
@@ -86,9 +86,12 @@
// 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
+ // The maximum angle in radians that the wrist can be from horizontal
// while it is near the intake.
static constexpr double kMaxWristAngleForMovingByIntake = 0.50;
+ // The minimum angle in radians that the wrist can be from horizontal
+ // while it is near the intake.
+ static constexpr double kMinWristAngleForMovingByIntake = -1.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
@@ -109,7 +112,7 @@
static constexpr double kZeroingVoltage = 5.0;
static constexpr double kOperatingVoltage = 12.0;
- static constexpr double kLandingShoulderDownVoltage = -2.0;
+ static constexpr double kLandingShoulderDownVoltage = -1.5;
// This is the angle above which we will do a HIGH_ARM_ZERO, and below which
// we will do a LOW_ARM_ZERO.
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index d278eb6..9fdec26 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -539,7 +539,8 @@
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(constants::Values::kIntakeRange.upper)
.angle_shoulder(constants::Values::kShoulderRange.upper)
- .angle_wrist(constants::Values::kWristRange.upper)
+ .angle_wrist(constants::Values::kWristRange.upper +
+ constants::Values::kShoulderRange.upper)
.Send());
// We have to wait for it to put the elevator in a safe position as well.
RunForTime(Time::InSeconds(15));
@@ -561,7 +562,7 @@
.angle_wrist(0.0)
.Send());
// We have to wait for it to put the elevator in a safe position as well.
- RunForTime(Time::InSeconds(15));
+ RunForTime(Time::InSeconds(20));
VerifyNearGoal();
}
@@ -1095,7 +1096,7 @@
.angle_wrist(0.0) // Stowed
.Send());
- RunForTime(Time::InSeconds(10));
+ RunForTime(Time::InSeconds(15));
ASSERT_TRUE(
superstructure_queue_.goal.MakeWithBuilder()
@@ -1104,7 +1105,7 @@
.angle_wrist(M_PI / 2.0) // down
.Send());
- RunForTime(Time::InSeconds(3));
+ RunForTime(Time::InSeconds(5));
superstructure_queue_.status.FetchLatest();
ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
@@ -1120,7 +1121,10 @@
EXPECT_NEAR(M_PI / 4.0, superstructure_queue_.status->shoulder.angle, 0.001);
// The wrist should be forced into a stowing position.
- EXPECT_NEAR(0, superstructure_queue_.status->wrist.angle, 0.001);
+ // Since the intake is kicked out, we can be within
+ // kMaxWristAngleForMovingByIntake
+ EXPECT_NEAR(0, superstructure_queue_.status->wrist.angle,
+ CollisionAvoidance::kMaxWristAngleForMovingByIntake + 0.001);
ASSERT_TRUE(
superstructure_queue_.goal.MakeWithBuilder()
@@ -1129,7 +1133,7 @@
.angle_wrist(M_PI) // forward
.Send());
- RunForTime(Time::InSeconds(3));
+ RunForTime(Time::InSeconds(5));
VerifyNearGoal();
}