Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / f7f267a91e70db6e5f62b09d88f0d02f15c904e8 / . / wpilibc / athena / include / RobotDrive.h
blob: 8e23e625ea05015ef162b0a2383bb87883db6a2c [file] [log] [blame]
Brian Silvermanf7f267a2017-02-04 16:16:08 -0800[diff] [blame^]1/*----------------------------------------------------------------------------*/
2/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
3/* Open Source Software - may be modified and shared by FRC teams. The code */
4/* must be accompanied by the FIRST BSD license file in the root directory of */
5/* the project. */
6/*----------------------------------------------------------------------------*/
7
8#pragma once
9
10#include <memory>
11#include <sstream>
12
13#include "ErrorBase.h"
14#include "MotorSafety.h"
15#include "MotorSafetyHelper.h"
16
17namespace frc {
18
19class SpeedController;
20class GenericHID;
21
22/**
23 * Utility class for handling Robot drive based on a definition of the motor
24 * configuration.
25 * The robot drive class handles basic driving for a robot. Currently, 2 and 4
26 * motor tank and mecanum drive trains are supported. In the future other drive
27 * types like swerve might be implemented. Motor channel numbers are passed
28 * supplied on creation of the class. Those are used for either the Drive
29 * function (intended for hand created drive code, such as autonomous) or with
30 * the Tank/Arcade functions intended to be used for Operator Control driving.
31 */
32class RobotDrive : public MotorSafety, public ErrorBase {
33 public:
34 enum MotorType {
35 kFrontLeftMotor = 0,
36 kFrontRightMotor = 1,
37 kRearLeftMotor = 2,
38 kRearRightMotor = 3
39 };
40
41 RobotDrive(int leftMotorChannel, int rightMotorChannel);
42 RobotDrive(int frontLeftMotorChannel, int rearLeftMotorChannel,
43 int frontRightMotorChannel, int rearRightMotorChannel);
44 RobotDrive(SpeedController* leftMotor, SpeedController* rightMotor);
45 RobotDrive(SpeedController& leftMotor, SpeedController& rightMotor);
46 RobotDrive(std::shared_ptr<SpeedController> leftMotor,
47 std::shared_ptr<SpeedController> rightMotor);
48 RobotDrive(SpeedController* frontLeftMotor, SpeedController* rearLeftMotor,
49 SpeedController* frontRightMotor, SpeedController* rearRightMotor);
50 RobotDrive(SpeedController& frontLeftMotor, SpeedController& rearLeftMotor,
51 SpeedController& frontRightMotor, SpeedController& rearRightMotor);
52 RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
53 std::shared_ptr<SpeedController> rearLeftMotor,
54 std::shared_ptr<SpeedController> frontRightMotor,
55 std::shared_ptr<SpeedController> rearRightMotor);
56 virtual ~RobotDrive() = default;
57
58 RobotDrive(const RobotDrive&) = delete;
59 RobotDrive& operator=(const RobotDrive&) = delete;
60
61 void Drive(double outputMagnitude, double curve);
62 void TankDrive(GenericHID* leftStick, GenericHID* rightStick,
63 bool squaredInputs = true);
64 void TankDrive(GenericHID& leftStick, GenericHID& rightStick,
65 bool squaredInputs = true);
66 void TankDrive(GenericHID* leftStick, int leftAxis, GenericHID* rightStick,
67 int rightAxis, bool squaredInputs = true);
68 void TankDrive(GenericHID& leftStick, int leftAxis, GenericHID& rightStick,
69 int rightAxis, bool squaredInputs = true);
70 void TankDrive(double leftValue, double rightValue,
71 bool squaredInputs = true);
72 void ArcadeDrive(GenericHID* stick, bool squaredInputs = true);
73 void ArcadeDrive(GenericHID& stick, bool squaredInputs = true);
74 void ArcadeDrive(GenericHID* moveStick, int moveChannel,
75 GenericHID* rotateStick, int rotateChannel,
76 bool squaredInputs = true);
77 void ArcadeDrive(GenericHID& moveStick, int moveChannel,
78 GenericHID& rotateStick, int rotateChannel,
79 bool squaredInputs = true);
80 void ArcadeDrive(double moveValue, double rotateValue,
81 bool squaredInputs = true);
82 void MecanumDrive_Cartesian(double x, double y, double rotation,
83 double gyroAngle = 0.0);
84 void MecanumDrive_Polar(double magnitude, double direction, double rotation);
85 void HolonomicDrive(double magnitude, double direction, double rotation);
86 virtual void SetLeftRightMotorOutputs(double leftOutput, double rightOutput);
87 void SetInvertedMotor(MotorType motor, bool isInverted);
88 void SetSensitivity(double sensitivity);
89 void SetMaxOutput(double maxOutput);
90
91 void SetExpiration(double timeout) override;
92 double GetExpiration() const override;
93 bool IsAlive() const override;
94 void StopMotor() override;
95 bool IsSafetyEnabled() const override;
96 void SetSafetyEnabled(bool enabled) override;
97 void GetDescription(std::ostringstream& desc) const override;
98
99 protected:
100 void InitRobotDrive();
101 double Limit(double num);
102 void Normalize(double* wheelSpeeds);
103 void RotateVector(double& x, double& y, double angle);
104
105 static const int kMaxNumberOfMotors = 4;
106 double m_sensitivity = 0.5;
107 double m_maxOutput = 1.0;
108 std::shared_ptr<SpeedController> m_frontLeftMotor;
109 std::shared_ptr<SpeedController> m_frontRightMotor;
110 std::shared_ptr<SpeedController> m_rearLeftMotor;
111 std::shared_ptr<SpeedController> m_rearRightMotor;
112 std::unique_ptr<MotorSafetyHelper> m_safetyHelper;
113
114 private:
115 int GetNumMotors() {
116 int motors = 0;
117 if (m_frontLeftMotor) motors++;
118 if (m_frontRightMotor) motors++;
119 if (m_rearLeftMotor) motors++;
120 if (m_rearRightMotor) motors++;
121 return motors;
122 }
123};
124
125} // namespace frc