Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 26e4e5289bfe3b1691be9c14bedfb70bdcd6dc56 / . / wpilibc / Athena / include / RobotDrive.h
blob: d91e80d32d0ee986634dac2deb526241e97e32a9 [file] [log] [blame]
Brian Silverman26e4e522015-12-17 01:56:40 -0500[diff] [blame^]1/*----------------------------------------------------------------------------*/
2/* Copyright (c) FIRST 2008. All Rights Reserved.
3 */
4/* Open Source Software - may be modified and shared by FRC teams. The code */
5/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
6/*----------------------------------------------------------------------------*/
7#pragma once
8
9#include "ErrorBase.h"
10#include <stdlib.h>
11#include <memory>
12#include <sstream>
13#include "HAL/HAL.hpp"
14#include "MotorSafety.h"
15#include "MotorSafetyHelper.h"
16
17class SpeedController;
18class GenericHID;
19
20/**
21 * Utility class for handling Robot drive based on a definition of the motor
22 * configuration.
23 * The robot drive class handles basic driving for a robot. Currently, 2 and 4
24 * motor tank and
25 * mecanum drive trains are supported. In the future other drive types like
26 * swerve might be
27 * implemented. Motor channel numbers are passed supplied on creation of the
28 * class. Those
29 * are used for either the Drive function (intended for hand created drive code,
30 * such as
31 * autonomous) or with the Tank/Arcade functions intended to be used for
32 * 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(uint32_t leftMotorChannel, uint32_t rightMotorChannel);
45 RobotDrive(uint32_t frontLeftMotorChannel, uint32_t rearLeftMotorChannel,
46 uint32_t frontRightMotorChannel, uint32_t 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(float outputMagnitude, float 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, uint32_t leftAxis,
70 GenericHID *rightStick, uint32_t rightAxis,
71 bool squaredInputs = true);
72 void TankDrive(GenericHID &leftStick, uint32_t leftAxis,
73 GenericHID &rightStick, uint32_t rightAxis,
74 bool squaredInputs = true);
75 void TankDrive(float leftValue, float rightValue, bool squaredInputs = true);
76 void ArcadeDrive(GenericHID *stick, bool squaredInputs = true);
77 void ArcadeDrive(GenericHID &stick, bool squaredInputs = true);
78 void ArcadeDrive(GenericHID *moveStick, uint32_t moveChannel,
79 GenericHID *rotateStick, uint32_t rotateChannel,
80 bool squaredInputs = true);
81 void ArcadeDrive(GenericHID &moveStick, uint32_t moveChannel,
82 GenericHID &rotateStick, uint32_t rotateChannel,
83 bool squaredInputs = true);
84 void ArcadeDrive(float moveValue, float rotateValue,
85 bool squaredInputs = true);
86 void MecanumDrive_Cartesian(float x, float y, float rotation,
87 float gyroAngle = 0.0);
88 void MecanumDrive_Polar(float magnitude, float direction, float rotation);
89 void HolonomicDrive(float magnitude, float direction, float rotation);
90 virtual void SetLeftRightMotorOutputs(float leftOutput, float rightOutput);
91 void SetInvertedMotor(MotorType motor, bool isInverted);
92 void SetSensitivity(float sensitivity);
93 void SetMaxOutput(double maxOutput);
94 void SetCANJaguarSyncGroup(uint8_t syncGroup);
95
96 void SetExpiration(float timeout) override;
97 float GetExpiration() const override;
98 bool IsAlive() const override;
99 void StopMotor() override;
100 bool IsSafetyEnabled() const override;
101 void SetSafetyEnabled(bool enabled) override;
102 void GetDescription(std::ostringstream& desc) const override;
103
104 protected:
105 void InitRobotDrive();
106 float Limit(float num);
107 void Normalize(double *wheelSpeeds);
108 void RotateVector(double &x, double &y, double angle);
109
110 static const int32_t kMaxNumberOfMotors = 4;
111 float m_sensitivity = 0.5;
112 double m_maxOutput = 1.0;
113 std::shared_ptr<SpeedController> m_frontLeftMotor;
114 std::shared_ptr<SpeedController> m_frontRightMotor;
115 std::shared_ptr<SpeedController> m_rearLeftMotor;
116 std::shared_ptr<SpeedController> m_rearRightMotor;
117 std::unique_ptr<MotorSafetyHelper> m_safetyHelper;
118 uint8_t m_syncGroup = 0;
119
120 private:
121 int32_t 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};