diff --git a/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto b/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto new file mode 100644 index 0000000..2b0de62 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto @@ -0,0 +1,99 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "DescendreBalayeuse" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Depot" + } + }, + { + "type": "named", + "data": { + "name": "Aspirer" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Tir" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Lancer" + } + }, + { + "type": "named", + "data": { + "name": "Aspirer" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "GrimperReservoir" + } + }, + { + "type": "named", + "data": { + "name": "GrimperReservoir" + } + }, + { + "type": "named", + "data": { + "name": "MonterGrimpeur" + } + }, + { + "type": "named", + "data": { + "name": "gauche" + } + }, + { + "type": "named", + "data": { + "name": "DescendreGrimpeur" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto index 326fc3d..e157521 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto @@ -14,6 +14,12 @@ "type": "deadline", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, { "type": "path", "data": { @@ -72,9 +78,9 @@ } }, { - "type": "path", + "type": "named", "data": { - "pathName": "MonterMur" + "name": "droite" } }, { diff --git a/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto b/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto index 0f80659..62f3ffc 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto @@ -14,6 +14,12 @@ "type": "deadline", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, { "type": "path", "data": { @@ -72,9 +78,9 @@ } }, { - "type": "path", + "type": "named", "data": { - "pathName": "MonterReservoir" + "name": "gauche" } }, { diff --git a/src/main/deploy/pathplanner/paths/MonterReservoir.path b/src/main/deploy/pathplanner/paths/MonterReservoir.path index 64b0950..85ada95 100644 --- a/src/main/deploy/pathplanner/paths/MonterReservoir.path +++ b/src/main/deploy/pathplanner/paths/MonterReservoir.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.0502282453637657, - "y": 2.4758915834522113 + "x": 1.0, + "y": 2.6 }, "prevControl": null, "nextControl": { - "x": 1.0408884389937225, - "y": 2.742159765250622 + "x": 0.9906601936299568, + "y": 2.866268181798411 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.0502282453637657, - "y": 2.7734807417974334 + "x": 1.0, + "y": 2.9 }, "prevControl": { - "x": 1.0547546338516156, - "y": 2.523521721541598 + "x": 1.00452638848785, + "y": 2.6500409797441646 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 59e696e..f4d6ba0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ 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.Inverser; import frc.robot.commands.Lancer; import frc.robot.commands.LancerAspirer; import frc.robot.commands.LancerBaseVitesse; @@ -30,6 +31,8 @@ import frc.robot.commands.ModeOposerDemeleur; import frc.robot.commands.MonterBalyeuse; import frc.robot.commands.MonterGrimpeur; import frc.robot.commands.ModeAuto.AspirerAuto; +import frc.robot.commands.ModeAuto.BougerDroiteAuto; +import frc.robot.commands.ModeAuto.BougerGaucheAuto; import frc.robot.commands.ModeAuto.GrimperMur; import frc.robot.commands.ModeAuto.GrimperReservoir; import frc.robot.commands.ModeAuto.LancerAuto; @@ -73,6 +76,8 @@ public class RobotContainer { public RobotContainer() { + NamedCommands.registerCommand("droite", new BougerDroiteAuto(drivetrain)); + NamedCommands.registerCommand("gauche", new BougerGaucheAuto(drivetrain)); NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain)); NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain)); NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G)); @@ -126,6 +131,9 @@ public class RobotContainer { manette1.b().whileTrue(new ModeOposer(lanceur)); manette1.a().whileTrue(new ModeOposerDemeleur(lanceur)); manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse)); + manette1.povRight().whileTrue(new BougerDroiteAuto(drivetrain)); + manette1.povLeft().whileTrue(new BougerGaucheAuto(drivetrain)); + manette1.start().whileTrue(new Inverser(drivetrain)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Inverser.java b/src/main/java/frc/robot/commands/Inverser.java new file mode 100644 index 0000000..3b8f0a0 --- /dev/null +++ b/src/main/java/frc/robot/commands/Inverser.java @@ -0,0 +1,42 @@ +// 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.CommandSwerveDrivetrain; + +/* 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 Inverser extends Command { + CommandSwerveDrivetrain drivetrain; + boolean inverse = false; + /** Creates a new Inverser. */ + public Inverser(CommandSwerveDrivetrain drivetrain) { + addRequirements(drivetrain); + // 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() { + if(!inverse){ + drivetrain.getPigeon2().setYaw(drivetrain.getPigeon2().getYaw().getValueAsDouble()+180); + inverse = true; + } + } + + // 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 e29f8b1..ace73e9 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -48,7 +48,9 @@ public class Lancer extends Command { public void execute() { double[] BotPose = new double[6]; if(limeLight3G.getV()){ - if(!alliance.isPresent()){return;} + if(!alliance.isPresent()){ + return; + } if(alliance.get() == Alliance.Blue){ BotPose = limeLight3G.getBotPoseBlue(); } @@ -60,10 +62,11 @@ public class Lancer extends Command { vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250; } if(limeLight3G.getV()){ - System.out.println(vitesse); + double output = pidController.calculate(lanceur.Vitesse(),vitesse); lanceur.Lancer(output); + System.out.println(output); if(lanceur.Vitesse() >= vitesse-800){ timer.start(); if(timer.get() >1){ diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index 55c31d8..ea9be23 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -53,7 +53,7 @@ public class Limelighter extends Command { double[] BotPose = new double[6]; System.out.println("e"); if (limelight3g.getV()) { - // BotPose = limelight3g.getBotPoseBlue(); + BotPose = limelight3g.getBotPoseBlue(); if (!alliance.isPresent()) { return; } @@ -65,25 +65,24 @@ public class Limelighter extends Command { x = 11.915394; BotPose = limelight3g.getBotPoseRed(); } - botx = BotPose[1]; - boty = BotPose[0]; - angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); + botx = BotPose[0]; + boty = BotPose[1]; calcul = limelight3g.Calcule(botx, x, boty, 4, angle); - if(angle > 180){ - angle -= 360; - } if(calcul > -5 && calcul < 5){ drivetrain.setControl( drive.withRotationalRate(0)); } else if(calcul > 5){ drivetrain.setControl( - drive.withRotationalRate(0.5*(2*Math.PI))); + drive.withRotationalRate(0.5*(2*Math.PI))); } else if(calcul < -5){ - drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); + drivetrain.setControl(drive.withRotationalRate(-0.5*(2*Math.PI))); } + // botx = BotPose[1]; + // boty = BotPose[0]; + // angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); + // calcul = limelight3g.Calcule(botx, x, boty, 4, angle); // if(calcul < -5 && calcul > -180){ // drivetrain.setControl( // drive.withRotationalRate(0.5*(2*Math.PI))); @@ -92,7 +91,7 @@ public class Limelighter extends Command { // drivetrain.setControl( // drive.withRotationalRate(-0.5*(2*Math.PI))); // } - // else if(calcul >= 180){ + // else if(calcul < -5){ // drivetrain.setControl( // drive.withRotationalRate(-0.5*(2*Math.PI))); // } diff --git a/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java b/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java new file mode 100644 index 0000000..72952e7 --- /dev/null +++ b/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java @@ -0,0 +1,80 @@ +// 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.ModeAuto; + +import static edu.wpi.first.units.Units.*; + +import java.util.Optional; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +/* 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 BougerDroiteAuto extends Command { + CommandSwerveDrivetrain drivetrain; + Timer timer = new Timer(); + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + Optional alliance; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + /** Creates a new AvanceAuto. */ + public BougerDroiteAuto(CommandSwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + addRequirements(drivetrain); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + alliance = DriverStation.getAlliance(); + timer.reset(); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(alliance.get() == Alliance.Blue){ + if(timer.get() < .75){ + System.out.println("8765"); + drivetrain.setControl(drive.withVelocityY(-1.5)); + } + else{ + drivetrain.setControl(drive.withVelocityY(0)); + } + } + else{ + if(timer.get() < 0.75){ + drivetrain.setControl(drive.withVelocityY(1.5)); + } + else{ + drivetrain.setControl(drive.withVelocityY(0)); + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.setControl(drive.withVelocityY(0)); + timer.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get() > 1; + } +} diff --git a/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java b/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java new file mode 100644 index 0000000..e4a3748 --- /dev/null +++ b/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java @@ -0,0 +1,79 @@ +// 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.ModeAuto; + +import static edu.wpi.first.units.Units.*; + +import java.util.Optional; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +/* 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 BougerGaucheAuto extends Command { + CommandSwerveDrivetrain drivetrain; + Timer timer = new Timer(); + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + Optional alliance; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + /** Creates a new AvanceAuto. */ + public BougerGaucheAuto(CommandSwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + addRequirements(drivetrain); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + alliance = DriverStation.getAlliance(); + timer.reset(); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(alliance.get() == Alliance.Blue){ + if(timer.get() < 0.75){ + drivetrain.setControl(drive.withVelocityY(1.5)); + } + else{ + drivetrain.setControl(drive.withVelocityY(0)); + } + } + else{ + if(timer.get() < 0.75){ + drivetrain.setControl(drive.withVelocityY(-1.5)); + } + else{ + drivetrain.setControl(drive.withVelocityY(0)); + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.setControl(drive.withVelocityY(0)); + timer.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get() > 1; + } +} diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index c0bcfa2..5eb1bf1 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -48,7 +48,10 @@ public class GrimperMur extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - alliance = DriverStation.getAlliance(); + // alliance = DriverStation.getAlliance(); + // if(drivetrain.Equipe()){ + // angle+=180; + // } } // Called every time the scheduler runs while the command is scheduled. @@ -62,7 +65,7 @@ public class GrimperMur extends Command { angle = angle + 360; } if(alliance.get() == Alliance.Blue){ - y = 2.961328; + y = 5; x = 1.11; angle = 0; if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index 8e13cd8..03e8629 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -14,6 +14,7 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; @@ -31,6 +32,7 @@ public class GrimperReservoir extends Command { double x; double y; double angle; + double pigeonAngle; private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); Optional alliance = DriverStation.getAlliance(); @@ -48,52 +50,62 @@ public class GrimperReservoir extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - alliance = DriverStation.getAlliance(); + // alliance = DriverStation.getAlliance(); + // if(drivetrain.Equipe()){ + // drivetrain.getPigeon2().setYaw(drivetrain.getPigeon2().getYaw().getValueAsDouble()+180); + // } } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - BotPose = limeLight3G.getBotPoseBlue(); - botx = BotPose[0]; - boty = BotPose[1]; - System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble()); - if(angle < 0){ - angle = angle + 360; - } - if(alliance.get() == Alliance.Red){ - y = 6.959326; - x = 13.57966; - angle = 180; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){ - drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); + if(limeLight3G.getV()){ + BotPose = limeLight3G.getBotPoseBlue(); + botx = BotPose[0]; + boty = BotPose[1]; + if(angle < 0){ + angle = angle + 360; + } + if(alliance.get() == Alliance.Red){ + y = 6.959326; + x = 13.57966; + angle = 180; + if(pigeonAngle< 190 && pigeonAngle> 170){ + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5),-2,2))); + } + else{ + if(pigeonAngle>0 && pigeonAngle<180){ + drivetrain.setControl(drive.withRotationalRate(1)); + } + else if(pigeonAngle>180){ + drivetrain.setControl(drive.withRotationalRate(-1)); + } + } + } + else{ + pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180; + System.out.println(pigeonAngle); + y = 2.6; + x = 1.11; + angle = 0; + if(pigeonAngle> 358 || pigeonAngle< 2){ + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0)); + } + else{ + if(pigeonAngle>0 && pigeonAngle<180){ + drivetrain.setControl(drive.withRotationalRate(-1)); + System.out.println("x"); + } + else if(pigeonAngle>180){ + System.out.println("e"); + drivetrain.setControl(drive.withRotationalRate(1)); + } + } + } } else{ - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); - } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); - } + drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)); } - } - else{ - y = 5.081328; - x = 1.11; - angle = 0; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ - drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); - } - else{ - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); - } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); - } - } - } - } // Called once the command ends or is interrupted. @@ -105,6 +117,6 @@ public class GrimperReservoir extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05); + return (y-boty < 0.1 && y-boty >-0.1) && (x-botx < 0.1 && x-botx > -0.1); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 21cb1e8..8175fc7 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -78,7 +78,7 @@ public class LimelighterAuto extends Command { } else if(calcul > 5){ drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); + drive.withRotationalRate(0.5*(2*Math.PI))); } else if(calcul < -5){ drivetrain.setControl( diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java index c2b9232..d57f0be 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java @@ -54,13 +54,18 @@ public class TournerVersReservoir extends Command { angle = 0; } } + if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10){ + drivetrain.setControl(drive.withRotationalRate(0)); + } + else{ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(-force*180/Math.PI)); + drivetrain.setControl(drive.withRotationalRate(-force*2)); } else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(force*180/Math.PI)); + drivetrain.setControl(drive.withRotationalRate(force*2)); } } + } // Called once the command ends or is interrupted. @Override @@ -69,6 +74,6 @@ public class TournerVersReservoir extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10; + return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10; } } diff --git a/src/main/java/frc/robot/commands/MonterGrimpeur.java b/src/main/java/frc/robot/commands/MonterGrimpeur.java index a8a3e84..d79b763 100644 --- a/src/main/java/frc/robot/commands/MonterGrimpeur.java +++ b/src/main/java/frc/robot/commands/MonterGrimpeur.java @@ -42,6 +42,6 @@ public class MonterGrimpeur extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return Math.abs(grimpeur.Position()) > grimpeur.PositionFinal(); } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 9b70aea..c20b5f1 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -22,10 +22,14 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -54,8 +58,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); + ShuffleboardTab teb = Shuffleboard.getTab("teb"); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - + private GenericEntry equipe = + teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( @@ -221,7 +227,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su } configureAutoBuilder(); } - + public boolean Equipe(){ + return equipe.getBoolean(false); + } /** * Returns a command that applies the specified control request to this swerve drivetrain. * @@ -254,7 +262,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su else if(getPigeon2().getYaw().getValueAsDouble() < 0){ getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()+360); } - /* * Periodically try to apply the operator perspective. * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java new file mode 100644 index 0000000..9bb2c7d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -0,0 +1,39 @@ +/* Generated by Phoenix Tuner X */ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.controls.SolidColor; +import com.ctre.phoenix6.hardware.CANdle; +import com.ctre.phoenix6.signals.RGBWColor; + +/** + * Subsystem that controls an addressable LED strip using a CANdle. + */ +public class LEDSubsystem extends SubsystemBase { + private final CANBus kCANBus = new CANBus("rio"); + private final CANdle m_candle = new CANdle(17, kCANBus); + public void Bleu(){ + m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(255, 0, 0, 0))); + } + public void Rouge(){ + new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 255, 0)); + } + public LEDSubsystem() { + setDefaultCommand(updateLEDs()); + } + + /** + * Updates the animations and LEDs of the CANdle. + * + * @return Command to run + */ + public Command updateLEDs() { + return run(() -> {}); + } +} + diff --git a/src/main/java/frc/robot/subsystems/Led.java b/src/main/java/frc/robot/subsystems/Led.java index f628715..da22522 100644 --- a/src/main/java/frc/robot/subsystems/Led.java +++ b/src/main/java/frc/robot/subsystems/Led.java @@ -16,13 +16,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Led extends SubsystemBase { ShuffleboardTab teb = Shuffleboard.getTab("teb"); - private GenericEntry equipe = - teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); - @SuppressWarnings("removal") CANdle CANDle = new CANdle(17); - @SuppressWarnings("removal") RainbowAnimation rainbowAnim = new RainbowAnimation(); - @SuppressWarnings("removal") public void bleu(){ CANDle.setLEDs(0, 0, 255,0,0,8); CANDle.setLEDs(0, 0, 255,0,16,8); @@ -47,7 +42,6 @@ public class Led extends SubsystemBase { // CANDle.setLEDs(0, 255, 0,0,120,8); // CANDle.setLEDs(0, 255, 0,0,136,8); } - @SuppressWarnings("removal") public void Rouge(){ CANDle.setLEDs(255, 0, 0,0,0,8); CANDle.setLEDs(255, 0, 0,0,16,8); @@ -59,7 +53,6 @@ public class Led extends SubsystemBase { // CANDle.setLEDs(255, 0, 0,0,112,8); // CANDle.setLEDs(255, 0, 0,0,128,8); } - @SuppressWarnings("removal") public void Jaune2(){ CANDle.setLEDs(255, 255, 0,0,8,8); CANDle.setLEDs(255, 255, 0,0,24,8); @@ -91,9 +84,6 @@ public class Led extends SubsystemBase { public void RainBowStop(){ CANDle.animate(null); } - public boolean Equipe(){ - return equipe.getBoolean(true); - } /** Creates a new Led. */ public Led() {} @@ -104,7 +94,7 @@ public class Led extends SubsystemBase { if(temps > 20 && temps < 30){ Vert1(); } - if(Equipe()){ + // if(Equipe()){ if(temps > 30 && temps < 55){ bleu(); } @@ -120,8 +110,8 @@ public class Led extends SubsystemBase { else{ RainBow(); } - } - else{ + // } + // else{ if(temps > 30 && temps < 55){ Rouge(); } @@ -140,6 +130,6 @@ public class Led extends SubsystemBase { } // This method will be called once per scheduler run - } + // } } diff --git a/src/main/java/frc/robot/subsystems/Limelight3G.java b/src/main/java/frc/robot/subsystems/Limelight3G.java index 5bc3bb6..1303b8f 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3G.java +++ b/src/main/java/frc/robot/subsystems/Limelight3G.java @@ -72,7 +72,7 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag") return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle; } else{ - return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)-360 - angle; + return -Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle; } } @Override