limelight
This commit is contained in:
@ -7,6 +7,7 @@ package frc.robot;
|
||||
import static edu.wpi.first.units.Units.*;
|
||||
|
||||
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.PointWheelsAt;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
@ -15,12 +16,15 @@ import com.pathplanner.lib.auto.NamedCommands;
|
||||
|
||||
import edu.wpi.first.cameraserver.CameraServer;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
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.InstantCommand;
|
||||
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;
|
||||
|
||||
@ -56,7 +60,12 @@ public class RobotContainer {
|
||||
Bougie bougie = new Bougie();
|
||||
Limelight3G limelight3g = new Limelight3G();
|
||||
Limelight3 limelight3 = new Limelight3();
|
||||
public RobotContainer() {
|
||||
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();
|
||||
@ -68,25 +77,25 @@ public class RobotContainer {
|
||||
drivetrain.setDefaultCommand(
|
||||
// Drivetrain will execute this command periodically
|
||||
drivetrain.applyRequest(() ->
|
||||
drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.15)) // Drive forward with negative Y (forward)
|
||||
.withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.15)) // Drive left with negative X (left)
|
||||
.withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.1)) // 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.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
|
||||
manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain));
|
||||
manette1.rightTrigger().whileTrue(new Forme3(limelight3));
|
||||
manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g));
|
||||
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 ParallelCommandGroup(
|
||||
autoChooser.getSelected(),
|
||||
new RainBow(bougie));
|
||||
}
|
||||
return new SequentialCommandGroup(autoChooser.getSelected(),
|
||||
new RainBow(bougie));
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -5,6 +5,8 @@
|
||||
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;
|
||||
|
||||
@ -18,16 +20,19 @@ 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 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) {
|
||||
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.
|
||||
@ -42,24 +47,30 @@ public class AprilTag3 extends Command {
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
double a = limelight3.getX();
|
||||
|
||||
if(limelight3.getV() == true){
|
||||
drivetrain.applyRequest(() ->
|
||||
drive.withRotationalRate(limelight3.getX()/48));
|
||||
System.out.println(limelight3.getX()/48);
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(a/10).
|
||||
withVelocityX(x.getAsDouble()).
|
||||
withVelocityY(y.getAsDouble()));
|
||||
System.out.println(a/10);
|
||||
}
|
||||
else{
|
||||
drivetrain.applyRequest(() ->
|
||||
drive.withRotationalRate(0)
|
||||
);
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(0).
|
||||
withVelocityX(0).
|
||||
withVelocityY(0));
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
drivetrain.applyRequest(() ->
|
||||
drive.withRotationalRate(0));
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(0)
|
||||
.withVelocityX(0)
|
||||
.withVelocityY(0));
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
|
@ -4,29 +4,37 @@
|
||||
|
||||
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 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) {
|
||||
public AprilTag3G(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
|
||||
this.limelight3g = limelight3g;
|
||||
addRequirements(limelight3g);
|
||||
this.drivetrain = drivetrain;
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
addRequirements(limelight3g,drivetrain);
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
@ -37,18 +45,29 @@ public class AprilTag3G extends Command {
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
double a = limelight3g.getX();
|
||||
if(limelight3g.getV() == true){
|
||||
drive.withRotationalRate(limelight3g.getX()/48);
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(-a/5).
|
||||
withVelocityX(x.getAsDouble()).
|
||||
withVelocityY(y.getAsDouble()));
|
||||
System.out.println(a/5);
|
||||
}
|
||||
else{
|
||||
drive.withRotationalRate(0);
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(0).
|
||||
withVelocityX(0).
|
||||
withVelocityY(0));
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
drive.withRotationalRate(0);
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(0).
|
||||
withVelocityX(0).
|
||||
withVelocityY(0));
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
|
@ -4,28 +4,36 @@
|
||||
|
||||
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 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) {
|
||||
public Forme3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
|
||||
this.limelight3 = limelight3;
|
||||
|
||||
addRequirements(limelight3);
|
||||
this.drivetrain = drivetrain;
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
addRequirements(limelight3,drivetrain);
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
@ -38,18 +46,29 @@ public class Forme3 extends Command {
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
double a = limelight3.getX();
|
||||
if(limelight3.getV() == true){
|
||||
drive.withRotationalRate(limelight3.getX()/48);
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(a/10).
|
||||
withVelocityX(x.getAsDouble()).
|
||||
withVelocityY(y.getAsDouble()));
|
||||
System.out.println(a/10);
|
||||
}
|
||||
else{
|
||||
drive.withRotationalRate(0);
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(0).
|
||||
withVelocityX(0).
|
||||
withVelocityY(0));
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
drive.withRotationalRate(0);
|
||||
drivetrain.setControl(drive.
|
||||
withRotationalRate(0).
|
||||
withVelocityX(0).
|
||||
withVelocityY(0));
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
|
@ -6,6 +6,7 @@ import java.util.function.Supplier;
|
||||
|
||||
import com.ctre.phoenix6.SignalLogger;
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
@ -53,6 +54,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)
|
||||
@ -255,7 +257,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
|
||||
return m_sysIdRoutineToApply.dynamic(direction);
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
/*
|
||||
|
Reference in New Issue
Block a user