Skip to content

Commit 8ef3910

Browse files
stwiggystwiggy
committed
Merge branch 'limelight' into bangbot
2 parents 48638f9 + 152c3a5 commit 8ef3910

File tree

9 files changed

+304
-79
lines changed

9 files changed

+304
-79
lines changed

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

Lines changed: 47 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -210,11 +210,11 @@ public static final class Drivetrainc {
210210
// seconds it takes to go from 0 to 12 volts(aka MAX)
211211
public static final double secsPer12Volts = 0.1;
212212

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;
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;
218218

219219
public static final boolean[] reversed = {false, false, false, false};
220220
// public static final boolean[] reversed = {true, true, true, true};
@@ -255,27 +255,27 @@ public static final class Drivetrainc {
255255
public static final double[] kForwardAccels = {1.1047/2, 0.79422/2, 0.77114/2, 1.1003/2};
256256
public static final double[] kBackwardAccels = kForwardAccels;
257257

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;
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;
264264

265-
public static final boolean isGyroReversed = true;
265+
public static final boolean isGyroReversed = true;
266266

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.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};
271271

272272
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);
273273

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

276-
//#endregion
276+
//#endregion
277277

278-
//#region Ports
278+
//#region Ports
279279

280280
public static final int driveFrontLeftPort = 11; //
281281
public static final int driveFrontRightPort = 19; //
@@ -292,21 +292,21 @@ public static final class Drivetrainc {
292292
public static final int canCoderPortBL = 2;
293293
public static final int canCoderPortBR = 1;
294294

295-
//#endregion
295+
//#endregion
296296

297-
//#region Command Constants
297+
//#region Command Constants
298298

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;
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;
305305

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.
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.
307307

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
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
310310

311311
//#endregion
312312
//#region Subsystem Constants
@@ -329,22 +329,27 @@ public static final class Autoc {
329329
//#endregion
330330
}
331331

332-
public static final class Limelight {
332+
public static final class Limelightc {
333333
public static final String INTAKE_LL_NAME = "intake-limelight";
334334
public static final String SHOOTER_LL_NAME = "shooter-limelight";
335335

336-
public static final double ERROR_TOLERANCE = 0.1;
337-
public static final double HORIZONTAL_FOV_DEG = 0;
338-
public static final double RESOLUTION_WIDTH = 640;
339-
public static final double MOUNT_ANGLE_DEG = 46.2; //23.228
340-
public static final double HEIGHT_FROM_GROUND_METERS = Units.inchesToMeters(9); //16.6
341-
public static final double ARM_TO_OUTTAKE_OFFSET_DEG= 115;
342-
public static final class Apriltag {
343-
public static final int RED_SPEAKER_CENTER_TAG_ID = 4;
344-
public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7;
345-
public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(56.7); //88.125
346-
}
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
347351
}
352+
}
348353

349354
public static final class OI {
350355
public static final class Driver {

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

Lines changed: 6 additions & 5 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;
@@ -64,7 +63,6 @@
6463
import com.pathplanner.lib.path.PathPlannerPath;
6564
import static com.pathplanner.lib.auto.AutoBuilder.*;
6665
import com.pathplanner.lib.auto.AutoBuilder;
67-
import org.carlmontrobotics.subsystems.Led;
6866

6967
//java
7068
import java.util.function.DoubleSupplier;
@@ -84,6 +82,7 @@ public class RobotContainer {
8482
private final Led led = new Led(intakeShooter);
8583
private final Arm arm = new Arm();
8684
private final Drivetrain drivetrain = new Drivetrain();
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+
}

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

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
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 org.carlmontrobotics.Constants.Limelightc;
8+
import org.carlmontrobotics.subsystems.Drivetrain;
9+
import org.carlmontrobotics.subsystems.Limelight;
10+
import org.carlmontrobotics.subsystems.LimelightHelpers;
11+
12+
import edu.wpi.first.math.util.Units;
13+
import edu.wpi.first.wpilibj.Timer;
14+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
15+
import edu.wpi.first.wpilibj2.command.Command;
16+
17+
public class MoveToNote extends Command {
18+
private final Drivetrain dt;
19+
private final Limelight ll;
20+
private Timer timer = new Timer();
21+
/** Creates a new MoveToNote. */
22+
public MoveToNote(Drivetrain dt, Limelight ll) {
23+
// Use addRequirements() here to declare subsystem dependencies.
24+
addRequirements(this.dt=dt);
25+
addRequirements(this.ll=ll);
26+
}
27+
28+
// Called when the command is initially scheduled.
29+
@Override
30+
public void initialize() {
31+
new AlignToNote(dt);
32+
timer.reset();
33+
timer.start();
34+
//new Intake().finallyDo(()->{this.end(false);});
35+
SmartDashboard.putBoolean("end", false);
36+
dt.setFieldOriented(false);
37+
}
38+
39+
// Called every time the scheduler runs while the command is scheduled.
40+
@Override
41+
public void execute() {
42+
double radErr = Units.degreesToRadians(LimelightHelpers.getTX(Limelightc.INTAKE_LL_NAME));
43+
double distErr = ll.getDistanceToNote(); //meters
44+
double forwardErr = distErr * Math.cos(radErr);
45+
// dt.drive(0,0,0);
46+
dt.drive(Math.max(forwardErr*2, .5), 0, 0);
47+
//180deg is about 6.2 rad/sec, min is .5rad/sec
48+
}
49+
50+
// Called once the command ends or is interrupted.
51+
@Override
52+
public void end(boolean interrupted) {
53+
dt.setFieldOriented(true);
54+
}
55+
56+
// Returns true when the command should end.
57+
@Override
58+
public boolean isFinished() {
59+
return timer.get() >= 0.5;
60+
}
61+
}

0 commit comments

Comments
 (0)