Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add specifying angular direction to turns #54

Merged
merged 3 commits into from
Feb 14, 2024
Merged

Add specifying angular direction to turns #54

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9e12bc3
add specifying angular direction to turns
AndrewLuGit Feb 14, 2024
770f506
Committing clang-format changes
github-actions[bot] Feb 14, 2024
ad3dab5
Fix a small little bug
Rocky14683 Feb 14, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 17 additions & 9 deletions include/VOSS/chassis/AbstractChassis.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ class AbstractChassis {
double exitTime);

void turn_task(controller_ptr controller, double max, voss::Flags flags,
double exitTime);
voss::AngularDirection direction, double exitTime);

public:
AbstractChassis(controller_ptr default_controller);
Expand All @@ -44,15 +44,23 @@ class AbstractChassis {
voss::Flags flags = voss::Flags::NONE, double exitTime = 22500);

void turn(double target, controller_ptr controller, double max = 100.0,
voss::Flags flags = voss::Flags::NONE, double exitTime = 22500);
voss::Flags flags = voss::Flags::NONE,
voss::AngularDirection direction = voss::AngularDirection::AUTO,
double exitTime = 22500);
void turn(double target, double max = 100.0,
voss::Flags flags = voss::Flags::NONE, double exitTime = 22500);
void turn_to(Point target, controller_ptr controller, double max = 100.0,
voss::Flags flags = voss::Flags::NONE,
double exitTime = 22500);
void turn_to(Point target, double max = 100.0,
voss::Flags flags = voss::Flags::NONE,
double exitTime = 22500);
voss::Flags flags = voss::Flags::NONE,
voss::AngularDirection direction = voss::AngularDirection::AUTO,
double exitTime = 22500);
void
turn_to(Point target, controller_ptr controller, double max = 100.0,
voss::Flags flags = voss::Flags::NONE,
voss::AngularDirection direction = voss::AngularDirection::AUTO,
double exitTime = 22500);
void
turn_to(Point target, double max = 100.0,
voss::Flags flags = voss::Flags::NONE,
voss::AngularDirection direction = voss::AngularDirection::AUTO,
double exitTime = 22500);
};

} // namespace voss::chassis
6 changes: 4 additions & 2 deletions include/VOSS/controller/AbstractController.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#include "VOSS/chassis/ChassisCommand.hpp"
#include "VOSS/localizer/AbstractLocalizer.hpp"
#include "VOSS/utils/flags.hpp"

namespace voss::controller {

Expand All @@ -16,8 +17,9 @@ class AbstractController {
AbstractController(std::shared_ptr<localizer::AbstractLocalizer> l);

virtual chassis::ChassisCommand get_command(bool reverse, bool thru) = 0;
virtual chassis::ChassisCommand get_angular_command(bool reverse,
bool thru) = 0;
virtual chassis::ChassisCommand
get_angular_command(bool reverse, bool thru,
voss::AngularDirection direction) = 0;

virtual void reset() = 0;

Expand Down
5 changes: 3 additions & 2 deletions include/VOSS/controller/ArcPIDController.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,8 +26,9 @@ class ArcPIDController : public AbstractController {
double linear_pid(double error);

chassis::ChassisCommand get_command(bool reverse, bool thru) override;
chassis::ChassisCommand get_angular_command(bool reverse,
bool thru) override;
chassis::ChassisCommand
get_angular_command(bool reverse, bool thru,
voss::AngularDirection direction) override;

void reset() override;

Expand Down
6 changes: 4 additions & 2 deletions include/VOSS/controller/BoomerangController.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include "PIDController.hpp"
#include "VOSS/chassis/ChassisCommand.hpp"
#include "VOSS/localizer/AbstractLocalizer.hpp"
#include "VOSS/utils/flags.hpp"

namespace voss::controller {

Expand All @@ -16,8 +17,9 @@ class BoomerangController : public AbstractController {
BoomerangController(std::shared_ptr<localizer::AbstractLocalizer> l);

chassis::ChassisCommand get_command(bool reverse, bool thru) override;
chassis::ChassisCommand get_angular_command(bool reverse,
bool thru) override;
chassis::ChassisCommand
get_angular_command(bool reverse, bool thru,
voss::AngularDirection direction) override;

void reset();

Expand Down
7 changes: 5 additions & 2 deletions include/VOSS/controller/PIDController.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#pragma once

#include "VOSS/controller/AbstractController.hpp"
#include "VOSS/utils/flags.hpp"

namespace voss::controller {

Expand All @@ -19,6 +20,7 @@ class PIDController : public AbstractController {
double close_2;
int counter;
double prev_angle;
bool turn_overshoot;

double prev_lin_err, total_lin_err, prev_ang_err, total_ang_err;

Expand All @@ -29,8 +31,9 @@ class PIDController : public AbstractController {
double angular_pid(double error);

chassis::ChassisCommand get_command(bool reverse, bool thru) override;
chassis::ChassisCommand get_angular_command(bool reverse,
bool thru) override;
chassis::ChassisCommand
get_angular_command(bool reverse, bool thru,
voss::AngularDirection direction) override;

void reset() override;

Expand Down
7 changes: 5 additions & 2 deletions include/VOSS/controller/SwingController.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#include "PIDController.hpp"
#include "VOSS/controller/AbstractController.hpp"
#include "VOSS/utils/flags.hpp"

namespace voss::controller {
class SwingController : public AbstractController {
Expand All @@ -14,6 +15,7 @@ class SwingController : public AbstractController {
double close;
double close_2;
int counter;
bool turn_overshoot;

double prev_ang_err, total_ang_err;
double prev_ang_speed;
Expand All @@ -22,8 +24,9 @@ class SwingController : public AbstractController {
SwingController(std::shared_ptr<localizer::AbstractLocalizer> l);

chassis::ChassisCommand get_command(bool reverse, bool thru) override;
chassis::ChassisCommand get_angular_command(bool reverse,
bool thru) override;
chassis::ChassisCommand
get_angular_command(bool reverse, bool thru,
voss::AngularDirection direction) override;

double angular_pid(double error);

Expand Down
22 changes: 18 additions & 4 deletions include/VOSS/utils/angle.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,13 +1,27 @@
#pragma once

#include "VOSS/utils/flags.hpp"
#include <cmath>

namespace voss {

double to_radians(double degrees);
inline double to_radians(double degrees) {
return degrees * M_PI / 180;
}

double to_degrees(double radians);
inline double to_degrees(double radians) {
return radians * 180 * M_1_PI;
}

double norm(double radians);
inline double norm(double radians) {
radians = fmod(radians, 2 * M_PI);
if (radians < 0)
radians += 2 * M_PI;
return radians;
}

double norm_delta(double radians);
inline double norm_delta(double radians) {
return std::remainder(radians, 2 * M_PI);
}

} // namespace voss
2 changes: 2 additions & 0 deletions include/VOSS/utils/flags.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,4 +21,6 @@ auto inline operator&(Flags flag1, Flags flag2) {
return static_cast<bool>(static_cast<uint8_t>(flag1) &
static_cast<uint8_t>(flag2));
}

enum class AngularDirection { AUTO, COUNTERCLOCKWISE, CLOCKWISE };
} // namespace voss
32 changes: 20 additions & 12 deletions src/VOSS/chassis/AbstractChassis.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,14 +41,16 @@ void AbstractChassis::move_task(controller_ptr controller, double max,
}

void AbstractChassis::turn_task(controller_ptr controller, double max,
voss::Flags flags, double exitTime) {
voss::Flags flags,
voss::AngularDirection direction,
double exitTime) {
int t = 0;
pros::Task running_t([&, controller]() {
controller->reset();
while (!this->execute(
controller->get_angular_command(flags & voss::Flags::REVERSE,
flags & voss::Flags::THRU),
max)) {
while (!this->execute(controller->get_angular_command(
flags & voss::Flags::REVERSE,
flags & voss::Flags::THRU, direction),
max)) {
if (pros::competition::is_disabled()) {
return;
}
Expand Down Expand Up @@ -95,33 +97,39 @@ void AbstractChassis::move(Pose target, controller_ptr controller, double max,
}

void AbstractChassis::turn(double target, double max, voss::Flags flags,
double exitTime) {
this->turn(target, this->default_controller, max, flags, exitTime);
voss::AngularDirection direction, double exitTime) {
this->turn(target, this->default_controller, max, flags, direction,
exitTime);
}

void AbstractChassis::turn(double target, controller_ptr controller, double max,
voss::Flags flags, double exitTime) {
voss::Flags flags, voss::AngularDirection direction,
double exitTime) {
// this->m.take();

controller->set_target({0, 0, 0}, false);
controller->set_angular_target(target, flags & voss::Flags::RELATIVE);

this->turn_task(controller, max, flags, exitTime);
this->turn_task(controller, max, flags, direction, exitTime);
}

void AbstractChassis::turn_to(Point target, double max, voss::Flags flags,
voss::AngularDirection direction,
double exitTime) {
this->turn_to(target, this->default_controller, max, flags, exitTime);
this->turn_to(target, this->default_controller, max, flags, direction,
exitTime);
}

void AbstractChassis::turn_to(Point target, controller_ptr controller,
double max, voss::Flags flags, double exitTime) {
double max, voss::Flags flags,
voss::AngularDirection direction,
double exitTime) {
// this->m.take();

controller->set_target({target.x, target.y, 361},
flags & voss::Flags::RELATIVE);

this->turn_task(controller, max, flags, exitTime);
this->turn_task(controller, max, flags, direction, exitTime);
}

} // namespace voss::chassis
5 changes: 3 additions & 2 deletions src/VOSS/controller/ArcPIDController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,8 +88,9 @@ chassis::ChassisCommand ArcPIDController::get_command(bool reverse, bool thru) {
return chassis::ChassisCommand{chassis::Voltages{left_speed, right_speed}};
}

chassis::ChassisCommand ArcPIDController::get_angular_command(bool reverse,
bool thru) {
chassis::ChassisCommand
ArcPIDController::get_angular_command(bool reverse, bool thru,
voss::AngularDirection direction) {
return chassis::ChassisCommand{chassis::Stop{}};
}

Expand Down
7 changes: 4 additions & 3 deletions src/VOSS/controller/BoomerangController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,11 +32,12 @@ chassis::ChassisCommand BoomerangController::get_command(bool reverse,
return child->get_command(reverse, thru);
}

chassis::ChassisCommand BoomerangController::get_angular_command(bool reverse,
bool thru) {
chassis::ChassisCommand
BoomerangController::get_angular_command(bool reverse, bool thru,
voss::AngularDirection direction) {
child->set_target(target, false);
child->set_angular_target(angular_target, false);
return child->get_angular_command(reverse, thru);
return child->get_angular_command(reverse, thru, direction);
}

void BoomerangController::reset() {
Expand Down
20 changes: 18 additions & 2 deletions src/VOSS/controller/PIDController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,8 +104,9 @@ chassis::ChassisCommand PIDController::get_command(bool reverse, bool thru) {
chassis::Voltages{lin_speed - ang_speed, lin_speed + ang_speed}};
}

chassis::ChassisCommand PIDController::get_angular_command(bool reverse,
bool thru) {
chassis::ChassisCommand
PIDController::get_angular_command(bool reverse, bool thru,
voss::AngularDirection direction) {
counter += 10;
double current_angle = this->l->get_orientation_rad();
double target_angle = 0;
Expand All @@ -121,6 +122,20 @@ chassis::ChassisCommand PIDController::get_angular_command(bool reverse,

angular_error = voss::norm_delta(angular_error);

if (fabs(angular_error) < voss::to_radians(5)) {
turn_overshoot = true;
}

if (!turn_overshoot) {
if (direction == voss::AngularDirection::COUNTERCLOCKWISE &&
angular_error < 0) {
angular_error += 2 * M_PI;
} else if (direction == voss::AngularDirection::CLOCKWISE &&
angular_error > 0) {
angular_error -= 2 * M_PI;
}
}

if (fabs(angular_error) < angular_exit_error) {
close += 10;
} else {
Expand Down Expand Up @@ -173,6 +188,7 @@ void PIDController::reset() {
this->total_ang_err = 0;
this->can_reverse = false;
this->counter = 0;
this->turn_overshoot = false;
}

} // namespace voss::controller
22 changes: 19 additions & 3 deletions src/VOSS/controller/SwingController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,9 @@ chassis::ChassisCommand SwingController::get_command(bool reverse, bool thru) {
return chassis::ChassisCommand{chassis::Stop{}};
}

chassis::ChassisCommand SwingController::get_angular_command(bool reverse,
bool thru) {
chassis::ChassisCommand
SwingController::get_angular_command(bool reverse, bool thru,
voss::AngularDirection direction) {
counter += 10;
double current_angle = this->l->get_orientation_rad();
double target_angle = 0;
Expand All @@ -27,6 +28,20 @@ chassis::ChassisCommand SwingController::get_angular_command(bool reverse,

angular_error = voss::norm_delta(angular_error);

if (fabs(angular_error) < voss::to_radians(5)) {
turn_overshoot = true;
}

if (!turn_overshoot) {
if (direction == voss::AngularDirection::COUNTERCLOCKWISE &&
angular_error < 0) {
angular_error += 2 * M_PI;
} else if (direction == voss::AngularDirection::CLOCKWISE &&
angular_error > 0) {
angular_error -= 2 * M_PI;
}
}

if (fabs(angular_error) < angular_exit_error) {
close += 10;
} else {
Expand All @@ -51,7 +66,7 @@ chassis::ChassisCommand SwingController::get_angular_command(bool reverse,
chassis::ChassisCommand command;
if (!((ang_speed >= 0.0) ^ (this->prev_ang_speed < 0.0)) &&
this->prev_ang_speed != 0) {
can_reverse = true;
can_reverse = !can_reverse;
}

if (!this->can_reverse) {
Expand Down Expand Up @@ -94,5 +109,6 @@ void SwingController::reset() {
this->total_ang_err = 0;
this->counter = 0;
this->can_reverse = false;
this->turn_overshoot = false;
}
}; // namespace voss::controller
Loading
Loading