/
DriveSubsystem.java
150 lines (124 loc) · 4.74 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
142
143
144
145
146
147
148
149
150
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class DriveSubsystem extends SubsystemBase {
private CANSparkMax left1 =
new CANSparkMax(
Constants.RobotMap.frontLeftMotorPort, CANSparkMaxLowLevel.MotorType.kBrushless);
private CANSparkMax right1 =
new CANSparkMax(
Constants.RobotMap.frontRightMotorPort, CANSparkMaxLowLevel.MotorType.kBrushless);
private CANSparkMax left2 =
new CANSparkMax(
Constants.RobotMap.backLeftMotorPort, CANSparkMaxLowLevel.MotorType.kBrushless);
private CANSparkMax right2 =
new CANSparkMax(
Constants.RobotMap.backRightMotorPort, CANSparkMaxLowLevel.MotorType.kBrushless);
ADXRS450_Gyro gyro = new ADXRS450_Gyro();
private final DifferentialDriveOdometry roboOdometry =
new DifferentialDriveOdometry(gyro.getRotation2d());
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
left1.restoreFactoryDefaults();
right1.restoreFactoryDefaults();
left2.restoreFactoryDefaults();
right2.restoreFactoryDefaults();
left2.follow(left1);
right2.follow(right1);
left1.setInverted(false);
right1.setInverted(true);
left1.setSmartCurrentLimit(30);
right1.setSmartCurrentLimit(30);
left1.getEncoder().setPositionConversionFactor(Constants.DriveConstants.distPerPulse);
right1.getEncoder().setPositionConversionFactor(Constants.DriveConstants.distPerPulse);
left1.getEncoder().setVelocityConversionFactor(Constants.DriveConstants.distPerPulse / 60);
right1.getEncoder().setVelocityConversionFactor(Constants.DriveConstants.distPerPulse / 60);
}
public void setHalfBrakeHalfCoast() {
left1.setIdleMode(IdleMode.kBrake);
left2.setIdleMode(IdleMode.kCoast);
right1.setIdleMode(IdleMode.kBrake);
right2.setIdleMode(IdleMode.kCoast);
}
public void setAllCoast() {
left1.setIdleMode(IdleMode.kCoast);
left2.setIdleMode(IdleMode.kCoast);
right1.setIdleMode(IdleMode.kCoast);
right2.setIdleMode(IdleMode.kCoast);
}
public RelativeEncoder getLeftEncoder() {
return left1.getEncoder();
}
public RelativeEncoder getRightEncoder() {
return right1.getEncoder();
}
public double getHeading() {
return Math.IEEEremainder(gyro.getAngle(), 360) * -1;
}
public double getDegrees() {
return gyro.getRotation2d().getDegrees();
}
public double getTurnRate() {
return gyro.getRate();
}
public Pose2d getPose() {
return roboOdometry.getPoseMeters();
}
public void resetOdometry(Pose2d pose) {
roboOdometry.resetPosition(pose, gyro.getRotation2d());
resetEncoders();
}
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
return new DifferentialDriveWheelSpeeds(
getLeftEncoder().getVelocity(), getRightEncoder().getVelocity());
}
public void resetEncoders() {
getLeftEncoder().setPosition(Constants.zero);
getRightEncoder().setPosition(Constants.zero);
}
public void resetGyro() {
gyro.reset();
}
public double getAverageEncoderDist() {
return ((getRightEncoder().getPosition() + getLeftEncoder().getPosition()) / 2.0);
}
public void tankDriveVolts(double left, double right) {
left1.setVoltage(left);
right1.setVoltage(right);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
roboOdometry.update(
Rotation2d.fromDegrees(getHeading()),
getLeftEncoder().getPosition(),
getRightEncoder().getPosition());
}
public void GTADrive(double leftTrigger, double rightTrigger, double turn) {
turn = MathUtil.applyDeadband(turn, Constants.DriveConstants.kJoystickTurnDeadzone);
turn = turn * turn * Math.signum(turn);
double left = rightTrigger - leftTrigger + turn;
double right = rightTrigger - leftTrigger - turn;
left = Math.min(1.0, Math.max(-1.0, left));
right = Math.max(-1.0, Math.min(1.0, right));
this.right1.set(right);
this.left1.set(left);
}
@Override
public void simulationPeriodic() {
// This method will be called once per scheduler run during simulation
}
}