Skip to content

Commit

Permalink
Implementing limelight pose + adjust speeds for direct drive shooter
Browse files Browse the repository at this point in the history
  • Loading branch information
@bobopop787
bobopop787 committed Mar 16, 2024
1 parent 0dcf049 commit 3dff3e5
Show file tree
Hide file tree
Showing 12 changed files with 977 additions and 67 deletions.
11 changes: 4 additions & 7 deletions simgui.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,18 +11,12 @@
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo",
"/Shuffleboard/Autos/Auto": "String Chooser",
"/SmartDashboard/Auto Visualization": "Field2d",
"/SmartDashboard/Field": "Field2d",
"/SmartDashboard/Swerve Visualization": "Mechanism2d",
"/SmartDashboard/Trajectory Field": "Field2d"
},
"windows": {
"/Shuffleboard/Autos/Auto": {
"window": {
"visible": true
}
},
"/SmartDashboard/Auto Visualization": {
"bottom": 1476,
"height": 8.210550308227539,
Expand Down Expand Up @@ -156,7 +150,10 @@
}
]
}
]
],
"window": {
"visible": false
}
}
}
}
6 changes: 3 additions & 3 deletions src/main/deploy/pathplanner/paths/New Path.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,11 +33,11 @@
{
"anchor": {
"x": 4.429102153502073,
"y": 2.0376855589664813
"y": 2.0611072320575454
},
"prevControl": {
"x": 4.475945499685199,
"y": 3.4195642713687
"x": 4.429102153502073,
"y": 3.443779671528314
},
"nextControl": null,
"isLocked": false,
Expand Down
22 changes: 11 additions & 11 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,14 +29,14 @@ public class RobotContainer {
private final CommandXboxController altController;
private final SwerveDrive swerveDrive;
private final Shooter shooter;
private final Intake intake;
// private final Intake intake;

public RobotContainer() {
Preferences.removeAll();

swerveDrive = new SwerveDrive();
shooter = new Shooter();
intake = new Intake();
// intake = new Intake();

driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort);
altController = new CommandXboxController(OperatorConstants.kAuxControllerPort);
Expand Down Expand Up @@ -91,21 +91,21 @@ private void configureBindings() {
altController.y().onTrue(shooter.runSetWristPosition(ShooterPosition.AMP));
altController.a().onTrue(shooter.runSetWristPosition(ShooterPosition.SOURCE));
altController.b().onTrue(shooter.runSetWristPosition(ShooterPosition.FLAT));
altController.rightStick().onTrue(shooter.runShooter(20));
altController.rightStick().onTrue(shooter.runShooter(10));
altController.rightStick().onFalse(shooter.runShooter(0));
altController.leftBumper().onTrue(shooter.runIndex(-15));
altController.leftBumper().onFalse(shooter.runIndex(0));
altController.leftTrigger().onTrue(shooter.runIndex(15));
altController.leftTrigger().onFalse(shooter.runIndex(0));

altController.povRight().onTrue(intake.runDeploy(-5));
altController.povRight().onFalse(intake.runDeploy(0));
altController.povLeft().onTrue(intake.runDeploy(5));
altController.povLeft().onFalse(intake.runDeploy(0));
altController.rightBumper().onTrue(intake.runRoller(-16));
altController.rightBumper().onFalse(intake.runRoller(0));
altController.rightTrigger().onTrue(intake.runRoller(16));
altController.rightTrigger().onFalse(intake.runRoller(0));
// altController.povRight().onTrue(intake.runDeploy(-5));
// altController.povRight().onFalse(intake.runDeploy(0));
// altController.povLeft().onTrue(intake.runDeploy(5));
// altController.povLeft().onFalse(intake.runDeploy(0));
// altController.rightBumper().onTrue(intake.runRoller(-16));
// altController.rightBumper().onFalse(intake.runRoller(0));
// altController.rightTrigger().onTrue(intake.runRoller(16));
// altController.rightTrigger().onFalse(intake.runRoller(0));
}

public Command getAutonomousCommand() {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/Autos.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ public static Trajectory getPathPlannerTrajectory(String filename, Rotation2d st
public static Command runSpeakerShot(Shooter shooter) {
return Commands.sequence(
shooter.runSetWristPosition(ShooterPosition.SPEAKER),
shooter.runShooter(23),
shooter.runShooter(10),
new WaitCommand(1.5),
shooter.runIndex(-15),
new WaitCommand(1.7),
Expand Down
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/commands/DriveTrajectory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,8 +105,10 @@ public void execute() {
desiredChassisSpeeds.omegaRadiansPerSecond
// Math.max(desiredChassisSpeeds.omegaRadiansPerSecond, SwerveConstants.kRotVelLimit
);
SmartDashboard.putNumber("auto/rotSpeed", desiredChassisSpeeds.omegaRadiansPerSecond);
SwerveModuleState[] states = SwerveConstants.kSwerveKinematics.toSwerveModuleStates(updatedChassisSpeeds);
swerveDrive.rawModuleInputs(states, false);
// swerveDrive.resetPose(currentState.poseMeters);

// only needed for simulating
if(curTime > trajectory.getTotalTimeSeconds() + 1) {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/auto/NothingAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ public class NothingAuto extends AutoCommand {
private Rotation2d startingRot;

public NothingAuto(SwerveDrive swerve) {
this(swerve, Rotation2d.fromDegrees(180));
this(swerve, Rotation2d.fromDegrees(179));
}

public NothingAuto(SwerveDrive swerve, Rotation2d startingRot) {
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/subsystems/shooter/Shooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@ public void setIndexVolts(double volts) {
}

public void setShootVolts(double volts) {
shooterLowerMotor.setVoltage(volts * speedFactor * 1.1);
shooterLowerMotor.setVoltage(volts * speedFactor * 1.2);
shooterUpperMotor.setVoltage(-volts * speedFactor);
}

Expand Down Expand Up @@ -228,13 +228,13 @@ public void periodic() {

switch(shooterPosition) {
case SOURCE:
speedFactor = 0.2;
speedFactor = 0.5;
break;
case SPEAKER:
speedFactor = -1;
break;
case AMP:
speedFactor = 0.5; // 0.3
speedFactor = 0.7; // 0.3
break;
default:
speedFactor = -0.04;
Expand Down
63 changes: 54 additions & 9 deletions src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
@@ -1,12 +1,14 @@
package frc.robot.subsystems.swerve;

import java.util.Arrays;
import java.util.Optional;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;

import com.ctre.phoenix6.hardware.Pigeon2;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand All @@ -33,6 +35,7 @@
import frc.robot.Constants.SwerveConstants;
import frc.robot.commands.Autos;
import frc.robot.commands.DriveTrajectory;
import frc.robot.util.LimelightHelpers;
import frc.robot.util.MechanismLigament2dWrapper;
import frc.robot.util.MotorPID;
import frc.robot.util.RateLimiter;
Expand All @@ -50,6 +53,8 @@ public class SwerveDrive extends SubsystemBase {
private SDSSwerveModule backRightModule;
private SDSSwerveModule[] modules;

private static final NetworkTable limelightNT = NetworkTableInstance.getDefault().getTable("limelight");

private Pigeon2 gyro;

private SwerveDriveKinematics swerveKinematics;
Expand Down Expand Up @@ -83,6 +88,7 @@ public class SwerveDrive extends SubsystemBase {
private double speedFactor;

private Field2d field;
private Field2d limelightField;

public SwerveDrive() {
initComponents();
Expand Down Expand Up @@ -164,10 +170,10 @@ private void initMathModels() {
// presetRotationPID.setTolerance(5);
presetRotationPID.enableContinuousInput(-180, 180);

speakerPosPID = new MotorPID(1.5, 0, 0);
speakerPosPID = new MotorPID(10, 0, 0);
speakerPosPID.setSetpoint(5.54);

ampPosPID = new MotorPID(1.5, 0, 0);
ampPosPID = new MotorPID(10, 0, 0);
ampPosPID.setSetpoint(DriverStation.getAlliance().equals(Optional.of(Alliance.Blue))? 1.9 : 16.542 - 1.9);

SwerveConstants.kSwerveKinematics.toSwerveModuleStates(new ChassisSpeeds(0, 0.01, 0));
Expand Down Expand Up @@ -202,6 +208,7 @@ private void initSimulations() {
rightFrame.append(backRightDirLigament.ligament);

field = new Field2d();
limelightField = new Field2d();

field.setRobotPose(new Pose2d(1, 1, new Rotation2d(0)));

Expand Down Expand Up @@ -327,13 +334,13 @@ public void adjustedDriveInputs(
private void addPresetRotDriveInputs(double xSpeed, double ySpeed, double rotSpeed, boolean noOptimize, boolean robotCentric) {
if(goSpeakerRot.getAsBoolean()) {
presetRotationPID.setSetpoint(180);
presetRotDriveInputs(xSpeed, ySpeed, noOptimize, robotCentric);
presetRotDriveInputs(xSpeed, ySpeed, noOptimize, false);
} else if(goAmpRot.getAsBoolean()) {
presetRotationPID.setSetpoint(DriverStation.getAlliance().equals(Optional.of(Alliance.Blue)) ? 90 : -90);
presetRotDriveInputs(xSpeed, ySpeed, noOptimize, robotCentric);
presetRotDriveInputs(xSpeed, ySpeed, noOptimize, false);
} else if(goSourceRot.getAsBoolean()) {
presetRotationPID.setSetpoint(DriverStation.getAlliance().equals(Optional.of(Alliance.Blue)) ? -60 : 60);
presetRotDriveInputs(xSpeed, ySpeed, noOptimize, robotCentric);
presetRotDriveInputs(xSpeed, ySpeed, noOptimize, false);
} else if(goAimedSpeaker.getAsBoolean()) {
double xOff = swerveOdometry.getEstimatedPosition().getY() - 5.54;
double yOff = swerveOdometry.getEstimatedPosition().getX() - (DriverStation.getAlliance().equals(Optional.of(Alliance.Blue)) ? -0.5 : 16.542 + 0.5);
Expand All @@ -353,17 +360,25 @@ private void presetRotDriveInputs(double rawXSpeed, double rawYSpeed, boolean no
private void addPresetPositionDriveInputs(double xSpeed, double ySpeed, double rotSpeed, boolean noOptimize, boolean robotCentric) {
// amp: blue 1.9, red 16.542 - 1.9 (meters)
// speaker: 5.54 (meters)
// System.out.println("e");
if(robotCentric) {
rawDriveInputs(xSpeed, ySpeed, rotSpeed, noOptimize, robotCentric);
return;
}
if(goAmpPos.getAsBoolean()) {
ampPosPID.setSetpoint(DriverStation.getAlliance().equals(Optional.of(Alliance.Blue))? 1.9 : 16.542 - 1.9);
rawDriveInputs(xSpeed, ampPosPID.calculate(swerveOdometry.getEstimatedPosition().getX()), rotSpeed, noOptimize, robotCentric);
double output = ampPosPID.calculate(swerveOdometry.getEstimatedPosition().getX());
output = MathUtil.clamp(output, -8, 8);
rawDriveInputs(xSpeed, output, rotSpeed, noOptimize, false);
} else if(goSpeakerPos.getAsBoolean()) {
rawDriveInputs(speakerPosPID.calculate(swerveOdometry.getEstimatedPosition().getY()), ySpeed, rotSpeed, noOptimize, robotCentric);
// System.out.println(-speakerPosPID.calculate(swerveOdometry.getEstimatedPosition().getY()));
double output = speakerPosPID.calculate(swerveOdometry.getEstimatedPosition().getY());
if(DriverStation.getAlliance().equals(Optional.of(Alliance.Blue))) output = -output;
output = MathUtil.clamp(output, -8, 8);
rawDriveInputs(output, ySpeed, rotSpeed, noOptimize, false);
} else {
rawDriveInputs(xSpeed, ySpeed, rotSpeed, noOptimize, robotCentric);
}
rawDriveInputs(xSpeed, ySpeed, rotSpeed, noOptimize, robotCentric);
}

public void rawDriveInputs(double rawXSpeed, double rawYSpeed, double rawRotSpeed, boolean noOptimize, boolean robotCentric) {
Expand Down Expand Up @@ -435,7 +450,7 @@ public void resetPose(Pose2d pose) {
public Command runTestDrive() {
return runOnce(() -> {
SwerveModuleState testSwerveState = new SwerveModuleState(Preferences.getDouble("kSwerveTestDrive", SwerveConstants.kDefaultTestDrive),
new Rotation2d(Preferences.getDouble("kSwerveTestTurn", SwerveConstants.kDefaultTestTurn))
new Rotation2d(Preferences.getDouble("kSwerveTestTurn", SwerveConstants.kDefaultTestTurn))
);
frontLeftModule.setDesiredSwerveState(testSwerveState);
// frontRightModule.setDesiredSwerveState(testSwerveState);
Expand All @@ -455,6 +470,7 @@ public Command runStopDrive() {

public Command runZeroGyro() {
return runOnce(() -> {
gyro.setYaw(0);
gyro.reset();
});
}
Expand Down Expand Up @@ -504,6 +520,13 @@ public Rotation2d getRotation2d() {
public ChassisSpeeds getRobotRelativeChassisSpeeds() {
return SwerveConstants.kSwerveKinematics.toChassisSpeeds(getModuleStates());
}

public Pose2d getLimelightPose(double[] poseArray) {
double x = poseArray[0];
double y = poseArray[1];
Rotation2d theta = Rotation2d.fromDegrees(poseArray[5]);
return new Pose2d(x, y, theta);
}

@Override
public void periodic() {
Expand All @@ -519,16 +542,38 @@ public void periodic() {

SmartDashboard.putNumber("Gyro", -gyro.getAngle());

double[] limelightPoseArr = limelightNT.getEntry("botpose_wpiblue").getDoubleArray(new double[]{});
// System.out.println(Arrays.toString(limelightPoseArr));
if(limelightPoseArr[7] == 1) {
// System.out.println("1 tag");
swerveOdometry.setVisionMeasurementStdDevs(VecBuilder.fill(6,6,1.5));
swerveOdometry.addVisionMeasurement(
getLimelightPose(limelightPoseArr),
LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight").timestampSeconds
);

} else if(limelightPoseArr[7] >= 2) {
// System.out.println("> 2 tags");
swerveOdometry.setVisionMeasurementStdDevs(VecBuilder.fill(3, 3, 1.5));
swerveOdometry.addVisionMeasurement(
getLimelightPose(limelightPoseArr),
LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight").timestampSeconds
);
}

Rotation2d robotRotation = gyro.getRotation2d();
if(DriverStation.getAlliance().equals(Optional.of(Alliance.Blue))) robotRotation = robotRotation.rotateBy(Rotation2d.fromDegrees(180));
swerveOdometry.update(
robotRotation,
getModulePositions()
);
field.setRobotPose(swerveOdometry.getEstimatedPosition());
limelightField.setRobotPose(getLimelightPose(limelightPoseArr));

swerveDriveNT.getEntry("poseX").setDouble(swerveOdometry.getEstimatedPosition().getX());
swerveDriveNT.getEntry("poseY").setDouble(swerveOdometry.getEstimatedPosition().getY());
SmartDashboard.putData("Field", field);
SmartDashboard.putData("Limelight Estimated Pose", limelightField);
SmartDashboard.putData("Auto Visualization", Autos.autoField);
DriveTrajectory.updateField();
}
Expand Down
Loading

0 comments on commit 3dff3e5

Please sign in to comment.