-
Notifications
You must be signed in to change notification settings - Fork 0
/
RobotMap.java
214 lines (179 loc) · 9.59 KB
/
RobotMap.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
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 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 org.usfirst.frc.team2906.robot;
import org.usfirst.frc.team2906.robot.subsystems.IXBar;
import org.usfirst.frc.team2906.robot.subsystems.Intake;
import org.usfirst.frc.team2906.robot.subsystems.Lift;
import com.kauailabs.navx.frc.AHRS;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
public class RobotMap {
public static DifferentialDrive driveWC;
public static Compressor pnueCompressor;
public static Intake intake;
public static IXBar ixBar;
public static Lift lift;
public static Spark DriveLI;
public static Spark DriveLII;
public static Spark DriveRI;
public static Spark DriveRII;
public static Spark IntakeR;
public static Spark IntakeL;
public static WPI_TalonSRX IXBar;
public static WPI_TalonSRX IXBarSlave;
public static WPI_TalonSRX Lift;
public static WPI_TalonSRX LiftSlave;
public static SpeedControllerGroup m_left;
public static SpeedControllerGroup m_right;
public static Relay leds;
public static Encoder DTELeft;
public static Encoder DTERight;
public static Encoder IXE;
public static DoubleSolenoid Extension;
public static AHRS navX;
public static CameraServer cam1;
public static DigitalInput LimIXTop; //Attach magnets to gear/sprocket with epoxy?
public static DigitalInput LimIXBottom;
public static DigitalInput LimIXPistonC; //tells the piston to retract if activated up (+1/-1)
public static int dTELeftReset = 0;
public static int dTERightReset = 0;
public static final double sensitivity = 0.1;
public static boolean cubeTargetTracked = false;
public static double azimuthToTarget = 0;
public static double distanceToTarget = 0;
public static double cubeCameraOffSetInDegrees = 0;
public static final double PIDCameraAimP = 0.5;
public static final double PIDCameraAimI = 0.03;
public static final double PIDCameraAimD = 0.5;
public static final double PIDCameraAimGainMultiplier = 0.15;
public static final double PIDNavxTurnGainMultiplier = 0.1;
public static final double PIDNavxTurnP = 0.5;
public static final double PIDNavxTurnI = 0.03;
public static final double PIDNavxTurnD = 0.5;
public static final double PIDDriveStraightGainMultiplier = 0.03; //.03
public static final double PIDDriveStraightP = 0.45; //.45 //1.0 (12/23)
public static final double PIDDriveStraightI = 0.015; //.015 //1.0 (12/23)
public static final double PIDDriveStraightD = 0.011; //.011 //3.0 (12/23) //8.0(12/23-2)
public static final double encoderCountsLeftToIn = 5.634251153907369;//27.675; //29.07(12/23) //27.851497(12-24)
public static final double encoderCountsRightToIn = 5.634251153907369; //NEED TO CHANGE BEFORE FINAL! New gbs +gear ration conversion
//CONSTANTS FOR TALONS SRX ENCODERS
//IXBar (POSITION) - encoder not functioning propperly
public static final int kSlotIdxIX = 0; //Which PID slot to pull gains from. Starting 2018, you can choose from 0,1,2 or 3. Only the first two (0,1) are visible in web-based configuration.
public static final int kPIDLoopIdxIX = 0; //Talon SRX/ Victor SPX will supported multiple (cascaded) PID loops. For now we just want the primary one.
public static final int kTimeoutMsIX = 10; // set to zero to skip waiting for confirmation, set to nonzero to wait andreport to DS if action fails.
public static boolean kSensorPhaseIX = true; //choose so that Talon does not report sensor out of phase
public static boolean kMotorInvertIX = false; // choose based on what direction you want to be positive,this does not affect motor invert.
public static double kFIX = 0.0; //typically remains 0
public static double kPIX = 0.01;
public static double kIIX = 0.0;
public static double kDIX = 0.0;
public static double GRIX = (7*5*5); //ex: if you want to move 90degs you would multiplie 90 by 42/12 to get a total movement of the output shaft at 315degs
public static double TtoDegIX = (360/(20*GRIX));
//LIFT (VELOCITY) //Change to position!!!
public static final int kSlotIdxL = 3; //Which PID slot to pull gains from. Starting 2018, you can choose from 0,1,2 or 3. Only the first two (0,1) are visible in web-based configuration.
public static final int kPIDLoopIdxL = 0; //Talon SRX/ Victor SPX will supported multiple (cascaded) PID loops. For now we just want the primary one.
public static final int kTimeoutMsL = 10; //set to zero to skip waiting for confirmation, set to nonzero to wait andreport to DS if action fails.
public static double kFL = 0.0;
public static double kPL = 0.0;
public static double kIL = 0.0;
public static double kDL = 0.0;
public static double GRL = 1.0;
public static void init(){
DriveLI = new Spark(0);
DriveLII = new Spark(1);
DriveRI = new Spark(2);
DriveRII = new Spark(3);
IntakeR = new Spark(4);
IntakeL = new Spark(5);
IXBar = new WPI_TalonSRX(1);
IXBarSlave = new WPI_TalonSRX(2);
Lift = new WPI_TalonSRX(3);
LiftSlave = new WPI_TalonSRX(4);
LiftSlave.follow(Lift);
IXBarSlave.follow(IXBar);
IXBar.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
IXBar.setSensorPhase(false);
// get the decoded pulse width encoder position, 4096 units per rotation
int pulseWidthPos = IXBar.getSensorCollection().getPulseWidthPosition();
// get the pulse width in us, rise-to-fall in microseconds
int pulseWidthUs = IXBar.getSensorCollection().getPulseWidthRiseToFallUs();
// get the period in us, rise-to-rise in microseconds
int periodUs = IXBar.getSensorCollection().getPulseWidthRiseToRiseUs();
// get measured velocity in units per 100ms, 4096 units is one rotation
int pulseWidthVel = IXBar.getSensorCollection().getPulseWidthVelocity();
// is sensor plugged in to Talon
boolean sensorPluggedIn = false;
if (periodUs != 0) {
sensorPluggedIn = true;
}
// +14 rotations forward when using CTRE Mag encoder
IXBar.configForwardSoftLimitThreshold(+1*4096, 10);
// -15 rotations reverse when using CTRE Mag encoder
IXBar.configReverseSoftLimitThreshold(-1*4096, 10);
IXBar.configForwardSoftLimitEnable(true, 10);
IXBar.configReverseSoftLimitEnable(true, 10);
// pass false to FORCE OFF the feature. Otherwise the enable flags above are honored
IXBar.overrideLimitSwitchesEnable(true);
IXBar.selectProfileSlot(kSlotIdxIX, 0);
IXBar.config_kF(kSlotIdxIX, kFIX, kTimeoutMsIX);
IXBar.config_kP(kSlotIdxIX, kPIX, kTimeoutMsIX);
IXBar.config_kI(kSlotIdxIX, kIIX, kTimeoutMsIX);
IXBar.config_kD(kSlotIdxIX, kDIX, kTimeoutMsIX);
IXBar.config_IntegralZone(0, 100, kTimeoutMsIX);
Lift.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, RobotMap.kTimeoutMsL);
Lift.setSensorPhase(true); //set the peak, nominal outputs */
Lift.configNominalOutputForward(0, RobotMap.kTimeoutMsL);
Lift.configNominalOutputReverse(0, RobotMap.kTimeoutMsL);
Lift.configPeakOutputForward(1, RobotMap.kTimeoutMsL);
Lift.configPeakOutputReverse(-1, RobotMap.kTimeoutMsL); //set closed loop gains in slot0
Lift.config_kF(RobotMap.kPIDLoopIdxL, RobotMap.kFL, RobotMap.kTimeoutMsL);
Lift.config_kP(RobotMap.kPIDLoopIdxL, RobotMap.kPL, RobotMap.kTimeoutMsL);
Lift.config_kI(RobotMap.kPIDLoopIdxL, RobotMap.kIL, RobotMap.kTimeoutMsL);
Lift.config_kD(RobotMap.kPIDLoopIdxL, RobotMap.kDL, RobotMap.kTimeoutMsL);
IXBar.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, RobotMap.kTimeoutMsIX);
IXBar.setSensorPhase(true); //set the peak, nominal outputs */
IXBar.configNominalOutputForward(0, RobotMap.kTimeoutMsIX);
IXBar.configNominalOutputReverse(0, RobotMap.kTimeoutMsIX);
IXBar.configPeakOutputForward(1, RobotMap.kTimeoutMsIX);
IXBar.configPeakOutputReverse(-1, RobotMap.kTimeoutMsIX); //set closed loop gains in slot0
IXBar.config_kF(RobotMap.kPIDLoopIdxIX, RobotMap.kFIX, RobotMap.kTimeoutMsIX);
IXBar.config_kP(RobotMap.kPIDLoopIdxIX, RobotMap.kPIX, RobotMap.kTimeoutMsIX);
IXBar.config_kI(RobotMap.kPIDLoopIdxIX, RobotMap.kIIX, RobotMap.kTimeoutMsIX);
IXBar.config_kD(RobotMap.kPIDLoopIdxIX, RobotMap.kDIX, RobotMap.kTimeoutMsIX);
m_left = new SpeedControllerGroup(DriveLI, DriveLII);
m_left.setInverted(false);
m_right = new SpeedControllerGroup(DriveRI, DriveRII);
m_right.setInverted(true);
driveWC = new DifferentialDrive(m_left, m_right);
driveWC.setExpiration(0.1);
driveWC.setMaxOutput(1.0);
driveWC.setSafetyEnabled(false);
leds = new Relay(0);
navX = new AHRS(SPI.Port.kMXP);
DTELeft = new Encoder(0, 1);
DTERight = new Encoder(2, 3);
IXE = new Encoder(4, 5);
LimIXTop = new DigitalInput(9);
LimIXBottom = new DigitalInput(8);
LimIXPistonC = new DigitalInput(7);
pnueCompressor = new Compressor(0);
Extension = new DoubleSolenoid(0, 1, 2);
/*CameraServer server1 = CameraServer.getInstance();
server1.startAutomaticCapture();*/
}
}