Skip to content

Commit

Permalink
Make motion parameters consistent
Browse files Browse the repository at this point in the history
  • Loading branch information
SizzinSeal committed Nov 20, 2023
1 parent 475d619 commit f30ce21
Show file tree
Hide file tree
Showing 6 changed files with 24 additions and 24 deletions.
10 changes: 5 additions & 5 deletions include/lemlib/chassis/differential.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -240,14 +240,14 @@ class Differential : public Chassis {
* @param y y location
* @param theta theta (in degrees). Target angle
* @param timeout longest time the robot can spend moving
* @param forwards whether the robot should move forwards or backwards. true for forwards (default), false for
* backwards
* @param reversed whether the robot should move forwards or backwards. true for backwards, false for
* forwards (default)
* @param lead the lead parameter. Determines how curved the robot will move. 0.6 by default (0 < lead < 1)
* @param chasePower higher values make the robot move faster but causes more overshoot on turns. 0 makes it
* default to global value
* @param maxSpeed the maximum speed the robot can move at. 127 at default
*/
void moveTo(float x, float y, float theta, int timeout, bool forwards = true, float chasePower = 0,
void moveTo(float x, float y, float theta, int timeout, bool reversed = false, float chasePower = 0,
float lead = 0.6, int maxSpeed = 127);

/**
Expand All @@ -257,10 +257,10 @@ class Differential : public Chassis {
* @param lookahead the lookahead distance. Units in inches. Larger values will make the robot move faster but
* will follow the path less accurately
* @param timeout the maximum time the robot can spend moving
* @param forwards whether the robot should follow the path going forwards. true by default
* @param reversed whether the robot should follow the path in reverse. false by default
* @param maxSpeed the maximum speed the robot can move at
*/
void follow(const asset& path, float lookahead, int timeout, bool forwards = true, int maxSpeed = 127);
void follow(const asset& path, float lookahead, int timeout, bool reversed = false, int maxSpeed = 127);

/**
* @brief Control the robot during the driver control period using the tank drive control scheme. In this
Expand Down
6 changes: 3 additions & 3 deletions include/lemlib/movements/boomerang.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,13 +18,13 @@ class Boomerang : public Movement {
* @param linearPID the linear PID to use
* @param angularPID the angular PID to use
* @param target the target pose
* @param forwards whether the robot should move forwards or backwards. true for forwards, false for
* @param reversed whether the robot should move forwards or backwards. false for forwards, true for
* backwards
* @param lead the lead parameter. Determines how curved the robot will move. (0 < lead < 1)
* @param chasePower higher values make the robot move faster but causes more overshoot on turns
* @param maxSpeed the maximum speed the robot can move at
*/
Boomerang(FAPID linearPID, FAPID angularPID, Pose target, bool forwards, float chasePower, float lead,
Boomerang(FAPID linearPID, FAPID angularPID, Pose target, bool reversed, float chasePower, float lead,
int maxSpeed);

/**
Expand All @@ -50,7 +50,7 @@ class Boomerang : public Movement {
FAPID linearPID;
Pose target;
Pose prevPose = Pose(0, 0, 0);
bool forwards;
bool reversed;
float chasePower;
float lead;
int maxSpeed;
Expand Down
6 changes: 3 additions & 3 deletions include/lemlib/movements/purepursuit.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,10 +20,10 @@ class PurePursuit : public Movement {
* @param lookaheadDist the lookahead distance. Units in inches. Recommended value is 15, but can be changed if
* needed
* @param timeout the maximum time the robot can spend moving
* @param forwards whether the chassis should move forwards or backwards. True by default
* @param reversed whether the chassis should move forwards (false) or backwards (true)
* @param maxSpeed the maximum speed the robot can move at. 127 by default
*/
PurePursuit(float trackWidth, const asset& path, float lookaheadDist, int timeout, bool forwards, int maxSpeed);
PurePursuit(float trackWidth, const asset& path, float lookaheadDist, int timeout, bool reversed, int maxSpeed);

/**
* @brief Get the distance travelled during the movement
Expand Down Expand Up @@ -51,7 +51,7 @@ class PurePursuit : public Movement {
int startTime;
float lookaheadDist;
int timeout;
bool forwards;
bool reversed;
int maxSpeed;

int compState;
Expand Down
8 changes: 4 additions & 4 deletions src/lemlib/chassis/differential.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,7 @@ void Differential::turnToHeading(float heading, int timeout, int maxSpeed) {
* It also needs to decide what the chasePower should be. Usually this will be the value set in
* the drivetrain struct, but it can be overridden by the user if needed.
*/
void Differential::moveTo(float x, float y, float theta, int timeout, bool forwards, float chasePower, float lead,
void Differential::moveTo(float x, float y, float theta, int timeout, bool reversed, float chasePower, float lead,
int maxSpeed) {
// if a movement is already running, wait until it is done
if (movement != nullptr) waitUntilDone();
Expand All @@ -157,7 +157,7 @@ void Differential::moveTo(float x, float y, float theta, int timeout, bool forwa
// if chasePower is 0, is the value defined in the drivetrain struct
if (chasePower == 0) chasePower = drivetrain.chasePower;
// create the movement
movement = std::make_unique<Boomerang>(linearPID, angularPID, target, forwards, chasePower, lead, maxSpeed);
movement = std::make_unique<Boomerang>(linearPID, angularPID, target, reversed, chasePower, lead, maxSpeed);
}

/**
Expand All @@ -166,11 +166,11 @@ void Differential::moveTo(float x, float y, float theta, int timeout, bool forwa
* Unlike the Differential::moveTo function, we can just pass the parameters directly to the
* Pure Pursuit constructor
*/
void Differential::follow(const asset& path, float lookahead, int timeout, bool forwards, int maxSpeed) {
void Differential::follow(const asset& path, float lookahead, int timeout, bool reversed, int maxSpeed) {
// if a movement is already running, wait until it is done
if (movement != nullptr) waitUntilDone();
// create the movement
movement = std::make_unique<PurePursuit>(drivetrain.trackWidth, path, lookahead, timeout, forwards, maxSpeed);
movement = std::make_unique<PurePursuit>(drivetrain.trackWidth, path, lookahead, timeout, reversed, maxSpeed);
}

/**
Expand Down
10 changes: 5 additions & 5 deletions src/lemlib/movements/boomerang.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,20 +14,20 @@ namespace lemlib {
* initial competition state, and manipulate the target heading based on
* whether the robot is going to be moving forwards or backwards
*/
Boomerang::Boomerang(FAPID linearPID, FAPID angularPID, Pose target, bool forwards, float chasePower, float lead,
Boomerang::Boomerang(FAPID linearPID, FAPID angularPID, Pose target, bool reversed, float chasePower, float lead,
int maxSpeed)
: Movement(),
linearPID(linearPID),
angularPID(angularPID),
target(target),
forwards(forwards),
reversed(reversed),
chasePower(chasePower),
lead(lead),
maxSpeed(maxSpeed) {
// get the current competition state. If this changes, the movement will stop
compState = pros::competition::get_status();
// flip target theta if moving backwards
if (!forwards) target.theta = fmod(target.theta + M_PI, 2 * M_PI);
if (reversed) target.theta = fmod(target.theta + M_PI, 2 * M_PI);
}

/**
Expand Down Expand Up @@ -68,7 +68,7 @@ std::pair<int, int> Boomerang::update(Pose pose) {
if (state == 2) return {128, 128};

// if going in reverse, flip the heading of the pose
if (!forwards) pose.theta += M_PI;
if (reversed) pose.theta += M_PI;

// update completion vars
if (dist == 0) { // if dist is 0, this is the first time update() has been called
Expand All @@ -86,7 +86,7 @@ std::pair<int, int> Boomerang::update(Pose pose) {
float angularError = angleError(pose.angle(carrot), pose.theta); // angular error
float linearError = pose.distance(carrot) * cos(angularError); // linear error
if (state == 1) angularError = angleError(target.theta, pose.theta); // settling behavior
if (!forwards) linearError = -linearError;
if (reversed) linearError = -linearError;

// get PID outputs
float angularPower = -angularPID.update(radToDeg(angularError), 0);
Expand Down
8 changes: 4 additions & 4 deletions src/lemlib/movements/purepursuit.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,13 +17,13 @@ namespace lemlib {
* it into a vector of strings. We then convert each of these strings into
* floats, and then push back a new waypoint to the path vector
*/
PurePursuit::PurePursuit(float trackWidth, const asset& path, float lookaheadDist, int timeout, bool forwards,
PurePursuit::PurePursuit(float trackWidth, const asset& path, float lookaheadDist, int timeout, bool reversed,
int maxSpeed)
: Movement(),
trackWidth(trackWidth),
lookaheadDist(lookaheadDist),
timeout(timeout),
forwards(forwards),
reversed(reversed),
maxSpeed(maxSpeed) {
// get the current competition state. If this changes, the movement will stop
compState = pros::competition::get_status();
Expand Down Expand Up @@ -74,7 +74,7 @@ std::pair<int, int> PurePursuit::update(Pose pose) {
if (state == 1) return {128, 128};

// add pi to theta if the robot is moving backwards
if (!forwards) pose.theta = std::fmod(pose.theta + M_PI, 2 * M_PI);
if (reversed) pose.theta = std::fmod(pose.theta + M_PI, 2 * M_PI);

// update completion vars
if (dist == 0) { // if dist is 0, this is the first time update() has been called
Expand Down Expand Up @@ -122,7 +122,7 @@ std::pair<int, int> PurePursuit::update(Pose pose) {
rightVel /= ratio;
}
// swap and reverse velocities if the robot is moving backwards
if (!forwards) {
if (reversed) {
std::swap(leftVel, rightVel);
leftVel *= -1;
rightVel *= -1;
Expand Down

0 comments on commit f30ce21

Please sign in to comment.