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

m

Browse Source
This commit is contained in:
samuel desharnais
2026-03-28 15:53:53 -04:00
parent fbd94fc33d
commit 799e3e34c3
4 changed files with 33 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -55,6 +55,7 @@ public class GrimperMur extends Command {
BotPose = limeLight3.getBotPoseBlue(); BotPose = limeLight3.getBotPoseBlue();
botx = BotPose[0]; botx = BotPose[0];
boty = BotPose[1]; boty = BotPose[1];
System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble());
if(angle < 0){ if(angle < 0){
angle = angle + 360; angle = angle + 360;
} }
@@ -62,15 +63,15 @@ public class GrimperMur extends Command {
y = 2.961328; y = 2.961328;
x = 1.11; x = 1.11;
angle = 0; angle = 0;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 355 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 5){ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){
drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
} }
else{ else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(-0.5)); drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI));
} }
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(0.5)); drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI));
} }
} }
} }
@@ -78,15 +79,15 @@ public class GrimperMur extends Command {
x = 11.45966; x = 11.45966;
y = 6.959326; y = 6.959326;
angle = 180; angle = 180;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 175 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 185){ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190){
drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
} }
else{ else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(0.5)); drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI));
} }
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(-0.5)); drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI));
} }
} }
} }

43
src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java
View File

@@ -55,38 +55,39 @@ public class GrimperReservoir extends Command {
BotPose = limeLight3G.getBotPoseBlue(); BotPose = limeLight3G.getBotPoseBlue();
botx = BotPose[0]; botx = BotPose[0];
boty = BotPose[1]; boty = BotPose[1];
System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble());
if(angle < 0){ if(angle < 0){
angle = angle + 360; angle = angle + 360;
} }
if(alliance.get() == Alliance.Blue){ if(alliance.get() == Alliance.Red){
y = 5.081328;
x = 1.11;
angle = 180;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 185 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 175){
drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
}
else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(0.5));
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(-0.5));
}
}
}
else{
y = 6.959326; y = 6.959326;
x = 13.57966; x = 13.57966;
angle = 0; angle = 180;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 355 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 5){ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){
drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
} }
else{ else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(-0.5)); drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI));
} }
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(0.5)); drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI));
}
}
}
else{
y = 5.081328;
x = 1.11;
angle = 0;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){
drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(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));
} }
} }
} }

4
src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java
View File

@@ -69,7 +69,7 @@ public class LimelighterAuto extends Command {
if(angle >180){ 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) / 3;
drivetrain.setControl( drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println(calcul); System.out.println(calcul);
@@ -92,7 +92,7 @@ public class LimelighterAuto extends Command {
// Returns true when the command should end. // Returns true when the command should end.
@Override @Override
public boolean isFinished() { public boolean isFinished() {
return calcul < 0.1 && calcul > -0.1; return calcul < 0.2 && calcul > -0.2;
} }
} }

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

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