WPILibC++  2020.3.2
DifferentialDrive.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2008-2019 FIRST. 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 <wpi/raw_ostream.h>
11 
12 #include "frc/drive/RobotDriveBase.h"
13 #include "frc/smartdashboard/Sendable.h"
14 #include "frc/smartdashboard/SendableHelper.h"
15 
16 namespace frc {
17 
18 class SpeedController;
19 
102  public Sendable,
103  public SendableHelper<DifferentialDrive> {
104  public:
105  static constexpr double kDefaultQuickStopThreshold = 0.2;
106  static constexpr double kDefaultQuickStopAlpha = 0.1;
107 
114  DifferentialDrive(SpeedController& leftMotor, SpeedController& rightMotor);
115 
116  ~DifferentialDrive() override = default;
117 
119  DifferentialDrive& operator=(DifferentialDrive&&) = default;
120 
133  void ArcadeDrive(double xSpeed, double zRotation, bool squareInputs = true);
134 
150  void CurvatureDrive(double xSpeed, double zRotation, bool isQuickTurn);
151 
161  void TankDrive(double leftSpeed, double rightSpeed, bool squareInputs = true);
162 
178  void SetQuickStopThreshold(double threshold);
179 
191  void SetQuickStopAlpha(double alpha);
192 
199  bool IsRightSideInverted() const;
200 
207  void SetRightSideInverted(bool rightSideInverted);
208 
209  void StopMotor() override;
210  void GetDescription(wpi::raw_ostream& desc) const override;
211 
212  void InitSendable(SendableBuilder& builder) override;
213 
214  private:
215  SpeedController* m_leftMotor;
216  SpeedController* m_rightMotor;
217 
218  double m_quickStopThreshold = kDefaultQuickStopThreshold;
219  double m_quickStopAlpha = kDefaultQuickStopAlpha;
220  double m_quickStopAccumulator = 0.0;
221  double m_rightSideInvertMultiplier = -1.0;
222 };
223 
224 } // namespace frc
frc::DifferentialDrive::DifferentialDrive
DifferentialDrive(SpeedController &leftMotor, SpeedController &rightMotor)
Construct a DifferentialDrive.
frc::RobotDriveBase
Common base class for drive platforms.
Definition: RobotDriveBase.h:24
frc::DifferentialDrive::ArcadeDrive
void ArcadeDrive(double xSpeed, double zRotation, bool squareInputs=true)
Arcade drive method for differential drive platform.
frc::DifferentialDrive::SetQuickStopAlpha
void SetQuickStopAlpha(double alpha)
Sets the low-pass filter gain for QuickStop in curvature drive.
frc::DifferentialDrive::IsRightSideInverted
bool IsRightSideInverted() const
Gets if the power sent to the right side of the drivetrain is multipled by -1.
frc::DifferentialDrive
A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive base...
Definition: DifferentialDrive.h:101
frc::DifferentialDrive::TankDrive
void TankDrive(double leftSpeed, double rightSpeed, bool squareInputs=true)
Tank drive method for differential drive platform.
frc::DifferentialDrive::SetRightSideInverted
void SetRightSideInverted(bool rightSideInverted)
Sets if the power sent to the right side of the drivetrain should be multipled by -1.
frc::DifferentialDrive::SetQuickStopThreshold
void SetQuickStopThreshold(double threshold)
Sets the QuickStop speed threshold in curvature drive.
frc::DifferentialDrive::InitSendable
void InitSendable(SendableBuilder &builder) override
Initializes this Sendable object.
frc::Sendable
Interface for Sendable objects.
Definition: Sendable.h:17
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc::SpeedController
Interface for speed controlling devices.
Definition: SpeedController.h:19
wpi::raw_ostream
This class implements an extremely fast bulk output stream that can only output to a stream.
Definition: raw_ostream.h:47
frc::DifferentialDrive::CurvatureDrive
void CurvatureDrive(double xSpeed, double zRotation, bool isQuickTurn)
Curvature drive method for differential drive platform.
frc::SendableHelper
A helper class for use with objects that add themselves to SendableRegistry.
Definition: SendableHelper.h:28
frc::SendableBuilder
Definition: SendableBuilder.h:23