Test overreaching arm is normalized

huskyroboticsteam · Sep 29, 2023 · 5b6bf0b · 5b6bf0b
1 parent d7b14ce
commit 5b6bf0b
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp
@@ -14,11 +14,12 @@ TEST_CASE("Test Planar Arm Controller", "[control][planararmcontroller]") {
 }
 
 TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") {
-	navtypes::Vectord<2> segLens({2.0, 3.0});
+	navtypes::Vectord<2> segLens({4.0, 6.0});
 	navtypes::Vectord<2> minAngles({-M_PI, -M_PI});
 	navtypes::Vectord<2> maxAngles({M_PI, M_PI});
 	kinematics::PlanarArmKinematics<2> kin_obj(segLens, minAngles, maxAngles, 0.0, 0);
 
 	PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR);
-	foo.set_setpoint({5.0, 0.0});
+	foo.set_setpoint({10.0, 0.0});
+	REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 1.0);
 }