-
Notifications
You must be signed in to change notification settings - Fork 607
/
DriveSubsystem.java
141 lines (122 loc) · 4.92 KB
/
DriveSubsystem.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kGyroReversed;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderPorts;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderReversed;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor1Port;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor2Port;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderPorts;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderReversed;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor1Port;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor2Port;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final SpeedControllerGroup m_leftMotors =
new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
new PWMVictorSPX(kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
new PWMVictorSPX(kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
// The gyro sensor
private final Gyro m_gyro = new ADXRS450_Gyro();
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
}
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
}
/**
* Resets the drive encoders to currently read a position of 0.
*/
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the two encoders.
*
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
}
/**
* Zeroes the heading of the robot.
*/
public void zeroHeading() {
m_gyro.reset();
}
/**
* Returns the heading of the robot.
*
* @return the robot's heading in degrees, from 180 to 180
*/
public double getHeading() {
return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
}
/**
* Returns the turn rate of the robot.
*
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
}
}