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