WPILibC++  unspecified
DifferentialDrive.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2008-2018 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 <llvm/raw_ostream.h>
11 
12 #include "Drive/RobotDriveBase.h"
13 
14 namespace frc {
15 
16 class SpeedController;
17 
100  public:
101  static constexpr double kDefaultQuickStopThreshold = 0.2;
102  static constexpr double kDefaultQuickStopAlpha = 0.1;
103 
104  DifferentialDrive(SpeedController& leftMotor, SpeedController& rightMotor);
105  ~DifferentialDrive() override = default;
106 
107  DifferentialDrive(const DifferentialDrive&) = delete;
108  DifferentialDrive& operator=(const DifferentialDrive&) = delete;
109 
110  void ArcadeDrive(double xSpeed, double zRotation, bool squaredInputs = true);
111  void CurvatureDrive(double xSpeed, double zRotation, bool isQuickTurn);
112  void TankDrive(double leftSpeed, double rightSpeed,
113  bool squaredInputs = true);
114 
115  void SetQuickStopThreshold(double threshold);
116  void SetQuickStopAlpha(double alpha);
117 
118  void StopMotor() override;
119  void GetDescription(llvm::raw_ostream& desc) const override;
120 
121  void InitSendable(SendableBuilder& builder) override;
122 
123  private:
124  SpeedController& m_leftMotor;
125  SpeedController& m_rightMotor;
126 
127  double m_quickStopThreshold = kDefaultQuickStopThreshold;
128  double m_quickStopAlpha = kDefaultQuickStopAlpha;
129  double m_quickStopAccumulator = 0.0;
130 };
131 
132 } // namespace frc
Interface for speed controlling devices.
Definition: SpeedController.h:17
Definition: RobotController.cpp:14
void TankDrive(double leftSpeed, double rightSpeed, bool squaredInputs=true)
Tank drive method for differential drive platform.
Definition: DifferentialDrive.cpp:189
A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive base...
Definition: DifferentialDrive.h:99
void InitSendable(SendableBuilder &builder) override
Initializes this Sendable object.
Definition: DifferentialDrive.cpp:260
void SetQuickStopAlpha(double alpha)
Sets the low-pass filter gain for QuickStop in curvature drive.
Definition: DifferentialDrive.cpp:246
void CurvatureDrive(double xSpeed, double zRotation, bool isQuickTurn)
Curvature drive method for differential drive platform.
Definition: DifferentialDrive.cpp:116
void SetQuickStopThreshold(double threshold)
Sets the QuickStop speed threshold in curvature drive.
Definition: DifferentialDrive.cpp:232
Definition: SendableBuilder.h:23
DifferentialDrive(SpeedController &leftMotor, SpeedController &rightMotor)
Construct a DifferentialDrive.
Definition: DifferentialDrive.cpp:25
void ArcadeDrive(double xSpeed, double zRotation, bool squaredInputs=true)
Arcade drive method for differential drive platform.
Definition: DifferentialDrive.cpp:47
This class implements an extremely fast bulk output stream that can only output to a stream...
Definition: raw_ostream.h:33
Common base class for drive platforms.
Definition: RobotDriveBase.h:26