diff --git a/src/main/deploy/pathplanner/paths/TirerSimple.path b/src/main/deploy/pathplanner/paths/TirerSimple.path index b039179..60f5642 100644 --- a/src/main/deploy/pathplanner/paths/TirerSimple.path +++ b/src/main/deploy/pathplanner/paths/TirerSimple.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 5.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index dcaad73..f6fcc16 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -16,12 +16,12 @@ "robotMass": 51.673, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, - "maxDriveSpeed": 5.374, + "driveWheelRadius": 0.099, + "driveGearing": 6.2, + "maxDriveSpeed": 9.82, "driveMotorType": "krakenX60", - "driveCurrentLimit": 60.0, - "wheelCOF": 1.2, + "driveCurrentLimit": 120.0, + "wheelCOF": 1.1, "flModuleX": 0.288925, "flModuleY": 0.269875, "frModuleX": 0.288925, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c7be8a6..f40b094 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,7 +20,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.Aspirer; import frc.robot.commands.DescendreBalyeuse; import frc.robot.commands.DescendreGrimpeur; -import frc.robot.commands.DistanceLancer; import frc.robot.commands.Lancer; import frc.robot.commands.LancerAspirer; import frc.robot.commands.LancerBaseVitesse; @@ -94,7 +93,6 @@ public class RobotContainer { } private void configureBindings() { - led.setDefaultCommand(new DistanceLancer(limeLight3G, led)); drivetrain.setDefaultCommand( drivetrain.applyRequest(() -> drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed, 0.05)) diff --git a/src/main/java/frc/robot/commands/DistanceLancer.java b/src/main/java/frc/robot/commands/DistanceLancer.java deleted file mode 100644 index 2b7522f..0000000 --- a/src/main/java/frc/robot/commands/DistanceLancer.java +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Led; -import frc.robot.subsystems.Limelight3G; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class DistanceLancer extends Command { - private Led led; - private Limelight3G limelight3g; - /** Creates a new DitanceLancer. */ - public DistanceLancer(Limelight3G limelight3g, Led led) { - this.led = led; - this.limelight3g = limelight3g; - addRequirements(led,limelight3g); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double botx = 0; - double boty = 0; - double[] BotPose = new double[6]; - double distance = 0; - if(limelight3g.getV()){ - BotPose = limelight3g.getBotPoseBlue(); - botx = BotPose[0]; - boty = BotPose[1]; - } - if(limelight3g.getV()){ - distance = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)); - } - if(distance > 1.7){ - led.Jaune2(); - } - else{ - - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index 1504d1f..ff3ca28 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -20,7 +20,7 @@ public class Lancer extends Command { public Lancer(Lanceur lanceur, Limelight3G limeLight3G) { this.lanceur = lanceur; this.timer = new Timer(); - this.limeLight3G = new Limelight3G(); + this.limeLight3G = limeLight3G; addRequirements(lanceur, limeLight3G); // Use addRequirements() here to declare subsystem dependencies. } diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index 9d35e05..97a882e 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -61,12 +61,12 @@ public class Limelighter extends Command { boty = BotPose[0]; angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); if(angle >180){ - angle =- 360; + angle -= 360; } - calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5; + calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 10; drivetrain.setControl( drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); - System.out.println(calcul); + System.out.println(angle); if (calcul < 0.2 && calcul > -0.2) { drivetrain.setControl(drive.withRotationalRate(0)); } diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 970ae28..132980c 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -7,16 +7,18 @@ package frc.robot.commands.ModeAuto; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; -import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Limelight3G; import static edu.wpi.first.units.Units.*; +import java.util.Optional; + /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class LimelighterAuto extends Command { - Timer timer; Limelight3G limelight3g; CommandSwerveDrivetrain drivetrain; private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); @@ -25,6 +27,7 @@ public class LimelighterAuto extends Command { double boty; double angle; double calcul; + Optional alliance = DriverStation.getAlliance(); private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); @@ -34,39 +37,37 @@ public class LimelighterAuto extends Command { this.limelight3g = limelight3g; this.drivetrain = drivetrain; addRequirements(drivetrain, limelight3g); - timer = new Timer(); // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override - public void initialize() { - timer.reset(); - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { double[] BotPose = new double[6]; + System.out.println("e"); if (limelight3g.getV()) { - BotPose = limelight3g.getBotPoseBlue(); + if(alliance.get() == Alliance.Blue){ + BotPose = limelight3g.getBotPoseBlue(); + } + else{ + BotPose = limelight3g.getBotPoseRed(); + } + botx = BotPose[1]; boty = BotPose[0]; angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); if(angle >180){ - angle =- 360; + angle -= 360; } calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5; drivetrain.setControl( drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); System.out.println(calcul); if (calcul < 0.2 && calcul > -0.2) { - // timer.start(); - // } else { - // timer.reset(); - // } - // } else { - // timer.stop(); drivetrain.setControl(drive.withRotationalRate(0)); } } @@ -80,7 +81,6 @@ public class LimelighterAuto extends Command { @Override public void end(boolean interrupted) { drivetrain.setControl(drive.withRotationalRate(0)); - timer.stop(); } // Returns true when the command should end. @@ -89,3 +89,4 @@ public class LimelighterAuto extends Command { return calcul < 0.1 && calcul > -0.1; } } + diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 7f44643..9b70aea 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -248,12 +248,12 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su @Override public void periodic() { - // if(getPigeon2().getYaw().getValueAsDouble() > 360){ - // getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()-360); - // } - // else if(getPigeon2().getYaw().getValueAsDouble() < 0){ - // getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()+360); - // } + if(getPigeon2().getYaw().getValueAsDouble() > 360){ + getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()-360); + } + else if(getPigeon2().getYaw().getValueAsDouble() < 0){ + getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()+360); + } /* * Periodically try to apply the operator perspective.