mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Work in progress
This commit is contained in:
mimigamin
@@ -30,18 +30,18 @@ import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
import frc4388.robot.commands.Swerve.StayInPosition;
|
||||
import frc4388.robot.commands.alignment.AutoAlign;
|
||||
import frc4388.robot.constants.Constants;
|
||||
import frc4388.robot.constants.Constants.OIConstants;
|
||||
import frc4388.robot.constants.Constants.SimConstants.Mode;
|
||||
// Subsystems
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.Lidar;
|
||||
import frc4388.robot.subsystems.intake.Intake;
|
||||
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
|
||||
import frc4388.robot.subsystems.led.LED;
|
||||
import frc4388.robot.subsystems.shooter.Shooter;
|
||||
import frc4388.robot.subsystems.shooter.ShooterConstants;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.robot.subsystems.vision.Lidar;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import frc4388.utility.compute.FieldPositions;
|
||||
@@ -84,148 +84,159 @@ public class RobotContainer {
|
||||
// private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
|
||||
|
||||
// public List<Subsystem> subsystems = new ArrayList<>();
|
||||
|
||||
// ! Teleop Commands
|
||||
public void stop() {
|
||||
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
|
||||
m_robotSwerveDrive.stopModules();
|
||||
Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
|
||||
}
|
||||
|
||||
// ! /* Autos */
|
||||
private SendableChooser<String> autoChooser;
|
||||
private Command autoCommand;
|
||||
|
||||
|
||||
private Command IntakeExtended = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake)
|
||||
);
|
||||
|
||||
// private Command LidarIntake = new SequentialCommandGroup(
|
||||
// // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball
|
||||
// // RobotIntakeDown,
|
||||
// // new WaitCommand(1),
|
||||
// // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
|
||||
// new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true)
|
||||
|
||||
// // new RunCommand(
|
||||
// // () -> m_robotSwerveDrive.driveWithInput(
|
||||
// // m_lidar.getClosestBall(),
|
||||
// // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
|
||||
// // false
|
||||
// // ),
|
||||
// // m_robotSwerveDrive
|
||||
// // )
|
||||
// // .withTimeout(5.0)
|
||||
// // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
|
||||
// );
|
||||
|
||||
private Command RobotRev = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
|
||||
IntakeExtended,
|
||||
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
|
||||
);
|
||||
|
||||
private Command IntakeRetracted = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
|
||||
);
|
||||
|
||||
private Command RobotShoot = new SequentialCommandGroup(
|
||||
// TEST NEW AUTO ALIGN
|
||||
//new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false),
|
||||
new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed),
|
||||
new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
|
||||
new WaitCommand(2),
|
||||
IntakeRetracted,
|
||||
new WaitCommand(5),
|
||||
new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter),
|
||||
new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter)
|
||||
);
|
||||
|
||||
private final StayInPosition m_stayInPosition = new StayInPosition(m_robotSwerveDrive);
|
||||
|
||||
public RobotContainer() {
|
||||
|
||||
configureButtonBindings();
|
||||
|
||||
// Called on first robot enable
|
||||
DeferredBlock.addBlock(() -> {
|
||||
m_robotSwerveDrive.resetGyro();
|
||||
}, false);
|
||||
|
||||
// Called on every robot enable
|
||||
DeferredBlock.addBlock(() -> {
|
||||
TimesNegativeOne.update();
|
||||
FieldPositions.update();
|
||||
|
||||
m_robotIntake.io.updateGains();
|
||||
m_robotShooter.io.updateGains();
|
||||
}, true);
|
||||
|
||||
NamedCommands.registerCommand("Robot Rev Up", RobotRev);
|
||||
NamedCommands.registerCommand("Intake Retracted", IntakeRetracted);
|
||||
NamedCommands.registerCommand("Robot Shoot", RobotShoot);
|
||||
// NamedCommands.registerCommand("Lidar Intake", LidarIntake);
|
||||
NamedCommands.registerCommand("Intake Extended", IntakeExtended);
|
||||
|
||||
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
|
||||
// Drive normally
|
||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.driveWithInput(
|
||||
getDeadbandedDriverController().getLeft(),
|
||||
getDeadbandedDriverController().getRight(),true);
|
||||
|
||||
}, m_robotSwerveDrive)
|
||||
.withName("SwerveDrive DefaultCommand"));
|
||||
|
||||
m_robotSwerveDrive.setToSlow();
|
||||
|
||||
makeAutoChooser();
|
||||
SmartDashboard.putData("Auto Chooser", autoChooser);
|
||||
|
||||
}
|
||||
private Pose2d currentPose = new Pose2d(0, 0, new Rotation2d());
|
||||
// ! Teleop Commands
|
||||
public void stop() {
|
||||
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
|
||||
m_robotSwerveDrive.stopModules();
|
||||
Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be
|
||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
|
||||
//Driver controls
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
// ! /* Autos */
|
||||
private SendableChooser<String> autoChooser;
|
||||
private Command autoCommand;
|
||||
|
||||
|
||||
private Command IntakeExtended = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake)
|
||||
);
|
||||
|
||||
// private Command LidarIntake = new SequentialCommandGroup(
|
||||
// // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball
|
||||
// // RobotIntakeDown,
|
||||
// // new WaitCommand(1),
|
||||
// // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
|
||||
// new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true)
|
||||
|
||||
// // new RunCommand(
|
||||
// // () -> m_robotSwerveDrive.driveWithInput(
|
||||
// // m_lidar.getClosestBall(),
|
||||
// // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
|
||||
// // false
|
||||
// // ),
|
||||
// // m_robotSwerveDrive
|
||||
// // )
|
||||
// // .withTimeout(5.0)
|
||||
// // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
|
||||
// );
|
||||
|
||||
private Command RobotRev = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
|
||||
IntakeExtended,
|
||||
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
|
||||
);
|
||||
|
||||
private Command IntakeRetracted = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
|
||||
);
|
||||
|
||||
private Command RobotShoot = new SequentialCommandGroup(
|
||||
// TEST NEW AUTO ALIGN
|
||||
//new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false),
|
||||
new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed),
|
||||
new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
|
||||
new WaitCommand(2),
|
||||
IntakeRetracted,
|
||||
new WaitCommand(5),
|
||||
new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter),
|
||||
new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter)
|
||||
);
|
||||
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
public RobotContainer() {
|
||||
|
||||
configureButtonBindings();
|
||||
|
||||
// Called on first robot enable
|
||||
DeferredBlock.addBlock(() -> {
|
||||
m_robotSwerveDrive.resetGyro();
|
||||
}, false);
|
||||
|
||||
// Called on every robot enable
|
||||
DeferredBlock.addBlock(() -> {
|
||||
TimesNegativeOne.update();
|
||||
FieldPositions.update();
|
||||
|
||||
m_robotIntake.io.updateGains();
|
||||
m_robotShooter.io.updateGains();
|
||||
}));
|
||||
|
||||
}, true);
|
||||
|
||||
NamedCommands.registerCommand("Robot Rev Up", RobotRev);
|
||||
NamedCommands.registerCommand("Intake Retracted", IntakeRetracted);
|
||||
NamedCommands.registerCommand("Robot Shoot", RobotShoot);
|
||||
// NamedCommands.registerCommand("Lidar Intake", LidarIntake);
|
||||
NamedCommands.registerCommand("Intake Extended", IntakeExtended);
|
||||
|
||||
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
|
||||
// Drive normally
|
||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.driveWithInput(
|
||||
getDeadbandedDriverController().getLeft(),
|
||||
getDeadbandedDriverController().getRight(),true);
|
||||
|
||||
}, m_robotSwerveDrive)
|
||||
.withName("SwerveDrive DefaultCommand"));
|
||||
|
||||
// IF the driver is holding the left trigger, intake driving
|
||||
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
||||
.whileTrue(new RunCommand(
|
||||
() -> {
|
||||
m_robotSwerveDrive.driveIntakeOrientation(
|
||||
getDeadbandedDriverController().getLeft(),
|
||||
getDeadbandedDriverController().getRight()
|
||||
|
||||
);
|
||||
}, m_robotSwerveDrive))
|
||||
m_robotSwerveDrive.setToSlow();
|
||||
|
||||
makeAutoChooser();
|
||||
SmartDashboard.putData("Auto Chooser", autoChooser);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be
|
||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
|
||||
//Driver controls
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
m_robotIntake.io.updateGains();
|
||||
m_robotShooter.io.updateGains();
|
||||
}));
|
||||
|
||||
|
||||
// TEST-> the driver is holding the left trigger, drive slow and rotation up
|
||||
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
m_robotSwerveDrive.setToSlow();
|
||||
m_robotSwerveDrive.shiftUpRot();
|
||||
}))
|
||||
.onFalse(new InstantCommand(() -> {
|
||||
m_robotSwerveDrive.setToFast();
|
||||
m_robotSwerveDrive.shiftDownRot();
|
||||
}));
|
||||
|
||||
//TEST - > Swerve drive pid on position
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
currentPose = m_robotSwerveDrive.getCurrentPose();
|
||||
}))
|
||||
.whileTrue(new RunCommand(() -> {
|
||||
m_stayInPosition.goToTargetPose(currentPose);
|
||||
}, m_robotSwerveDrive))
|
||||
.onFalse(new InstantCommand(() -> {
|
||||
m_robotSwerveDrive.softStop();
|
||||
}));
|
||||
|
||||
@@ -1,49 +0,0 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.time.Instant;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
// Command to repeat a joystick movement for a specific time.
|
||||
public class MoveForTimeCommand extends Command {
|
||||
private final SwerveDrive swerveDrive;
|
||||
private final Translation2d leftStick;
|
||||
private final Translation2d rightStick;
|
||||
private final long duration;
|
||||
private final boolean robotRelative;
|
||||
|
||||
private Instant startTime;
|
||||
|
||||
public MoveForTimeCommand(
|
||||
SwerveDrive swerveDrive,
|
||||
Translation2d leftStick,
|
||||
Translation2d rightStick,
|
||||
long millis,
|
||||
boolean robotRelative) {
|
||||
|
||||
addRequirements(swerveDrive);
|
||||
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.leftStick = leftStick;
|
||||
this.rightStick = rightStick;
|
||||
this.duration = millis;
|
||||
this.robotRelative = robotRelative;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
startTime = Instant.now();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration;
|
||||
}
|
||||
}
|
||||
@@ -1,45 +0,0 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
// Command to repeat a joystick movement for a specific time.
|
||||
public class MoveUntilSuply extends Command {
|
||||
private final SwerveDrive swerveDrive;
|
||||
private final Translation2d leftStick;
|
||||
private final Translation2d rightStick;
|
||||
private final Supplier<Boolean> truth;
|
||||
private final boolean robotRelative;
|
||||
|
||||
public MoveUntilSuply(
|
||||
SwerveDrive swerveDrive,
|
||||
Translation2d leftStick,
|
||||
Translation2d rightStick,
|
||||
Supplier<Boolean> truth,
|
||||
boolean robotRelative) {
|
||||
addRequirements(swerveDrive);
|
||||
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.leftStick = leftStick;
|
||||
this.rightStick = rightStick;
|
||||
this.truth = truth;
|
||||
this.robotRelative = robotRelative;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return truth.get();
|
||||
}
|
||||
}
|
||||
@@ -1,35 +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 frc4388.robot.commands.Swerve;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.commands.PID;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
public class RotateToAngle extends PID {
|
||||
|
||||
SwerveDrive drive;
|
||||
double targetAngle;
|
||||
|
||||
/** Creates a new RotateToAngle. */
|
||||
public RotateToAngle(SwerveDrive drive, double targetAngle) {
|
||||
super(0.3, 0.0, 0.0, 0.0, 1);
|
||||
|
||||
this.drive = drive;
|
||||
this.targetAngle = targetAngle;
|
||||
|
||||
addRequirements(drive);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
return targetAngle - drive.getGyroAngle();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
// 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 frc4388.robot.commands.Swerve;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.commands.PID;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
public class StayInPosition extends PID {
|
||||
|
||||
SwerveDrive drive;
|
||||
|
||||
/** Creates a new StayInPosition. */
|
||||
public StayInPosition(SwerveDrive drive) {
|
||||
super(0.3, 0.0, 0.0, 0.0, 1);
|
||||
|
||||
this.drive = drive;
|
||||
|
||||
addRequirements(drive);
|
||||
}
|
||||
|
||||
public void goToTargetPose(Pose2d targetPose){
|
||||
Pose2d currentPose = drive.getCurrentPose();
|
||||
double translationX = targetPose.getX() - currentPose.getX();
|
||||
double translationY = targetPose.getY() - currentPose.getY();
|
||||
Rotation2d deltaRotation = targetPose.getRotation().minus(currentPose.getRotation());
|
||||
Translation2d driveTranslation = new Translation2d(translationX, translationY);
|
||||
|
||||
drive.driveFieldAngle(driveTranslation, deltaRotation);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
return 0;
|
||||
// return targetAngle - drive.getGyroAngle();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
// drive.driveWithInput(new Pose2d(new Translation2d(0.0, 0.0), new Rotation2d(output / Math.abs(getError()))));
|
||||
}
|
||||
|
||||
// @Override
|
||||
// public boolean isFinished() {
|
||||
// Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation();
|
||||
// double ballAngleDeg = m_lidar.getLatestBallAngleDegrees();
|
||||
|
||||
// // TODO: Tune
|
||||
// return Math.abs(curRot.getDegrees() +ballAngleDeg) < 5;
|
||||
// }
|
||||
|
||||
}
|
||||
@@ -1,197 +0,0 @@
|
||||
package frc4388.robot.commands.Swerve;
|
||||
|
||||
import java.io.FileInputStream;
|
||||
import java.util.ArrayList;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.utility.compute.DataUtils;
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
|
||||
|
||||
|
||||
/**
|
||||
* The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s
|
||||
* @author Zachary Wilke
|
||||
*/
|
||||
public class neoJoystickPlayback extends Command {
|
||||
private final SwerveDrive swerve;
|
||||
private final VirtualController[] controllers;
|
||||
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
|
||||
private final Supplier<String> filenameGetter;
|
||||
private String filename;
|
||||
private int frame_index = 0;
|
||||
// private long startTime = 0;
|
||||
// private long playbackTime = 0;
|
||||
private boolean m_finished = false; // ! There is no better way.
|
||||
private boolean m_shouldfree = false; // should free memory on ending
|
||||
|
||||
private byte m_numAxes = 0;
|
||||
private byte m_numPOVs = 0;
|
||||
private byte m_numControllers = 0;
|
||||
private short m_numFrames = -1;
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
* @param shouldfree Unloads the auto on compleation or intruption.
|
||||
* @param instantload Load the auto on object instantiation
|
||||
*/
|
||||
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
|
||||
this.swerve = swerve;
|
||||
this.filenameGetter = filenameGetter;
|
||||
this.controllers = controllers;
|
||||
this.m_shouldfree = shouldfree;
|
||||
|
||||
if (instantload) loadAuto();
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param filename a String containing the name of the auto file you wish to playback.
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
* @param shouldfree unloads the auto on compleation or intruption.
|
||||
* @param instantload load the auto on object instantiation
|
||||
*/
|
||||
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
|
||||
this(swerve, () -> filename, controllers, shouldfree, instantload);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
*/
|
||||
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers) {
|
||||
this(swerve, filenameGetter, controllers, true, false);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param filename a String containing the name of the auto file you wish to playback.
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
*/
|
||||
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) {
|
||||
this(swerve, () -> filename, controllers, true, false);
|
||||
}
|
||||
|
||||
/**
|
||||
* Load the auto file from disk into memory
|
||||
* @return Returns true if loading was successful, else wise; return false
|
||||
* @implNote if the auto is already loaded, it will return true.
|
||||
*/
|
||||
public boolean loadAuto() {
|
||||
filename = filenameGetter.get();
|
||||
try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) {
|
||||
if (m_numFrames != -1 && m_numFrames == frames.size()) {
|
||||
System.out.println("AUTOPLAYBACK: Auto Already loaded.");
|
||||
return true;
|
||||
}
|
||||
|
||||
m_numAxes = stream.readNBytes(1)[0];
|
||||
m_numPOVs = stream.readNBytes(1)[0];
|
||||
m_numControllers = stream.readNBytes(1)[0];
|
||||
m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
||||
|
||||
if (m_numControllers > controllers.length) {
|
||||
System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers
|
||||
+ " virtual controllers but only " + controllers.length + " were given");
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < m_numFrames; i++) {
|
||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
||||
for (int j = 0; j < m_numControllers; j++) {
|
||||
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
|
||||
double[] axes = new double[m_numAxes];
|
||||
for (int k = 0; k < m_numAxes; k++) { // we love third level for loops.
|
||||
axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8));
|
||||
}
|
||||
short button = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
||||
short[] POV = new short[m_numPOVs];
|
||||
for (int k = 0; k < m_numPOVs; k++) {
|
||||
POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
||||
}
|
||||
controllerFrame.axes = axes;
|
||||
controllerFrame.button = button;
|
||||
controllerFrame.POV = POV;
|
||||
frame.controllerFrames[j] = controllerFrame;
|
||||
}
|
||||
frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4));
|
||||
frames.add(frame);
|
||||
}
|
||||
|
||||
System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long");
|
||||
return true;
|
||||
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`');
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Unloads the auto.
|
||||
*/
|
||||
public void unloadAuto() {
|
||||
System.out.println("AUTOPLAYBACK: Auto unloaded");
|
||||
frames.clear();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
// startTime = System.currentTimeMillis();
|
||||
// playbackTime = 0;
|
||||
frame_index = 0;
|
||||
|
||||
m_finished = !loadAuto();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
if (frame_index >= m_numFrames) m_finished = true;
|
||||
if (m_finished) return;
|
||||
|
||||
// if (frame_index == 0) {
|
||||
// startTime = System.currentTimeMillis();
|
||||
// playbackTime = 0;
|
||||
// } else {
|
||||
// playbackTime = System.currentTimeMillis() - startTime;
|
||||
// }
|
||||
|
||||
AutoRecordingFrame frame = frames.get(frame_index);
|
||||
for (int i = 0; i < controllers.length; i++) {
|
||||
AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i];
|
||||
controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV);
|
||||
if (i == 0) {
|
||||
this.swerve.driveWithInput(
|
||||
new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)),
|
||||
new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)),
|
||||
true);
|
||||
}
|
||||
}
|
||||
frame_index++;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
for (VirtualController controller : controllers) controller.zeroControls();
|
||||
swerve.stopModules();
|
||||
if (m_shouldfree) unloadAuto();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_finished;
|
||||
}
|
||||
}
|
||||
@@ -1,129 +0,0 @@
|
||||
package frc4388.robot.commands.Swerve;
|
||||
|
||||
import java.io.FileOutputStream;
|
||||
import java.util.ArrayList;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.utility.compute.DataUtils;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
|
||||
|
||||
/**
|
||||
* The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s
|
||||
* @author Zachary Wilke
|
||||
*/
|
||||
public class neoJoystickRecorder extends Command {
|
||||
private final SwerveDrive swerve;
|
||||
private final XboxController[] controllers;
|
||||
private String filename;
|
||||
private final Supplier<String> filenameGetter;
|
||||
private long startTime = -1;
|
||||
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
|
||||
*/
|
||||
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier<String> filenameGetter) {
|
||||
this.swerve = swerve;
|
||||
this.controllers = controllers;
|
||||
this.filenameGetter = filenameGetter;
|
||||
this.filename = "";
|
||||
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
* @param filename a String containing the name of the auto file you wish to playback.
|
||||
*/
|
||||
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) {
|
||||
this(swerve, controllers, () -> filename);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
frames.clear();
|
||||
|
||||
this.startTime = System.currentTimeMillis();
|
||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
||||
frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()};
|
||||
frames.add(frame);
|
||||
this.filename = this.filenameGetter.get();
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
System.out.println("AUTORECORD: RECORDING");
|
||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
||||
frame.timeStamp = (int) (System.currentTimeMillis() - startTime);
|
||||
for (int i = 0; i < controllers.length; i++) {
|
||||
XboxController controller = controllers[i];
|
||||
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
|
||||
double[] axes = {controller.getLeftX(), controller.getLeftY(),
|
||||
controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(),
|
||||
controller.getRightX(), controller.getRightY()};
|
||||
short button = 0;
|
||||
for (int j = 0; j < 10; j++)
|
||||
if (controller.getRawButton(j+1))
|
||||
button |= 1 << j;
|
||||
short[] POV = {(short)(controller.getPOV())};
|
||||
controllerFrame.axes = axes;
|
||||
controllerFrame.button = button;
|
||||
controllerFrame.POV = POV;
|
||||
frame.controllerFrames[i] = controllerFrame;
|
||||
}
|
||||
|
||||
frames.add(frame);
|
||||
|
||||
swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]),
|
||||
new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]),
|
||||
true); // Really jank way of doing this.
|
||||
|
||||
}
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) {
|
||||
// header: size of 0x5
|
||||
// byte Number of axes per controller
|
||||
// byte Number of POVs per controller
|
||||
// byte Number of controllers
|
||||
// short Number of frames
|
||||
stream.write(new byte[]{6, 1, (byte) controllers.length});
|
||||
stream.write(DataUtils.shortToByteArray((short) frames.size()));
|
||||
|
||||
// frame
|
||||
// controller frame * number of controllers
|
||||
// int unix time stamp.
|
||||
for (AutoRecordingFrame frame : frames) {
|
||||
// controller frame
|
||||
// double axis * Number of axes per controller
|
||||
// short button states
|
||||
// short POV * Number of POVs per controller
|
||||
for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) {
|
||||
for (double axis: controllerFrame.axes) {
|
||||
stream.write(DataUtils.doubleToByteArray(axis));
|
||||
}
|
||||
stream.write(DataUtils.shortToByteArray(controllerFrame.button));
|
||||
for (short POV: controllerFrame.POV) {
|
||||
stream.write(DataUtils.shortToByteArray(POV));
|
||||
}
|
||||
}
|
||||
stream.write(DataUtils.intToByteArray(frame.timeStamp));
|
||||
}
|
||||
System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long.");
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,104 +0,0 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
// 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.
|
||||
|
||||
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
/**
|
||||
* A command composition that runs one of two commands, depending on the value of the given
|
||||
* condition when this command is initialized.
|
||||
*
|
||||
* <p>The rules for command compositions apply: command instances that are passed to it cannot be
|
||||
* added to any other composition or scheduled individually, and the composition requires all
|
||||
* subsystems its components require.
|
||||
*
|
||||
* <p>This class is provided by the NewCommands VendorDep
|
||||
*/
|
||||
public class WhileTrueCommand extends Command {
|
||||
private final Command m_whileTrue;
|
||||
private final BooleanSupplier m_condition;
|
||||
|
||||
/**
|
||||
* Creates a new WhileTrueCommand.
|
||||
*
|
||||
* @param whileTrue the command to run while the condition is true
|
||||
* @param condition the condition to determine which command to run
|
||||
*/
|
||||
@SuppressWarnings("this-escape")
|
||||
public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
|
||||
m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand");
|
||||
m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
|
||||
|
||||
//CommandScheduler.getInstance().registerComposedCommands(whileTrue);
|
||||
|
||||
// addRequirements(whileTrue.getRequirements());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
if(m_condition.getAsBoolean())
|
||||
m_whileTrue.initialize();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_whileTrue.execute();
|
||||
|
||||
System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean());
|
||||
|
||||
if(!m_whileTrue.isFinished())
|
||||
return;
|
||||
|
||||
if(m_condition.getAsBoolean()){
|
||||
m_whileTrue.end(false);
|
||||
m_whileTrue.initialize();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_whileTrue.end(interrupted);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return !m_condition.getAsBoolean() && m_whileTrue.isFinished();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return m_whileTrue.runsWhenDisabled();
|
||||
}
|
||||
|
||||
@Override
|
||||
public InterruptionBehavior getInterruptionBehavior() {
|
||||
if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
|
||||
return InterruptionBehavior.kCancelSelf;
|
||||
} else {
|
||||
return InterruptionBehavior.kCancelIncoming;
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
super.initSendable(builder);
|
||||
builder.addStringProperty("whileTrue", m_whileTrue::getName, null);
|
||||
builder.addStringProperty(
|
||||
"selected",
|
||||
() -> {
|
||||
if (m_whileTrue == null) {
|
||||
return "null";
|
||||
} else {
|
||||
return m_whileTrue.getName();
|
||||
}
|
||||
},
|
||||
null);
|
||||
}
|
||||
}
|
||||
@@ -6,8 +6,8 @@ import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.subsystems.Lidar;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.robot.subsystems.vision.Lidar;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.utility.compute.FieldPositions;
|
||||
import frc4388.utility.structs.Gains;
|
||||
|
||||
@@ -1,37 +0,0 @@
|
||||
package frc4388.robot.commands.alignment;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
public class RotTo45 extends Command {
|
||||
|
||||
SwerveDrive m_SwerveDrive;
|
||||
Rotation2d targetAngle;
|
||||
|
||||
|
||||
public RotTo45(SwerveDrive swerveDrive) {
|
||||
m_SwerveDrive = swerveDrive;
|
||||
|
||||
addRequirements(swerveDrive);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
targetAngle = new Rotation2d();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_SwerveDrive.driveRelativeAngle(new Translation2d(), targetAngle);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation();
|
||||
// TODO: Tune
|
||||
return Math.abs(curRot.getDegrees() - targetAngle.getDegrees()) < 5;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,36 +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 frc4388.robot.commands.wait;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
|
||||
/* 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 waitSupplier extends Command {
|
||||
/** Creates a new waitSupplier. */
|
||||
private final Supplier<Boolean> truth;
|
||||
public waitSupplier(Supplier<Boolean> truth) {
|
||||
this.truth = truth;
|
||||
}
|
||||
|
||||
// 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() {}
|
||||
|
||||
// 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 truth.get();
|
||||
}
|
||||
}
|
||||
+1
-1
@@ -5,7 +5,7 @@
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
package frc4388.robot.subsystems.led;
|
||||
|
||||
import java.util.concurrent.TimeUnit;
|
||||
|
||||
@@ -15,8 +15,8 @@ import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import edu.wpi.first.units.measure.Current;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.intake.Intake;
|
||||
import frc4388.robot.subsystems.led.LED;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.utility.compute.FieldPositions;
|
||||
|
||||
|
||||
@@ -377,6 +377,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
driveFieldAngle(leftStick, ang);
|
||||
}
|
||||
|
||||
public Pose2d getCurrentPose(){
|
||||
return state.currentPose;
|
||||
}
|
||||
|
||||
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
|
||||
leftStick = leftStick.rotateBy(heading);
|
||||
|
||||
@@ -515,6 +519,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
|
||||
public void setPercentOutput(double speed) {
|
||||
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
|
||||
gear_index = -1;
|
||||
|
||||
+3
-3
@@ -1,4 +1,4 @@
|
||||
package frc4388.robot.subsystems;
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.LinkedList;
|
||||
@@ -20,8 +20,8 @@ import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.subsystems.RPLidarA1.PolarPoint;
|
||||
import frc4388.robot.subsystems.RPLidarA1.ScanListener;
|
||||
import frc4388.robot.subsystems.vision.RPLidarA1.PolarPoint;
|
||||
import frc4388.robot.subsystems.vision.RPLidarA1.ScanListener;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
import frc4388.utility.status.FaultA1M8;
|
||||
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
package frc4388.robot.subsystems;
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import com.fazecast.jSerialComm.SerialPort;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
Reference in New Issue
Block a user