Skip to content

Commit bc9cca2

Browse files
0x5b62656e5d0x5b62656e5d
authored
Merge pull request #49 from TASRobotics/auton
2 parents 4fc4d0d + 4c6602b commit bc9cca2

File tree

11 files changed

+319
-227
lines changed

11 files changed

+319
-227
lines changed

‎.vscode/settings.json‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -58,5 +58,6 @@
5858
"edu.wpi.first.math.**.struct.*",
5959
],
6060
"java.format.settings.url": "eclipse-formatter.xml",
61-
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
61+
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable",
62+
"wpilib.autoStartRioLog": false
6263
}

‎src/main/java/raidzero/lib/LazyCan.java‎

Lines changed: 18 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,8 @@ public class LazyCan {
1212
private LaserCan laserCan;
1313
private int canId;
1414

15+
private RegionOfInterest regionOfInterest;
16+
1517
private Measurement measurement;
1618

1719
private double threshold;
@@ -24,6 +26,8 @@ public class LazyCan {
2426
public LazyCan(int canId) {
2527
laserCan = new LaserCan(canId);
2628
this.canId = canId;
29+
laserCan = new LaserCan(canId);
30+
this.canId = canId;
2731
}
2832

2933
/**
@@ -33,17 +37,20 @@ public LazyCan(int canId) {
3337
*/
3438
public int getDistanceMm() {
3539
measurement = laserCan.getMeasurement();
40+
measurement = laserCan.getMeasurement();
3641

3742
return measurement != null ? measurement.distance_mm : -1;
3843
}
3944

4045
/**
41-
* Checks if the LaserCan finds an object within the distance threshold
46+
* Returns true if the LaserCan finds an object within the distance threshold
4247
*
43-
* @return True if there is an object within the distance threshold, false otherwise
48+
* @return if there is an object within the distance threshold
4449
*/
4550
public boolean withinThreshold() {
46-
return getDistanceMm() <= threshold;
51+
measurement = laserCan.getMeasurement();
52+
53+
return measurement != null ? measurement.distance_mm <= threshold : false;
4754
}
4855

4956
/**
@@ -53,25 +60,11 @@ public boolean withinThreshold() {
5360
* @param y the y start position for the reigon
5461
* @param w the width of the reigon
5562
* @param h the height of the reigon
56-
* @return The current {@link LazyCan} instance
63+
* @return the current LazyCan Object
5764
*/
5865
public LazyCan withRegionOfInterest(int x, int y, int w, int h) {
59-
try {
60-
laserCan.setRegionOfInterest(new RegionOfInterest(x, y, w, h));
61-
} catch (ConfigurationFailedException e) {
62-
DriverStation.reportError("LaserCan " + canId + ": RegionOfInterest Configuration failed! " + e, true);
63-
}
66+
regionOfInterest = new RegionOfInterest(x, y, w, h);
6467

65-
return this;
66-
}
67-
68-
/**
69-
* Sets the reigon of interest for the lasercan
70-
*
71-
* @param regionOfInterest The region of interest
72-
* @return The current {@link LazyCan} instance
73-
*/
74-
public LazyCan withRegionOfInterest(RegionOfInterest regionOfInterest) {
7568
try {
7669
laserCan.setRegionOfInterest(regionOfInterest);
7770
} catch (ConfigurationFailedException e) {
@@ -84,40 +77,38 @@ public LazyCan withRegionOfInterest(RegionOfInterest regionOfInterest) {
8477
/**
8578
* Sets the ranging mode of the LaserCan
8679
*
87-
* @param rangingMode The ranging mode
88-
* @return The current {@link LazyCan} instance
80+
* @param rangingMode the new ranging mode
81+
* @return the current LazyCan Object
8982
*/
9083
public LazyCan withRangingMode(RangingMode rangingMode) {
9184
try {
9285
laserCan.setRangingMode(rangingMode);
9386
} catch (ConfigurationFailedException e) {
9487
System.out.println("LaserCan " + canId + ": RangingMode Configuration failed! " + e);
9588
}
96-
9789
return this;
9890
}
9991

10092
/**
10193
* Sets the timing budget of the LaserCan
10294
*
103-
* @param timingBudget The timing budget
104-
* @return The current {@link LazyCan} instance
95+
* @param timingBudget the new timing budget
96+
* @return the current LazyCan Object
10597
*/
10698
public LazyCan withTimingBudget(TimingBudget timingBudget) {
10799
try {
108100
laserCan.setTimingBudget(timingBudget);
109101
} catch (ConfigurationFailedException e) {
110102
DriverStation.reportError("LaserCan " + canId + ": TimingBudget Configuration failed! " + e, true);
111103
}
112-
113104
return this;
114105
}
115106

116107
/**
117108
* Sets the distance threshold of the LaserCan
118109
*
119-
* @param threshold The threshold in milimeters
120-
* @return The current {@link LazyCan} instance
110+
* @param threshold the new threshold in milimeters
111+
* @return the current LazyCan object
121112
*/
122113
public LazyCan withThreshold(double threshold) {
123114
this.threshold = threshold;

‎src/main/java/raidzero/robot/Constants.java‎

Lines changed: 50 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
package raidzero.robot;
22

33
import com.ctre.phoenix6.signals.GravityTypeValue;
4+
import com.ctre.phoenix6.signals.InvertedValue;
5+
import com.ctre.phoenix6.signals.MotorArrangementValue;
46
import edu.wpi.first.math.geometry.Pose2d;
57
import edu.wpi.first.math.geometry.Rotation2d;
68
import java.util.ArrayList;
@@ -109,7 +111,7 @@ public static class Joint {
109111
public static class Winch {
110112
public static final int MOTOR_ID = 16;
111113

112-
public static final double SPEED = 0.75;
114+
public static final double SPEED = 1.0;
113115
}
114116
}
115117

@@ -127,9 +129,9 @@ public static enum REEFS {
127129

128130
public static final List<Pose2d> LEFT_REEF_WAYPOINTS = new ArrayList<Pose2d>(
129131
List.of(
130-
new Pose2d(3.70, 3.16, Rotation2d.fromDegrees(60)), // 17 Left
132+
new Pose2d(3.735, 3.14, Rotation2d.fromDegrees(60)), // 17 Left
131133
new Pose2d(3.30, 4.15, Rotation2d.fromDegrees(0)), // 18 Left
132-
new Pose2d(4.05, 5.1, Rotation2d.fromDegrees(300)), // 19 Left
134+
new Pose2d(4.06, 5.105, Rotation2d.fromDegrees(300)), // 19 Left
133135
new Pose2d(5.2619, 4.99953, Rotation2d.fromDegrees(240)), // 20 Left
134136
new Pose2d(5.70, 3.85, Rotation2d.fromDegrees(180)), // 21 Left
135137
new Pose2d(4.9113, 2.93927, Rotation2d.fromDegrees(120)) // 22 Left
@@ -140,47 +142,59 @@ public static enum REEFS {
140142
List.of(
141143
new Pose2d(4.05, 2.95, Rotation2d.fromDegrees(60)), // 17 Right
142144
new Pose2d(3.30, 3.85, Rotation2d.fromDegrees(0)), // 18 Right
143-
new Pose2d(3.70, 4.89, Rotation2d.fromDegrees(300)), // 19 Right
144-
new Pose2d(4.9419, 5.16453, Rotation2d.fromDegrees(240)), // 20 Right
145+
new Pose2d(3.713, 4.925, Rotation2d.fromDegrees(300)), // 19 Right
146+
new Pose2d(4.9489, 5.16, Rotation2d.fromDegrees(240)), // 20 Right
145147
new Pose2d(5.70, 4.20, Rotation2d.fromDegrees(180)), // 21 Right
146148
new Pose2d(5.2619, 3.05047, Rotation2d.fromDegrees(120)) // 22 Right
147149
)
148150
);
151+
152+
public static final Pose2d BLUE_PROCESSOR = new Pose2d(5.987542, 0.78, Rotation2d.fromDegrees(90));
153+
public static final Pose2d RED_PROCESSOR = new Pose2d(17.55 - 5.987542, 8.05 - 0.78, Rotation2d.fromDegrees(180));
149154
}
150155

151156
public static class TelescopingArm {
152157
public static class Intake {
153158
public static final int MOTOR_ID = 12;
154-
public static final int FOLLOW_ID = 13;
155159

156-
public static final double INTAKE_SPEED = 0.25;
157-
public static final double INTAKE_LOWER_SPEED = 0.04;
160+
public static final MotorArrangementValue MOTOR_ARRANGEMENT = MotorArrangementValue.Minion_JST;
158161

159-
public static final double SCOOCH_SPEED = 0.06;
162+
public static final InvertedValue INVERTED_VALUE = InvertedValue.CounterClockwise_Positive;
160163

161-
public static final double EXTAKE_SPEED = 0.1;
162-
public static final double EXTAKE_TIMEOUT_S = 1.0;
164+
public static final int STATOR_CURRENT_LIMIT = 30;
165+
public static final int SUPPLY_CURRENT_LIMIT = 30;
166+
public static final double SUPPLY_CURRENT_LOWER_TIME = 0.0;
163167

164-
public static final double LASERCAN_DISTANCE_THRESHOLD_MM = 50.0;
168+
public static final double TOP_LASER_THRESHOLD_MM = 50.0;
169+
public static final double BOTTOM_LASER_THRESHOLD_MM = 100.0;
165170

166-
public static final int CURRENT_LIMIT = 25;
171+
public static final double INTAKE_SPEED = 0.85;
172+
public static final double LOWER_SPEED = 0.25;
173+
public static final double EJECT_SPEED = -0.80;
174+
public static final double REVERSE_SPEED = -0.2;
167175

168-
public static final int BOTTOM_LASERCAN_ID = 0;
169-
public static final int TOP_LASERCAN_ID = 1;
176+
public static final double STALL_CURRENT_THRESHOLD = 20.0;
177+
public static final double CURRENT_SPIKE_THRESHOLD = 10.0;
178+
179+
public static final double EXTAKE_SPEED = 1.0;
180+
public static final double EXTAKE_TIMEOUT_S = 1.0;
170181

171-
public static final int SERVO_HUB_ID = 3;
182+
public static final double ALGAE_INTAKE_SPEED = 1.0;
183+
public static final double ALGAE_EJECT_SPEED = -1.0;
184+
public static final double HOLD_SPEED = 0.1;
185+
186+
public static final double KP = 1.0;
187+
public static final double KI = 0.0;
188+
public static final double KD = 0.0;
172189

173-
public static final int SERVO_RETRACTED = 1950;
174-
public static final int SERVO_EXTENDED = 1300;
175-
public static final int SERVO_CENTER_WIDTH = 1625;
176190
}
177191

178192
public static class Joint {
179193
public static final int MOTOR_ID = 11;
180194
public static final int CANCODER_ID = 11;
181195

182196
public static final double CANCODER_GEAR_RATIO = 28.0 / 80.0;
183-
public static final double CANCODER_OFFSET = -(0.352783 - (0.25 / CANCODER_GEAR_RATIO));
197+
public static final double CANCODER_OFFSET = -(0.325684 - (0.25 / CANCODER_GEAR_RATIO));
184198
public static final double CANCODER_DISCONTINUITY_POINT = 0.5;
185199

186200
public static final double CONVERSION_FACTOR = (120.0 / 12.0) * 20.0;
@@ -206,17 +220,21 @@ public static class Joint {
206220
}
207221

208222
public static class Positions {
209-
public static double[] L4_SCORING_POS_M = { -0.24, 2.65 };
210-
public static double[] L4_SCORING_POS_M_BLUE = { -0.17, 2.68 };
211-
public static double[] L4_CHECK_POSITION = { -0.25, 2.62 };
212-
public static double[] L4_GRAND_SLAM = { -0.2, 1.57 };
223+
public static final double[] L4_SCORING_POS_M = { -0.24, 2.75 };
224+
public static final double[] L4_SCORING_POS_M_BLUE = { -0.17, 2.68 };
225+
public static final double[] L4_CHECK_POSITION = { -0.25, 2.62 };
226+
public static final double[] L4_GRAND_SLAM = { -0.2, 1.57 };
227+
228+
public static final double[] L3_SCORING_POS_M = { -0.20, 1.57 };
229+
public static final double[] L2_SCORING_POS_M = { -0.15, 0.9 };
230+
public static final double[] L1_SCORING_POS_M = { 0.0, 0.0 };
213231

214-
public static double[] L3_SCORING_POS_M = { -0.20, 1.57 };
215-
public static double[] L2_SCORING_POS_M = { -0.2, 0.9 };
216-
public static double[] L1_SCORING_POS_M = { 0.0, 0.0 };
232+
public static final double[] INTAKE_POS_M = { 0.5, 0.835 };
233+
public static final double[] INTAKE_POS_M_BLUE = { 0.5, 0.875 };
217234

218-
public static double[] INTAKE_POS_M = { 0.5, 0.8425 };
219-
public static double[] INTAKE_POS_M_BLUE = { 0.5, 0.8425 };
235+
public static final double[] L3_ALGAE_POS_M = { 0.75, 1.3 };
236+
public static final double[] L2_ALGAE_POS_M = { 0.6, 0.7 };
237+
public static final double[] BARGE_SCORE_POS_M = { 0, 2.8 };
220238

221239
public static double[] HOME_POS_M = { 0.0, 0.0 };
222240
}
@@ -253,7 +271,9 @@ public static class Telescope {
253271
}
254272
}
255273

256-
public static final String CANIVORE_NAME = "CANdoAttitude";
274+
public static final String BASE_CANIVORE = "CANdoAttitude";
275+
public static final String KAYNE_BUS = "Kaynebus";
276+
public static final String RIO_BUS = "rio";
257277

258278
public static final double STICK_DEADBAND = 0.2;
259279
}

‎src/main/java/raidzero/robot/Robot.java‎

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@
55
package raidzero.robot;
66

77
import au.grapplerobotics.CanBridge;
8-
import com.ctre.phoenix6.controls.StaticBrake;
98
import edu.wpi.first.net.WebServer;
109
import edu.wpi.first.wpilibj.DriverStation;
1110
import edu.wpi.first.wpilibj.Filesystem;
@@ -24,12 +23,15 @@ public class Robot extends TimedRobot {
2423

2524
private final RobotContainer m_robotContainer;
2625

26+
private boolean alreadyEnabled;
27+
2728
public Robot() {
2829
m_robotContainer = new RobotContainer();
2930
CanBridge.runTCP();
3031

3132
WebServer.start(5800, Filesystem.getDeployDirectory().getPath());
3233
Elastic.selectTab("Setup");
34+
alreadyEnabled = false;
3335
}
3436

3537
@Override
@@ -44,14 +46,15 @@ public void disabledInit() {}
4446

4547
@Override
4648
public void disabledPeriodic() {
47-
Arm.system().updateCoastMode();
48-
49-
Swerve.system().initializeOtf();
49+
if (!alreadyEnabled) {
50+
Arm.system().updateCoastMode();
51+
CoralIntake.system().updateCoastMode();
52+
Swerve.system().initializeOtf();
53+
}
5054
}
5155

5256
@Override
5357
public void disabledExit() {
54-
CoralIntake.system().getRoller().setControl(new StaticBrake());
5558
ArmStrip.system().resetAnimation();
5659
}
5760

@@ -64,6 +67,7 @@ public void autonomousInit() {
6467
}
6568

6669
Elastic.selectTab("Autonomous");
70+
alreadyEnabled = true;
6771
}
6872

6973
@Override

0 commit comments

Comments
 (0)