updated ramsete controller

include/algorithms/path_following/pursuit.h

@@ -5,5 +5,6 @@ class Pursuit {
         Pursuit(float max_velocity);
         virtual bool is_arrived();
         virtual void set_path(std::vector<Coordinates> input_path);
+        virtual void set_waypoint(std::vector<Waypoint> input_waypoint);
         virtual ChassisVelocityPair step(RobotPosition position, bool reverse = false);
 };

include/algorithms/path_following/ramsete/ramsete.h

@@ -7,15 +7,15 @@ class Ramsete : public Pursuit {
         Ramsete(float max_velocity = 300);
         Ramsete(std::vector<Waypoint> input_path, float max_velocity = 300);
         bool is_arrived() override;
-        void set_path(std::vector<Waypoint> input_path);
+        void set_waypoint(std::vector<Waypoint> input_waypoint) override;
         void set_constants(float b, float zeta);
         ChassisVelocityPair step(RobotPosition position, bool reverse = false) override;
     private:
         bool arrived;
         float look_ahead_radius;
         float max_velocity;
-        float b = 0.5;
-        float zeta = 1;
+        float b = 0.1;
+        float zeta = 0.01;
 
         std::vector<Waypoint> path;
         int closest(RobotPosition position);

include/subsystems/chassis/chassis.h

@@ -1,5 +1,6 @@
 #include "algorithms/path_following/pursuit.h"
 #include "algorithms/path_following/pure_pursuit/pure_pursuit.h"
+#include "algorithms/path_following/ramsete/ramsete.h"
 
 using namespace okapi;
 
@@ -34,6 +35,7 @@ class Chassis {
         void simpleMoveToPointBackwards(float x, float y, float max_voltage = 5000, bool turn_on_intake = false);
 
         void followPath(std::vector<Coordinates> path, bool reverse = false);
+        void followPath(std::vector<Waypoint> path, bool reverse = false);
 
         // position sensing functions
         float getLeftPosition();

include/utilities/coordinates.h

@@ -32,7 +32,7 @@ class Waypoint : public Coordinates {
 	float ang_vel;
 
 public:
-	Waypoint(float x_coordinates, float y_coordinates, float lin_vel, float direction = 0, float ang_vel = 0);
+	Waypoint(float x_coordinates, float y_coordinates, float direction = 0, float lin_vel = 0, float ang_vel = 0);
 	float get_linear_vel();
 	float get_ang_vel();
 };

src/algorithms/path_following/pursuit.cpp

@@ -9,6 +9,10 @@ bool Pursuit::is_arrived() {
 }
 void Pursuit::set_path(std::vector<Coordinates> input_path) {
 
+}
+
+void Pursuit::set_waypoint(std::vector<Waypoint> input_waypoint) {
+
 }
 ChassisVelocityPair Pursuit::step(RobotPosition position, bool reverse) {
     ChassisVelocityPair pair = ChassisVelocityPair();

src/algorithms/path_following/ramsete/ramsete.cpp

@@ -23,6 +23,29 @@ Ramsete::Ramsete(std::vector<Waypoint> input_path, float max_velocity) : Pursuit
     this->arrived = false;
 }
 
+/**
+ * @brief Find the index number of the closest waypoint to the robot
+ * 
+ * @param position robot position
+ * @returns index 
+ */
+int Ramsete::closest(RobotPosition position) {
+    float x_dist = this->path[0].get_x() - position.x_pct;
+    float y_dist = this->path[0].get_y() - position.y_pct;
+    float min_distance = sqrt(x_dist * x_dist + y_dist * y_dist);
+    int min_idx = 0;
+    for (int i = 1; i < this->path.size(); i++) {
+        x_dist = this->path[i].get_x() - position.x_pct;
+        y_dist = this->path[i].get_y() - position.y_pct;
+        float dist = sqrt(x_dist * x_dist + y_dist * y_dist);
+        if (dist <= min_distance) {
+            min_distance = dist;
+            min_idx = i;
+        }
+    }
+    return min_idx;
+}
+
 /**
  * @brief Check if the robot has reached the destination
  * 
@@ -36,10 +59,10 @@ bool Ramsete::is_arrived() {
 /**
  * @brief Set the path to follow
  * 
- * @param input_path path to follow
+ * @param input_waypoint path to follow
  */
-void Ramsete::set_path(std::vector<Waypoint> input_path) {
-    this->path = input_path;
+void Ramsete::set_waypoint(std::vector<Waypoint> input_waypoint) {
+    this->path = input_waypoint;
     this->arrived = false;
 }
 
@@ -70,7 +93,7 @@ Waypoint Ramsete::absToLocal(RobotPosition position, Waypoint point) {
     float newX = yDist*cos(position.theta*M_PI/180.0) - xDist*sin(position.theta*M_PI/180.0);
     float newY = yDist*sin(position.theta*M_PI/180.0) + xDist*cos(position.theta*M_PI/180.0);
 
-    return Waypoint(newX, newY, position.theta, point.get_linear_vel(), point.get_ang_vel());
+    return Waypoint(newX, newY, point.get_direction(), point.get_linear_vel(), point.get_ang_vel());
 }
 
 /**
@@ -97,14 +120,20 @@ ChassisVelocityPair Ramsete::step(RobotPosition position, bool reverse) {
         float desired_angvel = look_ahead.get_ang_vel();
         float k = 2 * this->zeta * sqrt(desired_angvel * desired_angvel + this->b * desired_linvel * desired_linvel);
 
-        float target_linvel = desired_linvel * cos(e_theta) + k * e_y;
+        float target_linvel = desired_linvel * cos(e_theta * pi / 180) + k * e_y;
         float target_angvel;
 
         if (e_theta != 0) {
             target_angvel = desired_angvel + k * e_theta + (this->b*desired_linvel*sin(e_theta)* e_x) / e_theta;
         }
+
         velocity_pair.left_v = target_linvel + target_angvel;
         velocity_pair.right_v = target_linvel - target_angvel;
+    } else {
+        velocity_pair.left_v = 0;
+        velocity_pair.right_v = 0;
+
+        this->arrived = true;
     }
     return velocity_pair;
 }

src/main.cpp

@@ -176,29 +176,11 @@ void autonomous()
 void opcontrol()
 {
 	printf("[Main]: Opcontrol started\n");
-	// std::vector<Coordinates> ctlpoint = {
-	// 	Coordinates(10.030533885227815, 17.933841152045563, 0),
-	// 	Coordinates(17.81679342721255, 17.933841152045563, 0),
-	// 	Coordinates(44.992365946296516, 28.010177029908156, 0),
-	// 	Coordinates(30.183205640953005, 42.81933733525167, 0),
-	// 	Coordinates(38.27480869438812, 53.81170374746541, 0),
-	// 	Coordinates(49.72519037377744, 44.95674191540434, 0),
-	// 	Coordinates(66.21373999209804, 50.6055968772364, 0),
-	// 	Coordinates(50.3358773966782, 66.17811596120586, 0),
-	// 	Coordinates(49.877862129502624, 82.36132206807609, 0),
-	// 	Coordinates(66.51908350354843, 66.63613122838144, 0),
-	// 	Coordinates(81.9389308317927, 81.75063504517534, 0),
-	// 	Coordinates(90.79389266385377, 91.52162741158754, 0),
-	// };
-
-	// CatmullRom catmull = CatmullRom(ctlpoint);
-	// std::vector<Coordinates> path = catmull.get_processed_coordinates();
 	std::shared_ptr<Odom> odometry = std::make_shared<Odom>(&core, OdomMode::MOTOR_IMU);
 	odometry->setState(17.81679342721255, 17.933841152045563, 0);
-	Chassis chassis = Chassis(&core, odometry);
+	Chassis chassis = Chassis(&core, odometry, PursuitMode::RAMSETE);
 	chassis.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
 
-	// chassis.followPath(path);
 	while (true)
 	{
 		chassis.cheezyDrive(core.controller->getAnalog(Configuration::Controls::FORWARD_AXIS), core.controller->getAnalog(Configuration::Controls::TURN_AXIS));

src/subsystems/chassis/chassis.cpp

@@ -42,7 +42,11 @@ Chassis::Chassis(struct Core* core, std::shared_ptr<Odom> odom, PursuitMode purs
     this->pursuit_mode = pursuit_mode;
     if (pursuit_mode == PursuitMode::PID_PURE_PURSUIT) {
         this->pursuit = new PurePursuit(600);
-    }  else {
+    }  
+    else if (pursuit_mode == PursuitMode::RAMSETE) {
+        this->pursuit = new Ramsete(600);
+    }
+    else {
         this->pursuit = new PurePursuit(600);
     }
     // signatures
@@ -418,6 +422,27 @@ void Chassis::followPath(std::vector<Coordinates> path, bool reverse) {
 
 }
 
+/**
+ * @brief Follow a path
+ * 
+ * @param path path
+ * @param reverse whether the robot should move backwards to pursue the path
+ * @param enable_disk_pursuit whether the robot should pursue nearby disks
+ */
+void Chassis::followPath(std::vector<Waypoint> path, bool reverse) {
+    this->pursuit->set_waypoint(path);
+    while (!this->pursuit->is_arrived()) {
+        RobotPosition position = this->odom->getState();
+        ChassisVelocityPair velocity_pair;
+        velocity_pair = this->pursuit->step(position, reverse);
+        this->moveVelocity(velocity_pair.left_v, velocity_pair.right_v);
+        pros::delay(10);
+    }
+    this->moveVelocity(0);
+
+}
+
+
 /**
  * @brief Get left track motor position reading
  *