mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
121 lines
5.2 KiB
C++
121 lines
5.2 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
|
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
|
/* the project. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#pragma once
|
|
|
|
#include <memory>
|
|
#include <sstream>
|
|
|
|
#include "ErrorBase.h"
|
|
#include "MotorSafety.h"
|
|
#include "MotorSafetyHelper.h"
|
|
|
|
class SpeedController;
|
|
class GenericHID;
|
|
|
|
/**
|
|
* Utility class for handling Robot drive based on a definition of the motor
|
|
* configuration.
|
|
* The robot drive class handles basic driving for a robot. Currently, 2 and 4
|
|
* motor tank and mecanum drive trains are supported. In the future other drive
|
|
* types like swerve might be implemented. Motor channel numbers are passed
|
|
* supplied on creation of the class. Those are used for either the Drive
|
|
* function (intended for hand created drive code, such as autonomous) or with
|
|
* the Tank/Arcade functions intended to be used for Operator Control driving.
|
|
*/
|
|
class RobotDrive : public MotorSafety, public ErrorBase {
|
|
public:
|
|
enum MotorType {
|
|
kFrontLeftMotor = 0,
|
|
kFrontRightMotor = 1,
|
|
kRearLeftMotor = 2,
|
|
kRearRightMotor = 3
|
|
};
|
|
|
|
RobotDrive(int leftMotorChannel, int rightMotorChannel);
|
|
RobotDrive(int frontLeftMotorChannel, int rearLeftMotorChannel,
|
|
int frontRightMotorChannel, int rearRightMotorChannel);
|
|
RobotDrive(SpeedController* leftMotor, SpeedController* rightMotor);
|
|
RobotDrive(SpeedController& leftMotor, SpeedController& rightMotor);
|
|
RobotDrive(std::shared_ptr<SpeedController> leftMotor,
|
|
std::shared_ptr<SpeedController> rightMotor);
|
|
RobotDrive(SpeedController* frontLeftMotor, SpeedController* rearLeftMotor,
|
|
SpeedController* frontRightMotor, SpeedController* rearRightMotor);
|
|
RobotDrive(SpeedController& frontLeftMotor, SpeedController& rearLeftMotor,
|
|
SpeedController& frontRightMotor, SpeedController& rearRightMotor);
|
|
RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
|
|
std::shared_ptr<SpeedController> rearLeftMotor,
|
|
std::shared_ptr<SpeedController> frontRightMotor,
|
|
std::shared_ptr<SpeedController> rearRightMotor);
|
|
virtual ~RobotDrive() = default;
|
|
|
|
RobotDrive(const RobotDrive&) = delete;
|
|
RobotDrive& operator=(const RobotDrive&) = delete;
|
|
|
|
void Drive(float outputMagnitude, float curve);
|
|
void TankDrive(GenericHID* leftStick, GenericHID* rightStick,
|
|
bool squaredInputs = true);
|
|
void TankDrive(GenericHID& leftStick, GenericHID& rightStick,
|
|
bool squaredInputs = true);
|
|
void TankDrive(GenericHID* leftStick, int leftAxis, GenericHID* rightStick,
|
|
int rightAxis, bool squaredInputs = true);
|
|
void TankDrive(GenericHID& leftStick, int leftAxis, GenericHID& rightStick,
|
|
int rightAxis, bool squaredInputs = true);
|
|
void TankDrive(float leftValue, float rightValue, bool squaredInputs = true);
|
|
void ArcadeDrive(GenericHID* stick, bool squaredInputs = true);
|
|
void ArcadeDrive(GenericHID& stick, bool squaredInputs = true);
|
|
void ArcadeDrive(GenericHID* moveStick, int moveChannel,
|
|
GenericHID* rotateStick, int rotateChannel,
|
|
bool squaredInputs = true);
|
|
void ArcadeDrive(GenericHID& moveStick, int moveChannel,
|
|
GenericHID& rotateStick, int rotateChannel,
|
|
bool squaredInputs = true);
|
|
void ArcadeDrive(float moveValue, float rotateValue,
|
|
bool squaredInputs = true);
|
|
void MecanumDrive_Cartesian(float x, float y, float rotation,
|
|
float gyroAngle = 0.0);
|
|
void MecanumDrive_Polar(float magnitude, float direction, float rotation);
|
|
void HolonomicDrive(float magnitude, float direction, float rotation);
|
|
virtual void SetLeftRightMotorOutputs(float leftOutput, float rightOutput);
|
|
void SetInvertedMotor(MotorType motor, bool isInverted);
|
|
void SetSensitivity(float sensitivity);
|
|
void SetMaxOutput(double maxOutput);
|
|
|
|
void SetExpiration(float timeout) override;
|
|
float GetExpiration() const override;
|
|
bool IsAlive() const override;
|
|
void StopMotor() override;
|
|
bool IsSafetyEnabled() const override;
|
|
void SetSafetyEnabled(bool enabled) override;
|
|
void GetDescription(std::ostringstream& desc) const override;
|
|
|
|
protected:
|
|
void InitRobotDrive();
|
|
float Limit(float num);
|
|
void Normalize(double* wheelSpeeds);
|
|
void RotateVector(double& x, double& y, double angle);
|
|
|
|
static const int kMaxNumberOfMotors = 4;
|
|
float m_sensitivity = 0.5;
|
|
double m_maxOutput = 1.0;
|
|
std::shared_ptr<SpeedController> m_frontLeftMotor;
|
|
std::shared_ptr<SpeedController> m_frontRightMotor;
|
|
std::shared_ptr<SpeedController> m_rearLeftMotor;
|
|
std::shared_ptr<SpeedController> m_rearRightMotor;
|
|
std::unique_ptr<MotorSafetyHelper> m_safetyHelper;
|
|
|
|
private:
|
|
int GetNumMotors() {
|
|
int motors = 0;
|
|
if (m_frontLeftMotor) motors++;
|
|
if (m_frontRightMotor) motors++;
|
|
if (m_rearLeftMotor) motors++;
|
|
if (m_rearRightMotor) motors++;
|
|
return motors;
|
|
}
|
|
};
|