Added velocity limit checking.
Change-Id: Id58dd453e0577b0f9aab11ff7f7812ad3e5b1c02
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 79dd728..7c836cc 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -268,6 +268,19 @@
EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
EXPECT_GE(peak_wrist_acceleration_, wrist_acceleration);
EXPECT_LE(-peak_wrist_acceleration_, wrist_acceleration);
+
+ EXPECT_GE(peak_shoulder_velocity_,
+ superstructure_plant_.shoulder_angular_velocity());
+ EXPECT_LE(-peak_shoulder_velocity_,
+ superstructure_plant_.shoulder_angular_velocity());
+ EXPECT_GE(peak_intake_velocity_,
+ superstructure_plant_.intake_angular_velocity());
+ EXPECT_LE(-peak_intake_velocity_,
+ superstructure_plant_.intake_angular_velocity());
+ EXPECT_GE(peak_wrist_velocity_,
+ superstructure_plant_.wrist_angular_velocity());
+ EXPECT_LE(-peak_wrist_velocity_,
+ superstructure_plant_.wrist_angular_velocity());
}
}
@@ -282,6 +295,11 @@
void set_peak_wrist_acceleration(double value) {
peak_wrist_acceleration_ = value;
}
+ void set_peak_intake_velocity(double value) { peak_intake_velocity_ = value; }
+ void set_peak_shoulder_velocity(double value) {
+ peak_shoulder_velocity_ = value;
+ }
+ void set_peak_wrist_velocity(double value) { peak_wrist_velocity_ = value; }
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointed to
@@ -297,6 +315,10 @@
double peak_intake_acceleration_ = 1e10;
double peak_shoulder_acceleration_ = 1e10;
double peak_wrist_acceleration_ = 1e10;
+ // The velocity limits to check for while moving for the 3 axes.
+ double peak_intake_velocity_ = 1e10;
+ double peak_shoulder_velocity_ = 1e10;
+ double peak_wrist_velocity_ = 1e10;
};
// Tests that the superstructure does nothing when the goal is zero.