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

mode marche yesssssssss

Browse Source
This commit is contained in:
samuel desharnais
2026-03-31 19:53:25 -04:00
parent 4d1b353e25
commit 272f89962a
2 changed files with 14 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/main/java/frc/robot/commands/Lancer.java
View File

@@ -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){

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

@@ -32,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> alliance = DriverStation.getAlliance();
@@ -59,11 +60,11 @@ public class GrimperReservoir extends Command {
@Override
public void execute() {
if(limeLight3G.getV()){
BotPose = limeLight3G.getBotPoseBlue();
botx = BotPose[0];
boty = BotPose[1];
System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble());
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180;
System.out.println(pigeonAngle);
if(angle < 0){
angle = angle + 360;
}
@@ -71,14 +72,14 @@ public class GrimperReservoir extends Command {
y = 6.959326;
x = 13.57966;
angle = 180;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){
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(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
if(pigeonAngle>0 && pigeonAngle<180){
drivetrain.setControl(drive.withRotationalRate(1));
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
else if(pigeonAngle>180){
drivetrain.setControl(drive.withRotationalRate(-1));
}
}
@@ -87,15 +88,15 @@ public class GrimperReservoir extends Command {
y = 2.6;
x = 1.11;
angle = 0;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 358 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 2){
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(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
if(pigeonAngle>0 && pigeonAngle<180){
drivetrain.setControl(drive.withRotationalRate(-1));
System.out.println("x");
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
else if(pigeonAngle>180){
System.out.println("e");
drivetrain.setControl(drive.withRotationalRate(1));
}