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();
botx = BotPose[0];
boty = BotPose[1];
System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble());
if(angle < 0){
angle = angle + 360;
}
@@ -62,15 +63,15 @@ public class GrimperMur extends Command {
y = 2.961328;
x = 1.11;
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));
}
else{
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){
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;
y = 6.959326;
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));
}
else{
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){
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();
botx = BotPose[0];
boty = BotPose[1];
System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble());
if(angle < 0){
angle = angle + 360;
}
if(alliance.get() == Alliance.Blue){
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{
if(alliance.get() == Alliance.Red){
y = 6.959326;
x = 13.57966;
angle = 0;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 355 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 5){
angle = 180;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){
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));
drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI));
}
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){
angle -= 360;
}
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5;
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 3;
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println(calcul);
@@ -92,7 +92,7 @@ public class LimelighterAuto extends Command {
// Returns true when the command should end.
@Override
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();
DigitalInput limit = new DigitalInput(0);
private GenericEntry EncodeurGrimpeur =
teb.add("Position haut grimpeur", 100).getEntry();
teb.add("Position haut grimpeur", 101).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(100);
return EncodeurGrimpeur.getDouble(101);
}
/** Creates a new Grimpeur. */
public Grimpeur() {