Skip to content

Commit 53130a7

Browse files
DriverStationComputerDriverStationComputer
committed
Merge branch 'bangbot' of https://github.com/deepbluerobotics/RobotCode2024 into bangbot
2 parents f68c54b + 8ef3910 commit 53130a7

19 files changed

+423
-335
lines changed

‎simgui.json‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -87,7 +87,7 @@
8787
0.0,
8888
0.8500000238418579
8989
],
90-
"height": 338
90+
"height": 177
9191
}
9292
]
9393
}

‎src/main/java/org/carlmontrobotics/Constants.java‎

Lines changed: 57 additions & 63 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@
3131
import edu.wpi.first.wpilibj.XboxController.Button;
3232
import edu.wpi.first.wpilibj2.command.button.POVButton;
3333
import edu.wpi.first.wpilibj.util.Color;
34+
import org.carlmontrobotics.subsystems.Led;
3435

3536
/**
3637
* The Constants class provides a convenient place for teams to hold robot-wide
@@ -46,30 +47,17 @@
4647
*/
4748
public final class Constants {
4849
public static final double g = 9.81; // meters per second squared
49-
5050
public static final class Led {
51-
public static final int ledLength = 1000;//lower doesn't work but higher does
52-
public static final int ledPort = 5;
53-
54-
//public static final Color8Bit defaultColor = new Color8Bit(0, 0, 200);
55-
56-
57-
58-
public static final Color8Bit detectNote = new Color8Bit(250,140,3);
59-
public static final Color8Bit holding = new Color8Bit(0,250,0);
60-
public static final Color8Bit ejectColor = new Color8Bit(250,250,0);
61-
62-
public static final Color8Bit intakeColor = new Color8Bit(154,0,255); //intake detection = purple
63-
public static final Color8Bit outtakeColor = new Color8Bit(0,9,255); //outtake detection = blue
64-
public static final Color8Bit intakeouttakeColor = new Color8Bit(0,255,9); //intake and outtake (both) = green
65-
//Red when nothing, purple/blue when intake/outtake detect only, green when both
66-
67-
68-
//for weird alex stuf
69-
public static Color8Bit startingColor = new Color8Bit(255,0,0);
70-
51+
52+
53+
public static final Color8Bit DEFAULT_COLOR_BLUE = new Color8Bit(0, 0, 200);
54+
public static final Color8Bit DETECT_NOTE_YELLOW = new Color8Bit(255,255,0);
55+
public static final Color8Bit HOLDING_GREEN = new Color8Bit(0,250,0);
56+
public static final int ledPort = 0;
57+
//TODO: figure out how to get port of LED, it could be 0 or
7158
}
7259

60+
7361
public static final class Effectorc {
7462
// PID values
7563

@@ -222,11 +210,11 @@ public static final class Drivetrainc {
222210
// seconds it takes to go from 0 to 12 volts(aka MAX)
223211
public static final double secsPer12Volts = 0.1;
224212

225-
// maxRCW is the angular velocity of the robot.
226-
// Calculated by looking at one of the motors and treating it as a point mass moving around in a circle.
227-
// Tangential speed of this point mass is maxSpeed and the radius of the circle is sqrt((wheelBase/2)^2 + (trackWidth/2)^2)
228-
// Angular velocity = Tangential speed / radius
229-
public static final double maxRCW = maxSpeed / swerveRadius;
213+
// maxRCW is the angular velocity of the robot.
214+
// Calculated by looking at one of the motors and treating it as a point mass moving around in a circle.
215+
// Tangential speed of this point mass is maxSpeed and the radius of the circle is sqrt((wheelBase/2)^2 + (trackWidth/2)^2)
216+
// Angular velocity = Tangential speed / radius
217+
public static final double maxRCW = maxSpeed / swerveRadius;
230218

231219
public static final boolean[] reversed = {false, false, false, false};
232220
// public static final boolean[] reversed = {true, true, true, true};
@@ -267,27 +255,27 @@ public static final class Drivetrainc {
267255
public static final double[] kForwardAccels = {1.1047/2, 0.79422/2, 0.77114/2, 1.1003/2};
268256
public static final double[] kBackwardAccels = kForwardAccels;
269257

270-
public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second
271-
public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2
272-
public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java
273-
// The maximum acceleration the robot can achieve is equal to the coefficient of static friction times the gravitational acceleration
274-
// a = mu * 9.8 m/s^2
275-
public static final double autoCentripetalAccel = mu * g * 2;
258+
public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second
259+
public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2
260+
public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java
261+
// The maximum acceleration the robot can achieve is equal to the coefficient of static friction times the gravitational acceleration
262+
// a = mu * 9.8 m/s^2
263+
public static final double autoCentripetalAccel = mu * g * 2;
276264

277-
public static final boolean isGyroReversed = true;
265+
public static final boolean isGyroReversed = true;
278266

279-
// PID values are listed in the order kP, kI, and kD
280-
public static final double[] xPIDController = {4, 0.0, 0.0};
281-
public static final double[] yPIDController = {4, 0.0, 0.0};
282-
public static final double[] thetaPIDController = {0.05, 0.0, 0.001};
267+
// PID values are listed in the order kP, kI, and kD
268+
public static final double[] xPIDController = {4, 0.0, 0.0};
269+
public static final double[] yPIDController = {4, 0.0, 0.0};
270+
public static final double[] thetaPIDController = {0.1, 0.0, 0.00};
283271

284272
public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, driveInversion, reversed, driveModifier, turnInversion);
285273

286-
public static final Transform limelightTransformForPoseEstimation = Transform.BOTPOSE_WPIBLUE;
274+
//public static final Limelight.Transform limelightTransformForPoseEstimation = Transform.BOTPOSE_WPIBLUE;
287275

288-
//#endregion
276+
//#endregion
289277

290-
//#region Ports
278+
//#region Ports
291279

292280
public static final int driveFrontLeftPort = 11; //
293281
public static final int driveFrontRightPort = 19; //
@@ -304,21 +292,21 @@ public static final class Drivetrainc {
304292
public static final int canCoderPortBL = 2;
305293
public static final int canCoderPortBR = 1;
306294

307-
//#endregion
295+
//#endregion
308296

309-
//#region Command Constants
297+
//#region Command Constants
310298

311-
public static double kNormalDriveSpeed = 1; // Percent Multiplier
312-
public static double kNormalDriveRotation = 0.5; // Percent Multiplier
313-
public static double kSlowDriveSpeed = 0.4; // Percent Multiplier
314-
public static double kSlowDriveRotation = 0.250; // Percent Multiplier
315-
public static double kAlignMultiplier = 1D/3D;
316-
public static final double kAlignForward = 0.6;
299+
public static double kNormalDriveSpeed = 1; // Percent Multiplier
300+
public static double kNormalDriveRotation = 0.5; // Percent Multiplier
301+
public static double kSlowDriveSpeed = 0.4; // Percent Multiplier
302+
public static double kSlowDriveRotation = 0.250; // Percent Multiplier
303+
public static double kAlignMultiplier = 1D/3D;
304+
public static final double kAlignForward = 0.6;
317305

318-
public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to orient the wheels to the correct angle. This should be very small to avoid actually moving the robot.
306+
public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to orient the wheels to the correct angle. This should be very small to avoid actually moving the robot.
319307

320-
public static final double[] positionTolerance = {Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5}; // Meters, Meters, Degrees
321-
public static final double[] velocityTolerance = {Units.inchesToMeters(1), Units.inchesToMeters(1), 5}; // Meters, Meters, Degrees/Second
308+
public static final double[] positionTolerance = {Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5}; // Meters, Meters, Degrees
309+
public static final double[] velocityTolerance = {Units.inchesToMeters(1), Units.inchesToMeters(1), 5}; // Meters, Meters, Degrees/Second
322310

323311
//#endregion
324312
//#region Subsystem Constants
@@ -341,22 +329,27 @@ public static final class Autoc {
341329
//#endregion
342330
}
343331

344-
public static final class Limelight {
332+
public static final class Limelightc {
345333
public static final String INTAKE_LL_NAME = "intake-limelight";
346334
public static final String SHOOTER_LL_NAME = "shooter-limelight";
347335

348-
public static final double ERROR_TOLERANCE = 0.1;
349-
public static final double HORIZONTAL_FOV_DEG = 0;
350-
public static final double RESOLUTION_WIDTH = 640;
351-
public static final double MOUNT_ANGLE_DEG = 46.2; //23.228
352-
public static final double HEIGHT_FROM_GROUND_METERS = Units.inchesToMeters(9); //16.6
353-
public static final double ARM_TO_OUTTAKE_OFFSET_DEG= 115;
354-
public static final class Apriltag {
355-
public static final int RED_SPEAKER_CENTER_TAG_ID = 4;
356-
public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7;
357-
public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(56.7); //88.125
358-
}
336+
public static final double ERROR_TOLERANCE_RAD = 0.1;
337+
public static final double HORIZONTAL_FOV_DEG = 0;
338+
public static final double RESOLUTION_WIDTH_PIX = 640;
339+
public static final double MOUNT_ANGLE_DEG_SHOOTER = 25; //23.228
340+
public static final double MOUNT_ANGLE_DEG_INTAKE = -22; //23.228
341+
public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(56); //16.6
342+
public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52); //16.6
343+
public static final double ARM_TO_OUTTAKE_OFFSET_DEG= 115;
344+
public static final double NOTE_HEIGHT = Units.inchesToMeters(0);
345+
public static final double MIN_MOVEMENT_METERSPSEC = 0.5;
346+
public static final double MIN_MOVEMENT_RADSPSEC = 0.5;
347+
public static final class Apriltag {
348+
public static final int RED_SPEAKER_CENTER_TAG_ID = 4;
349+
public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7;
350+
public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(56.7); //88.125
359351
}
352+
}
360353

361354
public static final class OI {
362355
public static final class Driver {
@@ -397,6 +390,7 @@ public static final class Manipulator {
397390
public static final int RAISE_CLIMBER = Button.kA.value;
398391
public static final int LOWER_CLIMBER = Button.kY.value;
399392
}
393+
400394
public static final double JOY_THRESH = 0.01;
401395
public static final double MIN_AXIS_TRIGGER_VALUE = 0.2;//woah, this is high.
402396
}

‎src/main/java/org/carlmontrobotics/RobotContainer.java‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -21,12 +21,11 @@
2121
import org.json.simple.parser.JSONParser;
2222
//199 files
2323
import org.carlmontrobotics.subsystems.*;
24+
import org.carlmontrobotics.subsystems.Led;
2425
import org.carlmontrobotics.commands.*;
2526

2627
import static org.carlmontrobotics.Constants.OI;
27-
import static org.carlmontrobotics.Constants.Armc.AMP_ANGLE_RAD;
28-
import static org.carlmontrobotics.Constants.Armc.GROUND_INTAKE_POS;
29-
import static org.carlmontrobotics.Constants.Armc.HANG_ANGLE_RAD;
28+
import static org.carlmontrobotics.Constants.Armc.*;
3029
import static org.carlmontrobotics.Constants.OI.Manipulator.*;
3130

3231
import edu.wpi.first.math.geometry.Pose2d;
@@ -79,11 +78,11 @@ public class RobotContainer {
7978
// 2. Use absolute paths from constants to reduce confusion
8079
public final GenericHID driverController = new GenericHID(OI.Driver.port);
8180
public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port);
82-
8381
private final IntakeShooter intakeShooter = new IntakeShooter();
82+
private final Led led = new Led(intakeShooter);
8483
private final Arm arm = new Arm();
8584
private final Drivetrain drivetrain = new Drivetrain();
86-
private final AuxSystems auxSystems = new AuxSystems(arm, intakeShooter);
85+
private final Limelight limelight = new Limelight(drivetrain);
8786

8887
/* These must be equal to the pathPlanner path names from the GUI! */
8988
// Order matters - but the first one is index 1 on the physical selector - index 0 is reserved for null command.
@@ -145,7 +144,9 @@ private void setDefaultCommands() {
145144
}
146145
private void setBindingsDriver() {
147146
new JoystickButton(driverController, Driver.resetFieldOrientationButton).onTrue(new InstantCommand(drivetrain::resetFieldOrientation));
148-
147+
new JoystickButton(driverController, 1).whileTrue(new AlignToApriltag(drivetrain)); //button A
148+
new JoystickButton(driverController, 2).whileTrue(new AlignToNote(drivetrain)); //button b?
149+
new JoystickButton(driverController, 3).whileTrue(new AutoMATICALLYGetNote(drivetrain, limelight)); //button x?
149150
// new JoystickButton(driverController, OI.Driver.slowDriveButton).onTrue(new ParallelCommandGroup(
150151
// new InstantCommand(()->drivetrain.setFieldOriented(false)),
151152
// new PrintCommand("Setting to ROBOT ORIENTED!!\nRO\nRO\nRO\n"))

‎src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44

55
package org.carlmontrobotics.commands;
66

7-
import static org.carlmontrobotics.Constants.Limelight.*;
7+
import static org.carlmontrobotics.Constants.Limelightc.*;
88
import org.carlmontrobotics.subsystems.Drivetrain;
99
import edu.wpi.first.math.geometry.Rotation2d;
1010
import edu.wpi.first.wpilibj2.command.ProxyCommand;

‎src/main/java/org/carlmontrobotics/commands/AlignToNote.java‎

Lines changed: 47 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -4,20 +4,55 @@
44

55
package org.carlmontrobotics.commands;
66

7-
import static org.carlmontrobotics.Constants.Limelight.*;
87
import org.carlmontrobotics.subsystems.Drivetrain;
9-
import edu.wpi.first.math.geometry.Rotation2d;
10-
import edu.wpi.first.wpilibj2.command.ProxyCommand;
118
import org.carlmontrobotics.subsystems.LimelightHelpers;
129

13-
public class AlignToNote extends ProxyCommand {
10+
import edu.wpi.first.math.MathUtil;
11+
import edu.wpi.first.math.controller.PIDController;
12+
import edu.wpi.first.math.geometry.Rotation2d;
13+
import edu.wpi.first.util.sendable.SendableRegistry;
14+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
15+
import edu.wpi.first.wpilibj2.command.Command;
16+
import static org.carlmontrobotics.Constants.Drivetrainc.*;
17+
import static org.carlmontrobotics.Constants.Limelightc.INTAKE_LL_NAME;
18+
19+
public class AlignToNote extends Command {
20+
21+
public final TeleopDrive teleopDrive;
22+
public final Drivetrain drivetrain;
23+
24+
public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1], thetaPIDController[2]);
25+
26+
public AlignToNote(Drivetrain drivetrain) {
27+
this.drivetrain = drivetrain;
28+
this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand();
29+
30+
rotationPID.enableContinuousInput(-180, 180);
31+
Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()).minus(Rotation2d.fromDegrees(LimelightHelpers.getTX(INTAKE_LL_NAME)));
32+
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
33+
rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]);
34+
SendableRegistry.addChild(this, rotationPID);
35+
//SmartDashboard.pu
36+
37+
addRequirements(drivetrain);
38+
}
39+
40+
@Override
41+
public void execute() {
42+
Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()).minus(Rotation2d.fromDegrees(LimelightHelpers.getTX(INTAKE_LL_NAME)));
43+
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
44+
if (teleopDrive == null) drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading()));
45+
else {
46+
double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds();
47+
drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1], rotationPID.calculate(drivetrain.getHeading()));
48+
}
49+
}
1450

15-
public AlignToNote(Drivetrain dt) {
16-
super(() -> {
17-
Rotation2d fieldOrientedTargetAngle = Rotation2d.fromDegrees(LimelightHelpers.getTX(INTAKE_LL_NAME)).plus(Rotation2d.fromDegrees(dt.getHeading()));
18-
return new RotateToFieldRelativeAngle(fieldOrientedTargetAngle, dt);
19-
});
20-
super.addRequirements(dt);
21-
}
22-
//REMINDER TO UPLOAD SHOOTER LIMELIGHT WITH THE PIPELINE MODEL :D
51+
@Override
52+
public boolean isFinished() {
53+
return false;
54+
// SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint());
55+
// SmartDashboard.putNumber("Error", rotationPID.getPositionError());
56+
// return rotationPID.atSetpoint();
57+
}
2358
}

‎src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package org.carlmontrobotics.commands;
6+
7+
import static org.carlmontrobotics.Constants.Limelightc.*;
8+
9+
import org.carlmontrobotics.Constants.*;
10+
import org.carlmontrobotics.subsystems.*;
11+
12+
import edu.wpi.first.math.geometry.Rotation2d;
13+
import edu.wpi.first.math.util.Units;
14+
import edu.wpi.first.wpilibj.Timer;
15+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
16+
import edu.wpi.first.wpilibj2.command.Command;
17+
import edu.wpi.first.wpilibj2.command.InstantCommand;
18+
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
19+
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
20+
import edu.wpi.first.wpilibj2.command.ProxyCommand;
21+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
22+
import edu.wpi.first.wpilibj2.command.WaitCommand;
23+
24+
public class AutoMATICALLYGetNote extends Command {
25+
/** Creates a new AutoMATICALLYGetNote. */
26+
private Drivetrain dt;
27+
//private IntakeShooter effector;
28+
private Limelight ll;
29+
private Timer timer = new Timer();
30+
31+
public AutoMATICALLYGetNote(Drivetrain dt, Limelight ll /*IntakeShooter effector*/) {
32+
addRequirements(this.dt = dt);
33+
addRequirements(this.ll = ll);
34+
//addRequirements(this.effector = effector);
35+
// Use addRequirements() here to declare subsystem dependencies.
36+
}
37+
38+
@Override
39+
public void initialize() {
40+
timer.reset();
41+
timer.start();
42+
//new Intake().finallyDo(()->{this.end(false);});
43+
SmartDashboard.putBoolean("end", false);
44+
dt.setFieldOriented(false);
45+
}
46+
47+
@Override
48+
public void execute() {
49+
double angleErrRad = Units.degreesToRadians(LimelightHelpers.getTX(Limelightc.INTAKE_LL_NAME));
50+
double forwardDistErrMeters = ll.getDistanceToNote();
51+
double strafeDistErrMeters = forwardDistErrMeters * Math.tan(angleErrRad);
52+
// dt.drive(0,0,0);
53+
dt.drive(Math.max(forwardDistErrMeters*2, MIN_MOVEMENT_METERSPSEC), Math.max(strafeDistErrMeters*2, MIN_MOVEMENT_METERSPSEC), Math.max(angleErrRad*2,MIN_MOVEMENT_RADSPSEC));
54+
//180deg is about 6.2 rad/sec, min is .5rad/sec
55+
}
56+
57+
// Called once the command ends or is interrupted.
58+
@Override
59+
public void end(boolean interrupted) {
60+
dt.setFieldOriented(true);
61+
SmartDashboard.putBoolean("end", true);
62+
}
63+
64+
// Returns true when the command should end.
65+
@Override
66+
public boolean isFinished() {
67+
return false;
68+
//return timer.get() >= 0.5;
69+
}
70+
}

0 commit comments

Comments
 (0)