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