Implement superstructure class and tests
Change-Id: If5a2a296ada3dd625ad7eb74af106b36b36ab932
diff --git a/y2019/BUILD b/y2019/BUILD
index 576cf6c..44c9e9c 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -23,11 +23,12 @@
"//aos/mutex",
"//aos/network:team_number",
"//frc971:constants",
+ "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2019/control_loops/drivetrain:polydrivetrain_plants",
"//y2019/control_loops/superstructure/elevator:elevator_plants",
+ "//y2019/control_loops/superstructure/wrist:wrist_plants",
"//y2019/control_loops/superstructure/intake:intake_plants",
"//y2019/control_loops/superstructure/stilts:stilts_plants",
- "//y2019/control_loops/superstructure/wrist:wrist_plants",
],
)
diff --git a/y2019/constants.cc b/y2019/constants.cc
index e91a49c..574f299 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -12,14 +12,16 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
#include "aos/once.h"
-
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
+#include "y2019/control_loops/superstructure/elevator/integral_elevator_plant.h"
+#include "y2019/control_loops/superstructure/intake/integral_intake_plant.h"
+#include "y2019/control_loops/superstructure/stilts/integral_stilts_plant.h"
+#include "y2019/control_loops/superstructure/wrist/integral_wrist_plant.h"
namespace y2019 {
namespace constants {
+using ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator;
+
const int Values::kZeroingSampleSize;
namespace {
@@ -31,94 +33,140 @@
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
Values::PotAndAbsConstants *const elevator = &r->elevator;
- Values::Intake *const intake = &r->intake;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ *const elevator_params = &(elevator->subsystem_params);
Values::PotAndAbsConstants *const stilts = &r->stilts;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ *const stilts_params = &(stilts->subsystem_params);
Values::PotAndAbsConstants *const wrist = &r->wrist;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ *const wrist_params = &(wrist->subsystem_params);
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator> *const intake =
+ &r->intake;
- elevator->zeroing.average_filter_size = Values::kZeroingSampleSize;
- elevator->zeroing.one_revolution_distance =
+ // Elevator constants.
+ elevator_params->zeroing_voltage = 4.0;
+ elevator_params->operating_voltage = 12.0;
+ elevator_params->zeroing_profile_params = {0.1, 1.0};
+ elevator_params->default_profile_params = {4.0, 3.0};
+ elevator_params->range = Values::kElevatorRange();
+ elevator_params->make_integral_loop =
+ &control_loops::superstructure::elevator::MakeIntegralElevatorLoop;
+ elevator_params->zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ elevator_params->zeroing_constants.one_revolution_distance =
M_PI * 2.0 * constants::Values::kElevatorEncoderRatio();
- elevator->zeroing.zeroing_threshold = 0.0005;
- elevator->zeroing.moving_buffer_size = 20;
- elevator->zeroing.allowable_encoder_error = 0.9;
+ elevator_params->zeroing_constants.zeroing_threshold = 0.005;
+ elevator_params->zeroing_constants.moving_buffer_size = 20;
+ elevator_params->zeroing_constants.allowable_encoder_error = 0.9;
- intake->zeroing.average_filter_size = Values::kZeroingSampleSize;
- intake->zeroing.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
- intake->zeroing.zeroing_threshold = 0.0005;
- intake->zeroing.moving_buffer_size = 20;
- intake->zeroing.allowable_encoder_error = 0.9;
-
- stilts->zeroing.average_filter_size = Values::kZeroingSampleSize;
- stilts->zeroing.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kStiltsEncoderRatio();
- stilts->zeroing.zeroing_threshold = 0.0005;
- stilts->zeroing.moving_buffer_size = 20;
- stilts->zeroing.allowable_encoder_error = 0.9;
-
- wrist->zeroing.average_filter_size = Values::kZeroingSampleSize;
- wrist->zeroing.one_revolution_distance =
+ // Wrist constants.
+ wrist_params->zeroing_voltage = 4.0;
+ wrist_params->operating_voltage = 12.0;
+ wrist_params->zeroing_profile_params = {0.5, 2.0};
+ wrist_params->default_profile_params = {6.0, 5.0};
+ wrist_params->range = Values::kWristRange();
+ wrist_params->make_integral_loop =
+ &control_loops::superstructure::wrist::MakeIntegralWristLoop;
+ wrist_params->zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ wrist_params->zeroing_constants.one_revolution_distance =
M_PI * 2.0 * constants::Values::kWristEncoderRatio();
- wrist->zeroing.zeroing_threshold = 0.0005;
- wrist->zeroing.moving_buffer_size = 20;
- wrist->zeroing.allowable_encoder_error = 0.9;
+ wrist_params->zeroing_constants.zeroing_threshold = 0.0005;
+ wrist_params->zeroing_constants.moving_buffer_size = 20;
+ wrist_params->zeroing_constants.allowable_encoder_error = 0.9;
+
+ // Intake constants.
+ intake->zeroing_voltage = 4.0;
+ intake->operating_voltage = 12.0;
+ intake->zeroing_profile_params = {0.5, 3.0};
+ intake->default_profile_params = {6.0, 5.0};
+ intake->range = Values::kIntakeRange();
+ intake->make_integral_loop =
+ control_loops::superstructure::intake::MakeIntegralIntakeLoop;
+ intake->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
+ intake->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
+ intake->zeroing_constants.zeroing_threshold = 0.0005;
+ intake->zeroing_constants.moving_buffer_size = 20;
+ intake->zeroing_constants.allowable_encoder_error = 0.9;
+
+ // Stilts constants.
+ stilts_params->zeroing_voltage = 4.0;
+ stilts_params->operating_voltage = 12.0;
+ stilts_params->zeroing_profile_params = {0.1, 3.0};
+ stilts_params->default_profile_params = {2.0, 4.0};
+ stilts_params->range = Values::kStiltsRange();
+ stilts_params->make_integral_loop =
+ &control_loops::superstructure::stilts::MakeIntegralStiltsLoop;
+ stilts_params->zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ stilts_params->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kStiltsEncoderRatio();
+ stilts_params->zeroing_constants.zeroing_threshold = 0.0005;
+ stilts_params->zeroing_constants.moving_buffer_size = 20;
+ stilts_params->zeroing_constants.allowable_encoder_error = 0.9;
switch (team) {
// A set of constants for tests.
case 1:
- elevator->zeroing.measured_absolute_position = 0.0;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.0;
elevator->potentiometer_offset = 0.0;
- intake->zeroing.measured_absolute_position = 0.0;
- intake->zeroing.middle_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.middle_position = 0.0;
- stilts->zeroing.measured_absolute_position = 0.0;
- stilts->potentiometer_offset = 0.0;
-
- wrist->zeroing.measured_absolute_position = 0.0;
+ wrist_params->zeroing_constants.measured_absolute_position = 0.0;
wrist->potentiometer_offset = 0.0;
+
+ stilts_params->zeroing_constants.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
break;
case kCompTeamNumber:
- elevator->zeroing.measured_absolute_position = 0.0;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.0;
elevator->potentiometer_offset = 0.0;
- intake->zeroing.measured_absolute_position = 0.0;
- intake->zeroing.middle_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.middle_position = 0.0;
- stilts->zeroing.measured_absolute_position = 0.0;
- stilts->potentiometer_offset = 0.0;
-
- wrist->zeroing.measured_absolute_position = 0.0;
+ wrist_params->zeroing_constants.measured_absolute_position = 0.0;
wrist->potentiometer_offset = 0.0;
+
+ stilts_params->zeroing_constants.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
break;
case kPracticeTeamNumber:
- elevator->zeroing.measured_absolute_position = 0.0;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.0;
elevator->potentiometer_offset = 0.0;
- intake->zeroing.measured_absolute_position = 0.0;
- intake->zeroing.middle_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.middle_position = 0.0;
- stilts->zeroing.measured_absolute_position = 0.0;
- stilts->potentiometer_offset = 0.0;
-
- wrist->zeroing.measured_absolute_position = 0.0;
+ wrist_params->zeroing_constants.measured_absolute_position = 0.0;
wrist->potentiometer_offset = 0.0;
+
+ stilts_params->zeroing_constants.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
break;
case kCodingRobotTeamNumber:
- elevator->zeroing.measured_absolute_position = 0.0;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.0;
elevator->potentiometer_offset = 0.0;
- intake->zeroing.measured_absolute_position = 0.0;
- intake->zeroing.middle_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.middle_position = 0.0;
- stilts->zeroing.measured_absolute_position = 0.0;
- stilts->potentiometer_offset = 0.0;
-
- wrist->zeroing.measured_absolute_position = 0.0;
+ wrist_params->zeroing_constants.measured_absolute_position = 0.0;
wrist->potentiometer_offset = 0.0;
+
+ stilts_params->zeroing_constants.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
break;
default:
diff --git a/y2019/constants.h b/y2019/constants.h
index 21c061c..2cffaae 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -5,9 +5,10 @@
#include <stdint.h>
#include "frc971/constants.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2019/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2019/control_loops/superstructure/intake/intake_plant.h"
#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
+#include "y2019/control_loops/superstructure/intake/intake_plant.h"
#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
#include "y2019/control_loops/superstructure/wrist/wrist_plant.h"
@@ -27,6 +28,7 @@
struct Values {
static const int kZeroingSampleSize = 200;
+ // Drivetrain Constants
static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
static constexpr double kDrivetrainEncoderCountsPerRevolution() {
return kDrivetrainCyclesPerRevolution() * 4;
@@ -117,6 +119,7 @@
// Stilts
static constexpr double kStiltsEncoderCountsPerRevolution() { return 4096.0; }
+ // Stilts Constants
static constexpr double kStiltsEncoderRatio() {
return (1.0 /* Gear ratio */) *
control_loops::superstructure::stilts::kRadius;
@@ -144,17 +147,19 @@
}
struct PotAndAbsConstants {
- ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ subsystem_params;
double potentiometer_offset;
};
+
PotAndAbsConstants elevator;
PotAndAbsConstants wrist;
- PotAndAbsConstants stilts;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ intake;
- struct Intake {
- ::frc971::constants::AbsoluteEncoderZeroingConstants zeroing;
- };
- Intake intake;
+ PotAndAbsConstants stilts;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 453c680..f55e1ad 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -23,8 +23,30 @@
"superstructure.h",
],
deps = [
+ ":collision_avoidance",
":superstructure_queue",
"//aos/controls:control_loop",
+ "//y2019:constants",
+ ],
+)
+
+cc_test(
+ name = "superstructure_lib_test",
+ srcs = [
+ "superstructure_lib_test.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ ":superstructure_queue",
+ "//aos:math",
+ "//aos:queues",
+ "//aos/controls:control_loop_test",
+ "//aos/testing:googletest",
+ "//aos/time",
+ "//frc971/control_loops:capped_test_plant",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:team_number_test_environment",
+ "//y2019/control_loops/superstructure/intake:intake_plants",
],
)
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
index 7a26b90..ed6787f 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -128,8 +128,8 @@
}
if (unsafe_goal) {
- const double wrist_goal = unsafe_goal->wrist.angle;
- const double intake_goal = unsafe_goal->intake.joint_angle;
+ const double wrist_goal = unsafe_goal->wrist.unsafe_goal;
+ const double intake_goal = unsafe_goal->intake.unsafe_goal;
// Compute if we need to move the intake.
const bool intake_needs_to_move = (intake_position < kIntakeMiddleAngle) ^
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
index 92a1a70..17f9fdc 100644
--- a/y2019/control_loops/superstructure/collision_avoidance_tests.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -36,20 +36,20 @@
was_collided = avoidance.IsCollided(&status);
}
- safe_goal.wrist.angle =
- ::aos::Clip(unsafe_goal.wrist.angle, avoidance.min_wrist_goal(),
+ safe_goal.wrist.unsafe_goal =
+ ::aos::Clip(unsafe_goal.wrist.unsafe_goal, avoidance.min_wrist_goal(),
avoidance.max_wrist_goal());
- safe_goal.elevator.height = ::std::max(unsafe_goal.elevator.height,
+ safe_goal.elevator.unsafe_goal = ::std::max(unsafe_goal.elevator.unsafe_goal,
avoidance.min_elevator_goal());
- safe_goal.intake.joint_angle =
- ::aos::Clip(unsafe_goal.intake.joint_angle,
+ safe_goal.intake.unsafe_goal =
+ ::aos::Clip(unsafe_goal.intake.unsafe_goal,
avoidance.min_intake_goal(), avoidance.max_intake_goal());
- LimitedMove(&status.wrist.position, safe_goal.wrist.angle);
- LimitedMove(&status.elevator.position, safe_goal.elevator.height);
- LimitedMove(&status.intake.position, safe_goal.intake.joint_angle);
+ LimitedMove(&status.wrist.position, safe_goal.wrist.unsafe_goal);
+ LimitedMove(&status.elevator.position, safe_goal.elevator.unsafe_goal);
+ LimitedMove(&status.intake.position, safe_goal.intake.unsafe_goal);
if (IsMoving()) {
break;
}
@@ -78,9 +78,9 @@
void CheckGoals() {
// check to see if we reached the goals
- ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
- ASSERT_NEAR(unsafe_goal.elevator.height, status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.elevator.unsafe_goal, status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
private:
@@ -101,9 +101,9 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeInAngle - avoidance.kEpsIntake;
// sets the status position messgaes to be have the elevator at the bottom
@@ -123,9 +123,9 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
// sets the status position messgaes to be have the elevator at the half way
@@ -152,9 +152,9 @@
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
Iterate();
@@ -174,9 +174,9 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- unsafe_goal.wrist.angle = avoidance.kWristMinAngle + avoidance.kEpsWrist;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeInAngle - avoidance.kEpsIntake;
Iterate();
@@ -190,10 +190,10 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.angle =
+ unsafe_goal.wrist.unsafe_goal =
avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
// sets the status position messgaes to be have the elevator at the half way
@@ -213,10 +213,10 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.angle =
+ unsafe_goal.wrist.unsafe_goal =
avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
// sets the status position messgaes to be have the elevator at the half way
@@ -236,9 +236,9 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.angle = 4.0;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = 4.0;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
// sets the status position messgaes to be have the elevator at the half way
@@ -249,10 +249,10 @@
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
// Unreasonable Wrist Goal
@@ -260,9 +260,9 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.angle = avoidance.kWristMinAngle;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
(avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
// sets the status position messgaes to be have the elevator at the half way
@@ -274,18 +274,18 @@
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
// Fix Collision Wrist in Elevator
TEST_F(CollisionAvoidanceTests, FixElevatorCollision) {
// changes the goals
- unsafe_goal.wrist.angle = 3.14;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = 3.14;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
// sets the status position messgaes
@@ -295,18 +295,18 @@
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
// Fix Collision Wrist in Intake
TEST_F(CollisionAvoidanceTests, FixWristCollision) {
// changes the goals
- unsafe_goal.wrist.angle = avoidance.kWristMinAngle + avoidance.kEpsWrist;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
(avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
// sets the status position messgaes
@@ -317,17 +317,17 @@
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
// Fix Collision Wrist Above Elevator
TEST_F(CollisionAvoidanceTests, FixWristElevatorCollision) {
// changes the goals
- unsafe_goal.wrist.angle = 0.0;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = 0.0;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
// sets the status position messgaes
@@ -337,10 +337,10 @@
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
ASSERT_NEAR((avoidance.kElevatorClearHeight + avoidance.kEps),
status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
} // namespace testing
} // namespace superstructure
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index e87ef6e..efa6ccb 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -2,6 +2,7 @@
#include "aos/controls/control_loops.q.h"
#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
namespace y2019 {
namespace control_loops {
@@ -9,21 +10,60 @@
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name) {}
+ : aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
+ elevator_(constants::GetValues().elevator.subsystem_params),
+ wrist_(constants::GetValues().wrist.subsystem_params),
+ intake_(constants::GetValues().intake),
+ stilts_(constants::GetValues().stilts.subsystem_params) {}
-void Superstructure::RunIteration(
- const SuperstructureQueue::Goal *unsafe_goal,
- const SuperstructureQueue::Position *position,
- SuperstructureQueue::Output *output,
- SuperstructureQueue::Status *status) {
- (void)unsafe_goal;
- (void)position;
- (void)output;
- (void)status;
-
+void Superstructure::RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
+ const SuperstructureQueue::Position *position,
+ SuperstructureQueue::Output *output,
+ SuperstructureQueue::Status *status) {
if (WasReset()) {
LOG(ERROR, "WPILib reset, restarting\n");
+ elevator_.Reset();
+ wrist_.Reset();
+ intake_.Reset();
+ stilts_.Reset();
}
+
+ elevator_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->elevator) : nullptr,
+ &(position->elevator),
+ output != nullptr ? &(output->elevator_voltage) : nullptr,
+ &(status->elevator));
+
+ wrist_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->wrist) : nullptr,
+ &(position->wrist),
+ output != nullptr ? &(output->wrist_voltage) : nullptr,
+ &(status->wrist));
+
+ intake_.Iterate(
+ unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
+ &(position->intake_joint),
+ output != nullptr ? &(output->intake_joint_voltage) : nullptr,
+ &(status->intake));
+
+ stilts_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->stilts) : nullptr,
+ &(position->stilts),
+ output != nullptr ? &(output->stilts_voltage) : nullptr,
+ &(status->stilts));
+
+ status->zeroed = status->elevator.zeroed && status->wrist.zeroed &&
+ status->intake.zeroed && status->stilts.zeroed;
+
+ status->estopped = status->elevator.estopped || status->wrist.estopped ||
+ status->intake.estopped || status->stilts.estopped;
+
+ // TODO(theo) move these up when Iterate() is split
+ // update the goals
+ collision_avoidance_.UpdateGoal(status, unsafe_goal);
+
+ elevator_.set_min_position(collision_avoidance_.min_elevator_goal());
+ wrist_.set_min_position(collision_avoidance_.min_wrist_goal());
+ wrist_.set_max_position(collision_avoidance_.max_wrist_goal());
+ intake_.set_min_position(collision_avoidance_.min_intake_goal());
+ intake_.set_max_position(collision_avoidance_.max_intake_goal());
}
} // namespace superstructure
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 0d6765e..22dbcd6 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -2,6 +2,9 @@
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
#include "aos/controls/control_loop.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "y2019/constants.h"
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
namespace y2019 {
@@ -16,14 +19,41 @@
const ::std::string &name =
".y2019.control_loops.superstructure.superstructure_queue");
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+ using AbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
+ const PotAndAbsoluteEncoderSubsystem &elevator() const {
+ return elevator_;
+ }
+ const PotAndAbsoluteEncoderSubsystem &wrist() const {
+ return wrist_;
+ }
+ const AbsoluteEncoderSubsystem &intake() const {
+ return intake_;
+ }
+ const PotAndAbsoluteEncoderSubsystem &stilts() const {
+ return stilts_;
+ }
+
protected:
- virtual void RunIteration(
- const SuperstructureQueue::Goal *unsafe_goal,
- const SuperstructureQueue::Position *position,
- SuperstructureQueue::Output *output,
- SuperstructureQueue::Status *status) override;
+ virtual void RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
+ const SuperstructureQueue::Position *position,
+ SuperstructureQueue::Output *output,
+ SuperstructureQueue::Status *status) override;
private:
+ PotAndAbsoluteEncoderSubsystem elevator_;
+ PotAndAbsoluteEncoderSubsystem wrist_;
+ AbsoluteEncoderSubsystem intake_;
+ PotAndAbsoluteEncoderSubsystem stilts_;
+
+ CollisionAvoidance collision_avoidance_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index cd004ac..a71f81a 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -3,25 +3,6 @@
import "aos/controls/control_loops.q";
import "frc971/control_loops/profiled_subsystem.q";
-struct ElevatorGoal {
- // Meters, 0 = lowest position - mechanical hard stop,
- // positive = upward
- double height;
-
- .frc971.ProfileParameters profile_params;
-};
-
-struct IntakeGoal {
- // Positive is rollers intaking inward.
- double roller_voltage;
-
- // 0 = linkage on the sprocket is pointing straight up,
- // positive = forward
- double joint_angle;
-
- .frc971.ProfileParameters profile_params;
-};
-
struct SuctionGoal {
// True = open solenoid (apply suction)
// Top/bottom are when wrist is forward
@@ -29,31 +10,29 @@
bool bottom;
};
-struct StiltsGoal {
- // Distance stilts extended out of the bottom of the robot. Positive = down.
- // 0 is the height such that the bottom of the stilts is tangent to the bottom
- // of the middle wheels.
- double height;
-
- .frc971.ProfileParameters profile_params;
-};
-
-struct WristGoal {
- // 0 = Straight up parallel to elevator
- // Positive rotates toward intake from 0
- double angle;
- .frc971.ProfileParameters profile_params;
-};
-
queue_group SuperstructureQueue {
implements aos.control_loops.ControlLoop;
message Goal {
- ElevatorGoal elevator;
- IntakeGoal intake;
+ // Meters, 0 = lowest position - mechanical hard stop,
+ // positive = upward
+ .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal elevator;
+ // 0 = linkage on the sprocket is pointing straight up,
+ // positive = forward
+ .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal intake;
+ // 0 = Straight up parallel to elevator
+ // Positive rotates toward intake from 0
+ .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal wrist;
+
+ // Distance stilts extended out of the bottom of the robot. Positive = down.
+ // 0 is the height such that the bottom of the stilts is tangent to the bottom
+ // of the middle wheels.
+ .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal stilts;
+
+ // Positive is rollers intaking inward.
+ double roller_voltage;
+
SuctionGoal suction;
- StiltsGoal stilts;
- WristGoal wrist;
};
message Status {
@@ -69,7 +48,7 @@
// Status of each subsystem.
.frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus elevator;
.frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus wrist;
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
+ .frc971.control_loops.AbsoluteEncoderProfiledJointStatus intake;
.frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus stilts;
};
@@ -106,7 +85,8 @@
// Voltage sent to rollers on intake. Positive rolls inward.
double intake_roller_voltage;
- // Voltage sent to motors to move stilts height. Positive moves robot upward.
+ // Voltage sent to motors to move stilts height. Positive moves robot
+ // upward.
double stilts_voltage;
// True opens solenoid (applies suction)
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..de973c0
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,663 @@
+#include <unistd.h>
+
+#include <chrono>
+#include <memory>
+
+#include "aos/controls/control_loop_test.h"
+#include "aos/queue.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "gtest/gtest.h"
+#include "y2019/constants.h"
+#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
+#include "y2019/control_loops/superstructure/intake/intake_plant.h"
+#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
+#include "y2019/control_loops/superstructure/superstructure.h"
+#include "y2019/control_loops/superstructure/wrist/wrist_plant.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+namespace {
+constexpr double kNoiseScalar = 0.01;
+} // namespace
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::control_loops::CappedTestPlant;
+typedef Superstructure::PotAndAbsoluteEncoderSubsystem
+ PotAndAbsoluteEncoderSubsystem;
+typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
+
+// Class which simulates the superstructure and sends out queue messages with
+// the position.
+class SuperstructureSimulation {
+ public:
+ SuperstructureSimulation()
+ : elevator_plant_(
+ new CappedTestPlant(::y2019::control_loops::superstructure::
+ elevator::MakeElevatorPlant())),
+ elevator_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kElevatorEncoderRatio()),
+
+ wrist_plant_(new CappedTestPlant(
+ ::y2019::control_loops::superstructure::wrist::MakeWristPlant())),
+ wrist_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kWristEncoderRatio()),
+
+ intake_plant_(new CappedTestPlant(
+ ::y2019::control_loops::superstructure::intake::MakeIntakePlant())),
+ intake_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kIntakeEncoderRatio()),
+
+ stilts_plant_(new CappedTestPlant(
+ ::y2019::control_loops::superstructure::stilts::MakeStiltsPlant())),
+ stilts_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kStiltsEncoderRatio()),
+
+ superstructure_queue_(
+ ".y2019.control_loops.superstructure.superstructure_queue",
+ ".y2019.control_loops.superstructure.superstructure_queue.goal",
+ ".y2019.control_loops.superstructure.superstructure_queue.output",
+ ".y2019.control_loops.superstructure.superstructure_queue.status",
+ ".y2019.control_loops.superstructure.superstructure_queue."
+ "position") {
+ // Start the elevator out in the middle by default.
+ InitializeElevatorPosition(constants::Values::kElevatorRange().upper);
+
+ // Start the wrist out in the middle by default.
+ InitializeWristPosition(constants::Values::kWristRange().upper);
+
+ InitializeIntakePosition(constants::Values::kIntakeRange().upper);
+
+ // Start the stilts out in the middle by default.
+ InitializeStiltsPosition(constants::Values::kStiltsRange().lower);
+ }
+
+ void InitializeElevatorPosition(double start_pos) {
+ elevator_plant_->mutable_X(0, 0) = start_pos;
+ elevator_plant_->mutable_X(1, 0) = 0.0;
+
+ elevator_pot_encoder_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ constants::GetValues()
+ .elevator.subsystem_params.zeroing_constants
+ .measured_absolute_position);
+ }
+
+ void InitializeWristPosition(double start_pos) {
+ wrist_plant_->mutable_X(0, 0) = start_pos;
+ wrist_plant_->mutable_X(1, 0) = 0.0;
+ wrist_pot_encoder_.Initialize(start_pos, kNoiseScalar, 0.0,
+ constants::GetValues()
+ .wrist.subsystem_params.zeroing_constants
+ .measured_absolute_position);
+ }
+
+ void InitializeIntakePosition(double start_pos) {
+ intake_plant_->mutable_X(0, 0) = start_pos;
+ intake_plant_->mutable_X(1, 0) = 0.0;
+
+ intake_pot_encoder_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ constants::GetValues()
+ .intake.zeroing_constants.measured_absolute_position);
+ }
+
+ void InitializeStiltsPosition(double start_pos) {
+ stilts_plant_->mutable_X(0, 0) = start_pos;
+ stilts_plant_->mutable_X(1, 0) = 0.0;
+
+ stilts_pot_encoder_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ constants::GetValues()
+ .stilts.subsystem_params.zeroing_constants
+ .measured_absolute_position);
+ }
+
+ // Sends a queue message with the position of the superstructure.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<SuperstructureQueue::Position> position =
+ superstructure_queue_.position.MakeMessage();
+
+ elevator_pot_encoder_.GetSensorValues(&position->elevator);
+ wrist_pot_encoder_.GetSensorValues(&position->wrist);
+ intake_pot_encoder_.GetSensorValues(&position->intake_joint);
+ stilts_pot_encoder_.GetSensorValues(&position->stilts);
+ position.Send();
+ }
+
+ double elevator_position() const { return elevator_plant_->X(0, 0); }
+ double elevator_velocity() const { return elevator_plant_->X(1, 0); }
+
+ double wrist_position() const { return wrist_plant_->X(0, 0); }
+ double wrist_velocity() const { return wrist_plant_->X(1, 0); }
+
+ double intake_position() const { return intake_plant_->X(0, 0); }
+ double intake_velocity() const { return intake_plant_->X(1, 0); }
+
+ double stilts_position() const { return stilts_plant_->X(0, 0); }
+ double stilts_velocity() const { return stilts_plant_->X(1, 0); }
+
+ // Sets the difference between the commanded and applied powers.
+ // This lets us test that the integrators work.
+
+ void set_elevator_voltage_offset(double voltage_offset) {
+ elevator_plant_->set_voltage_offset(voltage_offset);
+ }
+
+ void set_wrist_voltage_offset(double voltage_offset) {
+ wrist_plant_->set_voltage_offset(voltage_offset);
+ }
+
+ void set_intake_voltage_offset(double voltage_offset) {
+ intake_plant_->set_voltage_offset(voltage_offset);
+ }
+
+ void set_stilts_voltage_offset(double voltage_offset) {
+ stilts_plant_->set_voltage_offset(voltage_offset);
+ }
+
+ // Simulates the superstructure for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+ EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
+
+ const double voltage_check_elevator =
+ (static_cast<PotAndAbsoluteEncoderSubsystem::State>(
+ superstructure_queue_.status->elevator.state) ==
+ PotAndAbsoluteEncoderSubsystem::State::RUNNING)
+ ? constants::GetValues().elevator.subsystem_params.operating_voltage
+ : constants::GetValues().elevator.subsystem_params.zeroing_voltage;
+
+ const double voltage_check_wrist =
+ (static_cast<PotAndAbsoluteEncoderSubsystem::State>(
+ superstructure_queue_.status->wrist.state) ==
+ PotAndAbsoluteEncoderSubsystem::State::RUNNING)
+ ? constants::GetValues().wrist.subsystem_params.operating_voltage
+ : constants::GetValues().wrist.subsystem_params.zeroing_voltage;
+
+ const double voltage_check_intake =
+ (static_cast<AbsoluteEncoderSubsystem::State>(
+ superstructure_queue_.status->intake.state) ==
+ AbsoluteEncoderSubsystem::State::RUNNING)
+ ? constants::GetValues().intake.operating_voltage
+ : constants::GetValues().intake.zeroing_voltage;
+
+ const double voltage_check_stilts =
+ (static_cast<PotAndAbsoluteEncoderSubsystem::State>(
+ superstructure_queue_.status->stilts.state) ==
+ PotAndAbsoluteEncoderSubsystem::State::RUNNING)
+ ? constants::GetValues().stilts.subsystem_params.operating_voltage
+ : constants::GetValues().stilts.subsystem_params.zeroing_voltage;
+
+ EXPECT_NEAR(superstructure_queue_.output->elevator_voltage, 0.0,
+ voltage_check_elevator);
+
+ EXPECT_NEAR(superstructure_queue_.output->wrist_voltage, 0.0,
+ voltage_check_wrist);
+
+ EXPECT_NEAR(superstructure_queue_.output->intake_joint_voltage, 0.0,
+ voltage_check_intake);
+
+ EXPECT_NEAR(superstructure_queue_.output->stilts_voltage, 0.0,
+ voltage_check_stilts);
+
+ ::Eigen::Matrix<double, 1, 1> elevator_U;
+ elevator_U << superstructure_queue_.output->elevator_voltage +
+ elevator_plant_->voltage_offset();
+
+ ::Eigen::Matrix<double, 1, 1> wrist_U;
+ wrist_U << superstructure_queue_.output->wrist_voltage +
+ wrist_plant_->voltage_offset();
+
+ ::Eigen::Matrix<double, 1, 1> intake_U;
+ intake_U << superstructure_queue_.output->intake_joint_voltage +
+ intake_plant_->voltage_offset();
+
+ ::Eigen::Matrix<double, 1, 1> stilts_U;
+ stilts_U << superstructure_queue_.output->stilts_voltage +
+ stilts_plant_->voltage_offset();
+
+ elevator_plant_->Update(elevator_U);
+ wrist_plant_->Update(wrist_U);
+ intake_plant_->Update(intake_U);
+ stilts_plant_->Update(stilts_U);
+
+ const double position_elevator = elevator_plant_->Y(0, 0);
+ const double position_wrist = wrist_plant_->Y(0, 0);
+ const double position_intake = intake_plant_->Y(0, 0);
+ const double position_stilts = stilts_plant_->Y(0, 0);
+
+ elevator_pot_encoder_.MoveTo(position_elevator);
+ wrist_pot_encoder_.MoveTo(position_wrist);
+ intake_pot_encoder_.MoveTo(position_intake);
+ stilts_pot_encoder_.MoveTo(position_stilts);
+
+ EXPECT_GE(position_elevator,
+ constants::Values::kElevatorRange().lower_hard);
+ EXPECT_LE(position_elevator,
+ constants::Values::kElevatorRange().upper_hard);
+
+ EXPECT_GE(position_wrist, constants::Values::kWristRange().lower_hard);
+ EXPECT_LE(position_wrist, constants::Values::kWristRange().upper_hard);
+
+ EXPECT_GE(position_intake, constants::Values::kIntakeRange().lower_hard);
+ EXPECT_LE(position_intake, constants::Values::kIntakeRange().upper_hard);
+
+ EXPECT_GE(position_stilts, constants::Values::kStiltsRange().lower_hard);
+ EXPECT_LE(position_stilts, constants::Values::kStiltsRange().upper_hard);
+ }
+
+ private:
+ ::std::unique_ptr<CappedTestPlant> elevator_plant_;
+ PositionSensorSimulator elevator_pot_encoder_;
+
+ ::std::unique_ptr<CappedTestPlant> wrist_plant_;
+ PositionSensorSimulator wrist_pot_encoder_;
+
+ ::std::unique_ptr<CappedTestPlant> intake_plant_;
+ PositionSensorSimulator intake_pot_encoder_;
+
+ ::std::unique_ptr<CappedTestPlant> stilts_plant_;
+ PositionSensorSimulator stilts_pot_encoder_;
+
+ SuperstructureQueue superstructure_queue_;
+};
+
+class SuperstructureTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ SuperstructureTest()
+ : superstructure_queue_(
+ ".y2019.control_loops.superstructure.superstructure_queue",
+ ".y2019.control_loops.superstructure.superstructure_queue.goal",
+ ".y2019.control_loops.superstructure.superstructure_queue.output",
+ ".y2019.control_loops.superstructure.superstructure_queue.status",
+ ".y2019.control_loops.superstructure.superstructure_queue."
+ "position"),
+ superstructure_(&event_loop_) {
+ set_team_id(::frc971::control_loops::testing::kTeamNumber);
+ }
+
+ void VerifyNearGoal() {
+ superstructure_queue_.goal.FetchLatest();
+ superstructure_queue_.status.FetchLatest();
+
+ EXPECT_NEAR(superstructure_queue_.goal->elevator.unsafe_goal,
+ superstructure_queue_.status->elevator.position, 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->wrist.unsafe_goal,
+ superstructure_plant_.wrist_position(), 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->intake.unsafe_goal,
+ superstructure_queue_.status->intake.position, 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->stilts.unsafe_goal,
+ superstructure_plant_.stilts_position(), 0.001);
+ }
+
+ // Runs one iteration of the whole simulation.
+ void RunIteration(bool enabled = true) {
+ SendMessages(enabled);
+
+ superstructure_plant_.SendPositionMessage();
+ superstructure_.Iterate();
+ superstructure_plant_.Simulate();
+
+ TickTime(::std::chrono::microseconds(5050));
+ }
+
+ void CheckCollisions() {
+ superstructure_queue_.status.FetchLatest();
+ ASSERT_FALSE(
+ collision_avoidance_.IsCollided(superstructure_queue_.status.get()));
+ }
+
+ void WaitUntilZeroed() {
+ int i = 0;
+ do {
+ i++;
+ RunIteration();
+ superstructure_queue_.status.FetchLatest();
+ // 2 Seconds
+ ASSERT_LE(i, 2 * 1.0 / .00505);
+ } while (!superstructure_queue_.status.get()->zeroed);
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const monotonic_clock::duration run_for, bool enabled = true,
+ bool check_collisions = true) {
+ const auto start_time = monotonic_clock::now();
+ while (monotonic_clock::now() < start_time + run_for) {
+ const auto loop_start_time = monotonic_clock::now();
+ double begin_elevator_velocity =
+ superstructure_plant_.elevator_velocity();
+ double begin_wrist_velocity = superstructure_plant_.wrist_velocity();
+ double begin_intake_velocity = superstructure_plant_.intake_velocity();
+ double begin_stilts_velocity = superstructure_plant_.stilts_velocity();
+
+ RunIteration(enabled);
+ if (check_collisions) {
+ CheckCollisions();
+ }
+
+ const double loop_time = chrono::duration_cast<chrono::duration<double>>(
+ monotonic_clock::now() - loop_start_time)
+ .count();
+
+ const double elevator_acceleration =
+ (superstructure_plant_.elevator_velocity() -
+ begin_elevator_velocity) /
+ loop_time;
+ const double wrist_acceleration =
+ (superstructure_plant_.wrist_velocity() - begin_wrist_velocity) /
+ loop_time;
+ const double intake_acceleration =
+ (superstructure_plant_.intake_velocity() - begin_intake_velocity) /
+ loop_time;
+ const double stilts_acceleration =
+ (superstructure_plant_.stilts_velocity() - begin_stilts_velocity) /
+ loop_time;
+
+ EXPECT_GE(peak_elevator_acceleration_, elevator_acceleration);
+ EXPECT_LE(-peak_elevator_acceleration_, elevator_acceleration);
+ EXPECT_GE(peak_elevator_velocity_,
+ superstructure_plant_.elevator_velocity());
+ EXPECT_LE(-peak_elevator_velocity_,
+ superstructure_plant_.elevator_velocity());
+
+ EXPECT_GE(peak_wrist_acceleration_, wrist_acceleration);
+ EXPECT_LE(-peak_wrist_acceleration_, wrist_acceleration);
+ EXPECT_GE(peak_wrist_velocity_, superstructure_plant_.wrist_velocity());
+ EXPECT_LE(-peak_wrist_velocity_, superstructure_plant_.wrist_velocity());
+
+ EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
+ EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
+ EXPECT_GE(peak_intake_velocity_, superstructure_plant_.intake_velocity());
+ EXPECT_LE(-peak_intake_velocity_,
+ superstructure_plant_.intake_velocity());
+
+ EXPECT_GE(peak_stilts_acceleration_, stilts_acceleration);
+ EXPECT_LE(-peak_stilts_acceleration_, stilts_acceleration);
+ EXPECT_GE(peak_stilts_velocity_, superstructure_plant_.stilts_velocity());
+ EXPECT_LE(-peak_stilts_velocity_,
+ superstructure_plant_.stilts_velocity());
+ }
+ }
+
+ void set_peak_elevator_acceleration(double value) {
+ peak_elevator_acceleration_ = value;
+ }
+ void set_peak_elevator_velocity(double value) {
+ peak_elevator_velocity_ = value;
+ }
+
+ void set_peak_wrist_acceleration(double value) {
+ peak_wrist_acceleration_ = value;
+ }
+ void set_peak_wrist_velocity(double value) { peak_wrist_velocity_ = value; }
+
+ void set_peak_intake_acceleration(double value) {
+ peak_intake_acceleration_ = value;
+ }
+ void set_peak_intake_velocity(double value) { peak_intake_velocity_ = value; }
+
+ void set_peak_stilts_acceleration(double value) {
+ peak_stilts_acceleration_ = value;
+ }
+ void set_peak_stilts_velocity(double value) { peak_stilts_velocity_ = value; }
+
+ ::aos::ShmEventLoop event_loop_;
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory
+ // that is no longer valid.
+ SuperstructureQueue superstructure_queue_;
+
+ // Create a control loop and simulation.
+ Superstructure superstructure_;
+ SuperstructureSimulation superstructure_plant_;
+
+ // Creat a collision avoidance object
+ CollisionAvoidance collision_avoidance_;
+
+ private:
+ // The acceleration limits to check for while moving.
+ double peak_elevator_acceleration_ = 1e10;
+ double peak_wrist_acceleration_ = 1e10;
+ double peak_intake_acceleration_ = 1e10;
+ double peak_stilts_acceleration_ = 1e10;
+
+ // The velocity limits to check for while moving.
+ double peak_elevator_velocity_ = 1e10;
+ double peak_wrist_velocity_ = 1e10;
+ double peak_intake_velocity_ = 1e10;
+ double peak_stilts_velocity_ = 1e10;
+};
+
+// Tests that the superstructure does nothing when the goal is zero.
+TEST_F(SuperstructureTest, DoesNothing) {
+ superstructure_plant_.InitializeElevatorPosition(1.4);
+ superstructure_plant_.InitializeWristPosition(1.0);
+ superstructure_plant_.InitializeIntakePosition(1.1);
+ superstructure_plant_.InitializeStiltsPosition(0.1);
+
+ WaitUntilZeroed();
+
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+
+ goal->elevator.unsafe_goal = 1.4;
+ goal->wrist.unsafe_goal = 1.0;
+ goal->intake.unsafe_goal = 1.1;
+ goal->stilts.unsafe_goal = 0.1;
+ ASSERT_TRUE(goal.Send());
+ }
+ RunForTime(chrono::seconds(10));
+ VerifyNearGoal();
+
+ EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+}
+
+// Tests that loops can reach a goal.
+TEST_F(SuperstructureTest, ReachesGoal) {
+ // Set a reasonable goal.
+
+ superstructure_plant_.InitializeElevatorPosition(1.4);
+ superstructure_plant_.InitializeWristPosition(1.0);
+ superstructure_plant_.InitializeIntakePosition(1.1);
+ superstructure_plant_.InitializeStiltsPosition(0.1);
+
+ WaitUntilZeroed();
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = 1.4;
+ goal->elevator.profile_params.max_velocity = 1;
+ goal->elevator.profile_params.max_acceleration = 0.5;
+
+ goal->wrist.unsafe_goal = 1.0;
+ goal->wrist.profile_params.max_velocity = 1;
+ goal->wrist.profile_params.max_acceleration = 0.5;
+
+ goal->intake.unsafe_goal = 1.1;
+ goal->intake.profile_params.max_velocity = 1;
+ goal->intake.profile_params.max_acceleration = 0.5;
+
+ goal->stilts.unsafe_goal = 0.1;
+ goal->stilts.profile_params.max_velocity = 1;
+ goal->stilts.profile_params.max_acceleration = 0.5;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ // Give it a lot of time to get there.
+ RunForTime(chrono::seconds(8));
+
+ VerifyNearGoal();
+}
+
+// Makes sure that the voltage on a motor is properly pulled back after
+// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
+//
+// We are going to disable collision detection to make this easier to implement.
+TEST_F(SuperstructureTest, SaturationTest) {
+ // Zero it before we move.
+ WaitUntilZeroed();
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+ goal->wrist.unsafe_goal = constants::Values::kWristRange().upper;
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
+ goal->stilts.unsafe_goal = constants::Values::kStiltsRange().upper;
+
+ ASSERT_TRUE(goal.Send());
+ }
+ RunForTime(chrono::seconds(8));
+ VerifyNearGoal();
+
+ // Try a low acceleration move with a high max velocity and verify the
+ // acceleration is capped like expected.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+ goal->elevator.profile_params.max_velocity = 20.0;
+ goal->elevator.profile_params.max_acceleration = 0.1;
+
+ goal->wrist.unsafe_goal = constants::Values::kWristRange().upper;
+ goal->wrist.profile_params.max_velocity = 20.0;
+ goal->wrist.profile_params.max_acceleration = 0.1;
+
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
+ goal->intake.profile_params.max_velocity = 20.0;
+ goal->intake.profile_params.max_acceleration = 0.1;
+
+ goal->stilts.unsafe_goal = constants::Values::kStiltsRange().lower;
+ goal->stilts.profile_params.max_velocity = 20.0;
+ goal->stilts.profile_params.max_acceleration = 0.1;
+
+ ASSERT_TRUE(goal.Send());
+ }
+ set_peak_elevator_velocity(23.0);
+ set_peak_elevator_acceleration(0.2);
+ set_peak_wrist_velocity(23.0);
+ set_peak_wrist_acceleration(0.2);
+ set_peak_intake_velocity(23.0);
+ set_peak_intake_acceleration(0.2);
+ set_peak_stilts_velocity(23.0);
+ set_peak_stilts_acceleration(0.2);
+
+ RunForTime(chrono::seconds(8));
+ VerifyNearGoal();
+
+ // Now do a high acceleration move with a low velocity limit.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().lower;
+ goal->elevator.profile_params.max_velocity = 0.1;
+ goal->elevator.profile_params.max_acceleration = 10.0;
+
+ goal->wrist.unsafe_goal = constants::Values::kWristRange().lower;
+ goal->wrist.profile_params.max_velocity = 0.1;
+ goal->wrist.profile_params.max_acceleration = 10.0;
+
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
+ goal->intake.profile_params.max_velocity = 0.1;
+ goal->intake.profile_params.max_acceleration = 10.0;
+
+ goal->stilts.unsafe_goal = constants::Values::kStiltsRange().lower;
+ goal->stilts.profile_params.max_velocity = 0.1;
+ goal->stilts.profile_params.max_acceleration = 10.0;
+ }
+ set_peak_elevator_velocity(0.2);
+ set_peak_elevator_acceleration(11.0);
+ set_peak_wrist_velocity(0.2);
+ set_peak_wrist_acceleration(11.0);
+ set_peak_intake_velocity(0.2);
+ set_peak_intake_acceleration(11.0);
+ set_peak_stilts_velocity(0.2);
+ set_peak_stilts_acceleration(11.0);
+
+ VerifyNearGoal();
+}
+
+// Tests if the robot zeroes properly... maybe redundant?
+TEST_F(SuperstructureTest, ZeroTest) {
+ superstructure_plant_.InitializeElevatorPosition(1.4);
+ superstructure_plant_.InitializeWristPosition(1.0);
+ superstructure_plant_.InitializeIntakePosition(1.1);
+ superstructure_plant_.InitializeStiltsPosition(0.1);
+
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+
+ goal->elevator.unsafe_goal = 1.4;
+ goal->elevator.profile_params.max_velocity = 1;
+ goal->elevator.profile_params.max_acceleration = 0.5;
+
+ goal->wrist.unsafe_goal = 1.0;
+ goal->wrist.profile_params.max_velocity = 1;
+ goal->wrist.profile_params.max_acceleration = 0.5;
+
+ goal->intake.unsafe_goal = 1.1;
+ goal->intake.profile_params.max_velocity = 1;
+ goal->intake.profile_params.max_acceleration = 0.5;
+
+ goal->stilts.unsafe_goal = 0.1;
+ goal->stilts.profile_params.max_velocity = 1;
+ goal->stilts.profile_params.max_acceleration = 0.5;
+
+ ASSERT_TRUE(goal.Send());
+ }
+ WaitUntilZeroed();
+ VerifyNearGoal();
+}
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(SuperstructureTest, ZeroNoGoal) {
+ WaitUntilZeroed();
+ RunForTime(chrono::seconds(2));
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.elevator().state());
+
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.wrist().state());
+
+ EXPECT_EQ(AbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.intake().state());
+
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.stilts().state());
+}
+
+// Move wrist front to back and see if we collide
+TEST_F(SuperstructureTest, CollisionTest) {
+ // Set a reasonable goal.
+ superstructure_plant_.InitializeElevatorPosition(
+ constants::Values::kElevatorRange().lower);
+ // 60 degrees forwards
+ superstructure_plant_.InitializeWristPosition(M_PI / 3.0);
+ superstructure_plant_.InitializeIntakePosition(
+ CollisionAvoidance::kIntakeOutAngle + CollisionAvoidance::kEpsIntake);
+ superstructure_plant_.InitializeStiltsPosition(0.1);
+
+ WaitUntilZeroed();
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().lower;
+ goal->wrist.unsafe_goal = -M_PI / 3.0;
+ goal->intake.unsafe_goal =
+ CollisionAvoidance::kIntakeInAngle - CollisionAvoidance::kEpsIntake;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ // Give it a lot of time to get there.
+ RunForTime(chrono::seconds(20), true, true);
+
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 7bb32b2..ed57a24 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -108,27 +108,27 @@
// TODO(sabina): get accurate angle.
if (data.IsPressed(kIntakeExtend)) {
- new_superstructure_goal->intake.joint_angle = 0.5;
+ new_superstructure_goal->intake.unsafe_goal = 0.5;
} else {
- new_superstructure_goal->intake.joint_angle = 0.0;
+ new_superstructure_goal->intake.unsafe_goal = 0.0;
}
if (data.IsPressed(kIntake)) {
new_superstructure_goal->suction.bottom = true;
if (superstructure_queue.status->has_piece == false) {
- new_superstructure_goal->intake.roller_voltage = 12.0;
+ new_superstructure_goal->roller_voltage = 12.0;
} else {
- new_superstructure_goal->intake.roller_voltage = 0.0;
+ new_superstructure_goal->roller_voltage = 0.0;
}
} else if (data.IsPressed(kSpit)) {
new_superstructure_goal->suction.bottom = false;
if (superstructure_queue.status->has_piece == false) {
- new_superstructure_goal->intake.roller_voltage = 12.0;
+ new_superstructure_goal->roller_voltage = 12.0;
} else {
- new_superstructure_goal->intake.roller_voltage = 0.0;
+ new_superstructure_goal->roller_voltage = 0.0;
}
} else {
- new_superstructure_goal->intake.roller_voltage = 0.0;
+ new_superstructure_goal->roller_voltage = 0.0;
}
// TODO(sabina): decide if we should really have disk suction as its own
@@ -147,9 +147,9 @@
// TODO(sabina): max height please?
if (data.IsPressed(kDeployStilt)) {
- new_superstructure_goal->stilts.height = 0;
+ new_superstructure_goal->stilts.unsafe_goal= 0;
} else if (data.IsPressed(kRetractStilt)) {
- new_superstructure_goal->stilts.height = 0;
+ new_superstructure_goal->stilts.unsafe_goal = 0;
} else {
}
@@ -157,8 +157,8 @@
wrist_angle_ = -wrist_angle_;
}
- new_superstructure_goal->elevator.height = elevator_height_;
- new_superstructure_goal->wrist.angle = wrist_angle_;
+ new_superstructure_goal->elevator.unsafe_goal = elevator_height_;
+ new_superstructure_goal->wrist.unsafe_goal = wrist_angle_;
LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
if (!new_superstructure_goal.Send()) {
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 428e723..a6aa098 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -111,10 +111,10 @@
Values::kMaxIntakeEncoderPulsesPerSecond());
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
"fast encoders are too fast");
-
constexpr double kMaxMediumEncoderPulsesPerSecond =
max(Values::kMaxElevatorEncoderPulsesPerSecond(),
Values::kMaxWristEncoderPulsesPerSecond());
+
static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
"medium encoders are too fast");