300 lines
13 KiB
C++
300 lines
13 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"
|
|
|
|
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 kD derivative constant for the chassis controller
|
|
* @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 kD, float smallError, float smallErrorTimeout, float largeError,
|
|
float largeErrorTimeout, float slew);
|
|
float kP;
|
|
float kD;
|
|
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 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 linearSettings settings for the linear 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);
|
|
/**
|
|
* @brief Get the speed of the robot
|
|
*
|
|
* @param radians true for theta in radians, false for degrees. False by default
|
|
* @return lemlib::Pose
|
|
*/
|
|
Pose getSpeed(bool radians = false);
|
|
/**
|
|
* @brief Get the local speed of the robot
|
|
*
|
|
* @param radians true for theta in radians, false for degrees. False by default
|
|
* @return lemlib::Pose
|
|
*/
|
|
Pose getLocalSpeed(bool radians = false);
|
|
/**
|
|
* @brief Estimate the pose of the robot after a certain amount of time
|
|
*
|
|
* @param time time in seconds
|
|
* @param radians False for degrees, true for radians. False by default
|
|
* @return lemlib::Pose
|
|
*/
|
|
Pose estimatePose(float time, bool radians = 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 forwards whether the robot should move forwards or backwards. true for forwards (default),
|
|
* false for backwards
|
|
* @param chasePower higher values make the robot move faster but causes more overshoot on turns. 0
|
|
* makes it default to global value
|
|
* @param lead the lead parameter. Determines how curved the robot will move. 0.6 by default (0 < lead <
|
|
* 1)
|
|
* @param maxSpeed the maximum speed the robot can move at. 127 at default
|
|
* @param async whether the function should be run asynchronously. true by default
|
|
*/
|
|
void moveTo(float x, float y, float theta, int timeout, bool forwards = true, float chasePower = 0,
|
|
float lead = 0.6, float maxSpeed = 127, 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 async whether the function should be run asynchronously. false by default
|
|
* @param maxSpeed the maximum speed the robot can move at. 127 by default
|
|
*/
|
|
void moveToOld(float x, float y, int timeout, bool forwards = true, bool async = false, float maxSpeed = 127);
|
|
/**
|
|
* @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);
|
|
private:
|
|
pros::Mutex mutex;
|
|
float distTravelled = 0;
|
|
|
|
ControllerSettings linearSettings;
|
|
ControllerSettings angularSettings;
|
|
Drivetrain drivetrain;
|
|
OdomSensors sensors;
|
|
DriveCurveFunction_t driveCurve;
|
|
};
|
|
} // namespace lemlib
|