vex/2024code
19
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial commit - test serial

Browse Source
This commit is contained in:
Cole A. Deck
2024-03-24 22:20:00 -05:00
commit a4b1c1b7ed
273 changed files with 43716 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

142
SerialTest/include/okapi/api/chassis/controller/chassisController.hpp Normal file
View File

@ -0,0 +1,142 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/chassis/controller/chassisScales.hpp"
#include "okapi/api/chassis/model/chassisModel.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/units/QAngle.hpp"
#include "okapi/api/units/QLength.hpp"
#include <memory>
#include <valarray>
namespace okapi {
class ChassisController {
public:
/**
* A ChassisController adds a closed-loop layer on top of a ChassisModel. moveDistance and
* turnAngle both use closed-loop control to move the robot. There are passthrough functions for
* everything defined in ChassisModel.
*
* @param imodel underlying ChassisModel
*/
explicit ChassisController() = default;
virtual ~ChassisController() = default;
/**
* Drives the robot straight for a distance (using closed-loop control).
*
* @param itarget distance to travel
*/
virtual void moveDistance(QLength itarget) = 0;
/**
* Drives the robot straight for a distance (using closed-loop control).
*
* @param itarget distance to travel in motor degrees
*/
virtual void moveRaw(double itarget) = 0;
/**
* Sets the target distance for the robot to drive straight (using closed-loop control).
*
* @param itarget distance to travel
*/
virtual void moveDistanceAsync(QLength itarget) = 0;
/**
* Sets the target distance for the robot to drive straight (using closed-loop control).
*
* @param itarget distance to travel in motor degrees
*/
virtual void moveRawAsync(double itarget) = 0;
/**
* Turns the robot clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for
*/
virtual void turnAngle(QAngle idegTarget) = 0;
/**
* Turns the robot clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for in motor degrees
*/
virtual void turnRaw(double idegTarget) = 0;
/**
* Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for
*/
virtual void turnAngleAsync(QAngle idegTarget) = 0;
/**
* Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for in motor degrees
*/
virtual void turnRawAsync(double idegTarget) = 0;
/**
* Sets whether turns should be mirrored.
*
* @param ishouldMirror whether turns should be mirrored
*/
virtual void setTurnsMirrored(bool ishouldMirror) = 0;
/**
* Checks whether the internal controllers are currently settled.
*
* @return Whether this ChassisController is settled.
*/
virtual bool isSettled() = 0;
/**
* Delays until the currently executing movement completes.
*/
virtual void waitUntilSettled() = 0;
/**
* Interrupts the current movement to stop the robot.
*/
virtual void stop() = 0;
/**
* Sets a new maximum velocity in RPM [0-600].
*
* @param imaxVelocity The new maximum velocity.
*/
virtual void setMaxVelocity(double imaxVelocity) = 0;
/**
* @return The maximum velocity in RPM [0-600].
*/
virtual double getMaxVelocity() const = 0;
/**
* Get the ChassisScales.
*/
virtual ChassisScales getChassisScales() const = 0;
/**
* Get the GearsetRatioPair.
*/
virtual AbstractMotor::GearsetRatioPair getGearsetRatioPair() const = 0;
/**
* @return The internal ChassisModel.
*/
virtual std::shared_ptr<ChassisModel> getModel() = 0;
/**
* @return The internal ChassisModel.
*/
virtual ChassisModel &model() = 0;
};
} // namespace okapi

184
SerialTest/include/okapi/api/chassis/controller/chassisControllerIntegrated.hpp Normal file
View File

@ -0,0 +1,184 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/chassis/controller/chassisController.hpp"
#include "okapi/api/control/async/asyncPosIntegratedController.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
namespace okapi {
class ChassisControllerIntegrated : public ChassisController {
public:
/**
* ChassisController using the V5 motor's integrated control. Puts the motors into encoder count
* units. Throws a `std::invalid_argument` exception if the gear ratio is zero. The initial
* model's max velocity will be propagated to the controllers.
*
* @param itimeUtil The TimeUtil.
* @param imodel The ChassisModel used to read from sensors/write to motors.
* @param ileftController The controller used for the left side motors.
* @param irightController The controller used for the right side motors.
* @param igearset The internal gearset and external ratio used on the drive motors.
* @param iscales The ChassisScales.
* @param ilogger The logger this instance will log to.
*/
ChassisControllerIntegrated(
const TimeUtil &itimeUtil,
std::shared_ptr<ChassisModel> imodel,
std::unique_ptr<AsyncPosIntegratedController> ileftController,
std::unique_ptr<AsyncPosIntegratedController> irightController,
const AbstractMotor::GearsetRatioPair &igearset = AbstractMotor::gearset::green,
const ChassisScales &iscales = ChassisScales({1, 1}, imev5GreenTPR),
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
/**
* Drives the robot straight for a distance (using closed-loop control).
*
* ```cpp
* // Drive forward 6 inches
* chassis->moveDistance(6_in);
*
* // Drive backward 0.2 meters
* chassis->moveDistance(-0.2_m);
* ```
*
* @param itarget distance to travel
*/
void moveDistance(QLength itarget) override;
/**
* Drives the robot straight for a distance (using closed-loop control).
*
* ```cpp
* // Drive forward by spinning the motors 400 degrees
* chassis->moveRaw(400);
* ```
*
* @param itarget distance to travel in motor degrees
*/
void moveRaw(double itarget) override;
/**
* Sets the target distance for the robot to drive straight (using closed-loop control).
*
* @param itarget distance to travel
*/
void moveDistanceAsync(QLength itarget) override;
/**
* Sets the target distance for the robot to drive straight (using closed-loop control).
*
* @param itarget distance to travel in motor degrees
*/
void moveRawAsync(double itarget) override;
/**
* Turns the robot clockwise in place (using closed-loop control).
*
* ```cpp
* // Turn 90 degrees clockwise
* chassis->turnAngle(90_deg);
* ```
*
* @param idegTarget angle to turn for
*/
void turnAngle(QAngle idegTarget) override;
/**
* Turns the robot clockwise in place (using closed-loop control).
*
* ```cpp
* // Turn clockwise by spinning the motors 200 degrees
* chassis->turnRaw(200);
* ```
*
* @param idegTarget angle to turn for in motor degrees
*/
void turnRaw(double idegTarget) override;
/**
* Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for
*/
void turnAngleAsync(QAngle idegTarget) override;
/**
* Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for in motor degrees
*/
void turnRawAsync(double idegTarget) override;
/**
* Sets whether turns should be mirrored.
*
* @param ishouldMirror whether turns should be mirrored
*/
void setTurnsMirrored(bool ishouldMirror) override;
/**
* Checks whether the internal controllers are currently settled.
*
* @return Whether this ChassisController is settled.
*/
bool isSettled() override;
/**
* Delays until the currently executing movement completes.
*/
void waitUntilSettled() override;
/**
* Interrupts the current movement to stop the robot.
*/
void stop() override;
/**
* Get the ChassisScales.
*/
ChassisScales getChassisScales() const override;
/**
* Get the GearsetRatioPair.
*/
AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;
/**
* @return The internal ChassisModel.
*/
std::shared_ptr<ChassisModel> getModel() override;
/**
* @return The internal ChassisModel.
*/
ChassisModel &model() override;
/**
* Sets a new maximum velocity in RPM [0-600].
*
* @param imaxVelocity The new maximum velocity.
*/
void setMaxVelocity(double imaxVelocity) override;
/**
* @return The maximum velocity in RPM [0-600].
*/
double getMaxVelocity() const override;
protected:
std::shared_ptr<Logger> logger;
bool normalTurns{true};
std::shared_ptr<ChassisModel> chassisModel;
TimeUtil timeUtil;
std::unique_ptr<AsyncPosIntegratedController> leftController;
std::unique_ptr<AsyncPosIntegratedController> rightController;
int lastTarget;
ChassisScales scales;
AbstractMotor::GearsetRatioPair gearsetRatioPair;
};
} // namespace okapi

275
SerialTest/include/okapi/api/chassis/controller/chassisControllerPid.hpp Normal file
View File

@ -0,0 +1,275 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/chassis/controller/chassisController.hpp"
#include "okapi/api/control/iterative/iterativePosPidController.hpp"
#include "okapi/api/util/abstractRate.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <atomic>
#include <memory>
#include <tuple>
namespace okapi {
class ChassisControllerPID : public ChassisController {
public:
/**
* ChassisController using PID control. Puts the motors into encoder count units. Throws a
* `std::invalid_argument` exception if the gear ratio is zero.
*
* @param itimeUtil The TimeUtil.
* @param imodel The ChassisModel used to read from sensors/write to motors.
* @param idistanceController The PID controller that controls chassis distance for driving
* straight.
* @param iturnController The PID controller that controls chassis angle for turning.
* @param iangleController The PID controller that controls chassis angle for driving straight.
* @param igearset The internal gearset and external ratio used on the drive motors.
* @param iscales The ChassisScales.
* @param ilogger The logger this instance will log to.
*/
ChassisControllerPID(
TimeUtil itimeUtil,
std::shared_ptr<ChassisModel> imodel,
std::unique_ptr<IterativePosPIDController> idistanceController,
std::unique_ptr<IterativePosPIDController> iturnController,
std::unique_ptr<IterativePosPIDController> iangleController,
const AbstractMotor::GearsetRatioPair &igearset = AbstractMotor::gearset::green,
const ChassisScales &iscales = ChassisScales({1, 1}, imev5GreenTPR),
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
ChassisControllerPID(const ChassisControllerPID &) = delete;
ChassisControllerPID(ChassisControllerPID &&other) = delete;
ChassisControllerPID &operator=(const ChassisControllerPID &other) = delete;
ChassisControllerPID &operator=(ChassisControllerPID &&other) = delete;
~ChassisControllerPID() override;
/**
* Drives the robot straight for a distance (using closed-loop control).
*
* ```cpp
* // Drive forward 6 inches
* chassis->moveDistance(6_in);
*
* // Drive backward 0.2 meters
* chassis->moveDistance(-0.2_m);
* ```
*
* @param itarget distance to travel
*/
void moveDistance(QLength itarget) override;
/**
* Drives the robot straight for a distance (using closed-loop control).
*
* ```cpp
* // Drive forward by spinning the motors 400 degrees
* chassis->moveRaw(400);
* ```
*
* @param itarget distance to travel in motor degrees
*/
void moveRaw(double itarget) override;
/**
* Sets the target distance for the robot to drive straight (using closed-loop control).
*
* @param itarget distance to travel
*/
void moveDistanceAsync(QLength itarget) override;
/**
* Sets the target distance for the robot to drive straight (using closed-loop control).
*
* @param itarget distance to travel in motor degrees
*/
void moveRawAsync(double itarget) override;
/**
* Turns the robot clockwise in place (using closed-loop control).
*
* ```cpp
* // Turn 90 degrees clockwise
* chassis->turnAngle(90_deg);
* ```
*
* @param idegTarget angle to turn for
*/
void turnAngle(QAngle idegTarget) override;
/**
* Turns the robot clockwise in place (using closed-loop control).
*
* ```cpp
* // Turn clockwise by spinning the motors 200 degrees
* chassis->turnRaw(200);
* ```
*
* @param idegTarget angle to turn for in motor degrees
*/
void turnRaw(double idegTarget) override;
/**
* Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for
*/
void turnAngleAsync(QAngle idegTarget) override;
/**
* Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for in motor degrees
*/
void turnRawAsync(double idegTarget) override;
/**
* Sets whether turns should be mirrored.
*
* @param ishouldMirror whether turns should be mirrored
*/
void setTurnsMirrored(bool ishouldMirror) override;
/**
* Checks whether the internal controllers are currently settled.
*
* @return Whether this ChassisController is settled.
*/
bool isSettled() override;
/**
* Delays until the currently executing movement completes.
*/
void waitUntilSettled() override;
/**
* Gets the ChassisScales.
*/
ChassisScales getChassisScales() const override;
/**
* Gets the GearsetRatioPair.
*/
AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;
/**
* Sets the velocity mode flag. When the controller is in velocity mode, the control loop will
* set motor velocities. When the controller is in voltage mode (`ivelocityMode = false`), the
* control loop will set motor voltages. Additionally, when the controller is in voltage mode,
* it will not obey maximum velocity limits.
*
* @param ivelocityMode Whether the controller should be in velocity or voltage mode.
*/
void setVelocityMode(bool ivelocityMode);
/**
* Sets the gains for all controllers.
*
* @param idistanceGains The distance controller gains.
* @param iturnGains The turn controller gains.
* @param iangleGains The angle controller gains.
*/
void setGains(const IterativePosPIDController::Gains &idistanceGains,
const IterativePosPIDController::Gains &iturnGains,
const IterativePosPIDController::Gains &iangleGains);
/**
* Gets the current controller gains.
*
* @return The current controller gains in the order: distance, turn, angle.
*/
std::tuple<IterativePosPIDController::Gains,
IterativePosPIDController::Gains,
IterativePosPIDController::Gains>
getGains() const;
/**
* Starts the internal thread. This method is called by the ChassisControllerBuilder when making a
* new instance of this class.
*/
void startThread();
/**
* Returns the underlying thread handle.
*
* @return The underlying thread handle.
*/
CrossplatformThread *getThread() const;
/**
* Interrupts the current movement to stop the robot.
*/
void stop() override;
/**
* Sets a new maximum velocity in RPM [0-600]. In voltage mode, the max velocity is ignored and a
* max voltage should be set on the underlying ChassisModel instead.
*
* @param imaxVelocity The new maximum velocity.
*/
void setMaxVelocity(double imaxVelocity) override;
/**
* @return The maximum velocity in RPM [0-600].
*/
double getMaxVelocity() const override;
/**
* @return The internal ChassisModel.
*/
std::shared_ptr<ChassisModel> getModel() override;
/**
* @return The internal ChassisModel.
*/
ChassisModel &model() override;
protected:
std::shared_ptr<Logger> logger;
bool normalTurns{true};
std::shared_ptr<ChassisModel> chassisModel;
TimeUtil timeUtil;
std::unique_ptr<IterativePosPIDController> distancePid;
std::unique_ptr<IterativePosPIDController> turnPid;
std::unique_ptr<IterativePosPIDController> anglePid;
ChassisScales scales;
AbstractMotor::GearsetRatioPair gearsetRatioPair;
bool velocityMode{true};
std::atomic_bool doneLooping{true};
std::atomic_bool doneLoopingSeen{true};
std::atomic_bool newMovement{false};
std::atomic_bool dtorCalled{false};
QTime threadSleepTime{10_ms};
static void trampoline(void *context);
void loop();
/**
* Wait for the distance setup (distancePid and anglePid) to settle.
*
* @return true if done settling; false if settling should be tried again
*/
bool waitForDistanceSettled();
/**
* Wait for the angle setup (anglePid) to settle.
*
* @return true if done settling; false if settling should be tried again
*/
bool waitForAngleSettled();
/**
* Stops all the controllers and the ChassisModel.
*/
void stopAfterSettled();
typedef enum { distance, angle, none } modeType;
modeType mode{none};
CrossplatformThread *task{nullptr};
};
} // namespace okapi

88
SerialTest/include/okapi/api/chassis/controller/chassisScales.hpp Normal file
View File

@ -0,0 +1,88 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/units/QAngle.hpp"
#include "okapi/api/units/QLength.hpp"
#include "okapi/api/units/RQuantity.hpp"
#include "okapi/api/util/logging.hpp"
#include <initializer_list>
#include <stdexcept>
#include <vector>
namespace okapi {
class ChassisScales {
public:
/**
* The scales a ChassisController needs to do all of its closed-loop control. The first element is
* the wheel diameter, the second element is the wheel track. For three-encoder configurations,
* the length from the center of rotation to the middle wheel and the middle wheel diameter are
* passed as the third and fourth elements.
*
* The wheel track is the center-to-center distance between the wheels (center-to-center
* meaning the width between the centers of both wheels). For example, if you are using four inch
* omni wheels and there are 11.5 inches between the centers of each wheel, you would call the
* constructor like so:
* `ChassisScales scales({4_in, 11.5_in}, imev5GreenTPR); // imev5GreenTPR for a green gearset`
*
* Wheel diameter
*
* +-+ Center of rotation
* | | |
* v v +----------+ Length to middle wheel
* | | from center of rotation
* +---> === | === |
* | + v + |
* | ++---------------++ |
* | | | v
* Wheel track | | |
* | | x |+| <-- Middle wheel
* | | |
* | | |
* | ++---------------++
* | + +
* +---> === ===
*
*
* @param idimensions {wheel diameter, wheel track} or {wheel diameter, wheel track, length to
* middle wheel, middle wheel diameter}.
* @param itpr The ticks per revolution of the encoders.
* @param ilogger The logger this instance will log to.
*/
ChassisScales(const std::initializer_list<QLength> &idimensions,
double itpr,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* The scales a ChassisController needs to do all of its closed-loop control. The first element is
* the straight scale, the second element is the turn scale. Optionally, the length from the
* center of rotation to the middle wheel and the middle scale can be passed as the third and
* fourth elements. The straight scale converts motor degrees to meters, the turn scale converts
* motor degrees to robot turn degrees, and the middle scale converts middle wheel degrees to
* meters.
*
* @param iscales {straight scale, turn scale} or {straight scale, turn scale, length to middle
* wheel in meters, middle scale}.
* @param itpr The ticks per revolution of the encoders.
* @param ilogger The logger this instance will log to.
*/
ChassisScales(const std::initializer_list<double> &iscales,
double itpr,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
QLength wheelDiameter;
QLength wheelTrack;
QLength middleWheelDistance;
QLength middleWheelDiameter;
double straight;
double turn;
double middle;
double tpr;
protected:
static void validateInputSize(std::size_t inputSize, const std::shared_ptr<Logger> &logger);
};
} // namespace okapi

183
SerialTest/include/okapi/api/chassis/controller/defaultOdomChassisController.hpp Normal file
View File

@ -0,0 +1,183 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/chassis/controller/chassisControllerIntegrated.hpp"
#include "okapi/api/chassis/controller/odomChassisController.hpp"
#include "okapi/api/chassis/model/skidSteerModel.hpp"
#include "okapi/api/odometry/odometry.hpp"
#include <memory>
namespace okapi {
class DefaultOdomChassisController : public OdomChassisController {
public:
/**
* Odometry based chassis controller that moves using a separately constructed chassis controller.
* Spins up a task at the default priority plus 1 for odometry when constructed.
*
* Moves the robot around in the odom frame. Instead of telling the robot to drive forward or
* turn some amount, you instead tell it to drive to a specific point on the field or turn to
* a specific angle, relative to its starting position.
*
* @param itimeUtil The TimeUtil.
* @param iodometry The odometry to read state estimates from.
* @param icontroller The chassis controller to delegate to.
* @param imode The new default StateMode used to interpret target points and query the Odometry
* state.
* @param imoveThreshold minimum length movement (smaller movements will be skipped)
* @param iturnThreshold minimum angle turn (smaller turns will be skipped)
* @param ilogger The logger this instance will log to.
*/
DefaultOdomChassisController(const TimeUtil &itimeUtil,
std::shared_ptr<Odometry> iodometry,
std::shared_ptr<ChassisController> icontroller,
const StateMode &imode = StateMode::FRAME_TRANSFORMATION,
QLength imoveThreshold = 0_mm,
QAngle iturnThreshold = 0_deg,
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
DefaultOdomChassisController(const DefaultOdomChassisController &) = delete;
DefaultOdomChassisController(DefaultOdomChassisController &&other) = delete;
DefaultOdomChassisController &operator=(const DefaultOdomChassisController &other) = delete;
DefaultOdomChassisController &operator=(DefaultOdomChassisController &&other) = delete;
/**
* Drives the robot straight to a point in the odom frame.
*
* @param ipoint The target point to navigate to.
* @param ibackwards Whether to drive to the target point backwards.
* @param ioffset An offset from the target point in the direction pointing towards the robot. The
* robot will stop this far away from the target point.
*/
void driveToPoint(const Point &ipoint,
bool ibackwards = false,
const QLength &ioffset = 0_mm) override;
/**
* Turns the robot to face a point in the odom frame.
*
* @param ipoint The target point to turn to face.
*/
void turnToPoint(const Point &ipoint) override;
/**
* @return The internal ChassisController.
*/
std::shared_ptr<ChassisController> getChassisController();
/**
* @return The internal ChassisController.
*/
ChassisController &chassisController();
/**
* This delegates to the input ChassisController.
*/
void turnToAngle(const QAngle &iangle) override;
/**
* This delegates to the input ChassisController.
*/
void moveDistance(QLength itarget) override;
/**
* This delegates to the input ChassisController.
*/
void moveRaw(double itarget) override;
/**
* This delegates to the input ChassisController.
*/
void moveDistanceAsync(QLength itarget) override;
/**
* This delegates to the input ChassisController.
*/
void moveRawAsync(double itarget) override;
/**
* Turns chassis to desired angle (turns in the direction of smallest angle)
* (ex. If current angle is 0 and target is 270, the chassis will turn -90 degrees)
*
* @param idegTarget target angle
*/
void turnAngle(QAngle idegTarget) override;
/**
* This delegates to the input ChassisController.
*/
void turnRaw(double idegTarget) override;
/**
* Turns chassis to desired angle (turns in the direction of smallest angle)
* (ex. If current angle is 0 and target is 270, the chassis will turn -90 degrees)
*
* @param idegTarget target angle
*/
void turnAngleAsync(QAngle idegTarget) override;
/**
* This delegates to the input ChassisController.
*/
void turnRawAsync(double idegTarget) override;
/**
* This delegates to the input ChassisController.
*/
void setTurnsMirrored(bool ishouldMirror) override;
/**
* This delegates to the input ChassisController.
*/
bool isSettled() override;
/**
* This delegates to the input ChassisController.
*/
void waitUntilSettled() override;
/**
* This delegates to the input ChassisController.
*/
void stop() override;
/**
* This delegates to the input ChassisController.
*/
void setMaxVelocity(double imaxVelocity) override;
/**
* This delegates to the input ChassisController.
*/
double getMaxVelocity() const override;
/**
* This delegates to the input ChassisController.
*/
ChassisScales getChassisScales() const override;
/**
* This delegates to the input ChassisController.
*/
AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;
/**
* This delegates to the input ChassisController.
*/
std::shared_ptr<ChassisModel> getModel() override;
/**
* This delegates to the input ChassisController.
*/
ChassisModel &model() override;
protected:
std::shared_ptr<Logger> logger;
std::shared_ptr<ChassisController> controller;
void waitForOdomTask();
};
} // namespace okapi

154
SerialTest/include/okapi/api/chassis/controller/odomChassisController.hpp Normal file
View File

@ -0,0 +1,154 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/chassis/controller/chassisController.hpp"
#include "okapi/api/chassis/model/skidSteerModel.hpp"
#include "okapi/api/coreProsAPI.hpp"
#include "okapi/api/odometry/odometry.hpp"
#include "okapi/api/odometry/point.hpp"
#include "okapi/api/units/QSpeed.hpp"
#include "okapi/api/util/abstractRate.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <atomic>
#include <memory>
#include <valarray>
namespace okapi {
class OdomChassisController : public ChassisController {
public:
/**
* Odometry based chassis controller. Starts task at the default for odometry when constructed,
* which calls `Odometry::step` every `10ms`. The default StateMode is
* `StateMode::FRAME_TRANSFORMATION`.
*
* Moves the robot around in the odom frame. Instead of telling the robot to drive forward or
* turn some amount, you instead tell it to drive to a specific point on the field or turn to
* a specific angle relative to its starting position.
*
* @param itimeUtil The TimeUtil.
* @param iodometry The Odometry instance to run in a new task.
* @param imode The new default StateMode used to interpret target points and query the Odometry
* state.
* @param imoveThreshold minimum length movement (smaller movements will be skipped)
* @param iturnThreshold minimum angle turn (smaller turns will be skipped)
*/
OdomChassisController(TimeUtil itimeUtil,
std::shared_ptr<Odometry> iodometry,
const StateMode &imode = StateMode::FRAME_TRANSFORMATION,
const QLength &imoveThreshold = 0_mm,
const QAngle &iturnThreshold = 0_deg,
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
~OdomChassisController() override;
OdomChassisController(const OdomChassisController &) = delete;
OdomChassisController(OdomChassisController &&other) = delete;
OdomChassisController &operator=(const OdomChassisController &other) = delete;
OdomChassisController &operator=(OdomChassisController &&other) = delete;
/**
* Drives the robot straight to a point in the odom frame.
*
* @param ipoint The target point to navigate to.
* @param ibackwards Whether to drive to the target point backwards.
* @param ioffset An offset from the target point in the direction pointing towards the robot. The
* robot will stop this far away from the target point.
*/
virtual void
driveToPoint(const Point &ipoint, bool ibackwards = false, const QLength &ioffset = 0_mm) = 0;
/**
* Turns the robot to face a point in the odom frame.
*
* @param ipoint The target point to turn to face.
*/
virtual void turnToPoint(const Point &ipoint) = 0;
/**
* Turns the robot to face an angle in the odom frame.
*
* @param iangle The angle to turn to.
*/
virtual void turnToAngle(const QAngle &iangle) = 0;
/**
* @return The current state.
*/
virtual OdomState getState() const;
/**
* Set a new state to be the current state. The default StateMode is
* `StateMode::FRAME_TRANSFORMATION`.
*
* @param istate The new state in the given format.
* @param imode The mode to treat the input state as.
*/
virtual void setState(const OdomState &istate);
/**
* Sets a default StateMode that will be used to interpret target points and query the Odometry
* state.
*
* @param imode The new default StateMode.
*/
void setDefaultStateMode(const StateMode &imode);
/**
* Set a new move threshold. Any requested movements smaller than this threshold will be skipped.
*
* @param imoveThreshold new move threshold
*/
virtual void setMoveThreshold(const QLength &imoveThreshold);
/**
* Set a new turn threshold. Any requested turns smaller than this threshold will be skipped.
*
* @param iturnTreshold new turn threshold
*/
virtual void setTurnThreshold(const QAngle &iturnTreshold);
/**
* @return The current move threshold.
*/
virtual QLength getMoveThreshold() const;
/**
* @return The current turn threshold.
*/
virtual QAngle getTurnThreshold() const;
/**
* Starts the internal odometry thread. This should not be called by normal users.
*/
void startOdomThread();
/**
* @return The underlying thread handle.
*/
CrossplatformThread *getOdomThread() const;
/**
* @return The internal odometry.
*/
std::shared_ptr<Odometry> getOdometry();
protected:
std::shared_ptr<Logger> logger;
TimeUtil timeUtil;
QLength moveThreshold;
QAngle turnThreshold;
std::shared_ptr<Odometry> odom;
CrossplatformThread *odomTask{nullptr};
std::atomic_bool dtorCalled{false};
StateMode defaultStateMode{StateMode::FRAME_TRANSFORMATION};
std::atomic_bool odomTaskRunning{false};
static void trampoline(void *context);
void loop();
};
} // namespace okapi

165
SerialTest/include/okapi/api/chassis/model/chassisModel.hpp Normal file
View File

@ -0,0 +1,165 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/chassis/model/readOnlyChassisModel.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include <array>
#include <initializer_list>
#include <memory>
#include <vector>
namespace okapi {
/**
* A version of the ReadOnlyChassisModel that also supports write methods, such as setting motor
* speed. Because this class can write to motors, there can only be one owner and as such copying
* is disabled.
*/
class ChassisModel : public ReadOnlyChassisModel {
public:
explicit ChassisModel() = default;
ChassisModel(const ChassisModel &) = delete;
ChassisModel &operator=(const ChassisModel &) = delete;
/**
* Drive the robot forwards (using open-loop control). Uses velocity mode.
*
* @param ipower motor power
*/
virtual void forward(double ispeed) = 0;
/**
* Drive the robot in an arc (using open-loop control). Uses velocity mode.
* The algorithm is (approximately):
* leftPower = forwardSpeed + yaw
* rightPower = forwardSpeed - yaw
*
* @param iforwadSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
*/
virtual void driveVector(double iforwardSpeed, double iyaw) = 0;
/**
* Drive the robot in an arc. Uses voltage mode.
* The algorithm is (approximately):
* leftPower = forwardSpeed + yaw
* rightPower = forwardSpeed - yaw
*
* @param iforwadSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
*/
virtual void driveVectorVoltage(double iforwardSpeed, double iyaw) = 0;
/**
* Turn the robot clockwise (using open-loop control). Uses velocity mode.
*
* @param ispeed motor power
*/
virtual void rotate(double ispeed) = 0;
/**
* Stop the robot (set all the motors to 0). Uses velocity mode.
*/
virtual void stop() = 0;
/**
* Drive the robot with a tank drive layout. Uses voltage mode.
*
* @param ileftSpeed left side speed
* @param irightSpeed right side speed
* @param ithreshold deadband on joystick values
*/
virtual void tank(double ileftSpeed, double irightSpeed, double ithreshold = 0) = 0;
/**
* Drive the robot with an arcade drive layout. Uses voltage mode.
*
* @param iforwardSpeed speed forward direction
* @param iyaw speed around the vertical axis
* @param ithreshold deadband on joystick values
*/
virtual void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) = 0;
/**
* Drive the robot with a curvature drive layout. The robot drives in constant radius turns
* where you control the curvature (inverse of radius) you drive in. This is advantageous
* because the forward speed will not affect the rate of turning. The algorithm switches to
* arcade if the forward speed is 0. Uses voltage mode.
*
* @param iforwardSpeed speed in the forward direction
* @param icurvature curvature (inverse of radius) to drive in
* @param ithreshold deadband on joystick values
*/
virtual void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) = 0;
/**
* Power the left side motors. Uses velocity mode.
*
* @param ipower motor power
*/
virtual void left(double ispeed) = 0;
/**
* Power the right side motors. Uses velocity mode.
*
* @param ipower motor power
*/
virtual void right(double ispeed) = 0;
/**
* Reset the sensors to their zero point.
*/
virtual void resetSensors() = 0;
/**
* Set the brake mode for each motor.
*
* @param mode new brake mode
*/
virtual void setBrakeMode(AbstractMotor::brakeMode mode) = 0;
/**
* Set the encoder units for each motor.
*
* @param units new motor encoder units
*/
virtual void setEncoderUnits(AbstractMotor::encoderUnits units) = 0;
/**
* Set the gearset for each motor.
*
* @param gearset new motor gearset
*/
virtual void setGearing(AbstractMotor::gearset gearset) = 0;
/**
* Sets a new maximum velocity in RPM. The usable maximum depends on the maximum velocity of the
* currently installed gearset. If the configured maximum velocity is greater than the attainable
* maximum velocity from the currently installed gearset, the ChassisModel will still scale to
* that velocity.
*
* @param imaxVelocity The new maximum velocity.
*/
virtual void setMaxVelocity(double imaxVelocity) = 0;
/**
* @return The current maximum velocity.
*/
virtual double getMaxVelocity() const = 0;
/**
* Sets a new maximum voltage in mV in the range `[0-12000]`.
*
* @param imaxVoltage The new maximum voltage.
*/
virtual void setMaxVoltage(double imaxVoltage) = 0;
/**
* @return The maximum voltage in mV `[0-12000]`.
*/
virtual double getMaxVoltage() const = 0;
};
} // namespace okapi

247
SerialTest/include/okapi/api/chassis/model/hDriveModel.hpp Normal file
View File

@ -0,0 +1,247 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/chassis/model/chassisModel.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/device/rotarysensor/continuousRotarySensor.hpp"
namespace okapi {
class HDriveModel : public ChassisModel {
public:
/**
* Model for an h-drive (wheels parallel with robot's direction of motion, with an additional
* wheel perpendicular to those). When the left and right side motors are powered +100%, the robot
* should move forward in a straight line. When the middle motor is powered +100%, the robot
* should strafe right in a straight line.
*
* @param ileftSideMotor The left side motor.
* @param irightSideMotor The right side motor.
* @param imiddleMotor The middle (perpendicular) motor.
* @param ileftEnc The left side encoder.
* @param irightEnc The right side encoder.
* @param imiddleEnc The middle encoder.
*/
HDriveModel(std::shared_ptr<AbstractMotor> ileftSideMotor,
std::shared_ptr<AbstractMotor> irightSideMotor,
std::shared_ptr<AbstractMotor> imiddleMotor,
std::shared_ptr<ContinuousRotarySensor> ileftEnc,
std::shared_ptr<ContinuousRotarySensor> irightEnc,
std::shared_ptr<ContinuousRotarySensor> imiddleEnc,
double imaxVelocity,
double imaxVoltage);
/**
* Drive the robot forwards (using open-loop control). Uses velocity mode. Sets the middle motor
* to zero velocity.
*
* @param ispeed motor power
*/
void forward(double ispeed) override;
/**
* Drive the robot in an arc (using open-loop control). Uses velocity mode. Sets the middle motor
* to zero velocity.
*
* The algorithm is (approximately):
* leftPower = ySpeed + zRotation
* rightPower = ySpeed - zRotation
*
* @param iySpeed speed on y axis (forward)
* @param izRotation speed around z axis (up)
*/
void driveVector(double iySpeed, double izRotation) override;
/**
* Drive the robot in an arc. Uses voltage mode. Sets the middle motor to zero velocity.
*
* The algorithm is (approximately):
* leftPower = forwardSpeed + yaw
* rightPower = forwardSpeed - yaw
*
* @param iforwadSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
*/
void driveVectorVoltage(double iforwardSpeed, double iyaw) override;
/**
* Turn the robot clockwise (using open-loop control). Uses velocity mode. Sets the middle motor
* to zero velocity.
*
* @param ispeed motor power
*/
void rotate(double ispeed) override;
/**
* Stop the robot (set all the motors to 0). Uses velocity mode.
*/
void stop() override;
/**
* Drive the robot with a tank drive layout. Uses voltage mode. Sets the middle motor to zero
* velocity.
*
* @param ileftSpeed left side speed
* @param irightSpeed right side speed
* @param ithreshold deadband on joystick values
*/
void tank(double ileftSpeed, double irightSpeed, double ithreshold = 0) override;
/**
* Drive the robot with an arcade drive layout. Uses voltage mode. Sets the middle motor to zero
* velocity.
*
* @param iforwardSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
* @param ithreshold deadband on joystick values
*/
void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) override;
/**
* Drive the robot with a curvature drive layout. The robot drives in constant radius turns
* where you control the curvature (inverse of radius) you drive in. This is advantageous
* because the forward speed will not affect the rate of turning. The algorithm switches to
* arcade if the forward speed is 0. Uses voltage mode. Sets the middle motor to zero velocity.
*
* @param iforwardSpeed speed in the forward direction
* @param icurvature curvature (inverse of radius) to drive in
* @param ithreshold deadband on joystick values
*/
void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) override;
/**
* Drive the robot with an arcade drive layout. Uses voltage mode.
*
* @param irightSpeed speed to the right
* @param iforwardSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
* @param ithreshold deadband on joystick values
*/
virtual void
hArcade(double irightSpeed, double iforwardSpeed, double iyaw, double ithreshold = 0);
/**
* Drive the robot with an curvature drive layout. Uses voltage mode.
*
* @param irightSpeed speed to the right
* @param iforwardSpeed speed in the forward direction
* @param icurvature curvature (inverse of radius) to drive in
* @param ithreshold deadband on joystick values
*/
virtual void
hCurvature(double irightSpeed, double iforwardSpeed, double icurvature, double ithreshold = 0);
/**
* Power the left side motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
void left(double ispeed) override;
/**
* Power the right side motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
void right(double ispeed) override;
/**
* Power the middle motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
virtual void middle(double ispeed);
/**
* Read the sensors.
*
* @return sensor readings in the format {left, right, middle}
*/
std::valarray<std::int32_t> getSensorVals() const override;
/**
* Reset the sensors to their zero point.
*/
void resetSensors() override;
/**
* Set the brake mode for each motor.
*
* @param mode new brake mode
*/
void setBrakeMode(AbstractMotor::brakeMode mode) override;
/**
* Set the encoder units for each motor.
*
* @param units new motor encoder units
*/
void setEncoderUnits(AbstractMotor::encoderUnits units) override;
/**
* Set the gearset for each motor.
*
* @param gearset new motor gearset
*/
void setGearing(AbstractMotor::gearset gearset) override;
/**
* Sets a new maximum velocity in RPM. The usable maximum depends on the maximum velocity of the
* currently installed gearset. If the configured maximum velocity is greater than the attainable
* maximum velocity from the currently installed gearset, the ChassisModel will still scale to
* that velocity.
*
* @param imaxVelocity The new maximum velocity.
*/
void setMaxVelocity(double imaxVelocity) override;
/**
* @return The current maximum velocity.
*/
double getMaxVelocity() const override;
/**
* Sets a new maximum voltage in mV in the range `[0-12000]`.
*
* @param imaxVoltage The new maximum voltage.
*/
void setMaxVoltage(double imaxVoltage) override;
/**
* @return The maximum voltage in mV in the range `[0-12000]`.
*/
double getMaxVoltage() const override;
/**
* Returns the left side motor.
*
* @return the left side motor
*/
std::shared_ptr<AbstractMotor> getLeftSideMotor() const;
/**
* Returns the left side motor.
*
* @return the left side motor
*/
std::shared_ptr<AbstractMotor> getRightSideMotor() const;
/**
* @return The middle motor.
*/
std::shared_ptr<AbstractMotor> getMiddleMotor() const;
protected:
double maxVelocity;
double maxVoltage;
std::shared_ptr<AbstractMotor> leftSideMotor;
std::shared_ptr<AbstractMotor> rightSideMotor;
std::shared_ptr<AbstractMotor> middleMotor;
std::shared_ptr<ContinuousRotarySensor> leftSensor;
std::shared_ptr<ContinuousRotarySensor> rightSensor;
std::shared_ptr<ContinuousRotarySensor> middleSensor;
};
} // namespace okapi

28
SerialTest/include/okapi/api/chassis/model/readOnlyChassisModel.hpp Normal file
View File

@ -0,0 +1,28 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/coreProsAPI.hpp"
#include <valarray>
namespace okapi {
/**
* A version of the ChassisModel that only supports read methods, such as querying sensor values.
* This class does not let you write to motors, so it supports having multiple owners and as a
* result copying is enabled.
*/
class ReadOnlyChassisModel {
public:
virtual ~ReadOnlyChassisModel() = default;
/**
* Read the sensors.
*
* @return sensor readings (format is implementation dependent)
*/
virtual std::valarray<std::int32_t> getSensorVals() const = 0;
};
} // namespace okapi

198
SerialTest/include/okapi/api/chassis/model/skidSteerModel.hpp Normal file
View File

@ -0,0 +1,198 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/chassis/model/chassisModel.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/device/rotarysensor/continuousRotarySensor.hpp"
namespace okapi {
class SkidSteerModel : public ChassisModel {
public:
/**
* Model for a skid steer drive (wheels parallel with robot's direction of motion). When all
* motors are powered +100%, the robot should move forward in a straight line.
*
* @param ileftSideMotor The left side motor.
* @param irightSideMotor The right side motor.
* @param ileftEnc The left side encoder.
* @param irightEnc The right side encoder.
*/
SkidSteerModel(std::shared_ptr<AbstractMotor> ileftSideMotor,
std::shared_ptr<AbstractMotor> irightSideMotor,
std::shared_ptr<ContinuousRotarySensor> ileftEnc,
std::shared_ptr<ContinuousRotarySensor> irightEnc,
double imaxVelocity,
double imaxVoltage);
/**
* Drive the robot forwards (using open-loop control). Uses velocity mode.
*
* @param ispeed motor power
*/
void forward(double ispeed) override;
/**
* Drive the robot in an arc (using open-loop control). Uses velocity mode.
* The algorithm is (approximately):
* leftPower = ySpeed + zRotation
* rightPower = ySpeed - zRotation
*
* @param iySpeed speed on y axis (forward)
* @param izRotation speed around z axis (up)
*/
void driveVector(double iySpeed, double izRotation) override;
/**
* Drive the robot in an arc. Uses voltage mode.
* The algorithm is (approximately):
* leftPower = forwardSpeed + yaw
* rightPower = forwardSpeed - yaw
*
* @param iforwadSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
*/
void driveVectorVoltage(double iforwardSpeed, double iyaw) override;
/**
* Turn the robot clockwise (using open-loop control). Uses velocity mode.
*
* @param ispeed motor power
*/
void rotate(double ispeed) override;
/**
* Stop the robot (set all the motors to 0). Uses velocity mode.
*/
void stop() override;
/**
* Drive the robot with a tank drive layout. Uses voltage mode.
*
* @param ileftSpeed left side speed
* @param irightSpeed right side speed
* @param ithreshold deadband on joystick values
*/
void tank(double ileftSpeed, double irightSpeed, double ithreshold = 0) override;
/**
* Drive the robot with an arcade drive layout. Uses voltage mode.
*
* @param iforwardSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
* @param ithreshold deadband on joystick values
*/
void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) override;
/**
* Drive the robot with a curvature drive layout. The robot drives in constant radius turns
* where you control the curvature (inverse of radius) you drive in. This is advantageous
* because the forward speed will not affect the rate of turning. The algorithm switches to
* arcade if the forward speed is 0. Uses voltage mode.
*
* @param iforwardSpeed speed in the forward direction
* @param icurvature curvature (inverse of radius) to drive in
* @param ithreshold deadband on joystick values
*/
void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) override;
/**
* Power the left side motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
void left(double ispeed) override;
/**
* Power the right side motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
void right(double ispeed) override;
/**
* Read the sensors.
*
* @return sensor readings in the format {left, right}
*/
std::valarray<std::int32_t> getSensorVals() const override;
/**
* Reset the sensors to their zero point.
*/
void resetSensors() override;
/**
* Set the brake mode for each motor.
*
* @param mode new brake mode
*/
void setBrakeMode(AbstractMotor::brakeMode mode) override;
/**
* Set the encoder units for each motor.
*
* @param units new motor encoder units
*/
void setEncoderUnits(AbstractMotor::encoderUnits units) override;
/**
* Set the gearset for each motor.
*
* @param gearset new motor gearset
*/
void setGearing(AbstractMotor::gearset gearset) override;
/**
* Sets a new maximum velocity in RPM. The usable maximum depends on the maximum velocity of the
* currently installed gearset. If the configured maximum velocity is greater than the attainable
* maximum velocity from the currently installed gearset, the ChassisModel will still scale to
* that velocity.
*
* @param imaxVelocity The new maximum velocity.
*/
void setMaxVelocity(double imaxVelocity) override;
/**
* @return The current maximum velocity.
*/
double getMaxVelocity() const override;
/**
* Sets a new maximum voltage in mV in the range `[0-12000]`.
*
* @param imaxVoltage The new maximum voltage.
*/
void setMaxVoltage(double imaxVoltage) override;
/**
* @return The maximum voltage in mV in the range `[0-12000]`.
*/
double getMaxVoltage() const override;
/**
* Returns the left side motor.
*
* @return the left side motor
*/
std::shared_ptr<AbstractMotor> getLeftSideMotor() const;
/**
* Returns the left side motor.
*
* @return the left side motor
*/
std::shared_ptr<AbstractMotor> getRightSideMotor() const;
protected:
double maxVelocity;
double maxVoltage;
std::shared_ptr<AbstractMotor> leftSideMotor;
std::shared_ptr<AbstractMotor> rightSideMotor;
std::shared_ptr<ContinuousRotarySensor> leftSensor;
std::shared_ptr<ContinuousRotarySensor> rightSensor;
};
} // namespace okapi

46
SerialTest/include/okapi/api/chassis/model/threeEncoderSkidSteerModel.hpp Normal file
View File

@ -0,0 +1,46 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/chassis/model/skidSteerModel.hpp"
namespace okapi {
class ThreeEncoderSkidSteerModel : public SkidSteerModel {
public:
/**
* Model for a skid steer drive (wheels parallel with robot's direction of motion). When all
* motors are powered +127, the robot should move forward in a straight line.
*
* @param ileftSideMotor left side motor
* @param irightSideMotor right side motor
* @param ileftEnc left side encoder
* @param imiddleEnc middle encoder (mounted perpendicular to the left and right side encoders)
* @param irightEnc right side encoder
*/
ThreeEncoderSkidSteerModel(std::shared_ptr<AbstractMotor> ileftSideMotor,
std::shared_ptr<AbstractMotor> irightSideMotor,
std::shared_ptr<ContinuousRotarySensor> ileftEnc,
std::shared_ptr<ContinuousRotarySensor> irightEnc,
std::shared_ptr<ContinuousRotarySensor> imiddleEnc,
double imaxVelocity,
double imaxVoltage);
/**
* Read the sensors.
*
* @return sensor readings in the format {left, right, middle}
*/
std::valarray<std::int32_t> getSensorVals() const override;
/**
* Reset the sensors to their zero point.
*/
void resetSensors() override;
protected:
std::shared_ptr<ContinuousRotarySensor> middleSensor;
};
} // namespace okapi

50
SerialTest/include/okapi/api/chassis/model/threeEncoderXDriveModel.hpp Normal file
View File

@ -0,0 +1,50 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/chassis/model/xDriveModel.hpp"
namespace okapi {
class ThreeEncoderXDriveModel : public XDriveModel {
public:
/**
* Model for an x drive (wheels at 45 deg from a skid steer drive). When all motors are powered
* +100%, the robot should move forward in a straight line.
*
* @param itopLeftMotor The top left motor.
* @param itopRightMotor The top right motor.
* @param ibottomRightMotor The bottom right motor.
* @param ibottomLeftMotor The bottom left motor.
* @param ileftEnc The left side encoder.
* @param irightEnc The right side encoder.
* @param imiddleEnc The middle encoder.
*/
ThreeEncoderXDriveModel(std::shared_ptr<AbstractMotor> itopLeftMotor,
std::shared_ptr<AbstractMotor> itopRightMotor,
std::shared_ptr<AbstractMotor> ibottomRightMotor,
std::shared_ptr<AbstractMotor> ibottomLeftMotor,
std::shared_ptr<ContinuousRotarySensor> ileftEnc,
std::shared_ptr<ContinuousRotarySensor> irightEnc,
std::shared_ptr<ContinuousRotarySensor> imiddleEnc,
double imaxVelocity,
double imaxVoltage);
/**
* Read the sensors.
*
* @return sensor readings in the format {left, right, middle}
*/
std::valarray<std::int32_t> getSensorVals() const override;
/**
* Reset the sensors to their zero point.
*/
void resetSensors() override;
protected:
std::shared_ptr<ContinuousRotarySensor> middleSensor;
};
} // namespace okapi

273
SerialTest/include/okapi/api/chassis/model/xDriveModel.hpp Normal file
View File

@ -0,0 +1,273 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/chassis/model/chassisModel.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/device/rotarysensor/continuousRotarySensor.hpp"
#include "okapi/api/units/QAngle.hpp"
namespace okapi {
class XDriveModel : public ChassisModel {
public:
/**
* Model for an x drive (wheels at 45 deg from a skid steer drive). When all motors are powered
* +100%, the robot should move forward in a straight line.
*
* @param itopLeftMotor The top left motor.
* @param itopRightMotor The top right motor.
* @param ibottomRightMotor The bottom right motor.
* @param ibottomLeftMotor The bottom left motor.
* @param ileftEnc The left side encoder.
* @param irightEnc The right side encoder.
*/
XDriveModel(std::shared_ptr<AbstractMotor> itopLeftMotor,
std::shared_ptr<AbstractMotor> itopRightMotor,
std::shared_ptr<AbstractMotor> ibottomRightMotor,
std::shared_ptr<AbstractMotor> ibottomLeftMotor,
std::shared_ptr<ContinuousRotarySensor> ileftEnc,
std::shared_ptr<ContinuousRotarySensor> irightEnc,
double imaxVelocity,
double imaxVoltage);
/**
* Drive the robot forwards (using open-loop control). Uses velocity mode.
*
* @param ispeed motor power
*/
void forward(double ipower) override;
/**
* Drive the robot in an arc (using open-loop control). Uses velocity mode.
* The algorithm is (approximately):
* leftPower = forwardSpeed + yaw
* rightPower = forwardSpeed - yaw
*
* @param iforwardSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
*/
void driveVector(double iforwardSpeed, double iyaw) override;
/**
* Drive the robot in an arc. Uses voltage mode.
* The algorithm is (approximately):
* leftPower = forwardSpeed + yaw
* rightPower = forwardSpeed - yaw
*
* @param iforwadSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
*/
void driveVectorVoltage(double iforwardSpeed, double iyaw) override;
/**
* Turn the robot clockwise (using open-loop control). Uses velocity mode.
*
* @param ipower motor power
*/
void rotate(double ipower) override;
/**
* Drive the robot side-ways (using open-loop control) where positive ipower is
* to the right and negative ipower is to the left. Uses velocity mode.
*
* @param ispeed motor power
*/
void strafe(double ipower);
/**
* Strafe the robot in an arc (using open-loop control) where positive istrafeSpeed is
* to the right and negative istrafeSpeed is to the left. Uses velocity mode.
* The algorithm is (approximately):
* topLeftPower = -1 * istrafeSpeed + yaw
* topRightPower = istrafeSpeed - yaw
* bottomRightPower = -1 * istrafeSpeed - yaw
* bottomLeftPower = istrafeSpeed + yaw
*
* @param istrafeSpeed speed to the right
* @param iyaw speed around the vertical axis
*/
void strafeVector(double istrafeSpeed, double iyaw);
/**
* Stop the robot (set all the motors to 0). Uses velocity mode.
*/
void stop() override;
/**
* Drive the robot with a tank drive layout. Uses voltage mode.
*
* @param ileftSpeed left side speed
* @param irightSpeed right side speed
* @param ithreshold deadband on joystick values
*/
void tank(double ileftSpeed, double irightSpeed, double ithreshold = 0) override;
/**
* Drive the robot with an arcade drive layout. Uses voltage mode.
*
* @param iforwardSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
* @param ithreshold deadband on joystick values
*/
void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) override;
/**
* Drive the robot with a curvature drive layout. The robot drives in constant radius turns
* where you control the curvature (inverse of radius) you drive in. This is advantageous
* because the forward speed will not affect the rate of turning. The algorithm switches to
* arcade if the forward speed is 0. Uses voltage mode.
*
* @param iforwardSpeed speed forward direction
* @param icurvature curvature (inverse of radius) to drive in
* @param ithreshold deadband on joystick values
*/
void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) override;
/**
* Drive the robot with an arcade drive layout. Uses voltage mode.
*
* @param irightSpeed speed to the right
* @param iforwardSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
* @param ithreshold deadband on joystick values
*/
virtual void
xArcade(double irightSpeed, double iforwardSpeed, double iyaw, double ithreshold = 0);
/**
* Drive the robot with a field-oriented arcade drive layout. Uses voltage mode.
* For example:
* Both `fieldOrientedXArcade(1, 0, 0, 0_deg)` and `fieldOrientedXArcade(1, 0, 0, 90_deg)`
* will drive the chassis in the forward/north direction. In other words, no matter
* the robot's heading, the robot will move forward/north when you tell it
* to move forward/north and will move right/east when you tell it to move right/east.
*
*
* @param ixSpeed forward speed -- (`+1`) forward, (`-1`) backward
* @param iySpeed sideways speed -- (`+1`) right, (`-1`) left
* @param iyaw turn speed -- (`+1`) clockwise, (`-1`) counter-clockwise
* @param iangle current chassis angle (`0_deg` = no correction, winds clockwise)
* @param ithreshold deadband on joystick values
*/
virtual void fieldOrientedXArcade(double ixSpeed,
double iySpeed,
double iyaw,
QAngle iangle,
double ithreshold = 0);
/**
* Power the left side motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
void left(double ispeed) override;
/**
* Power the right side motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
void right(double ispeed) override;
/**
* Read the sensors.
*
* @return sensor readings in the format {left, right}
*/
std::valarray<std::int32_t> getSensorVals() const override;
/**
* Reset the sensors to their zero point.
*/
void resetSensors() override;
/**
* Set the brake mode for each motor.
*
* @param mode new brake mode
*/
void setBrakeMode(AbstractMotor::brakeMode mode) override;
/**
* Set the encoder units for each motor.
*
* @param units new motor encoder units
*/
void setEncoderUnits(AbstractMotor::encoderUnits units) override;
/**
* Set the gearset for each motor.
*
* @param gearset new motor gearset
*/
void setGearing(AbstractMotor::gearset gearset) override;
/**
* Sets a new maximum velocity in RPM. The usable maximum depends on the maximum velocity of the
* currently installed gearset. If the configured maximum velocity is greater than the attainable
* maximum velocity from the currently installed gearset, the ChassisModel will still scale to
* that velocity.
*
* @param imaxVelocity The new maximum velocity.
*/
void setMaxVelocity(double imaxVelocity) override;
/**
* @return The current maximum velocity.
*/
double getMaxVelocity() const override;
/**
* Sets a new maximum voltage in mV in the range `[0-12000]`.
*
* @param imaxVoltage The new maximum voltage.
*/
void setMaxVoltage(double imaxVoltage) override;
/**
* @return The maximum voltage in mV in the range `[0-12000]`.
*/
double getMaxVoltage() const override;
/**
* Returns the top left motor.
*
* @return the top left motor
*/
std::shared_ptr<AbstractMotor> getTopLeftMotor() const;
/**
* Returns the top right motor.
*
* @return the top right motor
*/
std::shared_ptr<AbstractMotor> getTopRightMotor() const;
/**
* Returns the bottom right motor.
*
* @return the bottom right motor
*/
std::shared_ptr<AbstractMotor> getBottomRightMotor() const;
/**
* Returns the bottom left motor.
*
* @return the bottom left motor
*/
std::shared_ptr<AbstractMotor> getBottomLeftMotor() const;
protected:
double maxVelocity;
double maxVoltage;
std::shared_ptr<AbstractMotor> topLeftMotor;
std::shared_ptr<AbstractMotor> topRightMotor;
std::shared_ptr<AbstractMotor> bottomRightMotor;
std::shared_ptr<AbstractMotor> bottomLeftMotor;
std::shared_ptr<ContinuousRotarySensor> leftSensor;
std::shared_ptr<ContinuousRotarySensor> rightSensor;
};
} // namespace okapi