Compare commits
70 Commits
Swerve
...
a11f31a2a8
Author | SHA1 | Date | |
---|---|---|---|
a11f31a2a8 | |||
94aae66eed | |||
9f83b61c46 | |||
5d84d83f7d | |||
87c3abcb65 | |||
93e5bb0b46 | |||
7be5f8c2fc | |||
26d32e3707 | |||
1f4111ef6d | |||
9ff363dc24 | |||
bb9d5a5550 | |||
69ef6b6982 | |||
0be9356fa9 | |||
1627770572 | |||
f10dc1165d | |||
6ccef6bad9 | |||
7cf7483498 | |||
20e54ae142 | |||
7492e21135 | |||
71a90cd543 | |||
32989c510a | |||
33badb2f77 | |||
d692bab745 | |||
be9f3856f7 | |||
b1e32bd6df | |||
590f9556c2 | |||
c1bdf0aab7 | |||
8bc8f0390f | |||
0e04704942 | |||
c0c48a3f24 | |||
cbeb99c1a5 | |||
53adcf5701 | |||
319c370c6e | |||
2b94ebcc95 | |||
9796800b4e | |||
cf97a05e6f | |||
6fb4e0c1ac | |||
9af46de189 | |||
451d3c6e20 | |||
688315bcd0 | |||
2260580c81 | |||
9cd08cf07c | |||
8aca411a23 | |||
44f6030e99 | |||
6f75b9cc42 | |||
210d219793 | |||
06abfa4dbb | |||
26a4b9f9a3 | |||
b384585224 | |||
ee2c9ff4b2 | |||
bd180e9153 | |||
20c95efe93 | |||
4869632d6d | |||
afaec61f6d | |||
126058e9d4 | |||
1f17aaf4de | |||
eece0f47fa | |||
c81f118058 | |||
7848c4aaf2 | |||
1d1d6e962d | |||
d6420659e9 | |||
aafb2a62b5 | |||
0fdfa4269d | |||
017f168b3c | |||
bff426ef0f | |||
5ffa28596c | |||
e7b4b47928 | |||
0577ce368a | |||
7521c0d94e | |||
b16d11b70a |
1
.SysId/sysid.json
Normal file
1
.SysId/sysid.json
Normal file
@ -0,0 +1 @@
|
||||
{}
|
@ -1,4 +1,14 @@
|
||||
{
|
||||
"Joysticks": {
|
||||
"window": {
|
||||
"visible": false
|
||||
}
|
||||
},
|
||||
"System Joysticks": {
|
||||
"window": {
|
||||
"enabled": false
|
||||
}
|
||||
},
|
||||
"keyboardJoysticks": [
|
||||
{
|
||||
"axisConfig": [
|
||||
|
126
src/main/deploy/pathplanner/autos/BlueBas3 sans composantes.auto
Normal file
126
src/main/deploy/pathplanner/autos/BlueBas3 sans composantes.auto
Normal file
@ -0,0 +1,126 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "BlueBasStart"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AprilTag"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 1.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "BlueBasChercher"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AprilTag"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AprilTag"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "BlueBasPorter"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 1.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "BlueBasChercher2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": []
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AprilTag"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "BlueBasPorter2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AprilTag"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
19
src/main/deploy/pathplanner/autos/New Auto.auto
Normal file
19
src/main/deploy/pathplanner/autos/New Auto.auto
Normal file
@ -0,0 +1,19 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "New Path"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
@ -4,6 +4,12 @@
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 3.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
|
File diff suppressed because one or more lines are too long
@ -8,8 +8,8 @@
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.400623816881145,
|
||||
"y": 1.8077934662846444
|
||||
"x": 6.40062663880838,
|
||||
"y": 1.8077964923366423
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
@ -33,8 +33,8 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@ -50,5 +50,5 @@
|
||||
"velocity": 0,
|
||||
"rotation": 119.99999999999999
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
54
src/main/deploy/pathplanner/paths/New Path.path
Normal file
54
src/main/deploy/pathplanner/paths/New Path.path
Normal file
@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.8820696721311476,
|
||||
"y": 6.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.8706512438743716,
|
||||
"y": 5.690872003047288
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.8820696721311476,
|
||||
"y": 3.1279999999999997
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.8863317961339294,
|
||||
"y": 3.3779636659576444
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0.0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
@ -8,20 +8,20 @@
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 7.084733606557376,
|
||||
"y": 6.488473360655737
|
||||
"x": 7.119564732833961,
|
||||
"y": 6.478684258786044
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.849999999999999,
|
||||
"x": 5.815652951699465,
|
||||
"y": 6.464497950819672
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.32950819672131,
|
||||
"y": 6.464497950819672
|
||||
"x": 6.633711863494241,
|
||||
"y": 6.455611583097
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@ -33,10 +33,10 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 50.0,
|
||||
"maxAngularAcceleration": 100.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
@ -50,5 +50,5 @@
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
@ -9,12 +9,12 @@
|
||||
"defaultMaxAngVel": 540.0,
|
||||
"defaultMaxAngAccel": 720.0,
|
||||
"defaultNominalVoltage": 12.0,
|
||||
"robotMass": 74.088,
|
||||
"robotMass": 45.3592,
|
||||
"robotMOI": 6.883,
|
||||
"robotTrackwidth": 0.546,
|
||||
"driveWheelRadius": 0.048,
|
||||
"driveGearing": 5.143,
|
||||
"maxDriveSpeed": 5.45,
|
||||
"maxDriveSpeed": 5.261,
|
||||
"driveMotorType": "krakenX60",
|
||||
"driveCurrentLimit": 60.0,
|
||||
"wheelCOF": 1.2,
|
||||
|
1645
src/main/java/frc/robot/LimelightHelpers.java
Normal file
1645
src/main/java/frc/robot/LimelightHelpers.java
Normal file
File diff suppressed because it is too large
Load Diff
@ -5,31 +5,34 @@
|
||||
package frc.robot;
|
||||
|
||||
import static edu.wpi.first.units.Units.*;
|
||||
|
||||
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
||||
import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.PointWheelsAt;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.auto.NamedCommands;
|
||||
|
||||
import edu.wpi.first.cameraserver.CameraServer;
|
||||
import com.pathplanner.lib.commands.PathPlannerAuto;
|
||||
import com.pathplanner.lib.util.FlippingUtil;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
||||
import frc.robot.TunerConstants.TunerConstants;
|
||||
import frc.robot.commands.AprilTag3;
|
||||
import frc.robot.commands.AprilTag3G;
|
||||
import frc.robot.commands.Forme3;
|
||||
import frc.robot.commands.RainBow;
|
||||
import frc.robot.subsystems.Bougie;
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
import frc.robot.subsystems.Grimpeur;
|
||||
|
||||
import frc.robot.subsystems.Limelight3;
|
||||
import frc.robot.subsystems.Limelight3G;
|
||||
|
||||
public class RobotContainer {
|
||||
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
|
||||
@ -43,18 +46,24 @@ public class RobotContainer {
|
||||
private final Telemetry logger = new Telemetry(MaxSpeed);
|
||||
|
||||
private final CommandXboxController manette1 = new CommandXboxController(0);
|
||||
private final CommandXboxController manette2 = new CommandXboxController(0);
|
||||
private final CommandXboxController manette2 = new CommandXboxController(1);
|
||||
|
||||
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
||||
|
||||
private final SendableChooser<Command> autoChooser;
|
||||
Bougie bougie = new Bougie();
|
||||
|
||||
|
||||
public RobotContainer() {
|
||||
Limelight3G limelight3g = new Limelight3G();
|
||||
Limelight3 limelight3 = new Limelight3();
|
||||
Pose2d pose = new Pose2d();
|
||||
private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2
|
||||
public double getAngle() {
|
||||
return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot
|
||||
}
|
||||
public RobotContainer() {
|
||||
autoChooser = AutoBuilder.buildAutoChooser("New Auto");
|
||||
SmartDashboard.putData("Auto Mode", autoChooser);
|
||||
configureBindings();
|
||||
NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null));
|
||||
}
|
||||
|
||||
private void configureBindings() {
|
||||
@ -63,31 +72,34 @@ public class RobotContainer {
|
||||
drivetrain.setDefaultCommand(
|
||||
// Drivetrain will execute this command periodically
|
||||
drivetrain.applyRequest(() ->
|
||||
drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY(), 0.5)) // Drive forward with negative Y (forward)
|
||||
.withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX(), 0.5)) // Drive left with negative X (left)
|
||||
.withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.5)) // Drive counterclockwise with negative X (left)
|
||||
drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.2)) // Drive forward with negative Y (forward)
|
||||
.withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.2)) // Drive left with negative X (left)
|
||||
.withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.15)) // Drive counterclockwise with negative X (left)
|
||||
)
|
||||
);
|
||||
|
||||
|
||||
// reset the field-centric heading on left bumper press
|
||||
manette1.y().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
|
||||
manette1.a().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
|
||||
manette1.b().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
|
||||
manette1.x().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));
|
||||
manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
|
||||
|
||||
manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
||||
manette1.rightTrigger().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
||||
manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
||||
drivetrain.registerTelemetry(logger::telemeterize);
|
||||
}
|
||||
|
||||
public Command getAutonomousCommand() {
|
||||
return new SequentialCommandGroup(Commands.runOnce(()->{
|
||||
boolean flip = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;
|
||||
if(flip){
|
||||
drivetrain.resetPose(FlippingUtil.flipFieldPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose()));
|
||||
}
|
||||
else{
|
||||
drivetrain.resetPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose());
|
||||
}
|
||||
}),autoChooser.getSelected(), new RainBow(bougie));
|
||||
}
|
||||
|
||||
public Command getAutonomousCommand() {
|
||||
return
|
||||
new
|
||||
ParallelCommandGroup(
|
||||
autoChooser
|
||||
.getSelected(),
|
||||
new
|
||||
RainBow
|
||||
(
|
||||
bougie
|
||||
)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
@ -23,15 +23,19 @@ public class TunerConstants {
|
||||
|
||||
// The steer motor uses any SwerveModule.SteerRequestType control request with the
|
||||
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
|
||||
private static final Slot0Configs steerGains = new Slot0Configs()
|
||||
.withKP(100).withKI(0).withKD(0.5)
|
||||
.withKS(0.1).withKV(2.66).withKA(0)
|
||||
//private static final Slot0Configs steerGains = new Slot0Configs()
|
||||
// .withKP(68.294).withKI(0).withKD(4.7806)
|
||||
// .withKS(0.20754).withKV(2.4832).withKA(0.099824)
|
||||
// .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
|
||||
private static final Slot0Configs steerGains = new Slot0Configs()
|
||||
.withKP(43.502).withKI(0).withKD(2.7353)
|
||||
.withKS(0.027275).withKV(2.5818).withKA(0.1055)
|
||||
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
|
||||
// When using closed-loop control, the drive motor uses the control
|
||||
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
|
||||
private static final Slot0Configs driveGains = new Slot0Configs()
|
||||
.withKP(0.1).withKI(0).withKD(0)
|
||||
.withKS(0).withKV(0.124);
|
||||
.withKP(63.167).withKI(0).withKD(0.54521)
|
||||
.withKS(0.18227).withKV(0.12483);
|
||||
|
||||
// The closed-loop output type to use for the steer motors;
|
||||
// This affects the PID/FF gains for the steer motors
|
||||
|
41
src/main/java/frc/robot/commands/Algue1Test.java
Normal file
41
src/main/java/frc/robot/commands/Algue1Test.java
Normal file
@ -0,0 +1,41 @@
|
||||
// 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.Pince;
|
||||
|
||||
/* 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 Algue1Test extends Command {
|
||||
private Pince pince;
|
||||
/** Creates a new AlgueTest. */
|
||||
public Algue1Test(Pince pince) {
|
||||
this.pince = pince;
|
||||
addRequirements(pince);
|
||||
// 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() {
|
||||
pince.algue1Test(0.2);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
pince.algue1Test(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
41
src/main/java/frc/robot/commands/Algue2Test.java
Normal file
41
src/main/java/frc/robot/commands/Algue2Test.java
Normal file
@ -0,0 +1,41 @@
|
||||
// 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.Pince;
|
||||
|
||||
/* 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 Algue2Test extends Command {
|
||||
private Pince pince;
|
||||
/** Creates a new AlgueTest. */
|
||||
public Algue2Test(Pince pince) {
|
||||
this.pince = pince;
|
||||
addRequirements(pince);
|
||||
// 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() {
|
||||
pince.algue2Test(0.2);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
pince.algue2Test(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
51
src/main/java/frc/robot/commands/AlgueExpire.java
Normal file
51
src/main/java/frc/robot/commands/AlgueExpire.java
Normal file
@ -0,0 +1,51 @@
|
||||
// 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.Bougie;
|
||||
import frc.robot.subsystems.Pince;
|
||||
|
||||
/* 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 AlgueExpire extends Command {
|
||||
private Pince pince;
|
||||
private Bougie bougie;
|
||||
/** Creates a new CoralAlgue. */
|
||||
public AlgueExpire(Pince pince,Bougie bougie) {
|
||||
this.pince = pince;
|
||||
this.bougie = bougie;
|
||||
addRequirements(pince,bougie);
|
||||
// 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(pince.emperagealgue()>60){
|
||||
pince.aspirealgue(-0.5);
|
||||
}
|
||||
else{
|
||||
pince.aspirealgue(-0.5);
|
||||
bougie.Jaune();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
pince.aspirealgue(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
81
src/main/java/frc/robot/commands/AprilTag3.java
Normal file
81
src/main/java/frc/robot/commands/AprilTag3.java
Normal file
@ -0,0 +1,81 @@
|
||||
// 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 static edu.wpi.first.units.Units.*;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc.robot.TunerConstants.TunerConstants;
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
import frc.robot.subsystems.Limelight3;
|
||||
|
||||
/* 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 AprilTag3 extends Command {
|
||||
private Limelight3 limelight3;
|
||||
private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
||||
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
|
||||
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
|
||||
private DoubleSupplier x;
|
||||
private DoubleSupplier y;
|
||||
/* Setting up bindings for necessary control of the swerve drive platform */
|
||||
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
|
||||
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
|
||||
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
|
||||
/** Creates a new AprilTag3G. */
|
||||
public AprilTag3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
|
||||
this.limelight3 = limelight3;
|
||||
this.drivetrain = drivetrain;
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
addRequirements(limelight3,drivetrain);
|
||||
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
limelight3.Apriltag();
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
double a = limelight3.getX();
|
||||
|
||||
if(limelight3.getV() == true){
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(a/10).
|
||||
withVelocityX(x.getAsDouble()).
|
||||
withVelocityY(y.getAsDouble()));
|
||||
System.out.println(a/10);
|
||||
}
|
||||
else{
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(0).
|
||||
withVelocityX(0).
|
||||
withVelocityY(0));
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(0)
|
||||
.withVelocityX(0)
|
||||
.withVelocityY(0));
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
78
src/main/java/frc/robot/commands/AprilTag3G.java
Normal file
78
src/main/java/frc/robot/commands/AprilTag3G.java
Normal file
@ -0,0 +1,78 @@
|
||||
// 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 static edu.wpi.first.units.Units.*;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc.robot.TunerConstants.TunerConstants;
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
import frc.robot.subsystems.Limelight3G;
|
||||
|
||||
/* 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 AprilTag3G extends Command {
|
||||
private Limelight3G limelight3g;
|
||||
private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
||||
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
|
||||
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
|
||||
private DoubleSupplier x;
|
||||
private DoubleSupplier y;
|
||||
/* Setting up bindings for necessary control of the swerve drive platform */
|
||||
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
|
||||
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
|
||||
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
|
||||
/** Creates a new AprilTag3G. */
|
||||
/** Creates a new AprilTag3G. */
|
||||
public AprilTag3G(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
|
||||
this.limelight3g = limelight3g;
|
||||
this.drivetrain = drivetrain;
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
addRequirements(limelight3g,drivetrain);
|
||||
// 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() {
|
||||
double a = limelight3g.getX();
|
||||
if(limelight3g.getV() == true){
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(-a/5).
|
||||
withVelocityX(x.getAsDouble()).
|
||||
withVelocityY(y.getAsDouble()));
|
||||
System.out.println(a/5);
|
||||
}
|
||||
else{
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(0).
|
||||
withVelocityX(0).
|
||||
withVelocityY(0));
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(0).
|
||||
withVelocityX(0).
|
||||
withVelocityY(0));
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return limelight3g.getX()<1 && limelight3g.getX()>-1;
|
||||
}
|
||||
}
|
42
src/main/java/frc/robot/commands/CorailAspir.java
Normal file
42
src/main/java/frc/robot/commands/CorailAspir.java
Normal file
@ -0,0 +1,42 @@
|
||||
// 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.Pince;
|
||||
|
||||
/* 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 CorailAspir extends Command {
|
||||
/** Creates a new CorailAspir. */
|
||||
private Pince pince;
|
||||
public CorailAspir(Pince pince) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
this.pince = pince;
|
||||
addRequirements(pince);
|
||||
}
|
||||
|
||||
// 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() {
|
||||
|
||||
pince.aspirecoral(0.5);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
pince.aspirecoral(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
41
src/main/java/frc/robot/commands/CorailTest.java
Normal file
41
src/main/java/frc/robot/commands/CorailTest.java
Normal file
@ -0,0 +1,41 @@
|
||||
// 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.Pince;
|
||||
|
||||
/* 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 CorailTest extends Command {
|
||||
private Pince pince;
|
||||
/** Creates a new AlgueTest. */
|
||||
public CorailTest(Pince pince) {
|
||||
this.pince = pince;
|
||||
addRequirements(pince);
|
||||
// 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() {
|
||||
pince.aspirecoral(0.2);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
pince.aspirecoral(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
53
src/main/java/frc/robot/commands/CoralAlgueInspire.java
Normal file
53
src/main/java/frc/robot/commands/CoralAlgueInspire.java
Normal file
@ -0,0 +1,53 @@
|
||||
// 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.Bougie;
|
||||
import frc.robot.subsystems.Pince;
|
||||
|
||||
/* 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 CoralAlgueInspire extends Command {
|
||||
private Pince pince;
|
||||
private Bougie bougie;
|
||||
/** Creates a new CoralAlgue. */
|
||||
public CoralAlgueInspire(Pince pince, Bougie bougie) {
|
||||
this.pince = pince;
|
||||
this.bougie = bougie;
|
||||
addRequirements(pince,bougie);
|
||||
// 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() {
|
||||
pince.aspirecoral(-.5);
|
||||
if(pince.emperagealgue()>60){
|
||||
pince.aspirealgue(0);
|
||||
bougie.Bleu();
|
||||
}
|
||||
else{
|
||||
pince.aspirealgue(0.5);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
pince.aspirecoral(0);
|
||||
pince.aspirealgue(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
49
src/main/java/frc/robot/commands/CoralExpire.java
Normal file
49
src/main/java/frc/robot/commands/CoralExpire.java
Normal file
@ -0,0 +1,49 @@
|
||||
// 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.Pince;
|
||||
import frc.robot.subsystems.Bougie;
|
||||
/* 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 CoralExpire extends Command {
|
||||
private Pince pince;
|
||||
Bougie bougie;
|
||||
/** Creates a new CoralAlgue. */
|
||||
public CoralExpire(Pince pince, Bougie bougie) {
|
||||
this.pince = pince;
|
||||
this.bougie = bougie;
|
||||
addRequirements(pince,bougie);
|
||||
// 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(pince.emperagecoral() > 60){
|
||||
// pince.aspirecoral(0);
|
||||
//}
|
||||
//else{
|
||||
pince.aspirecoral(-.5);
|
||||
//bougie.Jaune();
|
||||
// }
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
pince.aspirecoral(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
52
src/main/java/frc/robot/commands/Depart.java
Normal file
52
src/main/java/frc/robot/commands/Depart.java
Normal file
@ -0,0 +1,52 @@
|
||||
// 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.Elevateur;
|
||||
import frc.robot.subsystems.Pince;
|
||||
|
||||
/* 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 Depart extends Command {
|
||||
private Elevateur elevateur;
|
||||
private Pince pince;
|
||||
/** Creates a new L2. */
|
||||
public Depart(Elevateur elevateur, Pince pince) {
|
||||
this.pince = pince;
|
||||
this.elevateur = elevateur;
|
||||
addRequirements(elevateur,pince);
|
||||
// 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(elevateur.limit2()==true){
|
||||
elevateur.vitesse(0);
|
||||
elevateur.reset();
|
||||
}
|
||||
else{
|
||||
elevateur.vitesse(.5);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
elevateur.vitesse(0);
|
||||
pince.pivote(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return elevateur.limit2()==true;
|
||||
}
|
||||
}
|
47
src/main/java/frc/robot/commands/DepartPince.java
Normal file
47
src/main/java/frc/robot/commands/DepartPince.java
Normal file
@ -0,0 +1,47 @@
|
||||
// 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.Pince;
|
||||
|
||||
/* 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 DepartPince extends Command {
|
||||
private Pince pince;
|
||||
/** Creates a new DepartPince. */
|
||||
public DepartPince(Pince pince) {
|
||||
this.pince = pince;
|
||||
addRequirements(pince);
|
||||
// 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(pince.position()==true){
|
||||
pince.pivote(0);
|
||||
pince.reset();
|
||||
}
|
||||
else{
|
||||
pince.pivote(.2);
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
pince.pivote(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
48
src/main/java/frc/robot/commands/ElevateurManuel.java
Normal file
48
src/main/java/frc/robot/commands/ElevateurManuel.java
Normal file
@ -0,0 +1,48 @@
|
||||
// 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 java.util.function.DoubleSupplier;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc.robot.subsystems.Elevateur;
|
||||
|
||||
/* 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 ElevateurManuel extends Command {
|
||||
private DoubleSupplier doubleSupplier;
|
||||
private Elevateur elevateur;
|
||||
/** Creates a new ElevateurManuel. */
|
||||
public ElevateurManuel(Elevateur elevateur,DoubleSupplier doubleSupplier) {
|
||||
this.doubleSupplier = doubleSupplier;
|
||||
this.elevateur = elevateur;
|
||||
addRequirements(elevateur);
|
||||
// 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(elevateur.limit2()==true){
|
||||
elevateur.vitesse(0);
|
||||
}
|
||||
elevateur.vitesse(doubleSupplier.getAsDouble()/7);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
elevateur.vitesse(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
79
src/main/java/frc/robot/commands/Forme3.java
Normal file
79
src/main/java/frc/robot/commands/Forme3.java
Normal file
@ -0,0 +1,79 @@
|
||||
// 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 static edu.wpi.first.units.Units.*;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc.robot.TunerConstants.TunerConstants;
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
import frc.robot.subsystems.Limelight3;
|
||||
|
||||
/* 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 Forme3 extends Command {
|
||||
private Limelight3 limelight3;
|
||||
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
|
||||
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
|
||||
private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
||||
private DoubleSupplier x;
|
||||
private DoubleSupplier y;
|
||||
/* Setting up bindings for necessary control of the swerve drive platform */
|
||||
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
|
||||
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
|
||||
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
|
||||
/** Creates a new Forme3. */
|
||||
public Forme3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
|
||||
this.limelight3 = limelight3;
|
||||
this.drivetrain = drivetrain;
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
addRequirements(limelight3,drivetrain);
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
limelight3.Forme();
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
double a = limelight3.getX();
|
||||
if(limelight3.getV() == true){
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(a/10).
|
||||
withVelocityX(x.getAsDouble()).
|
||||
withVelocityY(y.getAsDouble()));
|
||||
System.out.println(a/10);
|
||||
}
|
||||
else{
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(0).
|
||||
withVelocityX(0).
|
||||
withVelocityY(0));
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(0).
|
||||
withVelocityX(0).
|
||||
withVelocityY(0));
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
65
src/main/java/frc/robot/commands/L2.java
Normal file
65
src/main/java/frc/robot/commands/L2.java
Normal file
@ -0,0 +1,65 @@
|
||||
// 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.Elevateur;
|
||||
import frc.robot.subsystems.Pince;
|
||||
|
||||
/* 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 L2 extends Command {
|
||||
private Elevateur elevateur;
|
||||
private Pince pince;
|
||||
/** Creates a new L2. */
|
||||
public L2(Elevateur elevateur,Pince pince) {
|
||||
this.elevateur = elevateur;
|
||||
this.pince = pince;
|
||||
addRequirements(elevateur,pince);
|
||||
// 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(elevateur.position()>=500 && elevateur.position()<=510){
|
||||
elevateur.vitesse(0);
|
||||
}
|
||||
else if(elevateur.position()>=510){
|
||||
elevateur.vitesse(0.3);
|
||||
}
|
||||
else{
|
||||
elevateur.vitesse(.3);
|
||||
}
|
||||
if(pince.encodeurpivot()>=500 && pince.encodeurpivot()<=510){
|
||||
pince.pivote(0);
|
||||
|
||||
}
|
||||
else if(pince.encodeurpivot()>=510){
|
||||
pince.pivote(0.2);
|
||||
}
|
||||
else{
|
||||
pince.pivote(-0.2);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
elevateur.vitesse(0);
|
||||
pince.pivote(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
63
src/main/java/frc/robot/commands/L3.java
Normal file
63
src/main/java/frc/robot/commands/L3.java
Normal file
@ -0,0 +1,63 @@
|
||||
// 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.Elevateur;
|
||||
import frc.robot.subsystems.Pince;
|
||||
|
||||
/* 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 L3 extends Command {
|
||||
private Elevateur elevateur;
|
||||
private Pince pince;
|
||||
/** Creates a new L2. */
|
||||
public L3(Elevateur elevateur,Pince pince) {
|
||||
this.elevateur = elevateur;
|
||||
this.pince = pince;
|
||||
addRequirements(elevateur,pince);
|
||||
// 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(elevateur.position()>=700 && elevateur.position()<=710){
|
||||
elevateur.vitesse(0);
|
||||
}
|
||||
else if(elevateur.position()>=510){
|
||||
elevateur.vitesse(0.5);
|
||||
}
|
||||
else{
|
||||
elevateur.vitesse(-.5);
|
||||
}
|
||||
if(pince.encodeurpivot()>=700 && pince.encodeurpivot()<=710){
|
||||
pince.pivote(0);
|
||||
}
|
||||
else if(pince.encodeurpivot()>=710){
|
||||
pince.pivote(0.2);
|
||||
}
|
||||
else{
|
||||
pince.pivote(-0.2);
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
elevateur.vitesse(0);
|
||||
pince.pivote(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
63
src/main/java/frc/robot/commands/L4.java
Normal file
63
src/main/java/frc/robot/commands/L4.java
Normal file
@ -0,0 +1,63 @@
|
||||
// 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.Elevateur;
|
||||
import frc.robot.subsystems.Pince;
|
||||
|
||||
/* 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 L4 extends Command {
|
||||
private Elevateur elevateur;
|
||||
private Pince pince;
|
||||
/** Creates a new L2. */
|
||||
public L4(Elevateur elevateur,Pince pince) {
|
||||
this.elevateur = elevateur;
|
||||
this.pince = pince;
|
||||
addRequirements(elevateur,pince);
|
||||
// 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(elevateur.position()>=800 && elevateur.position()<=809){
|
||||
elevateur.vitesse(0);
|
||||
}
|
||||
else if(elevateur.position()>=810){
|
||||
elevateur.vitesse(0.5);
|
||||
}
|
||||
else{
|
||||
elevateur.vitesse(.5);
|
||||
}
|
||||
if(pince.encodeurpivot()>=800 && pince.encodeurpivot()<=809){
|
||||
pince.pivote(0);
|
||||
}
|
||||
else if(pince.encodeurpivot()>=810){
|
||||
pince.pivote(0.2);
|
||||
}
|
||||
else{
|
||||
pince.pivote(-0.2);
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
elevateur.vitesse(0);
|
||||
pince.pivote(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
48
src/main/java/frc/robot/commands/PinceManuel.java
Normal file
48
src/main/java/frc/robot/commands/PinceManuel.java
Normal file
@ -0,0 +1,48 @@
|
||||
// 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.Pince;
|
||||
|
||||
/* 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 PinceManuel extends Command {
|
||||
private Pince pince;
|
||||
/** Creates a new PinceManuel. */
|
||||
public PinceManuel(Pince pince
|
||||
//, DoubleSupplier doubleSupplier
|
||||
) {
|
||||
this.pince = pince;
|
||||
//this.doubleSupplier = doubleSupplier;
|
||||
addRequirements(pince);
|
||||
// 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(pince.position()){
|
||||
// pince.pivote(0);
|
||||
// }
|
||||
// else{
|
||||
pince.pivote(0.2);
|
||||
// }
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
pince.pivote(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
47
src/main/java/frc/robot/commands/PinceManuel2.java
Normal file
47
src/main/java/frc/robot/commands/PinceManuel2.java
Normal file
@ -0,0 +1,47 @@
|
||||
// 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.Pince;
|
||||
|
||||
/* 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 PinceManuel2 extends Command {
|
||||
private Pince pince;
|
||||
//private DoubleSupplier doubleSupplier;
|
||||
/** Creates a new PinceManuel. */
|
||||
public PinceManuel2(Pince pince
|
||||
|
||||
//,DoubleSupplier doubleSupplier
|
||||
) {
|
||||
this.pince = pince;
|
||||
// this.doubleSupplier = doubleSupplier;
|
||||
addRequirements(pince);
|
||||
// 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() {
|
||||
|
||||
pince.pivote(-0.2);
|
||||
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
pince.pivote(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
64
src/main/java/frc/robot/commands/StationPince.java
Normal file
64
src/main/java/frc/robot/commands/StationPince.java
Normal file
@ -0,0 +1,64 @@
|
||||
// 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.Elevateur;
|
||||
import frc.robot.subsystems.Pince;
|
||||
|
||||
/* 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 StationPince extends Command {
|
||||
|
||||
private Pince pince;
|
||||
private Elevateur elevateur;
|
||||
/** Creates a new L2Pince. */
|
||||
public StationPince(Pince pince,Elevateur elevateur) {
|
||||
this.elevateur = elevateur;
|
||||
this.pince = pince;
|
||||
addRequirements(pince,elevateur);
|
||||
// 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() {
|
||||
pince.aspirecoral(0.5);
|
||||
if(pince.encodeurpivot()>=900 && pince.encodeurpivot()<=910){
|
||||
pince.pivote(0);
|
||||
}
|
||||
else if(pince.encodeurpivot()>=910){
|
||||
pince.pivote(0.2);
|
||||
}
|
||||
else{
|
||||
pince.pivote(-0.2);
|
||||
}
|
||||
if(elevateur.position()>=400 && elevateur.position()<=410){
|
||||
elevateur.vitesse(0);
|
||||
}
|
||||
else if(elevateur.position()>=410){
|
||||
elevateur.vitesse(0.3);
|
||||
}
|
||||
else{
|
||||
elevateur.vitesse(-.3);
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
pince.pivote(0);
|
||||
pince.aspirecoral(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
41
src/main/java/frc/robot/commands/reset.java
Normal file
41
src/main/java/frc/robot/commands/reset.java
Normal file
@ -0,0 +1,41 @@
|
||||
// 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.Elevateur;
|
||||
|
||||
|
||||
|
||||
/* 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 reset extends Command {
|
||||
/** Creates a new reset. */
|
||||
private Elevateur elevateur;
|
||||
public reset(Elevateur elevateur) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
this.elevateur = elevateur;
|
||||
addRequirements(elevateur);
|
||||
}
|
||||
|
||||
// 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() {
|
||||
elevateur.reset();
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
@ -11,7 +11,7 @@ import com.ctre.phoenix.led.RainbowAnimation;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class Bougie extends SubsystemBase {
|
||||
CANdle candle = new CANdle(5);
|
||||
CANdle candle = new CANdle(23);
|
||||
CANdleConfiguration config = new CANdleConfiguration();
|
||||
RainbowAnimation rainbowAnim = new RainbowAnimation(1, 0.5, 64);
|
||||
/** Creates a new Bougie. */
|
||||
@ -20,14 +20,29 @@ public class Bougie extends SubsystemBase {
|
||||
candle.configAllSettings(config);
|
||||
}
|
||||
public void Rouge() {
|
||||
candle.setLEDs(255, 0, 0);
|
||||
candle.setLEDs(255, 0, 0,0,8,8);
|
||||
candle.setLEDs(255, 0, 0,0,24,8);
|
||||
candle.setLEDs(255, 0, 0,0,40,8);
|
||||
candle.setLEDs(255, 0, 0,0,56,8);
|
||||
}
|
||||
public void Vert() {
|
||||
candle.setLEDs(0, 255, 0);
|
||||
candle.setLEDs(0, 255, 0,0,8,8);
|
||||
candle.setLEDs(0, 255, 0,0,24,8);
|
||||
candle.setLEDs(0, 255, 0,0,40,8);
|
||||
candle.setLEDs(0, 255, 0,0,56,8);
|
||||
}
|
||||
public void Bleu() {
|
||||
candle.setLEDs(0, 0, 255);
|
||||
candle.setLEDs(0, 0, 255,0,16,8);
|
||||
candle.setLEDs(0, 0, 255,0,32,8);
|
||||
candle.setLEDs(0, 0, 255,0,48,8);
|
||||
candle.setLEDs(0, 0, 255,0,64,8);
|
||||
}
|
||||
public void Jaune() {
|
||||
candle.setLEDs(255, 215, 0,0,16,8);
|
||||
candle.setLEDs(255, 215, 0,0,32,8);
|
||||
candle.setLEDs(255, 215, 0,0,48,8);
|
||||
candle.setLEDs(255, 215, 0,0,64,8);
|
||||
}
|
||||
public void RainBow(){candle.animate(rainbowAnim);}
|
||||
public void RainBowStop(){candle.animate(null);}
|
||||
@Override
|
||||
@ -35,4 +50,3 @@ public class Bougie extends SubsystemBase {
|
||||
// This method will be called once per scheduler run
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -3,18 +3,15 @@ package frc.robot.subsystems;
|
||||
import static edu.wpi.first.units.Units.*;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import com.ctre.phoenix6.SignalLogger;
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.config.PIDConstants;
|
||||
import com.pathplanner.lib.config.RobotConfig;
|
||||
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
@ -30,7 +27,7 @@ import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain;
|
||||
|
||||
/**
|
||||
* Class that extends the Phoenix 6 SwerveDrivetrain class and implements
|
||||
* Subsystem so it can easily be used in command-based projects.
|
||||
* Subsystem so it can easily be used in command-based projeScts.
|
||||
*/
|
||||
public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
|
||||
|
||||
@ -53,6 +50,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
|
||||
|
||||
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
|
||||
|
||||
private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
|
||||
new SysIdRoutine.Config(
|
||||
null, // Use default ramp rate (1 V/s)
|
||||
@ -126,9 +124,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
),
|
||||
new PPHolonomicDriveController(
|
||||
// PID constants for translation
|
||||
new PIDConstants(10, 0, 0),
|
||||
new PIDConstants(63.167, 0, 0.54521),
|
||||
// PID constants for rotation
|
||||
new PIDConstants(7, 0, 0)
|
||||
new PIDConstants(7.9735, 0, 0.038499)
|
||||
),
|
||||
config,
|
||||
// Assume the path needs to be flipped for Red vs Blue, this is normally the case
|
||||
@ -138,10 +136,13 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
} catch (Exception ex) {
|
||||
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
|
||||
}
|
||||
}
|
||||
PPHolonomicDriveController.overrideRotationFeedback(()->{
|
||||
return 0;
|
||||
});
|
||||
}
|
||||
|
||||
/* The SysId routine to test */
|
||||
private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
|
||||
private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineSteer;
|
||||
|
||||
/**
|
||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||
|
37
src/main/java/frc/robot/subsystems/Elevateur.java
Normal file
37
src/main/java/frc/robot/subsystems/Elevateur.java
Normal file
@ -0,0 +1,37 @@
|
||||
// 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.subsystems;
|
||||
|
||||
import edu.wpi.first.networktables.GenericEntry;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
public class Elevateur extends SubsystemBase {
|
||||
/** Creates a new Elevateur. */
|
||||
public Elevateur() {}
|
||||
final SparkMax monte = new SparkMax(22, MotorType.kBrushless);
|
||||
final DigitalInput limit2 = new DigitalInput(1);
|
||||
GenericEntry teb = Shuffleboard.getTab("teb")
|
||||
.add("elevateur encodeur",position())
|
||||
.getEntry();
|
||||
public double position(){
|
||||
return monte.getEncoder().getPosition();
|
||||
}
|
||||
public void vitesse(double vitesse){
|
||||
monte.set(vitesse);
|
||||
}
|
||||
public boolean limit2(){
|
||||
return limit2.get();
|
||||
}
|
||||
public void reset(){
|
||||
monte.getEncoder().setPosition(0);
|
||||
}
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
}
|
||||
}
|
@ -1,26 +0,0 @@
|
||||
// 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.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class Grimpeur extends SubsystemBase {
|
||||
/** Creates a new Grimpeur. */
|
||||
public Grimpeur() {}
|
||||
final Spark grimpeur = new Spark(0);
|
||||
final DigitalInput limit1 = new DigitalInput(0);
|
||||
public void grimpe(double vitesse){
|
||||
grimpeur.set(vitesse);
|
||||
}
|
||||
final void stop(){
|
||||
limit1.get();
|
||||
}
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
}
|
||||
}
|
39
src/main/java/frc/robot/subsystems/Limelight3.java
Normal file
39
src/main/java/frc/robot/subsystems/Limelight3.java
Normal file
@ -0,0 +1,39 @@
|
||||
// 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.subsystems;
|
||||
|
||||
import edu.wpi.first.net.PortForwarder;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.LimelightHelpers;
|
||||
|
||||
public class Limelight3 extends SubsystemBase {
|
||||
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
|
||||
NetworkTableEntry pipeline = table.getEntry("pipeline");
|
||||
/** Creates a new Limelight3. */
|
||||
public Limelight3() {
|
||||
for(int port = 5800; port <=5807; port++){
|
||||
PortForwarder.add(port, "limelight.local", port);
|
||||
}
|
||||
}
|
||||
public double getX(){
|
||||
return LimelightHelpers.getTX("limelight-balon");
|
||||
}
|
||||
public boolean getV(){
|
||||
return LimelightHelpers.getTV("limelight-balon");
|
||||
}
|
||||
public void Apriltag(){
|
||||
pipeline.setNumber(1);
|
||||
}
|
||||
public void Forme(){
|
||||
pipeline.setNumber(0);
|
||||
}
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
}
|
||||
}
|
28
src/main/java/frc/robot/subsystems/Limelight3G.java
Normal file
28
src/main/java/frc/robot/subsystems/Limelight3G.java
Normal file
@ -0,0 +1,28 @@
|
||||
// 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.subsystems;
|
||||
|
||||
import edu.wpi.first.net.PortForwarder;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.LimelightHelpers;
|
||||
|
||||
public class Limelight3G extends SubsystemBase {
|
||||
/** Creates a new Limelight3. */
|
||||
public Limelight3G() {
|
||||
for(int port = 5800; port <=5807; port++){
|
||||
PortForwarder.add(port, "limelight.local", port);
|
||||
}
|
||||
}
|
||||
public double getX(){
|
||||
return LimelightHelpers.getTX("limelight-tag");
|
||||
}
|
||||
public boolean getV(){
|
||||
return LimelightHelpers.getTV("limelight-tag");
|
||||
}
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
}
|
||||
}
|
61
src/main/java/frc/robot/subsystems/Pince.java
Normal file
61
src/main/java/frc/robot/subsystems/Pince.java
Normal file
@ -0,0 +1,61 @@
|
||||
// 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.subsystems;
|
||||
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
|
||||
import edu.wpi.first.networktables.GenericEntry;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class Pince extends SubsystemBase {
|
||||
/** Creates a new Pince. */
|
||||
public Pince() {}
|
||||
final SparkMax coral = new SparkMax(20, MotorType.kBrushless);
|
||||
final SparkMax pivoti = new SparkMax(14, MotorType.kBrushless);
|
||||
final SparkMax algue1 = new SparkMax(16, MotorType.kBrushless);
|
||||
final SparkMax algue2 = new SparkMax(19, MotorType.kBrushless);
|
||||
final DigitalInput limit6 = new DigitalInput(0);
|
||||
GenericEntry teb = Shuffleboard.getTab("teb")
|
||||
.add("pince encodeur",encodeurpivot())
|
||||
.getEntry();
|
||||
public void aspirecoral(double vitesse){
|
||||
coral.set(vitesse);
|
||||
}
|
||||
public void pivote(double vitesse){
|
||||
pivoti.set(vitesse);
|
||||
}
|
||||
public void aspirealgue(double vitesse){
|
||||
algue2.set(vitesse);
|
||||
algue1.set(-vitesse);
|
||||
}
|
||||
public double encodeurpivot(){
|
||||
return pivoti.getEncoder().getPosition();
|
||||
}
|
||||
public boolean position(){
|
||||
return limit6.get();
|
||||
}
|
||||
public void reset(){
|
||||
pivoti.getEncoder().setPosition(0);
|
||||
}
|
||||
public double emperagecoral(){
|
||||
return coral.getOutputCurrent();
|
||||
}
|
||||
public double emperagealgue(){
|
||||
return algue1.getOutputCurrent();
|
||||
}
|
||||
public void algue1Test(double vitesse){
|
||||
algue1.set(vitesse);
|
||||
}
|
||||
public void algue2Test(double vitesse){
|
||||
algue2.set(vitesse);
|
||||
}
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
}
|
||||
}
|
@ -1,7 +1,7 @@
|
||||
{
|
||||
"fileName": "PathplannerLib-2025.2.2.json",
|
||||
"fileName": "PathplannerLib-2025.2.3.json",
|
||||
"name": "PathplannerLib",
|
||||
"version": "2025.2.2",
|
||||
"version": "2025.2.3",
|
||||
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
|
||||
"frcYear": "2025",
|
||||
"mavenUrls": [
|
||||
@ -12,7 +12,7 @@
|
||||
{
|
||||
"groupId": "com.pathplanner.lib",
|
||||
"artifactId": "PathplannerLib-java",
|
||||
"version": "2025.2.2"
|
||||
"version": "2025.2.3"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [],
|
||||
@ -20,7 +20,7 @@
|
||||
{
|
||||
"groupId": "com.pathplanner.lib",
|
||||
"artifactId": "PathplannerLib-cpp",
|
||||
"version": "2025.2.2",
|
||||
"version": "2025.2.3",
|
||||
"libName": "PathplannerLib",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
@ -1,7 +1,7 @@
|
||||
{
|
||||
"fileName": "Phoenix6-25.2.1.json",
|
||||
"fileName": "Phoenix6-25.2.2.json",
|
||||
"name": "CTRE-Phoenix (v6)",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"frcYear": "2025",
|
||||
"uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
|
||||
"mavenUrls": [
|
||||
@ -19,14 +19,14 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6",
|
||||
"artifactId": "wpiapi-java",
|
||||
"version": "25.2.1"
|
||||
"version": "25.2.2"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6",
|
||||
"artifactId": "api-cpp",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -40,7 +40,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6",
|
||||
"artifactId": "tools",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -54,7 +54,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "api-cpp-sim",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -68,7 +68,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "tools-sim",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -82,7 +82,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simTalonSRX",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -96,7 +96,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simVictorSPX",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -110,7 +110,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simPigeonIMU",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -124,7 +124,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simCANCoder",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -138,7 +138,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProTalonFX",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -152,7 +152,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProTalonFXS",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -166,7 +166,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProCANcoder",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -180,7 +180,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProPigeon2",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -194,7 +194,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProCANrange",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@ -210,7 +210,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6",
|
||||
"artifactId": "wpiapi-cpp",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_Phoenix6_WPI",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@ -226,7 +226,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6",
|
||||
"artifactId": "tools",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_PhoenixTools",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@ -242,7 +242,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "wpiapi-cpp-sim",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_Phoenix6_WPISim",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@ -258,7 +258,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "tools-sim",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_PhoenixTools_Sim",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@ -274,7 +274,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simTalonSRX",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_SimTalonSRX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@ -290,7 +290,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simVictorSPX",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_SimVictorSPX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@ -306,7 +306,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simPigeonIMU",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_SimPigeonIMU",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@ -322,7 +322,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simCANCoder",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_SimCANCoder",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@ -338,7 +338,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProTalonFX",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_SimProTalonFX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@ -354,7 +354,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProTalonFXS",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_SimProTalonFXS",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@ -370,7 +370,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProCANcoder",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_SimProCANcoder",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@ -386,7 +386,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProPigeon2",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_SimProPigeon2",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
@ -402,7 +402,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix6.sim",
|
||||
"artifactId": "simProCANrange",
|
||||
"version": "25.2.1",
|
||||
"version": "25.2.2",
|
||||
"libName": "CTRE_SimProCANrange",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
Reference in New Issue
Block a user