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