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

mode auto

Browse Source
This commit is contained in:
samuel desharnais
2026-03-31 17:53:41 -04:00
parent 6afc342006
commit 10b0110315
12 changed files with 190 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto Normal file
View File

@@ -0,0 +1,99 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 3.0
}
},
{
"type": "path",
"data": {
"pathName": "Depot"
}
},
{
"type": "named",
"data": {
"name": "Aspirer"
}
}
]
}
},
{
"type": "path",
"data": {
"pathName": "Tir"
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Lancer"
}
},
{
"type": "named",
"data": {
"name": "Aspirer"
}
}
]
}
},
{
"type": "path",
"data": {
"pathName": "GrimperReservoir"
}
},
{
"type": "named",
"data": {
"name": "GrimperReservoir"
}
},
{
"type": "named",
"data": {
"name": "MonterGrimpeur"
}
},
{
"type": "path",
"data": {
"pathName": "MonterReservoir"
}
},
{
"type": "named",
"data": {
"name": "DescendreGrimpeur"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

6
src/main/deploy/pathplanner/autos/DepotTirerMur.auto
View File

@@ -14,6 +14,12 @@
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 3.0
}
},
{
"type": "path",
"data": {

6
src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto
View File

@@ -14,6 +14,12 @@
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 3.0
}
},
{
"type": "path",
"data": {

16
src/main/deploy/pathplanner/paths/MonterReservoir.path
View File

@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 1.0502282453637657,
"y": 2.4758915834522113
"x": 1.0,
"y": 2.6
},
"prevControl": null,
"nextControl": {
"x": 1.0408884389937225,
"y": 2.742159765250622
"x": 0.9906601936299568,
"y": 2.866268181798411
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.0502282453637657,
"y": 2.7734807417974334
"x": 1.0,
"y": 2.9
},
"prevControl": {
"x": 1.0547546338516156,
"y": 2.523521721541598
"x": 1.00452638848785,
"y": 2.6500409797441646
},
"nextControl": null,
"isLocked": false,

1
src/main/java/frc/robot/RobotContainer.java
View File

@@ -126,6 +126,7 @@ public class RobotContainer {
manette1.b().whileTrue(new ModeOposer(lanceur));
manette1.a().whileTrue(new ModeOposerDemeleur(lanceur));
manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse));
manette1.povUp().whileTrue(new LancerAuto(lanceur, limeLight3G));
}
public Command getAutonomousCommand() {

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

@@ -56,7 +56,7 @@ public class Limelighter extends Command {
return;
}
if (alliance.get() == Alliance.Blue) {
x = 4.6;
x = 4;
}
else {
x = 11.915394;
@@ -64,30 +64,28 @@ public class Limelighter extends Command {
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
if(calcul < -5 && calcul > -180){
calcul = limelight3g.Calcule(botx, x, boty, 4.6, angle);
if(calcul < -3 && calcul > -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*(2*Math.PI)));
drive.withRotationalRate(-1.5));
}
else if(calcul > 5 && calcul < 180){
else if(calcul > 3 && calcul < 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
drive.withRotationalRate(-1.5));
}
else if(calcul >= 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
drive.withRotationalRate(-1.5));
}
else if(calcul <= -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*(2*Math.PI)));
drive.withRotationalRate(1.5));
}
else{
drivetrain.setControl(
drive.withRotationalRate(0));
}
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println(angle);
}
System.out.println(calcul);
if (calcul < 0.2 && calcul > -0.2) {
drivetrain.setControl(drive.withRotationalRate(0));
}

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

@@ -60,7 +60,7 @@ public class GrimperMur extends Command {
angle = angle + 360;
}
if(alliance.get() == Alliance.Blue){
y = 2.961328;
y = 5;
x = 1.11;
angle = 0;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){

84
src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.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.SwerveRequest;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
@@ -52,46 +53,53 @@ public class GrimperReservoir extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
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.Red){
y = 6.959326;
x = 13.57966;
angle = 180;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){
drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx));
if(limeLight3G.getV()){
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.Red){
y = 6.959326;
x = 13.57966;
angle = 180;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 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){
drivetrain.setControl(drive.withRotationalRate(1));
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(-1));
}
}
}
else{
y = 2.6;
x = 1.11;
angle = 0;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 358 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 2){
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5),-2,2)).withRotationalRate(0));
}
else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(-1));
System.out.println("x");
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
System.out.println("e");
drivetrain.setControl(drive.withRotationalRate(1));
}
}
}
}
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));
}
drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
}
}
else{
y = 5.081328;
x = 1.11;
angle = 0;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){
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));
}
}
}
}
// Called once the command ends or is interrupted.
@@ -103,6 +111,6 @@ public class GrimperReservoir 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.1 && y-boty >-0.1) && (x-botx < 0.1 && x-botx > -0.1);
}
}

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

@@ -85,8 +85,6 @@ public class LimelighterAuto extends Command {
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));

11
src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java
View File

@@ -52,13 +52,18 @@ public class TournerVersReservoir extends Command {
angle = 0;
}
}
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10){
drivetrain.setControl(drive.withRotationalRate(0));
}
else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(-force*180/Math.PI));
drivetrain.setControl(drive.withRotationalRate(-force*2));
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(force*180/Math.PI));
drivetrain.setControl(drive.withRotationalRate(force*2));
}
}
}
// Called once the command ends or is interrupted.
@Override
@@ -67,6 +72,6 @@ public class TournerVersReservoir extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10;
return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10;
}
}

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

@@ -42,6 +42,6 @@ public class MonterGrimpeur extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
return Math.abs(grimpeur.Position()) > grimpeur.PositionFinal();
}
}

8
src/main/java/frc/robot/subsystems/Limelight3G.java
View File

@@ -54,18 +54,18 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag")
{
if(x1 > x2){
if(y1 > y2){
return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle;
return Math.atan((x2 - x1) / (y2 - y1)* (180/Math.PI)) - angle;
}
else{
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+90 - angle;
return Math.atan((x2 - x1) / (y2 - y1)* (180/Math.PI)) + 180 - angle;
}
}
else{
if(y1 > y2){
return Math.atan((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle;
return 90 -(Math.atan((x2 - x1) / (y2 - y1)) * (180/Math.PI)) + 270 - angle;
}
else{
return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle;
return Math.atan((x2 - x1) / (y2 - y1)* (180/Math.PI)) - angle;
}
}
}