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

rebase from 2024

Browse Source
This commit is contained in:
C4llSiqn
2024-10-26 13:26:12 -06:00
parent 9f835cdd4f
commit b1213e7d43
42 changed files with 1637 additions and 681 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+78 -81
View File
@@ -8,9 +8,8 @@
package frc4388.robot;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import frc4388.utility.LEDPatterns;
import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -23,82 +22,98 @@ import frc4388.utility.Gains;
public final class Constants {
public static final class SwerveDriveConstants {
public static final double MAX_ROT_SPEED = 1.5;
public static final double MIN_ROT_SPEED = 0.8;
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 double SLOW_SPEED = 0.8;
public static final double FAST_SPEED = 1.0;
public static final double TURBO_SPEED = 4.0;
public static final class DefaultSwerveRotOffsets {
public static final double FRONT_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
public static final double FRONT_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
public static final double BACK_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
public static final double BACK_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
}
public static final class IDs {
public static final int LEFT_FRONT_WHEEL_ID = 2;
public static final int LEFT_FRONT_STEER_ID = 3;
public static final int LEFT_FRONT_ENCODER_ID = 10;
public static final int RIGHT_FRONT_WHEEL_ID = 4;
public static final int RIGHT_FRONT_STEER_ID = 5;
public static final int RIGHT_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 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(0.5, 0.0, 0.0, 0.0, 0, 1.0);
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 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 int CANCODER_TICKS_PER_ROTATION = 4096;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
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 = 2048;
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 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; // TODO: find the actual value
public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
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 = 5; // TODO: find the actual value
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
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;
@@ -111,31 +126,10 @@ public final class Constants {
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class VisionConstants {
public static final String NAME = "photonCamera";
public static final int LIME_HIXELS = 640;
public static final int LIME_VIXELS = 480;
public static final double H_FOV = 59.6;
public static final double V_FOV = 45.7;
public static final double LIME_HEIGHT = 6.0;
public static final double LIME_ANGLE = 55.0;
// public static final double HIGH_TARGET_HEIGHT = 46.0;
public static final double HIGH_TAPE_HEIGHT = 44.0;
// public static final double MID_TARGET_HEIGHT = 34.0;
public static final double MID_TAPE_HEIGHT = 24.0;
public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
}
public static final class DriveConstants {
public static final int DRIVE_PIGEON_ID = 6;
public static final class VisionConstants {
}
public static final class DriveConstants {
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
@@ -148,5 +142,8 @@ public final class Constants {
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/Robot.java
+9 -6
View File
@@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc4388.utility.DeferredBlock;
import frc4388.utility.RobotTime;
//import frc4388.robot.subsystems.LED;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the TimedRobot
@@ -22,16 +22,17 @@ import frc4388.utility.RobotTime;
*/
public class Robot extends TimedRobot {
Command m_autonomousCommand;
private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer;
//private LED mled = new LED();
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
@@ -40,14 +41,16 @@ public class Robot extends TimedRobot {
/**
* This function is called every robot packet, no matter the mode. Use
* this for items like diagnostics that you want ran during disabled,
* autonomous, teleoperated and test.
* autonomous, teleoperated and test.doubl
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
public void robotPeriodic() {
m_robotTime.updateTimes();
//System.out.println(m_robotContainer.limelight.isNearSpeaker());
//mled.updateLED();
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
@@ -119,7 +122,7 @@ public class Robot extends TimedRobot {
*/
@Override
public void teleopPeriodic() {
// m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft());
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+167 -54
View File
@@ -7,18 +7,33 @@
package frc4388.robot;
import edu.wpi.first.wpilibj.Joystick;
// 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.robot.Constants.OIConstants;
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.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.Swerve.JoystickPlayback;
import frc4388.robot.commands.Swerve.JoystickRecorder;
import frc4388.robot.subsystems.LED;
import frc4388.utility.LEDPatterns;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController;
// Autos
import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
// Subsystems
// 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
@@ -29,25 +44,84 @@ import frc4388.utility.controller.XboxController;
*/
public class RobotContainer {
/* RobotMap */
private final RobotMap m_robotMap = new RobotMap();
public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
private final LED m_robotLED = new LED(m_robotMap.LEDController);
// private final LED m_robotLED = new LED();
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 XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
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);
/* Virtual Controllers */
private final VirtualController m_virtualDriver = new VirtualController(0);
private final VirtualController m_virtualOperator = new VirtualController(1);
// ! Teleop Commands
// ! /* Autos */
private String lastAutoName = "defualt.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);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
configureButtonBindings();
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_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
// m_robotSwerveDrive.driveWithInput(
// getDeadbandedDriverController().getLeft(),
// getDeadbandedDriverController().getRight(),
// true);
// }, m_robotSwerveDrive));
}
/**
@@ -57,26 +131,68 @@ public class RobotContainer {
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
/* Driver Buttons */
// test command to spin the robot while pressing A on the driver controller
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
// () -> getDeadbandedDriverController().getLeftX(),
// () -> getDeadbandedDriverController().getLeftY(),
// () -> getDeadbandedDriverController().getRightX(),
// () -> getDeadbandedDriverController().getRightY(),
// "Blue1Path.txt"))
// .onFalse(new InstantCommand());
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
// .onFalse(new InstantCommand());
// ? /* 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 */
// ? /* 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());
}
/**
* This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}. <p/>
* Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons.
*/
private void configureVirtualButtonBindings() {
// ? /* Driver Buttons */
/* Notice: the following buttons have not been replicated
* Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback
* Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode.
* Auto Recording controls : We don't want an Null Ouroboros for an auto.
*/
// ? /* Operator Buttons */
/* Notice: the following buttons have not been replicated
* Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot.
* We don't need it in an auto.
* Climbing controls : We don't need to climb in auto.
*/
// ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto.
/* Operator Buttons */
// activates "Lit Mode"
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
}
/**
@@ -85,35 +201,32 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// no auto
return new InstantCommand();
return autoPlayback;
}
/**
* Add your docs here.
* A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller}
* @param joystickA A controller
* @param joystickB A controller
* @param buttonNumber The button to bind to
*/
public IHandController getDriverController() {
return m_driverXbox;
public Trigger DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) {
return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber)));
}
/**
* Add your docs here.
*/
public IHandController getOperatorController() {
return m_operatorXbox;
public DeadbandedXboxController getDeadbandedDriverController() {
return this.m_driverXbox;
}
/**
* Add your docs here.
*/
public Joystick getOperatorJoystick() {
return m_operatorXbox.getJoyStick();
public DeadbandedXboxController getDeadbandedOperatorController() {
return this.m_operatorXbox;
}
/**
* Add your docs here.
*/
public Joystick getDriverJoystick() {
return m_driverXbox.getJoyStick();
public VirtualController getVirtualDriverController() {
return m_virtualDriver;
}
public VirtualController getVirtualOperatorController() {
return m_virtualOperator;
}
}
src/main/java/frc4388/robot/RobotMap.java
+37 -12
View File
@@ -7,14 +7,14 @@
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.PigeonIMU;
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 edu.wpi.first.wpilibj.drive.DifferentialDrive;
import frc4388.robot.Constants.LEDConstants;
// import edu.wpi.first.wpilibj.motorcontrol.Spark;
// import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.SwerveModule;
import frc4388.utility.RobotGyro;
/**
@@ -22,19 +22,44 @@ import frc4388.utility.RobotGyro;
* 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);
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
void configureLEDMotorControllers() {
}
/* 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);
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);
}
}
src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java
+9 -2
View File
@@ -42,6 +42,7 @@ public class PlaybackChooser {
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);
@@ -66,9 +67,15 @@ public class PlaybackChooser {
public void nextChooser() {
SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
for (String auto : m_dir.list()) {
chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
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));
}
src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt
+20
View File
@@ -0,0 +1,20 @@
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
@@ -0,0 +1,107 @@
// 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/PID.java
+2 -2
View File
@@ -4,10 +4,10 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.utility.Gains;
public abstract class PID extends CommandBase {
public abstract class PID extends Command {
protected Gains gains;
private double output = 0;
private double tolerance = 0;
src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java
+8 -4
View File
@@ -9,11 +9,11 @@ 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.CommandBase;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickPlayback extends CommandBase {
public class JoystickPlayback extends Command {
private final SwerveDrive swerve;
private String filename;
private int mult = 1;
@@ -119,7 +119,11 @@ public class JoystickPlayback extends CommandBase {
// new Translation2d(out.rightX, out.rightY),
// true);
this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
// this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
// new Translation2d(lerpRX, lerpRY),
// true);
this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY),
new Translation2d(lerpRX, lerpRY),
true);
@@ -138,4 +142,4 @@ public class JoystickPlayback extends CommandBase {
public boolean isFinished() {
return m_finished;
}
}
}
src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
+5 -5
View File
@@ -11,11 +11,11 @@ import java.util.ArrayList;
import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickRecorder extends CommandBase {
public class JoystickRecorder extends Command {
public final SwerveDrive swerve;
public final Supplier<Double> leftX;
@@ -64,11 +64,11 @@ public class JoystickRecorder extends CommandBase {
outputs.add(inputs);
swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
new Translation2d(inputs.rightX, inputs.rightY),
true);
System.out.println("RECORDING");
//System.out.println("RECORDING");
}
// Called once the command ends or is interrupted.
@@ -94,4 +94,4 @@ public class JoystickRecorder extends CommandBase {
public boolean isFinished() {
return false;
}
}
}
src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java
+197
View File
@@ -0,0 +1,197 @@
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
@@ -0,0 +1,129 @@
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/subsystems/DiffDrive.java
+10 -7
View File
@@ -7,7 +7,8 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
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;
@@ -26,23 +27,25 @@ public class DiffDrive extends SubsystemBase {
private RobotTime m_robotTime = RobotTime.getInstance();
private WPI_TalonFX m_leftFrontMotor;
private WPI_TalonFX m_rightFrontMotor;
private WPI_TalonFX m_leftBackMotor;
private WPI_TalonFX m_rightBackMotor;
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(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor,
WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
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;
}
src/main/java/frc4388/robot/subsystems/LED.java
+48 -21
View File
@@ -7,6 +7,8 @@
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;
@@ -20,44 +22,69 @@ import frc4388.utility.LEDPatterns;
*/
public class LED extends SubsystemBase {
private LEDPatterns m_currentPattern;
private Spark m_LEDController;
static AddressableLED m_led;
static AddressableLEDBuffer m_ledBuffer;
static LED m_self;
/**
* Add your docs here.
*/
public LED(Spark LEDController){
m_LEDController = LEDController;
setPattern(LEDConstants.DEFAULT_PATTERN);
updateLED();
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(){
SmartDashboard.putNumber("LED", m_currentPattern.getValue());
//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 void updateLED(){
m_LEDController.set(m_currentPattern.getValue());
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());
}
/**
* Add your docs here.
*/
public void setPattern(LEDPatterns pattern){
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 LEDPatterns getPattern() {
return m_currentPattern;
public AddressableLEDBuffer getBuffer() {
return m_ledBuffer;
}
}
src/main/java/frc4388/robot/subsystems/Limelight.java
-165
View File
@@ -1,165 +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 java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.common.hardware.VisionLEDMode;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.VisionConstants;
public class Limelight extends SubsystemBase {
private PhotonCamera cam;
private PhotonPoseEstimator photonPoseEstimator;
private boolean lightOn;
/** Creates a new Limelight. */
public Limelight() {
cam = new PhotonCamera(VisionConstants.NAME);
cam.setDriverMode(false);
}
public void setLEDs(boolean on) {
lightOn = on;
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
public void toggleLEDs() {
lightOn = !lightOn;
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
public void setDriverMode(boolean driverMode) {
cam.setDriverMode(driverMode);
}
public void setToLimePipeline() {
cam.setPipelineIndex(1);
setLEDs(true);
}
public void setToAprilPipeline() {
cam.setPipelineIndex(0);
setLEDs(false);
}
public PhotonTrackedTarget getAprilPoint() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
return result.getBestTarget();
}
private List<TargetCorner> getAprilCorners() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
return result.getBestTarget().getDetectedCorners();
}
public double getAprilSkew() {
List<TargetCorner> corners = getAprilCorners();
ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
if (bottomSide == null) return 0;
TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
return bottomLeft.y - bottomRight.y;
}
private ArrayList<TargetCorner> getAprilBottomSide(List<TargetCorner> box) {
if (box == null) return null;
ArrayList<TargetCorner> bottomSide = new ArrayList<>();
TargetCorner l1 = new TargetCorner(-1, -1);
TargetCorner l2 = new TargetCorner(-1, -1);
for (TargetCorner c : box) {
if (c.y > l1.y) l1 = c;
}
for (TargetCorner c : box) {
if (c.y == l1.y) continue;
if (c.y > l2.y) l2 = c;
}
bottomSide.add(l1);
bottomSide.add(l2);
return bottomSide;
}
public double getDistanceToApril() {
PhotonTrackedTarget aprilPoint = getAprilPoint();
if (aprilPoint == null) return -1;
double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
double theta = 35.0 + aprilPoint.getPitch();
double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
return distanceToApril;
}
public PhotonTrackedTarget getLowestTape() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
PhotonTrackedTarget lowest = points.get(0);
for (PhotonTrackedTarget point : points) {
if (point.getPitch() < lowest.getPitch()) {
lowest = point;
}
}
return lowest;
}
public double getDistanceToTape() {
PhotonTrackedTarget tapePoint = getLowestTape();
if (tapePoint == null) return -1;
double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
double theta = 35.0 + tapePoint.getPitch();
double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
return distanceToTape;
}
@Override
public void periodic() {}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+176 -38
View File
@@ -4,17 +4,21 @@
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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.utility.RobotUnits;
public class SwerveDrive extends SubsystemBase {
public class SwerveDrive extends SubsystemBase {
private SwerveModule leftFront;
private SwerveModule rightFront;
@@ -24,17 +28,23 @@ public class SwerveDrive extends SubsystemBase {
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 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_SLOW; // * slow by default
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. */
@@ -45,20 +55,37 @@ public class SwerveDrive extends SubsystemBase {
this.rightBack = rightBack;
this.gyro = gyro;
reset_index();
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
boolean stopped = false;
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 = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
SmartDashboard.putBoolean("drift correction", false);
rot_correction = 0;
// rot = rightStick.getX();
// SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
@@ -66,8 +93,9 @@ public class SwerveDrive extends SubsystemBase {
stopped = true;
}
SmartDashboard.putBoolean("drift correction", true);
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
// SmartDashboard.putBoolean("drift correction", true);
// rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
}
@@ -76,14 +104,82 @@ public class SwerveDrive extends SubsystemBase {
// 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(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
} else {
// Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
// 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.
@@ -111,12 +207,33 @@ public class SwerveDrive extends SubsystemBase {
}
public double getGyroAngle() {
return gyro.getAngle();
return -gyro.getAngle();
}
public void add180() {
gyro.reset(gyro.getAngle()+180);
rotTarget = gyro.getAngle();
}
public void resetGyro() {
gyro.reset();
rotTarget = 0.0;
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() {
@@ -129,24 +246,45 @@ public class SwerveDrive extends SubsystemBase {
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 (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
} else {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
}
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 = SwerveDriveConstants.SLOW_SPEED;
this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED;
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
@@ -155,7 +293,7 @@ public class SwerveDrive extends SubsystemBase {
}
public void setToFast() {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED;
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
@@ -164,7 +302,7 @@ public class SwerveDrive extends SubsystemBase {
}
public void setToTurbo() {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED;
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
@@ -172,16 +310,6 @@ public class SwerveDrive extends SubsystemBase {
System.out.println("TURBO");
}
public void shiftUp() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
} else {
}
}
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;
@@ -192,4 +320,14 @@ public class SwerveDrive extends SubsystemBase {
}
}
public void shiftUpRot() {
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
}
public void shiftDownRot() {
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
}
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+136 -55
View File
@@ -4,56 +4,126 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
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 WPI_TalonFX driveMotor;
private WPI_TalonFX angleMotor;
private CANCoder encoder;
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(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
public SwerveModule(TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) {
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
this.encoder = encoder;
TalonFXConfiguration angleConfig = new TalonFXConfiguration();
angleConfig.slot0.kP = swerveGains.kP;
angleConfig.slot0.kI = swerveGains.kI;
angleConfig.slot0.kD = swerveGains.kD;
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)
);
// use the CANcoder as the remote sensor for the primary TalonFX PID
angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
angleMotor.configAllSettings(angleConfig);
encoder.configMagnetOffset(offset);
driveMotor.getConfigurator().apply(motorCfg);
driveMotor.setSelectedSensorPosition(0);
driveMotor.config_kP(0, 0.2);
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 WPI_TalonFX getDriveMotor() {
public TalonFX getDriveMotor() {
return this.driveMotor;
}
@@ -61,7 +131,7 @@ public class SwerveModule extends SubsystemBase {
* Get the angle motor of the SwerveModule
* @return the angle motor of the SwerveModule
*/
public WPI_TalonFX getAngleMotor() {
public TalonFX getAngleMotor() {
return this.angleMotor;
}
@@ -69,7 +139,7 @@ public class SwerveModule extends SubsystemBase {
* Get the CANcoder of the SwerveModule
* @return the CANcoder of the SwerveModule
*/
public CANCoder getEncoder() {
public CANcoder getEncoder() {
return this.encoder;
}
@@ -79,19 +149,23 @@ public class SwerveModule extends SubsystemBase {
*/
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.fromDegrees(encoder.getAbsolutePosition());
return Rotation2d.fromRotations(encoder.getPosition().getValue());
}
public double getAngularVel() {
return this.angleMotor.getSelectedSensorVelocity();
// return this.angleMotor.getSelectedSensorVelocity();
return angleMotor.getVelocity().getValueAsDouble();
}
public double getDrivePos() {
return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
// return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
return driveMotor.getPosition().getValueAsDouble();
}
public double getDriveVel() {
return this.driveMotor.getSelectedSensorVelocity(0);
// return this.driveMotor.getSelectedSensorVelocity(0);
return driveMotor.getVelocity().getValueAsDouble();
}
public void stop() {
@@ -100,7 +174,8 @@ public class SwerveModule extends SubsystemBase {
}
public void rotateToAngle(double angle) {
angleMotor.set(TalonFXControlMode.Position, angle);
final PositionVoltage m_request = new PositionVoltage(angle);
angleMotor.setControl(m_request);
}
/**
@@ -109,23 +184,30 @@ public class SwerveModule extends SubsystemBase {
*/
public SwerveModuleState getState() {
return new SwerveModuleState(
Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
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());
}
// */
// 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();
@@ -134,28 +216,27 @@ public class SwerveModule extends SubsystemBase {
// calculate the difference between our current rotational position and our new rotational position
Rotation2d rotationDelta = state.angle.minus(currentRotation);
// calculate the new absolute position of the SwerveModule based on the difference in rotation
double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND;
// convert the CANCoder from its position reading to ticks
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations());
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
driveMotor.set(Math.max(Math.min(speed, 1.), -1.));
}
public void reset(double position) {
encoder.setPositionToAbsolute();
public void reset() {
// encoder.setPosition(0);
}
public double getCurrent() {
return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
}
// public double getCurrent() {
// return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
// }
public double getVoltage() {
return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
}
// 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/utility/DataUtils.java
+35
View File
@@ -0,0 +1,35 @@
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/RobotGyro.java
+91 -22
View File
@@ -7,20 +7,22 @@
package frc4388.utility;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
// import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.kauailabs.navx.frc.AHRS;
// import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpilibj.interfaces.Gyro;
// import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
/**
* Gyro class that allows for interchangeable use between a pigeon and a navX
*/
public class RobotGyro implements Gyro {
public class RobotGyro {
private RobotTime m_robotTime = RobotTime.getInstance();
private WPI_Pigeon2 m_pigeon = null;
private Pigeon2 m_pigeon = null;
private AHRS m_navX = null;
public boolean m_isGyroAPigeon; //true if pigeon, false if navX
@@ -34,7 +36,7 @@ public class RobotGyro implements Gyro {
* Creates a Gyro based on a pigeon
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(WPI_Pigeon2 gyro) {
public RobotGyro(Pigeon2 gyro) {
m_pigeon = gyro;
m_isGyroAPigeon = true;
}
@@ -54,8 +56,8 @@ public class RobotGyro implements Gyro {
public void resetZeroValues() {
if (!m_isGyroAPigeon) return;
pitchZero = m_pigeon.getPitch();
rollZero = m_pigeon.getRoll();
// pitchZero = m_pigeon.getPitch();
// rollZero = m_pigeon.getRoll();
}
/**
@@ -84,16 +86,15 @@ public class RobotGyro implements Gyro {
* is typically done when the robot is first turned on while it's sitting at rest before the
* competition starts.
*/
@Override
public void calibrate() {
if (m_isGyroAPigeon) {
m_pigeon.calibrate();
} else {
m_navX.calibrate();
}
return;
// if (m_isGyroAPigeon) {
// m_pigeon.calibrate();
// } else {
// m_navX.calibrate();
// }
}
@Override
public void reset() {
resetZeroValues();
@@ -102,6 +103,73 @@ public class RobotGyro implements Gyro {
} else {
m_navX.reset();
}
}
public void reset(double val) {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(val);
} else {
m_navX.reset();
}
}
public void resetFlip() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(180);
} else {
m_navX.reset();
}
}
public void resetNinety() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(90);
} else {
m_navX.reset();
}
}
public void resetTwoSeventy() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(270);
} else {
m_navX.reset();
}
}
public void resetRightSideBlue() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(60);
} else {
m_navX.reset();
}
}
public void resetAmpSide() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(-60);
} else {
m_navX.reset();
}
}
/**
@@ -113,16 +181,19 @@ public class RobotGyro implements Gyro {
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
m_pigeon.getAngle();
var rotation = m_pigeon.getRotation3d();
return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())};
}
public Rotation2d getRotation2d() {
return m_pigeon.getRotation2d();
}
@Override
public double getAngle() {
if (m_isGyroAPigeon) {
return getPigeonAngles()[0];
return getPigeonAngles()[2];
} else {
return m_navX.getAngle();
}
@@ -176,7 +247,6 @@ public class RobotGyro implements Gyro {
}
}
@Override
public double getRate() {
if (m_isGyroAPigeon) {
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
@@ -185,7 +255,7 @@ public class RobotGyro implements Gyro {
}
}
public WPI_Pigeon2 getPigeon(){
public Pigeon2 getPigeon(){
return m_pigeon;
}
@@ -193,7 +263,6 @@ public class RobotGyro implements Gyro {
return m_navX;
}
@Override
public void close() throws Exception {
}
src/main/java/frc4388/utility/UtilityStructs.java
+14
View File
@@ -6,7 +6,21 @@ public class UtilityStructs {
public double leftY = 0.0;
public double rightX = 0.0;
public double rightY = 0.0;
public boolean OPLB;
public boolean OPRB;
public long timedOffset = 0;
}
public static class AutoRecordingControllerFrame {
public double[] axes = new double[6];
public short button = 0;
public short[] POV = new short[1];
}
public static class AutoRecordingFrame {
public AutoRecordingControllerFrame[] controllerFrames = new AutoRecordingControllerFrame[2];
public int timeStamp;
}
}
src/main/java/frc4388/utility/configurable/ConfigurableDouble.java
+23
View File
@@ -0,0 +1,23 @@
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
@@ -0,0 +1,23 @@
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);
}
}
src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+27
View File
@@ -0,0 +1,27 @@
package frc4388.utility.controller;
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.XboxController;
public class DeadbandedXboxController extends XboxController {
public DeadbandedXboxController(int port) { super(port); }
@Override public double getLeftX() { return getLeft().getX(); }
@Override public double getLeftY() { return getLeft().getY(); }
@Override public double getRightX() { return getRight().getX(); }
@Override public double getRightY() { return getRight().getY(); }
public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
return translation2d;
}
}
src/main/java/frc4388/utility/controller/VirtualController.java
+145
View File
@@ -0,0 +1,145 @@
package frc4388.utility.controller;
import edu.wpi.first.wpilibj.GenericHID;
/**
* A virtual controller that can be bound like an standard controller.
* @author Zachary Wilke
*/
public class VirtualController extends GenericHID {
private short m_buttonStates = 0;
private short m_buttonStatesLastFrame = 0;
private double[] m_axes = new double[6];
private short[] m_pov = new short[1];
/**
* Create an virtual controller
* @param port virtual port (merely a formality).
*/
public VirtualController(int port) {
super(port);
}
/**
* Set the curent inputs to the new frames.
* @param axes joystick axes, (i.e. joysticks and triggers).
* @param buttonFlags the bit packed button states.
* @param pov the array of dpads.
*/
public void setFrame(double[] axes, short buttonFlags, short[] pov) {
m_axes = axes;
setOutputs(buttonFlags);
m_pov = pov;
}
/**
* Zero outs the controls.
*/
public void zeroControls() {
m_axes = new double[6];
m_buttonStates = 0;
m_buttonStatesLastFrame = 0;
m_pov = new short[] {-1};
}
/**
* Gets the value of a bitflag from an int
* @param value int to search
* @param index index of bit
* @return if the bit is set
*/
public static boolean getFlag(int value, int index) {
return ((value & 1 << index) != 0);
}
@Override
public boolean getRawButton(int button) { // man why are buttons indexed at 1.
return getFlag(m_buttonStates, button - 1);
}
@Override
public boolean getRawButtonPressed(int button) {
return (!getFlag(m_buttonStatesLastFrame, button - 1) && getRawButton(button));
}
@Override
public boolean getRawButtonReleased(int button) {
return (getFlag(m_buttonStatesLastFrame, button - 1) && !getRawButton(button));
}
@Override
public double getRawAxis(int axis) {
return m_axes[axis];
}
@Override
public int getPOV(int pov) {
return m_pov[pov];
}
@Override
public int getAxisCount() {
return m_axes.length;
}
@Override
public int getPOVCount() {
return m_pov.length;
}
@Override
public int getButtonCount() {
return 10;
}
@Override
public boolean isConnected() {
return true;
}
@Override
public HIDType getType() {
return HIDType.kXInputGamepad;
}
@Override
public String getName() {
return "Virtual Controller";
}
@Override
public int getAxisType(int axis) {
return 1; /* ! Warning, does not return accurate data.
Hopefully this isn't a problem */
}
/**
* Use {@link VirtualController#setFrame} or {@link VirtualController#setOutputs}.
* this is an no-op overide.
*/
@Override
public void setOutput(int outputNumber, boolean value) {
// do not use
//m_buttonStatesLastFrame[outputNumber - 1] = m_buttonStates[outputNumber - 1];
//m_buttonStates[outputNumber - 1] = value;
}
/**
* Set buttons from a packed int, if you want to set joysticks and dpad use {@link VirtualController#SetFrame}
*/
@Override
public void setOutputs(int value) {
m_buttonStatesLastFrame = m_buttonStates;
m_buttonStates = (short) value;
}
/**
* Why are you Setting rumble on an virtual controller?
* @param type the rumble type (even though it won't do anything)
* @param value the rumble strength (always multiplyed by 0.0)
*/
@Override
public void setRumble(RumbleType type, double value) {
System.out.println("Why are you Setting rumble on an virtual controller?");
}
}
src/main/java/frc4388/utility/controller/XboxTriggerButton.java
+2 -2
View File
@@ -1,13 +1,13 @@
package frc4388.utility.controller;
import edu.wpi.first.wpilibj2.command.button.Button;
//import edu.wpi.first.wpilibj2.command.button.Trigger;
/**
* Mapping for the Xbox controller triggers to allow triggers to be defined as
* buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger
* exceeds a tolerance defined in {@link XboxController}.
*/
public class XboxTriggerButton extends Button {
public class XboxTriggerButton {//extends Trigger {
public static final int RIGHT_TRIGGER = 0;
public static final int LEFT_TRIGGER = 1;
public static final int RIGHT_AXIS_UP_TRIGGER = 2;