Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

L + lobotomy + checking for packet loss on a protocol that has no basic checks for packet loss

Browse Source
This commit is contained in:
C4llSiqn
2024-11-27 17:18:59 -07:00
parent f74be4b547
commit 334f286c22
33 changed files with 90 additions and 3258 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
-164
View File
@@ -7,11 +7,6 @@
package frc4388.robot;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be
@@ -21,168 +16,9 @@ import frc4388.utility.LEDPatterns;
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class SwerveDriveConstants {
public static final double MAX_ROT_SPEED = 3.5;
public static final double AUTO_MAX_ROT_SPEED = 1.5;
public static final double MIN_ROT_SPEED = 1.0;
public static double ROTATION_SPEED = MAX_ROT_SPEED;
public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
public static final String CANBUS_NAME = "IDK";
public static final double CORRECTION_MIN = 10;
public static final double CORRECTION_MAX = 50;
public static final double[] GEARS = {0.25, 0.5, 1.0};
public static final double SLOW_SPEED = 0.25;
public static final double FAST_SPEED = 0.5;
public static final double TURBO_SPEED = 1.0;
public static final class DefaultSwerveRotOffsets {
public static final double FRONT_LEFT_ROT_OFFSET = 0.36962890625 + 0.5;
public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 + 0.5;
public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875 + 0.5;
public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 + 0.5;
}
public static final class IDs {
public static final int RIGHT_FRONT_WHEEL_ID = 2;
public static final int RIGHT_FRONT_STEER_ID = 3;
public static final int RIGHT_FRONT_ENCODER_ID = 10;
public static final int LEFT_FRONT_WHEEL_ID = 4;
public static final int LEFT_FRONT_STEER_ID = 5;
public static final int LEFT_FRONT_ENCODER_ID = 11;
public static final int LEFT_BACK_WHEEL_ID = 6;
public static final int LEFT_BACK_STEER_ID = 7;
public static final int LEFT_BACK_ENCODER_ID = 12;
public static final int RIGHT_BACK_WHEEL_ID = 8;
public static final int RIGHT_BACK_STEER_ID = 9;
public static final int RIGHT_BACK_ENCODER_ID = 13;
public static final int DRIVE_PIGEON_ID = 14;
}
public static final class PIDConstants {
public static final int SWERVE_SLOT_IDX = 0;
public static final int SWERVE_PID_LOOP_IDX = 1;
public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0);
public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0);
}
public static final class AutoConstants {
public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
}
public static final class Conversions {
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5;
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
public static final double TICKS_PER_MOTOR_REV = 0.5;
public static final double WHEEL_DIAMETER_INCHES = 3.9;
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
public static final double TICK_TIME_TO_SECONDS = 10;
public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
}
public static final class Configurations {
public static final double OPEN_LOOP_RAMP_RATE = 0.2;
public static final double CLOSED_LOOP_RAMP_RATE = 0.2;
public static final double NEUTRAL_DEADBAND = 0.04;
}
public static final double MAX_SPEED_FEET_PER_SECOND = 20.4;
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
// dimensions
public static final double WIDTH = 18.5;
public static final double HEIGHT = 18.5;
public static final double HALF_WIDTH = WIDTH / 2.d;
public static final double HALF_HEIGHT = HEIGHT / 2.d;
// misc
public static final int TIMEOUT_MS = 30;
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class VisionConstants {
// public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609);
public static final Translation2d BlueSpeakerCenter = new Translation2d(-8.308975, 1.442593);
public static final double SpeakerBubbleDistance = 3;
public static final double targetPosDistance = 1.5;
}
public static final class DriveConstants {
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class LEDConstants {
public static final int LED_SPARK_ID = 0;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
}
public static final class ShooterConstants {
public static final int LEFT_SHOOTER_ID = 15; // final
public static final int RIGHT_SHOOTER_ID = 16; // final
public static final double SHOOTER_SPEED = 0.50; // final
public static final double SHOOTER_IDLE = 0.20; // final
public static final double SHOOTER_IDLE_LIMELIGHT = 0.20;
}
public static final class IntakeConstants {
public static final int INTAKE_MOTOR_ID = 18;
public static final int PIVOT_MOTOR_ID = 17;
public static final double INTAKE_SPEED = 0.75;
public static final double INTAKE_OUT_SPEED_UNPRESSED = 1.0;
public static final double INTAKE_OUT_SPEED_PRESSED = 0.5;
public static final double HANDOFF_SPEED = 0.4;
public static final double PIVOT_SPEED = 0.2;
public static final class ArmPID {
public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0);
}
}
public static final class ClimbConstants {
public static final int CLIMB_MOTOR_ID = 19;
public static final double CLIMB_IN_SPEED = -1.0;
public static final double CLIMB_OUT_SPEED = 0.87;
}
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
public static final int XBOX_PROGRAMMER_ID = 2;
public static final double LEFT_AXIS_DEADBAND = 0.1;
}
}
src/main/java/frc4388/robot/RobotContainer.java
+15 -221
View File
@@ -9,37 +9,17 @@ package frc4388.robot;
// Drive Systems
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.wpilibj.GenericHID;
import frc4388.utility.controller.XboxController;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.VirtualController;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.commands.packetLossCheckerInator;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
// Commands
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
// Autos
import frc4388.robot.commands.Intake.ArmIntakeIn;
import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
// Subsystems
import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.Shooter;
// import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
// Utilites
import frc4388.utility.DeferredBlock;
import frc4388.utility.configurable.ConfigurableString;
/**
* This class is where the bulk of the robot should be declared. Since
@@ -51,71 +31,11 @@ import frc4388.utility.configurable.ConfigurableString;
public class RobotContainer {
/* RobotMap */
public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
// private final LED m_robotLED = new LED();
private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
m_robotMap.rightFront,
m_robotMap.leftBack,
m_robotMap.rightBack,
m_robotMap.gyro);
/* Controllers */
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID);
private final Limelight limelight = new Limelight();
private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight);
private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor);
/* Virtual Controllers */
private final VirtualController m_virtualDriver = new VirtualController(0);
private final VirtualController m_virtualOperator = new VirtualController(1);
private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter);
// ! Teleop Commands
private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.PIDIn()),
new InstantCommand(() -> m_robotShooter.idle())
);
private SequentialCommandGroup intakeNotePullInIdle = new SequentialCommandGroup(
intakeToShootStuff, intakeToShoot,
new InstantCommand(() -> m_robotShooter.idle())
);
private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)
// new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup(
interrupt,
new InstantCommand(() -> m_robotIntake.PIDIn(), m_robotIntake),
new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)
);
// ! /* Autos */
private String lastAutoName = "four_note_taxi_kracken.auto";
private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive,
() -> autoplaybackName.get(), // lastAutoName
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false);
private neoJoystickPlayback ampShoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto",
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
false, true);
private final Command packetLostTester = new packetLossCheckerInator(m_robotMap.m_pigeon2);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -123,47 +43,8 @@ public class RobotContainer {
public RobotContainer() {
configureButtonBindings();
configureVirtualButtonBindings();
new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip());
DriverStation.silenceJoystickConnectionWarning(true);
// CameraServer.startAutomaticCapture();
/* Default Commands */
// ! Swerve Drive Default Command (Regular Rotation)
// drives the robot with a two-axis input from the driver controller
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
getDeadbandedDriverController().getRight(),
true);
}, m_robotSwerveDrive)
.withName("SwerveDrive DefaultCommand"));
m_robotSwerveDrive.setToSlow();
// ! Swerve Drive One Module Test
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
// m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft());
// }
// ! Swerve Drive Default Command (Orientation Rotation)
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
// m_robotSwerveDrive.driveWithInputOrientation(getDeadbandedDriverController().getLeft(),
// getDeadbandedDriverController().getRightX(),
// getDeadbandedDriverController().getRightY(),
// true);
// }, m_robotSwerveDrive))
// .withName("SwerveDrive OrientationCommand"));
// continually sends updates to the Blinkin LED controller to keep the lights on
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
// m_robotSwerveDrive.driveWithInput(
// getDeadbandedDriverController().getLeft(),
// getDeadbandedDriverController().getRight(),
// true);
// }, m_robotSwerveDrive));
}
/**
@@ -173,97 +54,10 @@ public class RobotContainer {
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// ? /* Driver Buttons */
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip()));
// ! /* Speed */
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
// ? /* Operator Buttons */
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.PIDIn()))
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.PIDOut()))
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.handoff()))
.onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors()));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
.onTrue(emergencyRetract);
// Override Intake Position encoder: out
new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-55), m_robotIntake));
// Override Intake Position encoder: in
new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-6.2), m_robotIntake));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter))
.onFalse(turnOffShoot);
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(intakeNotePullInIdle)
.onFalse(new InstantCommand(() -> m_robotIntake.PIDIn()));
// Spins up shooter, no wind down
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
.onTrue(emergencyRetract);
new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
.onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
.onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
.onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
.onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
// new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
// .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5)));
new Trigger(() -> getDeadbandedOperatorController().getPOV() != -1)
.onTrue(ampShoot)
.onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter, m_robotSwerveDrive));
// ? /* Programer Buttons (Controller 3)*/
// * /* Auto Recording */
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
() -> autoplaybackName.get()))
.onFalse(new InstantCommand());
new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
() -> autoplaybackName.get(),
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false))
.onFalse(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(packetLostTester);
}
/**
@@ -299,7 +93,7 @@ public class RobotContainer {
*/
public Command getAutonomousCommand() {
//no auto
return autoPlayback;
return null;
//return playbackChooser.getCommand();
// return new Command() {};
}
@@ -318,15 +112,15 @@ public class RobotContainer {
return this.m_driverXbox;
}
public DeadbandedXboxController getDeadbandedOperatorController() {
return this.m_operatorXbox;
}
// public DeadbandedXboxController getDeadbandedOperatorController() {
// return this.m_operatorXbox;
// }
public VirtualController getVirtualDriverController() {
return m_virtualDriver;
}
// public VirtualController getVirtualDriverController() {
// return m_virtualDriver;
// }
public VirtualController getVirtualOperatorController() {
return m_virtualOperator;
}
// public VirtualController getVirtualOperatorController() {
// return m_virtualOperator;
// }
}
src/main/java/frc4388/robot/RobotMap.java
+1 -66
View File
@@ -7,80 +7,15 @@
package frc4388.robot;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.Pigeon2;
// import edu.wpi.first.wpilibj.motorcontrol.Spark;
// import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.ClimbConstants;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.subsystems.SwerveModule;
import frc4388.utility.RobotGyro;
/**
* Defines and holds all I/O objects on the Roborio. This is useful for unit
* testing and modularization.
*/
public class RobotMap {
private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON_ID);
public RobotGyro gyro = new RobotGyro(m_pigeon2);
public SwerveModule leftFront;
public SwerveModule rightFront;
public SwerveModule leftBack;
public SwerveModule rightBack;
public RobotMap() {
configureLEDMotorControllers();
configureDriveMotorControllers();
}
/* LED Subsystem */
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
/* Swreve Drive Subsystem */
public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
/* Shooter Subsystem */
public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID);
public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID);
/* Intake Subsystem */
public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID);
public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID);
/* Climber Subsystem */
public final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID);
void configureLEDMotorControllers() {
}
void configureIntakeMotorControllers() {
}
void configureDriveMotorControllers() {
// initialize SwerveModules
this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
}
public Pigeon2 m_pigeon2 = new Pigeon2(14);
}
src/main/java/frc4388/robot/commands/Autos/AutoAlign.java
-208
View File
@@ -1,208 +0,0 @@
// package frc4388.robot.commands.Autos;
// import frc4388.robot.subsystems.Limelight;
// import frc4388.robot.subsystems.SwerveDrive;
// import frc4388.robot.Constants.AutoAlignConstants;
// import frc4388.robot.Constants.VisionConstants;
// import edu.wpi.first.wpilibj2.command.Command;
// import edu.wpi.first.math.geometry.Translation2d;
// import edu.wpi.first.math.geometry.Rotation2d;
// import java.util.Optional;
// import org.opencv.core.RotatedRect;
// import edu.wpi.first.math.geometry.Pose2d;
// import edu.wpi.first.wpilibj.DriverStation;
// import edu.wpi.first.wpilibj.DriverStation.Alliance;
// public class AutoAlign extends Command {
// private SwerveDrive swerve;
// private Limelight limelight;
// private Pose2d pose;
// private Translation2d targetPos;
// private Rotation2d targetRot;
// private Optional<Alliance> alliance;
// private boolean isRed;
// private boolean isFinished;
// private boolean isReverseFinished;
// private boolean reverseAfterFinish;
// private Translation2d[] moveStickReplayArr;
// private Translation2d[] rotStickReplayArr;
// private int replayIndex;
// // PID Stuff
// private double prevError;
// private double cumError;
// public AutoAlign(SwerveDrive swerve, Limelight limelight) {
// this.swerve = swerve;
// this.limelight = limelight;
// }
// // Calc the closest point on a circle, to the center of the speaker
// private Translation2d calcTargetPos(){
// final double R = VisionConstants.targetPosDistance;
// final double cX;
// final double cY;
// if(isRed){
// cX = VisionConstants.RedSpeakerCenter.getX();
// cY = VisionConstants.RedSpeakerCenter.getY();
// }else{
// cX = VisionConstants.BlueSpeakerCenter.getX();
// cY = VisionConstants.BlueSpeakerCenter.getY();
// }
// final double pX = pose.getX();
// final double pY = pose.getY();
// // Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point
// double vX = pX - cX;
// double vY = pY - cY;
// double magV = Math.sqrt(vX*vX + vY*vY);
// double aX = cX + vX / magV * R;
// double aY = cY + vY / magV * R;
// return new Translation2d(aX, aY);
// }
// // Calculate the angle facing the center, at the target rot
// private Rotation2d calcTargetRot() {
// final double R = VisionConstants.targetPosDistance;
// final double cX;
// final double cY;
// if(isRed){
// cX = VisionConstants.RedSpeakerCenter.getX();
// cY = VisionConstants.RedSpeakerCenter.getY();
// }else{
// cX = VisionConstants.BlueSpeakerCenter.getX();
// cY = VisionConstants.BlueSpeakerCenter.getY();
// }
// final double pX = pose.getX();
// final double pY = pose.getY();
// final double dX = pX - cX;
// final double dY = pY - cY;
// final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360);
// return Rotation2d.fromDegrees(yaw);
// }
// // Clamp to a circle, like an xbox controller
// private Translation2d clamp(double oldX, double oldY){
// // Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley
// final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360;
// final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI));
// final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI));
// final double newX = Math.max(-maxX, Math.min(maxX, oldX));
// final double newY = Math.max(-maxY, Math.min(maxY, oldY));
// return new Translation2d(newX, newY);
// }
// private Translation2d calcMoveStick(){
// final double curX = pose.getX();
// final double curY = pose.getY();
// // I think this might work, assuming the constants are good
// double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed;
// double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed;
// return clamp(stickX, stickY);
// }
// private Translation2d calcRotStick(){
// double error = pose.getRotation().getDegrees() - targetRot.getDegrees();
// cumError += error * .02; // 20 ms
// double delta = error - prevError;
// final double kP = 4;
// final double kI = 4;
// final double kD = 4;
// final double kF = 4;
// double output = error * kP;
// output += cumError * kI;
// output += delta * kD;
// output += kF;
// prevError = error;
// return clamp(output, 0);
// }
// public void reverse() {
// this.reverseAfterFinish = true;
// }
// // Called when the command is initially scheduled.
// @Override
// public final void initialize() {
// isRed = alliance.get() == DriverStation.Alliance.Red;
// if(reverseAfterFinish){
// // isReverseFinished = false;
// replayIndex = 0;
// }else{
// moveStickReplayArr = new Translation2d[]{};
// rotStickReplayArr = new Translation2d[]{};
// }
// }
// // Called every time the scheduler runs while the command is scheduled.
// @Override
// public void execute() {
// // Update limelight pos
// pose = limelight.getPose();
// // These must be 0, or it will error
// Translation2d moveStick = new Translation2d(0, 0);
// Translation2d rotStick = new Translation2d(0, 0);
// // Regular replay
// if(!isFinished){
// targetPos = calcTargetPos();
// targetRot = calcTargetRot();
// moveStick = calcMoveStick();
// rotStick = calcRotStick();
// // This is a way of appending...
// moveStickReplayArr[moveStickReplayArr.length] = moveStick;
// rotStickReplayArr[rotStickReplayArr.length] = rotStick;
// // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){
// // replayIndex
// // }
// isFinished = limelight.isNearSpeaker();
// // If reverseAfterFinish, then loop back over and replay in reverse
// }else if(reverseAfterFinish && !isReverseFinished){
// // Get reverse direction
// moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1];
// rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1];
// // Invert sticks
// moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1);
// rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1);
// replayIndex++;
// if(replayIndex >= moveStickReplayArr.length){
// reverseAfterFinish = true;
// }
// }
// // This would greatly benifit from having feild Relative implemented.
// swerve.driveWithInput(moveStick, rotStick, true);
// }
// // Returns true when the command should end.
// @Override
// public final boolean isFinished() {
// return isFinished && (isReverseFinished || !reverseAfterFinish);
// }
// }
src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java
-98
View File
@@ -1,98 +0,0 @@
package frc4388.robot.commands.Autos;
import java.io.File;
import java.util.ArrayList;
import java.util.HashMap;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc4388.robot.commands.Swerve.JoystickPlayback;
import frc4388.robot.subsystems.SwerveDrive;
public class PlaybackChooser {
private final ArrayList<SendableChooser<Command>> m_choosers = new ArrayList<>();
private SendableChooser<Command> m_playback = null;
private final ArrayList<ComplexWidget> m_widgets = new ArrayList<>();
private final HashMap<String, Command> m_commandPool = new HashMap<>();
private final File m_dir = new File("/home/lvuser/autos/");
private int m_cmdNum = 0;
private final SwerveDrive m_swerve;
// commands
private Command m_noAuto = new InstantCommand();
public PlaybackChooser(SwerveDrive swerve, Object... pool) {
m_swerve = swerve;
}
public PlaybackChooser addOption(String name, Command option) {
m_commandPool.put(name, option);
return this;
}
public PlaybackChooser buildDisplay() {
for (int i = 0; i < 10; i++) {
appendCommand();
}
m_playback = m_choosers.get(0);
nextChooser();
// ! This does not work, why?
Shuffleboard.getTab("Auto Chooser")
.add("Add Sequence", new InstantCommand(() -> nextChooser()))
.withPosition(4, 0);
return this;
}
// This will be bound to a button for the time being
public void appendCommand() {
var chooser = new SendableChooser<Command>();
chooser.setDefaultOption("No Auto", m_noAuto);
m_choosers.add(chooser);
ComplexWidget widget = Shuffleboard.getTab("Auto Chooser")
.add("Command: " + m_choosers.size(), chooser)
.withSize(4, 1)
.withPosition(0, m_choosers.size() - 1)
.withWidget(BuiltInWidgets.kSplitButtonChooser);
m_widgets.add(widget);
}
public void nextChooser() {
SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
String[] dirs = m_dir.list();
if(dirs != null){ // Fix funny error
for (String auto : dirs) {
chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
}
}
for (var cmd_name : m_commandPool.keySet()) {
chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
}
}
public Command getCommand() {
Command command = m_playback.getSelected();
command = command == null ? m_noAuto : command.asProxy();
Command[] commands = new Command[m_cmdNum - 1];
for (int i = 0; i < m_cmdNum - 1; i++) {
Command command2 = m_choosers.get(i + 1).getSelected();
command2 = command2 == null ? m_noAuto : command2.asProxy();
commands[i] = command2.asProxy();
}
return command.andThen(commands);
}
}
src/main/java/frc4388/robot/commands/Autos/Taxi.txt
-225
View File
@@ -1,225 +0,0 @@
0.0,0.0,0.0,0.0,0
0.0,0.0,0.0,0.0,0
0.0,0.0,0.0,0.0,12
0.0,0.0,0.0,0.0,26
0.0,0.0,0.0,0.0,37
0.0,0.0,0.0,0.0,50
0.0,0.0,0.0,0.0,62
0.0,0.0,0.0,0.0,73
0.0,0.0,0.0,0.0,88
0.0,0.0,0.0,0.0,103
0.0,0.0,0.0,0.0,116
0.0,0.0,0.0,0.0,160
0.0,0.0,0.0,0.0,173
0.0,0.0,0.0,0.0,185
0.0,0.0,0.0,0.0,200
0.0,0.0,0.0,0.0,211
0.0,0.0,0.0,0.0,223
0.0,0.0,0.0,0.0,235
0.0,0.0,0.0,0.0,247
0.0,0.0,0.0,0.0,263
0.0,0.0,0.0,0.0,283
0.0,0.0,0.0,0.0,303
0.0,-0.109375,0.0,0.0,323
0.0,-0.1484375,0.0,0.0,343
0.0,-0.2109375,0.0,0.0,363
0.0,-0.3671875,0.0,0.0,398
0.0,-0.4140625,0.0,0.0,411
0.0,-0.4765625,0.0,0.0,425
0.0,-0.5078125,0.0,0.0,443
0.0,-0.5078125,0.0,0.0,463
0.0,-0.53125,0.0,0.0,483
0.0,-0.5546875,0.0,0.0,504
0.0,-0.5625,0.0,0.0,523
0.0,-0.5625,0.0,0.0,544
0.0,-0.5703125,0.0,0.0,563
0.0,-0.5859375,0.0,0.0,584
0.0,-0.5859375,0.0,0.0,603
0.0,-0.5859375,0.0,0.0,640
0.0,-0.59375,0.0,0.0,657
0.0,-0.6015625,0.0,0.0,672
0.0,-0.6015625,0.0,0.0,685
0.0,-0.6015625,0.0,0.0,703
0.0,-0.6015625,0.0,0.0,723
0.0,-0.6015625,0.0,0.0,743
0.0,-0.6015625,0.0,0.0,763
0.0,-0.6015625,0.0,0.0,783
0.0,-0.6015625,0.0,0.0,803
0.0,-0.6015625,0.0,0.0,823
0.0,-0.6015625,0.0,0.0,844
0.0,-0.6015625,0.0,0.0,878
0.0,-0.6015625,0.0,0.0,893
0.0,-0.6015625,0.0,0.0,907
0.0,-0.6015625,0.0,0.0,924
0.0,-0.609375,0.0,0.0,943
0.0,-0.609375,0.0,0.0,963
0.0,-0.609375,0.0,0.0,983
0.0,-0.609375,0.0,0.0,1004
0.0,-0.609375,0.0,0.0,1023
0.0,-0.609375,0.0,0.0,1043
0.0,-0.609375,0.0,0.0,1064
0.0,-0.609375,0.0,0.0,1083
0.0,-0.609375,0.0,0.0,1156
0.0,-0.609375,0.0,0.0,1172
0.0,-0.609375,0.0,0.0,1185
0.0,-0.609375,0.0,0.0,1200
0.0,-0.609375,0.0,0.0,1215
0.0,-0.609375,0.0,0.0,1225
0.0,-0.609375,0.0,0.0,1236
0.0,-0.609375,0.0,0.0,1249
0.0,-0.609375,0.0,0.0,1263
0.0,-0.609375,0.0,0.0,1283
0.0,-0.609375,0.0,0.0,1303
0.0,-0.609375,0.0,0.0,1323
0.0,-0.609375,0.0,0.0,1363
0.0,-0.6015625,0.0,0.0,1376
0.0,-0.6015625,0.0,0.0,1394
0.0,-0.6015625,0.0,0.0,1405
0.0,-0.6015625,0.0,0.0,1423
0.0,-0.6015625,0.0,0.0,1443
0.0,-0.6015625,0.0,0.0,1463
0.0,-0.6015625,0.0,0.0,1483
0.0,-0.6015625,0.0,0.0,1503
0.0,-0.6015625,0.0,0.0,1523
0.0,-0.6015625,0.0,0.0,1543
0.0,-0.6015625,0.0,0.0,1563
0.0,-0.6015625,0.0,0.0,1597
0.0,-0.6015625,0.0,0.0,1608
0.0,-0.6015625,0.0,0.0,1624
0.0,-0.6015625,0.0,0.0,1643
0.0,-0.6015625,0.0,0.0,1664
0.0,-0.5859375,0.0,0.0,1683
0.0,-0.5859375,0.0,0.0,1703
0.0,-0.5625,0.0,0.0,1723
0.0,-0.5625,0.0,0.0,1743
0.0,-0.5625,0.0,0.0,1763
0.0,-0.5625,0.0,0.0,1783
0.0,-0.5625,0.0,0.0,1803
0.0,-0.5625,0.0,0.0,1843
0.0,-0.5625,0.0,0.0,1855
0.0,-0.5625,0.0,0.0,1868
0.0,-0.5625,0.0,0.0,1883
0.0,-0.5625,0.0,0.0,1903
0.0,-0.5625,0.0,0.0,1923
0.0,-0.5625,0.0,0.0,1943
0.0,-0.5625,0.0,0.0,1963
0.0,-0.5625,0.0,0.0,1983
0.0,-0.5625,0.0,0.0,2003
0.0,-0.5625,0.0,0.0,2024
0.0,-0.5625,0.0,0.0,2043
0.0,-0.5625,0.0,0.0,2081
0.0,-0.5625,0.0,0.0,2093
0.0,-0.5625,0.0,0.0,2105
0.0,-0.5625,0.0,0.0,2123
0.0,-0.5625,0.0,0.0,2143
0.0,-0.5625,0.0,0.0,2163
0.0,-0.5625,0.0,0.0,2183
0.0,-0.5625,0.0,0.0,2203
0.0,-0.5625,0.0,0.0,2223
0.0,-0.5625,0.0,0.0,2243
0.0,-0.5625,0.0,0.0,2263
0.0,-0.5625,0.0,0.0,2283
0.0,-0.5625,0.0,0.0,2366
0.0,-0.5625,0.0,0.0,2377
0.0,-0.5625,0.0,0.0,2394
0.0,-0.5703125,0.0,0.0,2405
0.0,-0.5703125,0.0,0.0,2418
0.0,-0.5703125,0.0,0.0,2431
0.0,-0.5703125,0.0,0.0,2444
0.0,-0.5703125,0.0,0.0,2458
0.0,-0.5703125,0.0,0.0,2470
0.0,-0.5703125,0.0,0.0,2485
0.0,-0.5703125,0.0,0.0,2503
0.0,-0.5703125,0.0,0.0,2523
0.0,-0.5703125,0.0,0.0,2563
0.0,-0.5703125,0.0,0.0,2577
0.0,-0.5703125,0.0,0.0,2591
0.0,-0.5703125,0.0,0.0,2608
0.0,-0.5703125,0.0,0.0,2624
0.0,-0.5703125,0.0,0.0,2643
0.0,-0.5703125,0.0,0.0,2677
0.0,-0.5703125,0.0,0.0,2698
0.0,-0.5703125,0.0,0.0,2711
0.0,-0.5703125,0.0,0.0,2725
0.0,-0.5703125,0.0,0.0,2743
0.0,-0.5703125,0.0,0.0,2764
0.0,-0.5703125,0.0,0.0,2810
0.0,-0.5703125,0.0,0.0,2820
0.0,-0.5703125,0.0,0.0,2833
0.0,-0.5703125,0.0,0.0,2845
0.0,-0.5703125,0.0,0.0,2863
0.0,-0.5703125,0.0,0.0,2883
0.0,-0.5703125,0.0,0.0,2904
0.0,-0.5703125,0.0,0.0,2924
0.0,-0.5703125,0.0,0.0,2943
0.0,-0.5703125,0.0,0.0,2963
0.0,-0.5703125,0.0,0.0,2983
0.0,-0.5703125,0.0,0.0,3003
0.0,-0.5703125,0.0,0.0,3033
0.0,-0.5703125,0.0,0.0,3050
0.0,-0.5703125,0.0,0.0,3065
0.0,-0.5703125,0.0,0.0,3083
0.0,-0.5703125,0.0,0.0,3103
0.0,-0.5703125,0.0,0.0,3123
0.0,-0.5703125,0.0,0.0,3144
0.0,-0.5703125,0.0,0.0,3164
0.0,-0.5703125,0.0,0.0,3184
0.0,-0.5703125,0.0,0.0,3203
0.0,-0.5703125,0.0,0.0,3223
0.0,-0.5703125,0.0,0.0,3243
0.0,-0.5703125,0.0,0.0,3272
0.0,-0.5703125,0.0,0.0,3289
0.0,-0.5703125,0.0,0.0,3303
0.0,-0.5703125,0.0,0.0,3323
0.0,-0.5703125,0.0,0.0,3343
0.0,-0.5703125,0.0,0.0,3363
0.0,-0.5703125,0.0,0.0,3383
0.0,-0.5703125,0.0,0.0,3403
0.0,-0.5703125,0.0,0.0,3423
0.0,-0.5703125,0.0,0.0,3443
0.0,-0.5703125,0.0,0.0,3463
0.0,-0.5703125,0.0,0.0,3483
0.0,-0.5703125,0.0,0.0,3566
0.0,-0.5703125,0.0,0.0,3578
0.0,-0.5703125,0.0,0.0,3596
0.0,-0.5703125,0.0,0.0,3610
0.0,-0.5703125,0.0,0.0,3623
0.0,-0.5703125,0.0,0.0,3640
0.0,-0.5703125,0.0,0.0,3651
0.0,-0.5703125,0.0,0.0,3663
0.0,-0.5703125,0.0,0.0,3678
0.0,-0.5703125,0.0,0.0,3691
0.0,-0.5703125,0.0,0.0,3706
0.0,-0.5703125,0.0,0.0,3723
0.0,-0.5703125,0.0,0.0,3766
0.0,-0.5703125,0.0,0.0,3778
0.0,-0.5703125,0.0,0.0,3792
0.0,-0.5703125,0.0,0.0,3807
0.0,-0.5703125,0.0,0.0,3823
0.0,-0.5703125,0.0,0.0,3843
0.0,-0.53125,0.0,0.0,3863
0.0,-0.53125,0.0,0.0,3884
0.0,-0.421875,0.0,0.0,3904
0.0,0.0,0.0,0.0,3924
0.0,0.0,0.0,0.0,3944
0.0,0.0,0.0,0.0,3963
0.0,0.0,0.0,0.0,3999
0.0,0.0,0.0,0.0,4010
0.0,0.0,0.0,0.0,4025
0.0,0.0,0.0,0.0,4043
0.0,0.0,0.0,0.0,4063
0.0,0.0,0.0,0.0,4083
0.0,0.0,0.0,0.0,4103
0.0,0.0,0.0,0.0,4123
0.0,0.0,0.0,0.0,4143
0.0,0.0,0.0,0.0,4163
0.0,0.0,0.0,0.0,4183
0.0,0.0,0.0,0.0,4203
0.0,0.0,0.0,0.0,4236
0.0,0.0,0.0,0.0,4247
0.0,0.0,0.0,0.0,4264
0.0,0.0,0.0,0.0,4284
0.0,0.0,0.0,0.0,4304
0.0,0.0,0.0,0.0,4324
0.0,0.0,0.0,0.0,4343
0.0,0.0,0.0,0.0,4363
src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt
-20
View File
@@ -1,20 +0,0 @@
AUTO file format
HEADER static size 0x5
0x00 BYTE NUM AXES: defualts to 6
0x01 BYTE NUM POV: defualts to 1
0x02 BYTE NUM CONTROLLERS: defualts to 2
0x03 SHORT FRAMES: any value greator or equal than one.
FRAME PER CONTROLLER: defualt size 0x34
0x00 DOUBLE AXES[NUM AXES]
0x30 SHORT BUTTONS
0x32 SHORT POVs[NUM POV]
FRAME: size varrys
FRAME PER CONTROLLER[NUM CONTROLLERS]
INT UNIXTIMESTAMP
FILE:
HEADER
FRAME[FRAMES]
src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java
-107
View File
@@ -1,107 +0,0 @@
// package frc4388.robot.commands.Autos;
// import java.io.File;
// import java.util.ArrayList;
// import java.util.HashMap;
// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
// import edu.wpi.first.wpilibj2.command.Command;
// import edu.wpi.first.wpilibj2.command.InstantCommand;
// import frc4388.robot.commands.Swerve.JoystickPlayback;
// import frc4388.robot.commands.Swerve.neoJoystickPlayback;
// import frc4388.robot.subsystems.SwerveDrive;
// import frc4388.utility.controller.VirtualController;
// public class neoPlaybackChooser {
// private final SendableChooser<String> m_teamChosser = new SendableChooser<String>();
// private final SendableChooser<String> m_possitionChosser = new SendableChooser<String>();
// private final SendableChooser<String> m_autoNameChosser = new SendableChooser<String>();
// private final SwerveDrive m_swerve;
// private final VirtualController[] m_controllers;
// // private final ArrayList<SendableChooser<Command>> m_choosers = new ArrayList<>();
// // private SendableChooser<Command> m_playback = null;
// private final ArrayList<ComplexWidget> m_widgets = new ArrayList<>();
// // private final HashMap<String, Command> m_commandPool = new HashMap<>();
// // private final File m_dir = new File("/home/lvuser/autos/");
// // private int m_cmdNum = 0;
// // commands
// private Command m_noAuto = new InstantCommand();
// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) {
// m_teamChosser.addOption("Red", "red");
// m_teamChosser.setDefaultOption("Blue", "blue");
// m_teamChosser.addOption("Nuetral", "nuetral");
// m_possitionChosser.addOption("AMP", "amp");
// m_possitionChosser.setDefaultOption("Center", "center");
// m_possitionChosser.addOption("Source", "source");
// m_swerve = swerve;
// m_controllers = controllers;
// }
// public neoPlaybackChooser addOption(String name, String option) {
// m_autoNameChosser.addOption(name, option);
// return this;
// }
// // public PlaybackChooser buildDisplay() {
// // for (int i = 0; i < 10; i++) {
// // appendCommand();
// // }
// // m_playback = m_choosers.get(0);
// // nextChooser();
// // // ! This does not work, why?
// // Shuffleboard.getTab("Auto Chooser")
// // .add("Add Sequence", new InstantCommand(() -> nextChooser()))
// // .withPosition(4, 0);
// // return this;
// // }
// // This will be bound to a button for the time being
// public void render() {
// // var chooser = new SendableChooser<Command>();
// // // chooser.setDefaultOption("No Auto", m_noAuto);
// // m_choosers.add(chooser);
// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser")
// .add("Command: " + m_choosers.size(), chooser)
// .withSize(4, 1)
// .withPosition(0, m_choosers.size() - 1)
// .withWidget(BuiltInWidgets.kSplitButtonChooser)
// .withWidget(BuiltInWidgets.kComboBoxChooser);
// m_widgets.add(widget);
// }
// // public void nextChooser() {
// // SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
// // String[] dirs = m_dir.list();
// // if(dirs != null){ // Fix funny error
// // for (String auto : dirs) {
// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
// // }
// // }
// // for (var cmd_name : m_commandPool.keySet()) {
// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
// // }
// // }
// public String autoName() {
// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto";
// }
// public Command getCommand() {
// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true);
// }
// }
src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java
-52
View File
@@ -1,52 +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.Intake;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Shooter;
public class ArmIntakeIn extends Command {
/** Creates a new ArmIntakeIn. */
private Intake robotIntake;
private Shooter robotShooter;
public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) {
// Use addRequirements() here to declare subsystem dependencies.
this.robotIntake = robotIntake;
this.robotShooter = robotShooter;
addRequirements(this.robotIntake, this.robotShooter);
}
// 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() {
robotIntake.PIDOut();
robotIntake.spinIntakeMotor();
}
// 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 robotIntake.getIntakeLimitSwitchState();
// if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0)))
// {
// return !true==true;
// }
// else
// {
// return !false==!(!(true));
// }
}
}
src/main/java/frc4388/robot/commands/PID.java
-60
View File
@@ -1,60 +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;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.utility.Gains;
public abstract class PID extends Command {
protected Gains gains;
private double output = 0;
private double tolerance = 0;
/** Creates a new PelvicInflammatoryDisease. */
public PID(double kp, double ki, double kd, double kf, double tolerance) {
gains = new Gains(kp, ki, kd, kf, 0);
this.tolerance = tolerance;
}
public PID(Gains gains, double tolerance) {
this.gains = gains;
this.tolerance = tolerance;
}
/** produces the error from the setpoint */
public abstract double getError();
/** todo: javadoc */
public abstract void runWithOutput(double output);
// Called when the command is initially scheduled.
@Override
public final void initialize() {
output = 0;
}
private double prevError, cumError = 0;
// Called every time the scheduler runs while the command is scheduled.
@Override
public final void execute() {
double error = getError();
cumError += error * .02; // 20 ms
double delta = error - prevError;
output = error * gains.kP;
output += cumError * gains.kI;
output += delta * gains.kD;
output += gains.kF;
runWithOutput(output);
}
// Returns true when the command should end.
@Override
public final boolean isFinished() {
return Math.abs(getError()) < tolerance;
}
}
src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java
-145
View File
@@ -1,145 +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 java.io.File;
import java.io.FileNotFoundException;
import java.util.ArrayList;
import java.util.Scanner;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickPlayback extends Command {
private final SwerveDrive swerve;
private String filename;
private int mult = 1;
private Scanner input;
private final ArrayList<TimedOutput> outputs = new ArrayList<>();
private int counter = 0;
private long startTime = 0;
private long playbackTime = 0;
private int lastIndex;
private boolean m_finished = false; // ! find a better way
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
this.swerve = swerve;
this.filename = filename;
this.mult = mult;
addRequirements(this.swerve);
}
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename) {
this(swerve, filename, 1);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
outputs.clear();
m_finished = false;
startTime = System.currentTimeMillis();
playbackTime = 0;
lastIndex = 0;
try {
input = new Scanner(new File("/home/lvuser/autos/" + filename));
String line = "";
while (input.hasNextLine()) {
line = input.nextLine();
if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
continue;
}
String[] values = line.split(",");
System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
var out = new TimedOutput();
out.leftX = Double.parseDouble(values[0]) * mult;
out.leftY = Double.parseDouble(values[1]);
out.rightX = Double.parseDouble(values[2]);
out.rightY = Double.parseDouble(values[3]);
out.timedOffset = Long.parseLong(values[4]);
outputs.add(out);
}
input.close();
} catch (FileNotFoundException e) {
e.printStackTrace();
}
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (counter == 0) {
startTime = System.currentTimeMillis();
playbackTime = 0;
} else {
playbackTime = System.currentTimeMillis() - startTime;
}
// skip to reasonable time frame
// too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
{
int i = lastIndex == 0 ? 1 : lastIndex;
while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
i++;
}
if (i >= outputs.size()) {
m_finished = true; // ! kind of a hack
return;
}
lastIndex = i;
}
TimedOutput lastOut = outputs.get(lastIndex - 1);
TimedOutput out = outputs.get(lastIndex);
double deltaTime = out.timedOffset - lastOut.timedOffset;
double playbackDelta = playbackTime - lastOut.timedOffset;
double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime);
double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime);
double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime);
double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
// this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY),
// new Translation2d(out.rightX, out.rightY),
// true);
// this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
// new Translation2d(lerpRX, lerpRY),
// true);
this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY),
new Translation2d(lerpRX, lerpRY),
true);
counter++;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
input.close();
swerve.stopModules();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return m_finished;
}
}
src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
-97
View File
@@ -1,97 +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 java.io.File;
import java.io.IOException;
import java.io.PrintWriter;
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.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickRecorder extends Command {
public final SwerveDrive swerve;
public final Supplier<Double> leftX;
public final Supplier<Double> leftY;
public final Supplier<Double> rightX;
public final Supplier<Double> rightY;
private String filename;
public final ArrayList<TimedOutput> outputs = new ArrayList<>();
private long startTime = -1;
/** Creates a new JoystickRecorder. */
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY,
Supplier<Double> rightX, Supplier<Double> rightY,
String filename)
{
this.swerve = swerve;
this.leftX = leftX;
this.leftY = leftY;
this.rightX = rightX;
this.rightY = rightY;
this.filename = filename;
addRequirements(this.swerve);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
outputs.clear();
this.startTime = System.currentTimeMillis();
outputs.add(new TimedOutput());
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
var inputs = new TimedOutput();
inputs.leftX = leftX.get();
inputs.leftY = leftY.get();
inputs.rightX = rightX.get();
inputs.rightY = rightY.get();
inputs.timedOffset = System.currentTimeMillis() - startTime;
outputs.add(inputs);
swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
new Translation2d(inputs.rightX, inputs.rightY),
true);
//System.out.println("RECORDING");
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
File output = new File("/home/lvuser/autos/" + filename);
try (PrintWriter writer = new PrintWriter(output)) {
for (var input : outputs) {
writer.println( input.leftX + "," + input.leftY + "," +
input.rightX + "," + input.rightY + "," +
input.timedOffset);
}
writer.close();
} catch (IOException e) {
e.printStackTrace();
}
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java
-35
View File
@@ -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.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);
}
}
src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java
-197
View File
@@ -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.SwerveDrive;
import frc4388.utility.DataUtils;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
import frc4388.utility.controller.VirtualController;
/**
* 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;
}
}
src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java
-129
View File
@@ -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.SwerveDrive;
import frc4388.utility.DataUtils;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
import frc4388.utility.controller.DeadbandedXboxController;
/**
* 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();
}
}
}
src/main/java/frc4388/robot/commands/packetLossCheckerInator.java
+74
View File
@@ -0,0 +1,74 @@
// 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;
import java.util.HashMap;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.wpilibj2.command.Command;
// archaic communication protocols with modern problems require out of the box thinking
public class packetLossCheckerInator extends Command {
private final Pigeon2 pigeon;
private int startTime;
private final int amountOfCycles = 10_000; // Estemated at 200s of robot time.
private int lastAngle = 0;
private int errorCount = 0;
private int iterations = 0;
private HashMap<Integer, Integer> statusMap;
/** Creates a new packetLossCheckerInator. */
public packetLossCheckerInator(Pigeon2 pigeon) {
// Use addRequirements() here to declare subsystem dependencies.
this.pigeon = pigeon;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
System.out.println("### Test started, this will take a bit. ###");
startTime = (int) System.currentTimeMillis();
statusMap = new HashMap<Integer, Integer>();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double angle = pigeon.getAngle();
if ((int) Math.round(angle) != lastAngle) errorCount++;
angle += 1;
lastAngle = (int) angle;
StatusCode e = pigeon.setYaw(lastAngle);
if (statusMap.containsKey(e.value)) statusMap.put(e.value, statusMap.get(e.value) + 1);
else statusMap.put(e.value, 1); //statusMap.get(e.value)
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
int tElapsed = (int) System.currentTimeMillis() - startTime;
System.out.println("### Test concluded ###");
System.out.print("Obvious Falures: ");
System.out.println(errorCount);
System.out.print("Cycles");
System.out.println(amountOfCycles);
System.out.print("Time it took: ");
System.out.println(tElapsed);
System.out.println("## Status map counts ##");
for (int key : statusMap.keySet()) {
System.out.print(key);
System.out.print(": ");
System.out.println(statusMap.get(key));
}
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return iterations >= amountOfCycles;
}
}
src/main/java/frc4388/robot/subsystems/Apriltags.java
-36
View File
@@ -1,36 +0,0 @@
package frc4388.robot.subsystems;
//import edu.wpi.first.apriltag.AprilTag;
//import edu.wpi.first.math.geometry.Pose3d;
//import edu.wpi.first.math.geometry.Rotation3d;
//import edu.wpi.first.networktables.NetworkTable;
//import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
public class Apriltags {
public static class Tag {
public boolean visible = true;
public double x, y, z = 0;
public double ry, rp, rr = 0;
}
public Tag getTagPosRot() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
final Tag tag = new Tag();
tag.visible = isAprilTag();
tag.x = tagTable.getEntry("TagPosX").getDouble(0);
tag.y = tagTable.getEntry("TagPosY").getDouble(0);
tag.z = tagTable.getEntry("TagPosZ").getDouble(0);
tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
return tag;
}
public boolean isAprilTag() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
return tagTable.getEntry("IsTag").getBoolean(false);
}
}
src/main/java/frc4388/robot/subsystems/Climber.java
-57
View File
@@ -1,57 +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.subsystems;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ClimbConstants;
public class Climber extends SubsystemBase {
/** Creates a new Climber. */
TalonFX climbMotor;
public Climber(TalonFX climbMotor) {
this.climbMotor = climbMotor;
this.climbMotor.setInverted(true);
climbMotor.setNeutralMode(NeutralModeValue.Brake);
var slot0Configs = new Slot0Configs();
slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output
slot0Configs.kI = 0.0; // no output for integrated error
slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output
climbMotor.getConfigurator().apply(slot0Configs);
}
public void climbOut() {
//PositionVoltage request = new PositionVoltage(0);
//climbMotor.setControl(request.withPosition(-520));
climbMotor.set(ClimbConstants.CLIMB_OUT_SPEED);
}
public void climbIn() {
//PositionVoltage request = new PositionVoltage(-520);
//climbMotor.setControl(request.withPosition(0));
climbMotor.set(ClimbConstants.CLIMB_IN_SPEED);
}
public void stopClimb() {
climbMotor.set(0.d);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
//SmartDashboard.putNumber("climber pos", climbMotor.getPosition().getValue());
}
}
src/main/java/frc4388/robot/subsystems/DiffDrive.java
-88
View File
@@ -1,88 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.utility.RobotGyro;
import frc4388.utility.RobotTime;
/**
* Add your docs here.
*/
public class DiffDrive extends SubsystemBase {
// Put methods for controlling this subsystem
// here. Call these from Commands.
private RobotTime m_robotTime = RobotTime.getInstance();
private TalonFX m_leftFrontMotor;
private TalonFX m_rightFrontMotor;
private TalonFX m_leftBackMotor;
private TalonFX m_rightBackMotor;
private DifferentialDrive m_driveTrain;
private RobotGyro m_gyro;
/**
* Add your docs here.
*/
public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
m_leftFrontMotor = leftFrontMotor;
m_rightFrontMotor = rightFrontMotor;
m_leftBackMotor = leftBackMotor;
m_rightBackMotor = rightBackMotor;
m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false));
m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false));
m_driveTrain = driveTrain;
m_gyro = gyro;
}
@Override
public void periodic() {
m_gyro.updatePigeonDeltas();
if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
updateSmartDashboard();
}
}
/**
* Add your docs here.
*/
public void driveWithInput(double move, double steer) {
m_driveTrain.arcadeDrive(move, steer);
}
/**
* Add your docs here.
*/
public void tankDriveWithInput(double leftMove, double rightMove) {
m_leftFrontMotor.set(leftMove);
m_rightFrontMotor.set(rightMove);
}
/**
* Add your docs here.
*/
private void updateSmartDashboard() {
// Examples of the functionality of RobotGyro
SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
//SmartDashboard.putData(m_gyro);
}
}
src/main/java/frc4388/robot/subsystems/Intake.java
-110
View File
@@ -1,110 +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.subsystems;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.utility.Gains;
public class Intake extends SubsystemBase {
private TalonFX intakeMotor;
private TalonFX pivotMotor;
public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS;
/** Creates a new Intake. */
public Intake(TalonFX intakeMotor, TalonFX pivotMotor) {
this.intakeMotor = intakeMotor;
this.pivotMotor = pivotMotor;
intakeMotor.getConfigurator().apply(new TalonFXConfiguration());
pivotMotor.getConfigurator().apply(new TalonFXConfiguration());
intakeMotor.setNeutralMode(NeutralModeValue.Brake);
pivotMotor.setNeutralMode(NeutralModeValue.Brake);
// in init function, set slot 0 gains
var slot0Configs = new Slot0Configs();
slot0Configs.kP = 1.3; // An error of 0.5 rotations results in 12 V output
slot0Configs.kI = 0.0; // no output for integrated error
slot0Configs.kD = 0.21; // A velocity of 1 rps results in 0.1 V output
pivotMotor.getConfigurator().apply(slot0Configs);
}
// ! Talon Methods
public void PIDIn() {
PIDPosition(0);
}
public void PIDOut() {
PIDPosition(-53);
}
public void PIDPosition(double pos) {
PositionVoltage request = new PositionVoltage(pos);
pivotMotor.setControl(request);
}
public void handoff() {
intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
}
public void spinIntakeMotor() {
intakeMotor.set(IntakeConstants.INTAKE_SPEED);
}
public void spinIntakeMotor(double speed) {
intakeMotor.set(speed);
}
public boolean getIntakeLimitSwitchState() {
return intakeMotor.getForwardLimit().getValue().value == 0;
}
public void setPivotEncoderPosition(double val) {
pivotMotor.setPosition(val);
}
public void stopIntakeMotors() {
intakeMotor.set(0);
}
public void stopArmMotor() {
pivotMotor.set(0);
}
public void stop() {
intakeMotor.set(0);
pivotMotor.set(0);
}
public double getArmPos() {
return pivotMotor.getPosition().getValue();
}
public void resetArmPosition() {
if (getIntakeLimitSwitchState()) {
// talonPivot.setPosition(0);
}
}
public void ampPosition() {
PIDPosition(-59); //TODO: Find actual value
}
public void ampOuttake(double speed) {
spinIntakeMotor(speed);
}
@Override
public void periodic() {
resetArmPosition();
}
}
src/main/java/frc4388/robot/subsystems/LED.java
-90
View File
@@ -1,90 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.AddressableLED;
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.LEDConstants;
import frc4388.utility.LEDPatterns;
/**
* Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
* Driver
*/
public class LED extends SubsystemBase {
static AddressableLED m_led;
static AddressableLEDBuffer m_ledBuffer;
static LED m_self;
/**
* Add your docs here.
*/
public LED(){
if(m_self != null)
return;
m_led = new AddressableLED(9);
m_ledBuffer = new AddressableLEDBuffer(10);
m_led.setLength(m_ledBuffer.getLength());
m_led.setData(m_ledBuffer);
m_led.start();
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
}
public static LED getInstance() {
if(m_self == null)
m_self = new LED();
return m_self;
}
@Override
public void periodic(){
//gamermode();
//SmartDashboard.putNumber("LED", m_currentPattern.getValue());
return;
}
static int firstcolor = 0;
static void gamermode() {
for(int i = 0; i < m_ledBuffer.getLength(); i++) {
final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180;
setLEDHSV(i, hue, 255, 128);
}
firstcolor +=3;
firstcolor %= 180;
}
/**
* Add your docs here.
*/
public static void updateLED (){
gamermode();
// m_LEDController.set(m_currentPattern.getValue());
}
/**
* Add your docs here.
*/
public static void setLEDRGB(int lednum, int r, int g, int b){
m_ledBuffer.setRGB(lednum, r, g, b);
//m_currentPattern = pattern;
// m_LEDController.set(m_currentPattern.getValue());
}
public static void setLEDHSV(int lednum, int hue, int sat, int val){
m_ledBuffer.setRGB(lednum, hue, sat, val);
//m_currentPattern = pattern;
// m_LEDController.set(m_currentPattern.getValue());
}
/**
* Add your docs here.
* @return
*/
public AddressableLEDBuffer getBuffer() {
return m_ledBuffer;
}
}
src/main/java/frc4388/robot/subsystems/Limelight.java
-82
View File
@@ -1,82 +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.subsystems;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.VisionConstants;
// Look at vvv for networktables stuff
// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data
public class Limelight extends SubsystemBase {
// // [X, Y, Z, Roll, Pitch, Yaw]
// private double[] cameraPose;
// private boolean isTag;
// private Pose2d pose;
// private boolean isNearSpeaker;
// public boolean getIsTag() {
// return isTag;
// }
// private void update() {
// SmartDashboard.putBoolean("Apriltag", isTag);
// if(!isTag){
// return;
// }
// double x = cameraPose[0];
// double y = cameraPose[1];
// double yaw = cameraPose[5];
// Rotation2d rot = Rotation2d.fromDegrees(yaw);
// pose = new Pose2d(x, y, rot);
// boolean isRed = DriverStation.getAlliance().get() == Alliance.Red;
// double distance;
// if(isRed){
// distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter);
// }else{
// distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter);
// }
// isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance;
// //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
// //SmartDashboard.putNumber("speakerDistance", distance);
// }
// public Pose2d getPose() {
// return pose;
// }
// public boolean isNearSpeaker() {
// return isNearSpeaker;
// }
// @Override
// public void periodic() {
// // This method will be called once per scheduler run
// //isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0;
// //double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]);
// //if(newPose != cameraPose){
// // cameraPose = newPose;
// //update();
// //}
// }
}
src/main/java/frc4388/robot/subsystems/Shooter.java
-114
View File
@@ -1,114 +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.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Limelight;
// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
// import com.ctre.phoenix.motorcontrol.NeutralMode;
public class Shooter extends SubsystemBase {
private TalonFX leftShooter;
private TalonFX rightShooter;
private Limelight limelight;
private int spinMode = 0;
// 0 = Stop / Coast
// 1 = Idle
// 2 = Idle Near Speaker
// 3 = Spin
// 4 = SingleSpin
private double smartDashboardShooterSpeed;
/** Creates a new Shooter. */
public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) {
leftShooter = leftTalonFX;
rightShooter = rightTalonFX;
limelight = tmplimelight;
leftShooter.setNeutralMode(NeutralModeValue.Coast);
rightShooter.setNeutralMode(NeutralModeValue.Coast);
SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
}
public Shooter(TalonFX leftShooter) {
this.leftShooter = leftShooter;
leftShooter.setNeutralMode(NeutralModeValue.Coast);
}
public void singleSpin() {
leftShooter.set(1.0);
spinMode = 4;
}
public void singleSpin(double speed) {
leftShooter.set(speed);
spinMode = 4;
}
public void spin() {
spin(smartDashboardShooterSpeed);
spinMode = 3;
}
public void spin(double speed) {
leftShooter.set(-speed);
rightShooter.set(-speed);
spinMode = 3;
}
public void spin(double leftSpeed, double rightSpeed) {
leftShooter.set(leftSpeed);
rightShooter.set(-rightSpeed);
spinMode = 3;
}
public void stop() {
spin(0.d);
spinMode = 0;
}
public void idle() {
spin(ShooterConstants.SHOOTER_IDLE);
spinMode = 1;
}
public int getMode(){
return spinMode;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
// SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
//SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
//smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
// If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed.
// Else if the robot is not near the speaker, then set the speed back to idle.
// if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){
// leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
// rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
// spinMode = 2;
// }else if(!limelight.isNearSpeaker() && spinMode == 2){
// idle();
// }
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
-333
View File
@@ -1,333 +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.subsystems;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.SwerveDriveConstants.Conversions;
import frc4388.utility.RobotGyro;
import frc4388.utility.RobotUnits;
public class SwerveDrive extends SubsystemBase {
private SwerveModule leftFront;
private SwerveModule rightFront;
private SwerveModule leftBack;
private SwerveModule rightBack;
private SwerveModule[] modules;
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
private RobotGyro gyro;
private int gear_index;
private boolean stopped = false;
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
this.leftFront = leftFront;
this.rightFront = rightFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
this.gyro = gyro;
reset_index();
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
// double ang = Math.atan2(rightStick.getY(), rightStick.getX());
// rightStick.getAngle()
double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
// System.out.println(ang);
// module.go(ang);
// Rotation2d rot = Rotation2d.fromRadians(ang);
Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
SwerveModuleState state = new SwerveModuleState(speed, rot);
module.setDesiredState(state);
}
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0;
SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction));
if (fieldRelative) {
double rot = 0;
// ! drift correction
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot_correction = 0;
// rot = rightStick.getX();
// SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
stopModules();
stopped = true;
}
// SmartDashboard.putBoolean("drift correction", true);
// rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
}
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds.
// chassisSpeeds = chassisSpeeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
} else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
double rot = 0;
// ! drift correction
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
// SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
stopModules();
stopped = true;
}
// SmartDashboard.putBoolean("drift correction", true);
// double rot_correction = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
}
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
} else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
}
// setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
// Translation2d rightStick = new Translation2d(-rightX, rightY);
double rightX = rightStick.getX();
double rightY = rightStick.getY();
double rot_correction = 0;
// double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
if(fieldRelative) {
double rot = 0;
if(rightStick.getNorm() > 0.5) {
orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5));
double min = tmp.getDegrees();
min = Math.max(Math.abs(min), 2);
if(tmp.getDegrees() < 0)
min*=-1;
tmp = new Rotation2d(min * Math.PI / 180);
rot = tmp.getRadians(); // x x - y/x
}
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + rot_correction, gyro.getRotation2d()).times(1);
} else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
// setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
/**
* Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state);
}
}
public boolean rotateToTarget(double angle) {
double currentAngle = getGyroAngle();
double error = angle - currentAngle;
driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
if (Math.abs(angle - getGyroAngle()) < 5.0) {
return true;
}
return false;
}
public double getGyroAngle() {
return -gyro.getAngle();
}
public void add180() {
gyro.reset(gyro.getAngle()+180);
rotTarget = gyro.getAngle();
}
public void resetGyro() {
gyro.reset();
rotTarget = gyro.getAngle();
}
public void resetGyroFlip() {
gyro.resetFlip();
rotTarget = gyro.getAngle();
}
public void resetGyroRightBlue() {
gyro.resetRightSideBlue();
rotTarget = gyro.getAngle();
}
public void resetGyroRightAmp() {
gyro.resetAmpSide();
rotTarget = gyro.getAngle();
}
public void stopModules() {
for (SwerveModule module : this.modules) {
module.stop();
}
}
public SwerveDriveKinematics getKinematics() {
return this.kinematics;
}
public boolean getSpeedState() {
return false;
}
@Override
public void periodic() {
// This method will be called once per scheduler run\
SmartDashboard.putNumber("Gyro", getGyroAngle());
SmartDashboard.putNumber("RotTartget", rotTarget);
}
private void reset_index() {
gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?)
}
public void shiftDown() {
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
int i = gear_index - 1;
if (i == -1) i = 0;
setPercentOutput(SwerveDriveConstants.GEARS[i]);
gear_index = i;
}
public void shiftUp() {
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
int i = gear_index + 1;
if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1;
setPercentOutput(SwerveDriveConstants.GEARS[i]);
gear_index = i;
}
public void setPercentOutput(double speed) {
speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed;
gear_index = -1;
}
public void setToSlow() {
this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED;
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
}
public void setToFast() {
this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED;
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
}
public void setToTurbo() {
this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED;
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
}
public void toggleGear(double angle) {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
} else {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
}
}
public void shiftUpRot() {
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
}
public void shiftDownRot() {
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
}
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
-242
View File
@@ -1,242 +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.subsystems;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.FeedbackConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import com.ctre.phoenix6.hardware.CANcoder;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
public class SwerveModule extends SubsystemBase {
private TalonFX driveMotor;
private TalonFX angleMotor;
private CANcoder encoder;
// private final StatusSignal<Double> cc_pos;
// private final StatusSignal<Double> cc_vel;
// private int selfid;
// private ConfigurableDouble offsetGetter;
private static int swerveId = 0;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
/** Creates a new SwerveModule. */
public SwerveModule(TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) {
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
this.encoder = encoder;
var motorCfg = new TalonFXConfiguration()
.withOpenLoopRamps(
new OpenLoopRampsConfigs()
.withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
).withClosedLoopRamps(
new ClosedLoopRampsConfigs()
.withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Brake)
.withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
).withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(100)
.withStatorCurrentLimitEnable(true)
.withSupplyCurrentLimit(100)
.withSupplyCurrentLimitEnable(true)
);
driveMotor.getConfigurator().apply(motorCfg);
TalonFXConfiguration angleConfig = new TalonFXConfiguration()
.withOpenLoopRamps(
new OpenLoopRampsConfigs()
.withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
).withClosedLoopRamps(
new ClosedLoopRampsConfigs()
.withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Brake)
.withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
);
angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
angleConfig.Slot0.kP = swerveGains.kP;
angleConfig.Slot0.kI = swerveGains.kI;
angleConfig.Slot0.kD = swerveGains.kD;
angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID();
angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
angleMotor.getConfigurator().apply(angleConfig);
CANcoderConfiguration canconfig = new CANcoderConfiguration();
canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
canconfig.MagnetSensor.MagnetOffset = offset;
encoder.getConfigurator().apply(canconfig);
rotateToAngle(0);
}
// public void go(double ang){
// // double curang = this.encoder.getAbsolutePosition().getValue();
// System.out.println(getAngle().getDegrees());
// rotateToAngle(ang);
// }
@Override
public void periodic() {
//encoder.configMagnetOffset(offsetGetter.get());
//SmartDashboard.putString("Error Code: " + selfid, getstuff());
// SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees());
// SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel());
// SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos());
// SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel());
}
/**
* Get the drive motor of the SwerveModule
* @return the drive motor of the SwerveModule
*/
public TalonFX getDriveMotor() {
return this.driveMotor;
}
/**
* Get the angle motor of the SwerveModule
* @return the angle motor of the SwerveModule
*/
public TalonFX getAngleMotor() {
return this.angleMotor;
}
/**
* Get the CANcoder of the SwerveModule
* @return the CANcoder of the SwerveModule
*/
public CANcoder getEncoder() {
return this.encoder;
}
/**
* Get the angle of a SwerveModule as a Rotation2d
* @return the angle of a SwerveModule as a Rotation2d
*/
public Rotation2d getAngle() {
// * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
// return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
return Rotation2d.fromRotations(encoder.getPosition().getValue());
}
public double getAngularVel() {
// return this.angleMotor.getSelectedSensorVelocity();
return angleMotor.getVelocity().getValueAsDouble();
}
public double getDrivePos() {
// return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
return driveMotor.getPosition().getValueAsDouble();
}
public double getDriveVel() {
// return this.driveMotor.getSelectedSensorVelocity(0);
return driveMotor.getVelocity().getValueAsDouble();
}
public void stop() {
driveMotor.set(0);
angleMotor.set(0);
}
public void rotateToAngle(double angle) {
final PositionVoltage m_request = new PositionVoltage(angle);
angleMotor.setControl(m_request);
}
/**
* Get state of swerve module
* @return speed in m/s and angle in degrees
*/
public SwerveModuleState getState() {
return new SwerveModuleState(
Units.inchesToMeters(driveMotor.getVelocity().getValue() *
SwerveDriveConstants.Conversions.INCHES_PER_WHEEL_REV *
SwerveDriveConstants.Conversions.WHEEL_REV_PER_MOTOR_REV),
getAngle()
);
}
// private SwerveModuleState optimizeState(SwerveModuleState desiredState) {
// Rotation2d curRot = this.getAngle();
// }
/**
* Returns the current position of the SwerveModule
* @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
// */
// public SwerveModulePosition getPosition() {
// return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
// }
/**
* Set the speed and rotation of the SwerveModule from a SwerveModuleState object
* @param desiredState a SwerveModuleState representing the desired new state of the module
// */
public void setDesiredState(SwerveModuleState desiredState) {
Rotation2d currentRotation = this.getAngle();
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
// calculate the difference between our current rotational position and our new rotational position
Rotation2d rotationDelta = state.angle.minus(currentRotation);
double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND;
rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations());
driveMotor.set(Math.max(Math.min(speed, 1.), -1.));
}
public void reset() {
// encoder.setPosition(0);
}
// public double getCurrent() {
// return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
// }
// public double getVoltage() {
// return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
// }
// public String getstuff() {
// encoder.getPosition();
// return "" + encoder.getLastError().value;
// }
}
src/main/java/frc4388/robot/subsystems/Vision.java
-38
View File
@@ -1,38 +0,0 @@
package frc4388.robot.subsystems;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
public class Vision {
private final NetworkTableEntry m_isTags;
private final NetworkTableEntry m_xPoses;
private final NetworkTableEntry m_yPoses;
private final NetworkTableEntry m_zPoses;
public Vision() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
m_isTags = tagTable.getEntry("IsTag");
m_xPoses = tagTable.getEntry("TagPosX");
m_yPoses = tagTable.getEntry("TagPosY");
m_zPoses = tagTable.getEntry("TagPosZ");
}
public AprilTag[] getAprilTags() {
if (!m_isTags.getBoolean(false)) return new AprilTag[0];
double xarr[] = m_xPoses.getDoubleArray(new double[] {});
double yarr[] = m_yPoses.getDoubleArray(new double[] {});
double zarr[] = m_zPoses.getDoubleArray(new double[] {});
AprilTag tags[] = new AprilTag[xarr.length];
for (int i = 0; i < tags.length; i++) {
tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
}
return tags;
}
}
src/main/java/frc4388/utility/AprilTag.java
-13
View File
@@ -1,13 +0,0 @@
package frc4388.utility;
// This is a seperate class in case I want to encode rotation or other
// information about the tag
public class AprilTag {
public final double x, y, z;
public AprilTag(double _x, double _y, double _z) {
x = _x;
y = _y;
z = _z;
}
}
src/main/java/frc4388/utility/DataUtils.java
-35
View File
@@ -1,35 +0,0 @@
package frc4388.utility;
import java.nio.ByteBuffer;
public class DataUtils {
public static byte[] doubleToByteArray(double value) {
byte[] bytes = new byte[8];
ByteBuffer.wrap(bytes).putDouble(value);
return bytes;
}
public static double byteArrayToDouble(byte[] bytes) {
return ByteBuffer.wrap(bytes).getDouble();
}
public static byte[] intToByteArray(int value) {
byte[] bytes = new byte[4];
ByteBuffer.wrap(bytes).putInt(value);
return bytes;
}
public static int byteArrayToInt(byte[] bytes) {
return ByteBuffer.wrap(bytes).getInt();
}
public static byte[] shortToByteArray(short value) {
byte[] bytes = new byte[2];
ByteBuffer.wrap(bytes).putShort(value);
return bytes;
}
public static short byteArrayToShort(byte[] bytes) {
return ByteBuffer.wrap(bytes).getShort();
}
}
src/main/java/frc4388/utility/DualJoystickButton.java
-22
View File
@@ -1,22 +0,0 @@
package frc4388.utility;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj2.command.button.Trigger;
/**
* A button binding for two controllers, preferably an {@link frc4388.utility.controller.DeadbandedXboxController Xbox Controller} and {@link frc4388.utility.controller.VirtualController Virtual Xbox Controller}
* @author Zachary Wilke
*/
public class DualJoystickButton extends Trigger {
/**
* Creates an Button binding on two controllers
* @param joystickA A controller
* @param joystickB A controller
* @param buttonNumber The button to bind to
*/
public DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) {
super(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber)));
}
}
src/main/java/frc4388/utility/Gains.java
-83
View File
@@ -1,83 +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.utility;
/** Add your docs here. */
public class Gains {
public double kP;
public double kI;
public double kD;
public double kF;
public int kIZone;
public double kPeakOutput;
public double kMaxOutput;
public double kMinOutput;
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIZone The zone of the I value.
* @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
*/
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
this.kF = kF;
this.kIZone = kIZone;
this.kPeakOutput = kPeakOutput;
this.kMaxOutput = kPeakOutput;
this.kMinOutput = -kPeakOutput;
}
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIZone The zone of the I value.
*/
public Gains(double kP, double kI, double kD, double kF, int kIZone) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
this.kF = kF;
this.kIZone = kIZone;
this.kPeakOutput = 1.0;
this.kMaxOutput = 1.0;
this.kMinOutput = -1.0;
}
public Gains(double kP, double kI, double kD) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
}
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIZone The zone of the I value.
* @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
* @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
*/
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
this.kF = kF;
this.kIZone = kIZone;
this.kMaxOutput = kMaxOutput;
this.kMinOutput = kMinOutput;
this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
}
}
src/main/java/frc4388/utility/LEDPatterns.java
-45
View File
@@ -1,45 +0,0 @@
package frc4388.utility;
/**
* Add your docs here.
*/
public enum LEDPatterns {
/* PALLETTE PATTERNS */
RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f),
PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f),
PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f),
RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f),
RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f),
RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f),
BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f),
GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
/* COLOR 1 PATTERNS */
C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f),
C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f),
/* COLOR 2 PATTERNS */
C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f),
C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f),
/* COLOR 1 AND 2 PATTERNS */
C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
C1C2_WAVES(0.53f), C1C2_SINELON(0.55f),
/* SOLID COLORS */
SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f),
SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f),
SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f),
SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f),
SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f);
/* GETTERS/SETTERS */
private final float id;
LEDPatterns(float id) {
this.id = id;
}
public float getValue() {
return id;
}
}
src/main/java/frc4388/utility/configurable/ConfigurableDouble.java
-23
View File
@@ -1,23 +0,0 @@
package frc4388.utility.configurable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class ConfigurableDouble {
private double defualtValue;
private String name;
/**
* Creates an new ConfigurableDouble through Smart Dashboard.
* @param name the name of the Smart Dashboard key.
* @param defualtValue the initilization value
*/
public ConfigurableDouble(String name, double defualtValue) {
this.name = name;
this.defualtValue = defualtValue;
SmartDashboard.putNumber(name, defualtValue);
}
public double get() {
return SmartDashboard.getNumber(name, defualtValue);
}
}
src/main/java/frc4388/utility/configurable/ConfigurableString.java
-23
View File
@@ -1,23 +0,0 @@
package frc4388.utility.configurable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class ConfigurableString {
private String defualtValue;
private String name;
/**
* Creates an new ConfigurableString through Smart Dashboard.
* @param name the name of the Smart Dashboard key.
* @param defualtValue the initilization value
*/
public ConfigurableString(String name, String defualtValue) {
this.name = name;
this.defualtValue = defualtValue;
SmartDashboard.putString(name, defualtValue);
}
public String get() {
return SmartDashboard.getString(name, defualtValue);
}
}