This repository has been archived on 2020-09-21. You can view files and clone it, but cannot push or open issues or pull requests.
FRC2016-old/wpilib/cpp/current/include/RobotDrive.h
2016-01-28 11:33:19 -05:00

131 lines
5.4 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 "ErrorBase.h"
#include <stdlib.h>
#include <memory>
#include <sstream>
#include "HAL/HAL.hpp"
#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(uint32_t leftMotorChannel, uint32_t rightMotorChannel);
RobotDrive(uint32_t frontLeftMotorChannel, uint32_t rearLeftMotorChannel,
uint32_t frontRightMotorChannel, uint32_t 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, uint32_t leftAxis,
GenericHID *rightStick, uint32_t rightAxis,
bool squaredInputs = true);
void TankDrive(GenericHID &leftStick, uint32_t leftAxis,
GenericHID &rightStick, uint32_t 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, uint32_t moveChannel,
GenericHID *rotateStick, uint32_t rotateChannel,
bool squaredInputs = true);
void ArcadeDrive(GenericHID &moveStick, uint32_t moveChannel,
GenericHID &rotateStick, uint32_t 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 SetCANJaguarSyncGroup(uint8_t syncGroup);
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 int32_t 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;
uint8_t m_syncGroup = 0;
private:
int32_t GetNumMotors() {
int motors = 0;
if (m_frontLeftMotor) motors++;
if (m_frontRightMotor) motors++;
if (m_rearLeftMotor) motors++;
if (m_rearRightMotor) motors++;
return motors;
}
};