Merge changes I9b620d06,I471030b3
* changes:
Add a convenient vector wrapper
Add a file reading utility function
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index e78a786..e535ae7 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -148,13 +148,9 @@
int plant_index() const { return plant_index_; }
void set_plant_index(int plant_index) {
- if (plant_index < 0) {
- plant_index_ = 0;
- } else if (plant_index >= static_cast<int>(coefficients_.size())) {
- plant_index_ = static_cast<int>(coefficients_.size()) - 1;
- } else {
- plant_index_ = plant_index;
- }
+ assert(plant_index >= 0);
+ assert(plant_index < static_cast<int>(coefficients_.size()));
+ plant_index_ = plant_index;
}
void Reset() {
diff --git a/y2016/BUILD b/y2016/BUILD
index ccb1315..57892ea 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -39,6 +39,7 @@
'//frc971/autonomous:auto_queue',
'//y2016/control_loops/shooter:shooter_queue',
'//y2016/control_loops/superstructure:superstructure_queue',
+ '//y2016/queues:ball_detector',
],
)
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/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 2f1ae4a..699903d 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -80,6 +80,13 @@
// Update position/goal for our two shooter sides.
left_.set_goal(goal->angular_velocity);
right_.set_goal(goal->angular_velocity);
+
+ // Turn the lights on if we are supposed to spin.
+ if (output) {
+ if (::std::abs(goal->angular_velocity) > 0.0) {
+ output->lights_on = true;
+ }
+ }
}
left_.set_position(position->theta_left);
diff --git a/y2016/control_loops/shooter/shooter.q b/y2016/control_loops/shooter/shooter.q
index 5559bbb..00149a8 100644
--- a/y2016/control_loops/shooter/shooter.q
+++ b/y2016/control_loops/shooter/shooter.q
@@ -55,6 +55,9 @@
// See comments on the identical fields in Goal for details.
bool clamp_open;
bool push_to_shooter;
+
+ // If true, the lights are on.
+ bool lights_on;
};
queue Goal goal;
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 925faab..b82d2d9 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,12 +50,25 @@
// 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 (intake_angle < kMaxIntakeAngleBeforeArmInterference + kSafetyMargin) {
+ if (::std::abs(wrist_angle) > kMaxWristAngleForMovingByIntake) {
+ shoulder_angle_goal =
+ ::std::max(original_shoulder_angle_goal,
+ kMinShoulderAngleForIntakeInterference + kSafetyMargin);
+ }
+ } else {
+ 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,
@@ -63,18 +78,19 @@
// 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 +98,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 +113,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 +142,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 +585,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 +683,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..fd00fd1 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.50;
+
// 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_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index 0a0a825..eb6432d 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -354,11 +354,15 @@
// Shoulder saturated
if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
+ LOG(DEBUG, "Moving shoulder state. U: %f, %f\n", loop_->U(0, 0),
+ loop_->U_uncapped(0, 0));
shoulder_profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
}
// Wrist saturated
if (!disable && loop_->U(1, 0) != loop_->U_uncapped(1, 0)) {
+ LOG(DEBUG, "Moving shooter state. U: %f, %f\n", loop_->U(1, 0),
+ loop_->U_uncapped(1, 0));
wrist_profile_.MoveCurrentState(loop_->R().block<2, 1>(2, 0));
}
}
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index b4fbead..2934011 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -145,6 +145,35 @@
}
private:
+ void CapU() override {
+ // U(0)
+ // U(1) = coupling * U(0) + ...
+ // So, when modifying U(0), remove the coupling.
+ if (U(0, 0) > max_voltage(0)) {
+ const double overage_amount = U(0, 0) - max_voltage(0);
+ mutable_U(0, 0) = max_voltage(0);
+ const double coupled_amount =
+ (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) * overage_amount;
+ LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
+ mutable_U(1, 0) += coupled_amount;
+ }
+ if (U(0, 0) < min_voltage(0)) {
+ const double under_amount = U(0, 0) - min_voltage(0);
+ mutable_U(0, 0) = min_voltage(0);
+ const double coupled_amount =
+ (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) *
+ under_amount;
+ LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
+ mutable_U(1, 0) += coupled_amount;
+ }
+
+ // Uncapping U above isn't actually a problem with U for the shoulder.
+ // Reset any change.
+ mutable_U_uncapped(1, 0) = U(1, 0);
+ mutable_U(1, 0) =
+ ::std::min(max_voltage(1), ::std::max(min_voltage(1), U(1, 0)));
+ }
+
bool IsAccelerating(double bemf_voltage, double voltage) {
if (bemf_voltage > 0) {
return voltage > bemf_voltage;
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.
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index a6e6c93..f31b708 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -14,6 +14,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2016/control_loops/shooter/shooter.q.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/queues/ball_detector.q.h"
#include "y2016/constants.h"
#include "frc971/queues/gyro.q.h"
@@ -24,8 +25,9 @@
using ::y2016::control_loops::superstructure_queue;
using ::aos::input::driver_station::ButtonLocation;
-using ::aos::input::driver_station::JoystickAxis;
using ::aos::input::driver_station::ControlBit;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::POVLocation;
namespace y2016 {
namespace input {
@@ -39,14 +41,15 @@
const ButtonLocation kTurn2(1, 11);
// Buttons on the lexan driver station to get things running on bring-up day.
-const ButtonLocation kTest1(3, 6);
-const ButtonLocation kTest2(3, 2);
-const ButtonLocation kTest3(3, 11);
-const ButtonLocation kTest4(3, 9);
+const ButtonLocation kIntakeDown(3, 11);
+const POVLocation kFrontLong(3, 180);
+const POVLocation kBackLong(3, 0);
+const ButtonLocation kTest3(3, 7);
+const ButtonLocation kIntakeIn(3, 12);
const ButtonLocation kTest5(3, 8);
-const ButtonLocation kTest6(3, 3);
+const ButtonLocation kFire(3, 3);
const ButtonLocation kTest7(3, 5);
-const ButtonLocation kTest8(3, 4);
+const ButtonLocation kIntakeOut(3, 9);
class Reader : public ::aos::input::JoystickInput {
public:
@@ -144,37 +147,45 @@
waiting_for_zero_ = true;
}
- if (data.IsPressed(kTest1)) {
- intake_goal_ = 1.6;
- } else {
+ if (data.IsPressed(kIntakeDown)) {
intake_goal_ = 0.1;
+ } else {
+ intake_goal_ = 1.6;
}
- if (data.IsPressed(kTest2)) {
+ if (data.IsPressed(kFrontLong)) {
+ // Forwards shot
shoulder_goal_ = M_PI / 2.0 - 0.2;
+ wrist_goal_ = M_PI + 0.42;
+ shooter_velocity_ = 640.0;
+ } else if (data.IsPressed(kBackLong)) {
+ // Backwards shot
+ shoulder_goal_ = M_PI / 2.0 - 0.2;
+ wrist_goal_ = -0.59;
+ shooter_velocity_ = 640.0;
} else {
+ wrist_goal_ = 0.0;
shoulder_goal_ = -0.010;
+ shooter_velocity_ = 0.0;
}
if (data.IsPressed(kTest3)) {
wrist_goal_ = 0.0;
- } else {
- // Backwards shot
- wrist_goal_ = -0.59;
- // Forwards shot
- //wrist_goal_ = M_PI + 0.42;
}
- is_intaking_ = data.IsPressed(kTest4);
-
- if (data.IsPressed(kTest5)) {
- //shooter_velocity_ = 600.0;
- shooter_velocity_ = 640.0;
- } else {
- shooter_velocity_ = 0.0;
+ bool ball_detected = false;
+ ::y2016::sensors::ball_detector.FetchLatest();
+ if (::y2016::sensors::ball_detector.get()) {
+ ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
+ }
+ if (data.PosEdge(kIntakeIn)) {
+ saw_ball_when_started_intaking_ = ball_detected;
}
- if (data.IsPressed(kTest6) && shooter_velocity_ != 0.0) {
+ is_intaking_ = data.IsPressed(kIntakeIn) &&
+ (!ball_detected || saw_ball_when_started_intaking_);
+
+ if (data.IsPressed(kFire) && shooter_velocity_ != 0.0) {
fire_ = true;
} else {
fire_ = false;
@@ -183,10 +194,7 @@
if (data.PosEdge(kTest7)) {
}
- if (data.PosEdge(kTest8)) {
- }
-
- is_outtaking_ = data.IsPressed(kTest8);
+ is_outtaking_ = data.IsPressed(kIntakeOut);
if (!waiting_for_zero_) {
if (!action_queue_.Running()) {
@@ -197,13 +205,13 @@
new_superstructure_goal->max_angular_velocity_intake = 7.0;
new_superstructure_goal->max_angular_velocity_shoulder = 4.0;
- new_superstructure_goal->max_angular_velocity_wrist = 11.0;
+ new_superstructure_goal->max_angular_velocity_wrist = 10.0;
new_superstructure_goal->max_angular_acceleration_intake = 40.0;
- new_superstructure_goal->max_angular_acceleration_shoulder = 8.0;
+ new_superstructure_goal->max_angular_acceleration_shoulder = 10.0;
new_superstructure_goal->max_angular_acceleration_wrist = 25.0;
- /*
// Granny mode
+ /*
new_superstructure_goal->max_angular_velocity_intake = 0.2;
new_superstructure_goal->max_angular_velocity_shoulder = 0.2;
new_superstructure_goal->max_angular_velocity_wrist = 0.2;
@@ -266,6 +274,9 @@
// If we're waiting for the subsystems to zero.
bool waiting_for_zero_ = true;
+ // If true, the ball was present when the intaking button was pressed.
+ bool saw_ball_when_started_intaking_ = false;
+
bool is_intaking_ = false;
bool is_outtaking_ = false;
bool fire_ = false;
diff --git a/y2016/queues/BUILD b/y2016/queues/BUILD
index 3b282fd..81f75d4 100644
--- a/y2016/queues/BUILD
+++ b/y2016/queues/BUILD
@@ -3,6 +3,13 @@
load('/aos/build/queues', 'queue_library')
queue_library(
+ name = 'ball_detector',
+ srcs = [
+ 'ball_detector.q',
+ ],
+)
+
+queue_library(
name = 'profile_params',
srcs = [
'profile_params.q',
diff --git a/y2016/queues/ball_detector.q b/y2016/queues/ball_detector.q
new file mode 100644
index 0000000..948cc25
--- /dev/null
+++ b/y2016/queues/ball_detector.q
@@ -0,0 +1,13 @@
+package y2016.sensors;
+
+message BallDetector {
+ // Voltage measured by the ball detector sensor.
+
+ // Higher voltage means ball is closer to detector, lower voltage means ball
+ // is far from the sensor or not in the robot at all.
+ // TODO(comran): Check to see if our sensor's output corresponds with the
+ // comment above.
+
+ double voltage;
+};
+queue BallDetector ball_detector;
diff --git a/y2016/wpilib/BUILD b/y2016/wpilib/BUILD
index bd55575..2cb2937 100644
--- a/y2016/wpilib/BUILD
+++ b/y2016/wpilib/BUILD
@@ -37,5 +37,6 @@
'//y2016/control_loops/drivetrain:polydrivetrain_plants',
'//y2016/control_loops/shooter:shooter_queue',
'//y2016/control_loops/superstructure:superstructure_queue',
+ '//y2016/queues:ball_detector',
],
)
diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 2adddd9..7c58f15 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -36,6 +36,7 @@
#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2016/control_loops/shooter/shooter.q.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/queues/ball_detector.q.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/loop_output_handler.h"
@@ -243,6 +244,12 @@
wrist_encoder_.set_index(::std::move(index));
}
+ // Ball detector setter.
+ void set_ball_detector(::std::unique_ptr<AnalogInput> analog) {
+ ball_detector_ = ::std::move(analog);
+ }
+
+
// All of the DMA-related set_* calls must be made before this, and it doesn't
// hurt to do all of them.
@@ -333,6 +340,14 @@
superstructure_message.Send();
}
+
+ {
+ auto ball_detector_message =
+ ::y2016::sensors::ball_detector.MakeMessage();
+ ball_detector_message->voltage = ball_detector_->GetVoltage();
+ LOG_STRUCT(DEBUG, "ball detector", *ball_detector_message);
+ ball_detector_message.Send();
+ }
}
void Quit() { run_ = false; }
@@ -371,6 +386,7 @@
::std::unique_ptr<Encoder> shooter_left_encoder_, shooter_right_encoder_;
::frc971::wpilib::DMAEncoderAndPotentiometer intake_encoder_,
shoulder_encoder_, wrist_encoder_;
+ ::std::unique_ptr<AnalogInput> ball_detector_;
::std::atomic<bool> run_{true};
DigitalGlitchFilter drivetrain_shooter_encoder_filter_, hall_filter_,
@@ -408,6 +424,11 @@
shooter_pusher_ = ::std::move(s);
}
+ void set_lights(
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ lights_ = ::std::move(s);
+ }
+
void operator()() {
compressor_->Start();
::aos::SetCurrentThreadName("Solenoids");
@@ -439,6 +460,7 @@
LOG_STRUCT(DEBUG, "solenoids", *shooter_);
shooter_clamp_->Set(shooter_->clamp_open);
shooter_pusher_->Set(shooter_->push_to_shooter);
+ lights_->Set(shooter_->lights_on);
}
}
@@ -461,7 +483,7 @@
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_left_,
- drivetrain_right_, shooter_clamp_, shooter_pusher_;
+ drivetrain_right_, shooter_clamp_, shooter_pusher_, lights_;
::std::unique_ptr<Compressor> compressor_;
::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
@@ -605,7 +627,6 @@
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
SensorReader reader;
- // TODO(constants): Update these input numbers.
reader.set_drivetrain_left_encoder(make_encoder(5));
reader.set_drivetrain_right_encoder(make_encoder(6));
reader.set_drivetrain_left_hall(make_unique<AnalogInput>(5));
@@ -626,6 +647,8 @@
reader.set_wrist_index(make_unique<DigitalInput>(1));
reader.set_wrist_potentiometer(make_unique<AnalogInput>(1));
+ reader.set_ball_detector(make_unique<AnalogInput>(7));
+
reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
@@ -673,6 +696,7 @@
solenoid_writer.set_drivetrain_right(pcm->MakeSolenoid(0));
solenoid_writer.set_shooter_clamp(pcm->MakeSolenoid(4));
solenoid_writer.set_shooter_pusher(pcm->MakeSolenoid(5));
+ solenoid_writer.set_lights(pcm->MakeSolenoid(6));
solenoid_writer.set_compressor(make_unique<Compressor>());