From 4b9c8d231f949275a9d83ce26d603d69fb439661 Mon Sep 17 00:00:00 2001 From: acezxn Date: Thu, 16 Mar 2023 16:15:41 +0800 Subject: [PATCH] updated ramsete controller --- include/algorithms/path_following/pursuit.h | 1 + .../path_following/ramsete/ramsete.h | 6 +-- include/subsystems/chassis/chassis.h | 2 + include/utilities/coordinates.h | 2 +- src/algorithms/path_following/pursuit.cpp | 4 ++ .../path_following/ramsete/ramsete.cpp | 39 ++++++++++++++++--- src/main.cpp | 20 +--------- src/subsystems/chassis/chassis.cpp | 27 ++++++++++++- 8 files changed, 72 insertions(+), 29 deletions(-) diff --git a/include/algorithms/path_following/pursuit.h b/include/algorithms/path_following/pursuit.h index eb62d7d..4ff6e4a 100644 --- a/include/algorithms/path_following/pursuit.h +++ b/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 input_path); + virtual void set_waypoint(std::vector input_waypoint); virtual ChassisVelocityPair step(RobotPosition position, bool reverse = false); }; \ No newline at end of file diff --git a/include/algorithms/path_following/ramsete/ramsete.h b/include/algorithms/path_following/ramsete/ramsete.h index dfbb9ee..89b6334 100644 --- a/include/algorithms/path_following/ramsete/ramsete.h +++ b/include/algorithms/path_following/ramsete/ramsete.h @@ -7,15 +7,15 @@ class Ramsete : public Pursuit { Ramsete(float max_velocity = 300); Ramsete(std::vector input_path, float max_velocity = 300); bool is_arrived() override; - void set_path(std::vector input_path); + void set_waypoint(std::vector 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 path; int closest(RobotPosition position); diff --git a/include/subsystems/chassis/chassis.h b/include/subsystems/chassis/chassis.h index f18d0f0..7afb20f 100644 --- a/include/subsystems/chassis/chassis.h +++ b/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 path, bool reverse = false); + void followPath(std::vector path, bool reverse = false); // position sensing functions float getLeftPosition(); diff --git a/include/utilities/coordinates.h b/include/utilities/coordinates.h index b68d46a..dee3a3f 100644 --- a/include/utilities/coordinates.h +++ b/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(); }; \ No newline at end of file diff --git a/src/algorithms/path_following/pursuit.cpp b/src/algorithms/path_following/pursuit.cpp index 116200b..5bba96a 100644 --- a/src/algorithms/path_following/pursuit.cpp +++ b/src/algorithms/path_following/pursuit.cpp @@ -9,6 +9,10 @@ bool Pursuit::is_arrived() { } void Pursuit::set_path(std::vector input_path) { +} + +void Pursuit::set_waypoint(std::vector input_waypoint) { + } ChassisVelocityPair Pursuit::step(RobotPosition position, bool reverse) { ChassisVelocityPair pair = ChassisVelocityPair(); diff --git a/src/algorithms/path_following/ramsete/ramsete.cpp b/src/algorithms/path_following/ramsete/ramsete.cpp index f762524..0132132 100644 --- a/src/algorithms/path_following/ramsete/ramsete.cpp +++ b/src/algorithms/path_following/ramsete/ramsete.cpp @@ -23,6 +23,29 @@ Ramsete::Ramsete(std::vector 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 input_path) { - this->path = input_path; +void Ramsete::set_waypoint(std::vector 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; } \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 78e193f..5a8625d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -176,29 +176,11 @@ void autonomous() void opcontrol() { printf("[Main]: Opcontrol started\n"); - // std::vector 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 path = catmull.get_processed_coordinates(); std::shared_ptr odometry = std::make_shared(&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)); diff --git a/src/subsystems/chassis/chassis.cpp b/src/subsystems/chassis/chassis.cpp index 578a605..d299a85 100644 --- a/src/subsystems/chassis/chassis.cpp +++ b/src/subsystems/chassis/chassis.cpp @@ -42,7 +42,11 @@ Chassis::Chassis(struct Core* core, std::shared_ptr 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 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 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 *