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

Merge branch 'main' of https://git.demerso.net/sdesharnais/Rebuilt-2026

Browse Source
This commit is contained in:
samuel desharnais
2026-04-02 17:13:57 -04:00
parent 2472c67adf c8d0ee3a03
commit 6885b80d6a
2 changed files with 48 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -14,6 +14,7 @@ import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest; import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
@@ -32,6 +33,7 @@ public class GrimperMur extends Command {
double x; double x;
double y; double y;
double angle; double angle;
double pigeonAngle;
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
Optional<Alliance> alliance = DriverStation.getAlliance(); Optional<Alliance> alliance = DriverStation.getAlliance();
@@ -62,42 +64,50 @@ 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;
} }
if(alliance.get() == Alliance.Blue){ if(alliance.get() == Alliance.Red){
y = 5.4; pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
System.out.println(pigeonAngle);
y = 2.6;
x = 15.6;
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)));
}
else{
if(pigeonAngle>180){
drivetrain.setControl(drive.withRotationalRate(-1));
}
else{
drivetrain.setControl(drive.withRotationalRate(1));
}
}
}
else{
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180;
System.out.println(pigeonAngle);
y = 5.2;
x = 1.11; x = 1.11;
angle = 0; angle = 0;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ if(pigeonAngle> 358 || pigeonAngle< 2){
drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0));
} }
else{ else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ if(pigeonAngle>0 && pigeonAngle<180){
drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); 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() >180){
drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI));
} }
} }
} }
else{ else{
x = 15.6; drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
y = 6.959326;
angle = 180;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190){
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));
}
}
}
} }
} }

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

@@ -69,7 +69,7 @@ public class GrimperReservoir extends Command {
if(alliance.get() == Alliance.Red){ if(alliance.get() == Alliance.Red){
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
System.out.println(pigeonAngle); System.out.println(pigeonAngle);
y = 5.4; y = 5.2;
x = 15.6; x = 15.6;
angle = 180; angle = 180;
if(pigeonAngle< 190 && pigeonAngle> 170){ if(pigeonAngle< 190 && pigeonAngle> 170){