sdesharnais/Rebuilt-2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

grimpe mode auto

Browse Source
This commit is contained in:
samuel desharnais
2026-04-02 20:34:53 -04:00
parent 6885b80d6a
commit 6c86b2ed5c
8 changed files with 40 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/deploy/pathplanner/autos/DepotTirerMur.auto
View File

@@ -41,12 +41,6 @@
"pathName": "Tir"
}
},
{
"type": "named",
"data": {
"name": "Limelighter"
}
},
{
"type": "deadline",
"data": {

8
src/main/deploy/pathplanner/paths/GrimperMur.path
View File

@@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 1.0761055634807417,
"y": 4.85660485021398
"x": 1.0577108433734939,
"y": 5.532096385542169
},
"prevControl": {
"x": 1.503081312410841,
"y": 4.869543509272469
"x": 1.4846865923035932,
"y": 5.545035044600658
},
"nextControl": null,
"isLocked": false,

30
src/main/java/frc/robot/commands/Limelighter.java
View File

@@ -7,6 +7,7 @@ package frc.robot.commands;
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;
@@ -51,7 +52,6 @@ public class Limelighter extends Command {
@Override
public void execute() {
double[] BotPose = new double[6];
System.out.println("e");
if (limelight3g.getV()) {
BotPose = limelight3g.getBotPoseBlue();
if (!alliance.isPresent()) {
@@ -69,19 +69,21 @@ public class Limelighter extends Command {
botx = BotPose[0];
boty = BotPose[1];
angle = BotPose[5];
calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
if(calcul > -5 && calcul < 5){
drivetrain.setControl(
drive.withRotationalRate(0));
}
else if(calcul > 5){
drivetrain.setControl(
drive.withRotationalRate(2));
}
else if(calcul < -5){
drivetrain.setControl(drive.withRotationalRate(-2));
}
// botx = BotPose[1];
calcul = Math.toRadians(limelight3g.Calcule(boty, 4,botx, x, angle));
System.out.println(calcul);
drivetrain.setControl(drive.withRotationalRate(MathUtil.clamp(calcul, -3, 3)));
// if(calcul > -5 && calcul < 5){
// drivetrain.setControl(
// drive.withRotationalRate(0));
// }
// else if(calcul > 5){
// drivetrain.setControl(
// drive.withRotationalRate(2));
// }
// else if(calcul < -5){
// drivetrain.setControl(drive.withRotationalRate(-2));
// }
// // botx = BotPose[1];
// boty = BotPose[0];
// angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
// calcul = limelight3g.Calcule(botx, x, boty, 4, angle);

13
src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java
View File

@@ -46,23 +46,12 @@ public class BougerDroiteAuto extends Command {
// 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");
if(timer.get() < 1.25){
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.

26
src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java
View File

@@ -51,7 +51,7 @@ 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;
// }
@@ -70,38 +70,38 @@ public class GrimperMur extends Command {
if(alliance.get() == Alliance.Red){
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
System.out.println(pigeonAngle);
y = 2.6;
x = 15.6;
y = 3.6;
x = 14.9;
angle = 180;
if(pigeonAngle< 190 && pigeonAngle> 170){
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*6.5), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*6.5),-2,2)));
if(pigeonAngle> 358 || pigeonAngle< 2){
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*6.5), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*6.5),-2,2)).withRotationalRate(0));
}
else{
if(pigeonAngle>180){
drivetrain.setControl(drive.withRotationalRate(-1));
drivetrain.setControl(drive.withVelocityY(0).withVelocityX(0).withRotationalRate(1));
}
else{
drivetrain.setControl(drive.withRotationalRate(1));
drivetrain.setControl(drive.withVelocityY(0).withVelocityX(0).withRotationalRate(-1));
}
}
}
else{
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180;
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
System.out.println(pigeonAngle);
y = 5.2;
x = 1.11;
y = 4.4;
x = 1.7;
angle = 0;
if(pigeonAngle> 358 || pigeonAngle< 2){
if(pigeonAngle> 182 || pigeonAngle< 178){
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));
drivetrain.setControl(drive.withVelocityY(0).withVelocityX(0).withRotationalRate(-1));
System.out.println("x");
}
else if(pigeonAngle>180){
System.out.println("e");
drivetrain.setControl(drive.withRotationalRate(1));
drivetrain.setControl(drive.withVelocityY(0).withVelocityX(0).withRotationalRate(1));
}
}
}

4
src/main/java/frc/robot/subsystems/Grimpeur.java
View File

@@ -21,7 +21,7 @@ public class Grimpeur extends SubsystemBase {
SparkMaxConfig slaveConfig = new SparkMaxConfig();
DigitalInput limit = new DigitalInput(0);
private GenericEntry EncodeurGrimpeur =
teb.add("Position haut grimpeur", 101).getEntry();
teb.add("Position haut grimpeur", 100).getEntry();
public void Grimper(double vitesse){
grimpeur1.set(vitesse);
grimpeur2.set(vitesse);
@@ -36,7 +36,7 @@ public class Grimpeur extends SubsystemBase {
return limit.get();
}
public double PositionFinal(){
return EncodeurGrimpeur.getDouble(101);
return EncodeurGrimpeur.getDouble(100);
}
/** Creates a new Grimpeur. */
public Grimpeur() {

4
src/main/java/frc/robot/subsystems/LimeLight3.java
View File

@@ -22,13 +22,13 @@ public class LimeLight3 extends SubsystemBase {
}
public double[] getBotPoseBlue(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}
public double[] getBotPoseRed(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}

4
src/main/java/frc/robot/subsystems/Limelight3G.java
View File

@@ -69,10 +69,10 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag")
// }
// }
if(y1 > y2){
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
return Math.toDegrees(Math.atan((x2 - x1) /(y2 - y1))) - angle;
}
else{
return -Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
return Math.toDegrees(-Math.atan((x2 - x1) / (y2 - y1))) - angle;
}
}
@Override