-
Notifications
You must be signed in to change notification settings - Fork 607
/
DriveSubsystem.java
110 lines (95 loc) · 4.04 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
/*----------------------------------------------------------------------------*/
/* 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.frisbeebot.subsystems;
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.wpilibj2.command.SubsystemBase;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderPorts;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderReversed;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor1Port;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor2Port;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderPorts;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderReversed;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor1Port;
import static edu.wpi.first.wpilibj.examples.frisbeebot.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);
/**
* 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);
}
}