Skip to content

Commit

Permalink
Start work on abstract chassis
Browse files Browse the repository at this point in the history
  • Loading branch information
@SizzinSeal
SizzinSeal committed Oct 30, 2023
1 parent c6869de commit 2de6969
Show file tree
Hide file tree
Showing 2 changed files with 250 additions and 215 deletions.
218 changes: 3 additions & 215 deletions include/lemlib/chassis/chassis.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,143 +11,24 @@

#pragma once

#include <functional>
#include <memory>

#include "pros/abstract_motor.hpp"
#include "pros/rtos.hpp"
#include "pros/motors.hpp"
#include "pros/imu.hpp"

#include "lemlib/asset.hpp"
#include "lemlib/pose.hpp"
#include "lemlib/movements/movement.hpp"
#include "lemlib/devices/trackingWheel.hpp"
#include "lemlib/odom/odom.hpp"

namespace lemlib {
/**
* @brief Struct containing all the sensors used for odometry
*
* 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
*/
typedef struct {
TrackingWheel* vertical1;
TrackingWheel* vertical2;
TrackingWheel* horizontal1;
TrackingWheel* horizontal2;
pros::Imu* imu;
} OdomSensors_t;

/**
* @brief Struct containing constants for a chassis controller
*
* 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
*/
typedef struct {
float kP;
float kD;
float smallError;
float smallErrorTimeout;
float largeError;
float largeErrorTimeout;
float slew;
} ChassisController_t;

/**
* @brief Struct containing constants for a 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
*/
typedef struct {
std::shared_ptr<pros::MotorGroup> leftMotors;
std::shared_ptr<pros::MotorGroup> rightMotors;
float trackWidth;
float wheelDiameter;
float rpm;
float chasePower;
} Drivetrain_t;

/**
* @brief Construct a shared pointer to a tracking wheel.
*
* This function exists to reduce complexity for the client. The client could make their own
* shared pointer to a motor group, but this function makes it easy
*
* @param ports array of signed ports. Negative ports mean the motor should be reversed
* @param gears the gearbox used by the motors
* @return std::shared_ptr<pros::MotorGroup> a shared pointer to the motor group
*/
std::shared_ptr<pros::MotorGroup> makeMotorGroup(const std::initializer_list<int8_t> ports,
const pros::v5::MotorGears gears);

/**
* @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_t drivetrain, ChassisController_t lateralSettings, ChassisController_t angularSettings,
OdomSensors_t sensors, DriveCurveFunction_t driveCurve = &defaultDriveCurve);

/**
* @brief Initialize the chassis
*
* @param calibrateIMU whether to calibrate the IMU. True by default
*/
void initialize(bool calibrateIMU = true);
virtual void initialize() = 0;

/**
* @brief Set the pose of the chassis
Expand Down Expand Up @@ -190,116 +71,23 @@ class Chassis {
*/
void waitUntilDone();

/**
* @brief Turn the chassis so it is facing a 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 reversed whether the robot should turn to face the point with the back of the robot. false by default
* @param maxSpeed the maximum speed the robot can turn at. Default is 127
*/
void turnToPose(float x, float y, int timeout, bool reversed = false, int maxSpeed = 127);

/**
* @brief Turn the chassis so it is facing a heading
*
* The PID logging id is "angularPID"
*
* @param heading the heading the robot should face. Units are in degrees
* @param timeout longest time the robot can spend moving
* @param maxSpeed the maximum speed the robot can turn at. Default is 127
*/
void turnToHeading(float heading, int timeout, int maxSpeed = 127);

/**
* @brief Move the chassis towards the target pose
*
* Uses the boomerang controller
*
* @param x x location
* @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 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,
float lead = 0.6, int maxSpeed = 127);

/**
* @brief Move the chassis using a custom motion algorithm
*
* @param movement shared pointer to the custom movement
*/
void moveCustom(std::unique_ptr<Movement> movement);

/**
* @brief Move the chassis along a path
*
* @param filePath the filename of the path 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 maxSpeed the maximum speed the robot can move at
*/
void follow(const asset& path, float lookahead, int timeout, bool forwards = true, int maxSpeed = 127);

/**
* @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:
protected:
/**
* @brief Chassis update function. Updates chassis motion and odometry
*
*/
void update();
virtual void update() = 0;

float prevDist = 0; // the previous distance travelled by the movement

std::unique_ptr<Odom> odom;
std::unique_ptr<Movement> movement;
std::unique_ptr<pros::Task> task;

ChassisController_t lateralSettings;
ChassisController_t angularSettings;
Drivetrain_t drivetrain;
DriveCurveFunction_t driveCurve;
};
} // namespace lemlib
Loading

0 comments on commit 2de6969

Please sign in to comment.