/
Mk2SwerveModule.java
148 lines (118 loc) · 4.56 KB
/
Mk2SwerveModule.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
package org.frcteam2910.c2019.drivers;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Spark;
import org.frcteam2910.common.control.PidConstants;
import org.frcteam2910.common.control.PidController;
import org.frcteam2910.common.drivers.SwerveModule;
import org.frcteam2910.common.math.Vector2;
import java.util.concurrent.atomic.AtomicLong;
public class Mk2SwerveModule extends SwerveModule {
private static final PidConstants ANGLE_CONSTANTS = new PidConstants(0.5, 0.0, 0.0001);
private static final double DRIVE_TICKS_PER_INCH = 1.0 / (4.0 * Math.PI / 60.0 * 15.0 / 20.0 * 24.0 / 38.0 * 18.0); // 0.707947
private static final double CAN_UPDATE_RATE = 200.0;
private final double angleOffset;
private Spark angleMotor;
private AnalogInput angleEncoder;
private CANSparkMax driveMotor;
private CANEncoder driveEncoder;
private final Object canLock = new Object();
private double driveEncoderTicks = 0.0;
private double drivePercentOutput = 0.0;
private double driveVelocityRpm = 0.0;
private double driveCurrent = 0.0;
private Notifier canUpdateNotifier = new Notifier(() -> {
double driveEncoderTicks = driveEncoder.getPosition();
synchronized (canLock) {
Mk2SwerveModule.this.driveEncoderTicks = driveEncoderTicks;
}
double driveVelocityRpm = driveEncoder.getVelocity();
synchronized (canLock) {
Mk2SwerveModule.this.driveVelocityRpm = driveVelocityRpm;
}
double localDriveCurrent = driveMotor.getOutputCurrent();
synchronized (canLock) {
driveCurrent = localDriveCurrent;
}
double drivePercentOutput;
synchronized (canLock) {
drivePercentOutput = Mk2SwerveModule.this.drivePercentOutput;
}
driveMotor.set(drivePercentOutput);
});
private PidController angleController = new PidController(ANGLE_CONSTANTS);
public Mk2SwerveModule(Vector2 modulePosition, double angleOffset,
Spark angleMotor, CANSparkMax driveMotor, AnalogInput angleEncoder) {
super(modulePosition);
this.angleOffset = angleOffset;
this.angleMotor = angleMotor;
this.angleEncoder = angleEncoder;
this.driveMotor = driveMotor;
this.driveEncoder = new CANEncoder(driveMotor);
driveMotor.setSmartCurrentLimit(60);
driveMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 500);
driveMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, 3);
angleController.setInputRange(0.0, 2.0 * Math.PI);
angleController.setContinuous(true);
angleController.setOutputRange(-0.5, 0.5);
canUpdateNotifier.startPeriodic(1.0 / CAN_UPDATE_RATE);
}
@Override
protected double readAngle() {
double angle = (1.0 - angleEncoder.getVoltage() / RobotController.getVoltage5V()) * 2.0 * Math.PI + angleOffset;
angle %= 2.0 * Math.PI;
if (angle < 0.0) {
angle += 2.0 * Math.PI;
}
return angle;
}
@Override
protected double readDistance() {
double driveEncoderTicks;
synchronized (canLock) {
driveEncoderTicks = this.driveEncoderTicks;
}
return driveEncoderTicks / DRIVE_TICKS_PER_INCH;
}
protected double readVelocity() {
double driveVelocityRpm;
synchronized (canLock) {
driveVelocityRpm = this.driveVelocityRpm;
}
return driveVelocityRpm * (1.0 / 60.0) / DRIVE_TICKS_PER_INCH;
}
protected double readDriveCurrent() {
double localDriveCurrent;
synchronized (canLock) {
localDriveCurrent = driveCurrent;
}
return localDriveCurrent;
}
@Override
public double getCurrentVelocity() {
return readVelocity();
}
@Override
public double getDriveCurrent() {
return readDriveCurrent();
}
@Override
protected void setTargetAngle(double angle) {
angleController.setSetpoint(angle);
}
@Override
protected void setDriveOutput(double output) {
synchronized (canLock) {
this.drivePercentOutput = output;
}
}
@Override
public void updateState(double dt) {
super.updateState(dt);
angleMotor.set(angleController.calculate(getCurrentAngle(), dt));
}
}