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

competiton

Browse Source
This commit is contained in:
samuel desharnais
2026-04-11 08:16:06 -04:00
parent 7908d4c5df
commit 41f6e4747f
16 changed files with 315 additions and 251 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/java/frc/robot/commands/DescendreGrimpeur.java
View File

@@ -13,6 +13,7 @@ public class DescendreGrimpeur extends Command {
/** Creates a new DescendreGrimpeur. */
public DescendreGrimpeur(Grimpeur grimpeur) {
this.grimpeur = grimpeur;
addRequirements(grimpeur);
// Use addRequirements() here to declare subsystem dependencies.
}

50
src/main/java/frc/robot/commands/DescendreGrimpeurPlus.java Normal file
View File

@@ -0,0 +1,50 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Grimpeur;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class DescendreGrimpeurPlus extends Command {
private Grimpeur grimpeur;
/** Creates a new DescendreGrimpeur. */
public DescendreGrimpeurPlus(Grimpeur grimpeur) {
this.grimpeur = grimpeur;
addRequirements(grimpeur);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(!grimpeur.Limit()){
grimpeur.GrimperGauche(-0.4);
grimpeur.GrimperDroit(-0.4);
}
else{
grimpeur.Reset();
grimpeur.GrimperGauche(0);
grimpeur.GrimperDroit(0);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
grimpeur.GrimperGauche(0);
grimpeur.GrimperDroit(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

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

@@ -20,7 +20,7 @@ public class Lancer extends Command {
private PIDController pidController;
private Limelight3G limeLight3G;
private Timer timer;
double vitesse = 0.5;
double vitesse = 0;
double botx = 0;
double boty = 0;
Optional<Alliance> alliance = DriverStation.getAlliance();
@@ -61,9 +61,9 @@ public class Lancer extends Command {
boty = BotPose[1];
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()){
// if(limeLight3G.getV()){
if(vitesse > 2000){
double output = pidController.calculate(lanceur.Vitesse(),vitesse);
lanceur.Lancer(output);
System.out.println(output);
@@ -72,9 +72,12 @@ public class Lancer extends Command {
if(timer.get() >1){
lanceur.Demeler(1);
}
}
else{
lanceur.Lancer(3500);
}
}
}
// }
}
// Called once the command ends or is interrupted.

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

@@ -71,48 +71,48 @@ public class Limelighter extends Command {
angle = BotPose[5];
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);
// if(calcul < -5 && calcul > -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else if(calcul > 5 && calcul < 180){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul < -5){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul <= -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else{
// drivetrain.setControl(
// drive.withRotationalRate(0));
// }
// drivetrain.setControl(
// drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
// System.out.println(angle);
// if (calcul < 0.2 && calcul > -0.2) {
// drivetrain.setControl(drive.withRotationalRate(0));
// }
//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);
if(calcul < -5 && calcul > -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*(2*Math.PI)));
}
else if(calcul > 5 && calcul < 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
else if(calcul < -5){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
else if(calcul <= -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*(2*Math.PI)));
}
else{
drivetrain.setControl(
drive.withRotationalRate(0));
}
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println(angle);
if (calcul < 0.2 && calcul > -0.2) {
drivetrain.setControl(drive.withRotationalRate(0));
}
}
else{
drivetrain.setControl(drive.withRotationalRate(0));

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

@@ -71,10 +71,10 @@ public class GrimperMur extends Command {
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
System.out.println(pigeonAngle);
y = 3.6;
x = 14.9;
x = 15;
angle = 180;
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));
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*10), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*10),-2,2)).withRotationalRate(0));
}
else{
if(pigeonAngle>180){
@@ -89,10 +89,10 @@ public class GrimperMur extends Command {
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
System.out.println(pigeonAngle);
y = 4.4;
x = 1.7;
x = 1.6;
angle = 0;
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));
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*10), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*10),-2,2)).withRotationalRate(0));
}
else{
if(pigeonAngle>0 && pigeonAngle<180){
@@ -120,6 +120,6 @@ public class GrimperMur extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05);
return (y-boty < 0.2 && y-boty >-0.2) && (x-botx < 0.06 && x-botx > -0.06);
}
}

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

@@ -73,7 +73,7 @@ public class GrimperReservoir extends Command {
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)));
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*10), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*10),-2,2)));
}
else{
if(pigeonAngle>180){
@@ -91,7 +91,7 @@ public class GrimperReservoir extends Command {
x = 1.11;
angle = 0;
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));
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*10), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*10),-2,2)).withRotationalRate(0));
}
else{
if(pigeonAngle>0 && pigeonAngle<180){
@@ -119,6 +119,6 @@ public class GrimperReservoir extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (y-boty < 0.1 && y-boty >-0.1) && (x-botx < 0.1 && x-botx > -0.1);
return (y-boty < 0.2 && y-boty >-0.2) && (x-botx < 0.06 && x-botx > -0.06);
}
}

1
src/main/java/frc/robot/commands/MonterGrimpeur.java
View File

@@ -13,6 +13,7 @@ public class MonterGrimpeur extends Command {
/** Creates a new MonterGrimpeur. */
public MonterGrimpeur(Grimpeur grimpeur) {
this.grimpeur = grimpeur;
addRequirements(grimpeur);
// Use addRequirements() here to declare subsystem dependencies.
}