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

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