-
Notifications
You must be signed in to change notification settings - Fork 35
/
TalonSRXFactory.java
143 lines (110 loc) · 6.17 KB
/
TalonSRXFactory.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
package com.team254.lib.drivers;
import com.ctre.phoenix.ParamEnum;
import com.ctre.phoenix.motorcontrol.*;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
/**
* Creates CANTalon objects and configures all the parameters we care about to factory defaults. Closed-loop and sensor
* parameters are not set, as these are expected to be set by the application.
*/
public class TalonSRXFactory {
private final static int kTimeoutMs = 100;
public static class Configuration {
public NeutralMode NEUTRAL_MODE = NeutralMode.Coast;
// factory default
public double NEUTRAL_DEADBAND = 0.04;
public boolean ENABLE_CURRENT_LIMIT = false;
public boolean ENABLE_SOFT_LIMIT = true;
public boolean ENABLE_LIMIT_SWITCH = false;
public int FORWARD_SOFT_LIMIT = 0;
public int REVERSE_SOFT_LIMIT = 0;
public boolean INVERTED = false;
public boolean SENSOR_PHASE = false;
public int CONTROL_FRAME_PERIOD_MS = 5;
public int MOTION_CONTROL_FRAME_PERIOD_MS = 100;
public int GENERAL_STATUS_FRAME_RATE_MS = 5;
public int FEEDBACK_STATUS_FRAME_RATE_MS = 100;
public int QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000;
public int ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000;
public int PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000;
public VelocityMeasPeriod VELOCITY_MEASUREMENT_PERIOD = VelocityMeasPeriod.Period_100Ms;
public int VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW = 64;
public double OPEN_LOOP_RAMP_RATE = 0.0;
public double CLOSED_LOOP_RAMP_RATE = 0.0;
}
private static final Configuration kDefaultConfiguration = new Configuration();
private static final Configuration kSlaveConfiguration = new Configuration();
static {
// This control frame value seems to need to be something reasonable to avoid the Talon's
// LEDs behaving erratically. Potentially try to increase as much as possible.
kSlaveConfiguration.CONTROL_FRAME_PERIOD_MS = 100;
kSlaveConfiguration.MOTION_CONTROL_FRAME_PERIOD_MS = 1000;
kSlaveConfiguration.GENERAL_STATUS_FRAME_RATE_MS = 1000;
kSlaveConfiguration.FEEDBACK_STATUS_FRAME_RATE_MS = 1000;
kSlaveConfiguration.QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000;
kSlaveConfiguration.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000;
kSlaveConfiguration.PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000;
kSlaveConfiguration.ENABLE_SOFT_LIMIT = false;
}
// create a CANTalon with the default (out of the box) configuration
public static TalonSRX createDefaultTalon(int id) {
return createTalon(id, kDefaultConfiguration);
}
public static TalonSRX createPermanentSlaveTalon(int id, int master_id) {
final TalonSRX talon = createTalon(id, kSlaveConfiguration);
talon.set(ControlMode.Follower, master_id);
return talon;
}
public static TalonSRX createTalon(int id, Configuration config) {
TalonSRX talon = new LazyTalonSRX(id);
talon.set(ControlMode.PercentOutput, 0.0);
talon.changeMotionControlFramePeriod(config.MOTION_CONTROL_FRAME_PERIOD_MS);
talon.clearMotionProfileHasUnderrun(kTimeoutMs);
talon.clearMotionProfileTrajectories();
talon.clearStickyFaults(kTimeoutMs);
talon.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector,
LimitSwitchNormal.Disabled, kTimeoutMs);
talon.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector,
LimitSwitchNormal.Disabled, kTimeoutMs);
talon.overrideLimitSwitchesEnable(config.ENABLE_LIMIT_SWITCH);
// Turn off re-zeroing by default.
talon.configSetParameter(
ParamEnum.eClearPositionOnLimitF, 0, 0, 0, kTimeoutMs);
talon.configSetParameter(
ParamEnum.eClearPositionOnLimitR, 0, 0, 0, kTimeoutMs);
talon.configNominalOutputForward(0, kTimeoutMs);
talon.configNominalOutputReverse(0, kTimeoutMs);
talon.configNeutralDeadband(config.NEUTRAL_DEADBAND, kTimeoutMs);
talon.configPeakOutputForward(1.0, kTimeoutMs);
talon.configPeakOutputReverse(-1.0, kTimeoutMs);
talon.setNeutralMode(config.NEUTRAL_MODE);
talon.configForwardSoftLimitThreshold(config.FORWARD_SOFT_LIMIT, kTimeoutMs);
talon.configForwardSoftLimitEnable(config.ENABLE_SOFT_LIMIT, kTimeoutMs);
talon.configReverseSoftLimitThreshold(config.REVERSE_SOFT_LIMIT, kTimeoutMs);
talon.configReverseSoftLimitEnable(config.ENABLE_SOFT_LIMIT, kTimeoutMs);
talon.overrideSoftLimitsEnable(config.ENABLE_SOFT_LIMIT);
talon.setInverted(config.INVERTED);
talon.setSensorPhase(config.SENSOR_PHASE);
talon.selectProfileSlot(0, 0);
talon.configVelocityMeasurementPeriod(config.VELOCITY_MEASUREMENT_PERIOD, kTimeoutMs);
talon.configVelocityMeasurementWindow(config.VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW,
kTimeoutMs);
talon.configOpenloopRamp(config.OPEN_LOOP_RAMP_RATE, kTimeoutMs);
talon.configClosedloopRamp(config.CLOSED_LOOP_RAMP_RATE, kTimeoutMs);
talon.configVoltageCompSaturation(0.0, kTimeoutMs);
talon.configVoltageMeasurementFilter(32, kTimeoutMs);
talon.enableVoltageCompensation(false);
talon.enableCurrentLimit(config.ENABLE_CURRENT_LIMIT);
talon.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General,
config.GENERAL_STATUS_FRAME_RATE_MS, kTimeoutMs);
talon.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0,
config.FEEDBACK_STATUS_FRAME_RATE_MS, kTimeoutMs);
talon.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature,
config.QUAD_ENCODER_STATUS_FRAME_RATE_MS, kTimeoutMs);
talon.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat,
config.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS, kTimeoutMs);
talon.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth,
config.PULSE_WIDTH_STATUS_FRAME_RATE_MS, kTimeoutMs);
talon.setControlFramePeriod(ControlFrame.Control_3_General, config.CONTROL_FRAME_PERIOD_MS);
return talon;
}
}