add support for custom motion algs

5150VEX · Oct 10, 2023 · fac73f8 · fac73f8
1 parent 00523c2
commit fac73f8
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 9 deletions.
diff --git a/include/lemlib/chassis/chassis.hpp b/include/lemlib/chassis/chassis.hpp
@@ -160,6 +160,13 @@ class Chassis {
         void moveTo(float x, float y, float theta, int timeout, bool forwards = true, float chasePower = 0,
                     float lead = 0.6, int maxSpeed = 127);
 
+        /**
+         * @brief Move the chassis using a custom motion algorithm
+         *
+         * @param movement shared pointer to the custom movement
+         */
+        void moveCustom(std::unique_ptr<Movement> movement);
+
         /**
          * @brief Move the chassis along a path
          *
@@ -214,7 +221,7 @@ class Chassis {
         float prevDist = 0; // the previous distance travelled by the movement
 
         Odometry odom;
-        std::shared_ptr<Movement> movement;
+        std::unique_ptr<Movement> movement;
         std::unique_ptr<pros::Task> task;
 
         ChassisController_t lateralSettings;

diff --git a/src/lemlib/chassis/chassis.cpp b/src/lemlib/chassis/chassis.cpp
@@ -125,13 +125,13 @@ void Chassis::waitUntilDone() {
  */
 void Chassis::turnToPose(float x, float y, int timeout, bool reversed, int maxSpeed) {
     // if a movement is already running, wait until it is done
-    if (movement == nullptr) waitUntilDone();
+    if (movement != nullptr) waitUntilDone();
     // set up the PID
     FAPID angularPID(0, 0, angularSettings.kP, 0, angularSettings.kD, "angularPID");
     angularPID.setExit(angularSettings.largeError, angularSettings.smallError, angularSettings.largeErrorTimeout,
                        angularSettings.smallErrorTimeout, timeout);
     // create the movement
-    movement = std::make_shared<Turn>(angularPID, Pose(x, y), reversed, maxSpeed);
+    movement = std::make_unique<Turn>(angularPID, Pose(x, y), reversed, maxSpeed);
 }
 
 /**
@@ -148,15 +148,15 @@ void Chassis::turnToPose(float x, float y, int timeout, bool reversed, int maxSp
  */
 void Chassis::turnToHeading(float heading, int timeout, int maxSpeed) {
     // if a movement is already running, wait until it is done
-    if (movement == nullptr) waitUntilDone();
+    if (movement != nullptr) waitUntilDone();
     // convert heading to radians and standard form
     float newHeading = M_PI_2 - degToRad(heading);
     // set up the PID
     FAPID angularPID(0, 0, angularSettings.kP, 0, angularSettings.kD, "angularPID");
     angularPID.setExit(angularSettings.largeError, angularSettings.smallError, angularSettings.largeErrorTimeout,
                        angularSettings.smallErrorTimeout, timeout);
     // create the movement
-    movement = std::make_shared<Turn>(angularPID, newHeading, maxSpeed);
+    movement = std::make_unique<Turn>(angularPID, newHeading, maxSpeed);
 }
 
 /**
@@ -174,7 +174,7 @@ void Chassis::turnToHeading(float heading, int timeout, int maxSpeed) {
 void Chassis::moveTo(float x, float y, float theta, int timeout, bool forwards, float chasePower, float lead,
                      int maxSpeed) {
     // if a movement is already running, wait until it is done
-    if (movement == nullptr) waitUntilDone();
+    if (movement != nullptr) waitUntilDone();
     // convert target theta to radians and standard form
     Pose target = Pose(x, y, M_PI_2 - degToRad(theta));
     // set up PIDs
@@ -185,7 +185,17 @@ void Chassis::moveTo(float x, float y, float theta, int timeout, bool forwards,
     // if chasePower is 0, is the value defined in the drivetrain struct
     if (chasePower == 0) chasePower = drivetrain.chasePower;
     // create the movement
-    movement = std::make_shared<Boomerang>(linearPID, angularPID, target, forwards, chasePower, lead, maxSpeed);
+    movement = std::make_unique<Boomerang>(linearPID, angularPID, target, forwards, chasePower, lead, maxSpeed);
+}
+
+/**
+ * Move the robot with a custom motion algorithm
+ */
+void Chassis::moveCustom(std::unique_ptr<Movement> movement) {
+    // if a movement is already running, wait until it is done
+    if (movement != nullptr) waitUntilDone();
+    // create the movement
+    this->movement = std::move(movement);
 }
 
 /**
@@ -196,9 +206,9 @@ void Chassis::moveTo(float x, float y, float theta, int timeout, bool forwards,
  */
 void Chassis::follow(const asset& path, float lookahead, int timeout, bool forwards, int maxSpeed) {
     // if a movement is already running, wait until it is done
-    if (movement == nullptr) waitUntilDone();
+    if (movement != nullptr) waitUntilDone();
     // create the movement
-    movement = std::make_shared<PurePursuit>(drivetrain.trackWidth, path, lookahead, timeout, forwards, maxSpeed);
+    movement = std::make_unique<PurePursuit>(drivetrain.trackWidth, path, lookahead, timeout, forwards, maxSpeed);
 }
 
 /**