350 lines
15 KiB
C++
350 lines
15 KiB
C++
/**
|
|
* @file include/lemlib/chassis/chassis.hpp
|
|
* @author LemLib Team
|
|
* @brief Chassis class declarations
|
|
* @version 0.4.5
|
|
* @date 2023-01-23
|
|
*
|
|
* @copyright Copyright (c) 2023
|
|
*
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <functional>
|
|
#include "pros/rtos.hpp"
|
|
#include "pros/motors.hpp"
|
|
#include "pros/imu.hpp"
|
|
#include "lemlib/asset.hpp"
|
|
#include "lemlib/chassis/trackingWheel.hpp"
|
|
#include "lemlib/pose.hpp"
|
|
#include "lemlib/pid.hpp"
|
|
#include "lemlib/exitcondition.hpp"
|
|
|
|
namespace lemlib {
|
|
/**
|
|
* @brief Struct containing all the sensors used for odometry
|
|
*
|
|
*/
|
|
struct OdomSensors {
|
|
/**
|
|
* The sensors are stored in a struct so that they can be easily passed to the chassis class
|
|
* The variables are pointers so that they can be set to nullptr if they are not used
|
|
* Otherwise the chassis class would have to have a constructor for each possible combination of sensors
|
|
*
|
|
* @param vertical1 pointer to the first vertical tracking wheel
|
|
* @param vertical2 pointer to the second vertical tracking wheel
|
|
* @param horizontal1 pointer to the first horizontal tracking wheel
|
|
* @param horizontal2 pointer to the second horizontal tracking wheel
|
|
* @param imu pointer to the IMU
|
|
*/
|
|
OdomSensors(TrackingWheel* vertical1, TrackingWheel* vertical2, TrackingWheel* horizontal1,
|
|
TrackingWheel* horizontal2, pros::Imu* imu);
|
|
TrackingWheel* vertical1;
|
|
TrackingWheel* vertical2;
|
|
TrackingWheel* horizontal1;
|
|
TrackingWheel* horizontal2;
|
|
pros::Imu* imu;
|
|
};
|
|
|
|
/**
|
|
* @brief Struct containing constants for a chassis controller
|
|
*
|
|
*/
|
|
struct ControllerSettings {
|
|
/**
|
|
* The constants are stored in a struct so that they can be easily passed to the chassis class
|
|
* Set a constant to 0 and it will be ignored
|
|
*
|
|
* @param kP proportional constant for the chassis controller
|
|
* @param kI integral constant for the chassis controller
|
|
* @param kD derivative constant for the chassis controller
|
|
* @param antiWindup
|
|
* @param smallError the error at which the chassis controller will switch to a slower control loop
|
|
* @param smallErrorTimeout the time the chassis controller will wait before switching to a slower control loop
|
|
* @param largeError the error at which the chassis controller will switch to a faster control loop
|
|
* @param largeErrorTimeout the time the chassis controller will wait before switching to a faster control loop
|
|
* @param slew the maximum acceleration of the chassis controller
|
|
*/
|
|
ControllerSettings(float kP, float kI, float kD, float windupRange, float smallError, float smallErrorTimeout,
|
|
float largeError, float largeErrorTimeout, float slew)
|
|
: kP(kP),
|
|
kI(kI),
|
|
kD(kD),
|
|
windupRange(windupRange),
|
|
smallError(smallError),
|
|
smallErrorTimeout(smallErrorTimeout),
|
|
largeError(largeError),
|
|
largeErrorTimeout(largeErrorTimeout),
|
|
slew(slew) {}
|
|
|
|
float kP;
|
|
float kI;
|
|
float kD;
|
|
float windupRange;
|
|
float smallError;
|
|
float smallErrorTimeout;
|
|
float largeError;
|
|
float largeErrorTimeout;
|
|
float slew;
|
|
};
|
|
|
|
/**
|
|
* @brief Struct containing constants for a drivetrain
|
|
*
|
|
*/
|
|
struct Drivetrain {
|
|
/**
|
|
* The constants are stored in a struct so that they can be easily passed to the chassis class
|
|
* Set a constant to 0 and it will be ignored
|
|
*
|
|
* @param leftMotors pointer to the left motors
|
|
* @param rightMotors pointer to the right motors
|
|
* @param trackWidth the track width of the robot
|
|
* @param wheelDiameter the diameter of the wheel used on the drivetrain
|
|
* @param rpm the rpm of the wheels
|
|
* @param chasePower higher values make the robot move faster but causes more overshoot on turns
|
|
*/
|
|
Drivetrain(pros::MotorGroup* leftMotors, pros::MotorGroup* rightMotors, float trackWidth, float wheelDiameter,
|
|
float rpm, float chasePower);
|
|
pros::Motor_Group* leftMotors;
|
|
pros::Motor_Group* rightMotors;
|
|
float trackWidth;
|
|
float wheelDiameter;
|
|
float rpm;
|
|
float chasePower;
|
|
};
|
|
|
|
/**
|
|
* @brief Parameters for Chassis::moveToPose
|
|
*
|
|
* We use a struct to simplify customization. Chassis::moveToPose has many
|
|
* parameters and specifying them all just to set one optional param ruins
|
|
* readability. By passing a struct to the function, we can have named
|
|
* parameters, overcoming the c/c++ limitation
|
|
*
|
|
* @param forwards whether the robot should move forwards or backwards. True by default
|
|
* @param chasePower how fast the robot will move around corners. Recommended value 2-15.
|
|
* 0 means use chasePower set in chassis class. 0 by default.
|
|
* @param lead carrot point multiplier. value between 0 and 1. Higher values result in
|
|
* curvier movements. 0.6 by default
|
|
* @param maxSpeed the maximum speed the robot can travel at. Value between 0-127.
|
|
* 127 by default
|
|
* @param minSpeed the minimum speed the robot can travel at. If set to a non-zero value,
|
|
* the exit conditions will switch to less accurate but smoother ones. Value between 0-127.
|
|
* 0 by default
|
|
* @param earlyExitRange distance between the robot and target point where the movement will
|
|
* exit. Only has an effect if minSpeed is non-zero.
|
|
*/
|
|
struct MoveToPoseParams {
|
|
bool forwards = true;
|
|
float chasePower = 0;
|
|
float lead = 0.6;
|
|
float maxSpeed = 127;
|
|
float minSpeed = 0;
|
|
float earlyExitRange = 0;
|
|
};
|
|
|
|
/**
|
|
* @brief Function pointer type for drive curve functions.
|
|
* @param input The control input in the range [-127, 127].
|
|
* @param scale The scaling factor, which can be optionally ignored.
|
|
* @return The new value to be used.
|
|
*/
|
|
typedef std::function<float(float, float)> DriveCurveFunction_t;
|
|
|
|
/**
|
|
* @brief Default drive curve. Modifies the input with an exponential curve. If the input is 127, the function
|
|
* will always output 127, no matter the value of scale, likewise for -127. This curve was inspired by team
|
|
* 5225, the Pilons. A Desmos graph of this curve can be found here:
|
|
* https://www.desmos.com/calculator/rcfjjg83zx
|
|
* @param input value from -127 to 127
|
|
* @param scale how steep the curve should be.
|
|
* @return The new value to be used.
|
|
*/
|
|
float defaultDriveCurve(float input, float scale);
|
|
|
|
/**
|
|
* @brief Chassis class
|
|
*
|
|
*/
|
|
class Chassis {
|
|
public:
|
|
/**
|
|
* @brief Construct a new Chassis
|
|
*
|
|
* @param drivetrain drivetrain to be used for the chassis
|
|
* @param lateralSettings settings for the lateral controller
|
|
* @param angularSettings settings for the angular controller
|
|
* @param sensors sensors to be used for odometry
|
|
* @param driveCurve drive curve to be used. defaults to `defaultDriveCurve`
|
|
*/
|
|
Chassis(Drivetrain drivetrain, ControllerSettings linearSettings, ControllerSettings angularSettings,
|
|
OdomSensors sensors, DriveCurveFunction_t driveCurve = &defaultDriveCurve);
|
|
/**
|
|
* @brief Calibrate the chassis sensors
|
|
*
|
|
* @param calibrateIMU whether the IMU should be calibrated. true by default
|
|
*/
|
|
void calibrate(bool calibrateIMU = true);
|
|
/**
|
|
* @brief Set the pose of the chassis
|
|
*
|
|
* @param x new x value
|
|
* @param y new y value
|
|
* @param theta new theta value
|
|
* @param radians true if theta is in radians, false if not. False by default
|
|
*/
|
|
void setPose(float x, float y, float theta, bool radians = false);
|
|
/**
|
|
* @brief Set the pose of the chassis
|
|
*
|
|
* @param pose the new pose
|
|
* @param radians whether pose theta is in radians (true) or not (false). false by default
|
|
*/
|
|
void setPose(Pose pose, bool radians = false);
|
|
/**
|
|
* @brief Get the pose of the chassis
|
|
*
|
|
* @param radians whether theta should be in radians (true) or degrees (false). false by default
|
|
* @return Pose
|
|
*/
|
|
Pose getPose(bool radians = false, bool standardPos = false);
|
|
/**
|
|
* @brief Wait until the robot has traveled a certain distance along the path
|
|
*
|
|
* @note Units are in inches if current motion is moveTo or follow, degrees if using turnTo
|
|
*
|
|
* @param dist the distance the robot needs to travel before returning
|
|
*/
|
|
void waitUntil(float dist);
|
|
/**
|
|
* @brief Wait until the robot has completed the path
|
|
*
|
|
*/
|
|
void waitUntilDone();
|
|
/**
|
|
* @brief Turn the chassis so it is facing the target point
|
|
*
|
|
* The PID logging id is "angularPID"
|
|
*
|
|
* @param x x location
|
|
* @param y y location
|
|
* @param timeout longest time the robot can spend moving
|
|
* @param forwards whether the robot should turn to face the point with the front of the robot. true by
|
|
* default
|
|
* @param maxSpeed the maximum speed the robot can turn at. Default is 127
|
|
* @param async whether the function should be run asynchronously. true by default
|
|
*/
|
|
void turnTo(float x, float y, int timeout, bool forwards = true, float maxSpeed = 127, bool async = true);
|
|
/**
|
|
* @brief Move the chassis towards the target pose
|
|
*
|
|
* Uses the boomerang controller
|
|
*
|
|
* @param x x location
|
|
* @param y y location
|
|
* @param theta target heading in degrees.
|
|
* @param timeout longest time the robot can spend moving
|
|
* @param params struct to simulate named parameters
|
|
* @param async whether the function should be run asynchronously. true by default
|
|
*/
|
|
void moveToPose(float x, float y, float theta, int timeout, MoveToPoseParams params = {}, bool async = true);
|
|
/**
|
|
* @brief Move the chassis towards a target point
|
|
*
|
|
* @param x x location
|
|
* @param y y location
|
|
* @param timeout longest time the robot can spend moving
|
|
* @param maxSpeed the maximum speed the robot can move at. 127 by default
|
|
* @param async whether the function should be run asynchronously. true by default
|
|
*/
|
|
void moveToPoint(float x, float y, int timeout, bool forwards = true, float maxSpeed = 127, bool async = true);
|
|
/**
|
|
* @brief Move the chassis along a path
|
|
*
|
|
* @param path the path asset to follow
|
|
* @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 async whether the function should be run asynchronously. true by default
|
|
*/
|
|
void follow(const asset& path, float lookahead, int timeout, bool forwards = true, bool async = true);
|
|
/**
|
|
* @brief Control the robot during the driver control period using the tank drive control scheme. In
|
|
* this control scheme one joystick axis controls one half of the robot, and another joystick axis
|
|
* controls another.
|
|
* @param left speed of the left side of the drivetrain. Takes an input from -127 to 127.
|
|
* @param right speed of the right side of the drivetrain. Takes an input from -127 to 127.
|
|
* @param curveGain control how steep the drive curve is. The larger the number, the steeper the curve.
|
|
* A value of 0 disables the curve entirely.
|
|
*/
|
|
void tank(int left, int right, float curveGain = 0.0);
|
|
/**
|
|
* @brief Control the robot during the driver using the arcade drive control scheme. In this control
|
|
* scheme one joystick axis controls the forwards and backwards movement of the robot, while the other
|
|
* joystick axis controls the robot's turning
|
|
* @param throttle speed to move forward or backward. Takes an input from -127 to 127.
|
|
* @param turn speed to turn. Takes an input from -127 to 127.
|
|
* @param curveGain the scale inputted into the drive curve function. If you are using the default drive
|
|
* curve, refer to the `defaultDriveCurve` documentation.
|
|
*/
|
|
void arcade(int throttle, int turn, float curveGain = 0.0);
|
|
/**
|
|
* @brief Control the robot during the driver using the curvature drive control scheme. This control
|
|
* scheme is very similar to arcade drive, except the second joystick axis controls the radius of the
|
|
* curve that the drivetrain makes, rather than the speed. This means that the driver can accelerate in
|
|
* a turn without changing the radius of that turn. This control scheme defaults to arcade when forward
|
|
* is zero.
|
|
* @param throttle speed to move forward or backward. Takes an input from -127 to 127.
|
|
* @param turn speed to turn. Takes an input from -127 to 127.
|
|
* @param curveGain the scale inputted into the drive curve function. If you are using the default drive
|
|
* curve, refer to the `defaultDriveCurve` documentation.
|
|
*/
|
|
void curvature(int throttle, int turn, float cureGain = 0.0);
|
|
/**
|
|
* @brief Cancels the currently running motion.
|
|
* If there is a queued motion, then that queued motion will run.
|
|
*/
|
|
void cancelMotion();
|
|
/**
|
|
* @brief Cancels all motions, even those that are queued.
|
|
* After this, the chassis will not be in motion.
|
|
*/
|
|
void cancelAllMotions();
|
|
/**
|
|
* @return whether a motion is currently running
|
|
*/
|
|
bool isInMotion() const;
|
|
protected:
|
|
/**
|
|
* @brief Indicates that this motion is queued and blocks current task until this motion reaches front of queue
|
|
*/
|
|
void requestMotionStart();
|
|
/**
|
|
* @brief Dequeues this motion and permits queued task to run
|
|
*/
|
|
void endMotion();
|
|
private:
|
|
bool motionRunning = false;
|
|
bool motionQueued = false;
|
|
|
|
pros::Mutex mutex;
|
|
float distTravelled = 0;
|
|
|
|
ControllerSettings lateralSettings;
|
|
ControllerSettings angularSettings;
|
|
Drivetrain drivetrain;
|
|
OdomSensors sensors;
|
|
DriveCurveFunction_t driveCurve;
|
|
|
|
PID lateralPID;
|
|
PID angularPID;
|
|
ExitCondition lateralLargeExit;
|
|
ExitCondition lateralSmallExit;
|
|
ExitCondition angularLargeExit;
|
|
ExitCondition angularSmallExit;
|
|
};
|
|
} // namespace lemlib
|