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

Merge remote-tracking branch 'origin/swerve' into robot-logger

Browse Source
This commit is contained in:
nathanrsxtn
2022-02-04 19:09:28 -07:00
parent c4acef61ea dd30f1dc6f
commit 19d4b2accc
37 changed files with 115155 additions and 561 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/First Test Path.path
+1
View File
@@ -0,0 +1 @@
{"waypoints":[{"anchorPoint":{"x":3.0,"y":3.0},"prevControl":null,"nextControl":{"x":3.410442907302426,"y":3.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.5,"y":4.5},"prevControl":{"x":4.288229266113705,"y":4.529967680625016},"nextControl":{"x":4.67537248192198,"y":4.475183036710464},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.0,"y":3.0},"prevControl":{"x":5.973638727554094,"y":3.050086417647222},"nextControl":{"x":6.10510365848318,"y":2.8003030488819602},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.5,"y":1.5029823163094613},"prevControl":{"x":4.6862524414704,"y":1.722990549429376},"nextControl":{"x":4.3137475585296,"y":1.2829740831895466},"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":3.0},"prevControl":{"x":3.5480060960121915,"y":2.4748364324314855},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":5.0,"maxAcceleration":5.0,"isReversed":null}
src/main/deploy/pathplanner/Move Down.path
+1
View File
@@ -0,0 +1 @@
{"waypoints":[{"anchorPoint":{"x":5.0,"y":5.0},"prevControl":null,"nextControl":{"x":5.034465241344275,"y":5.002934143799322},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":4.992423777951004,"y":2.9954542667706034},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
src/main/deploy/pathplanner/Move Forward.path
+1
View File
@@ -0,0 +1 @@
{"waypoints":[{"anchorPoint":{"x":2.0390109745736735,"y":2.9639231692256494},"prevControl":null,"nextControl":{"x":2.049521340421991,"y":2.9534128033773315},"holonomicAngle":1.2188752351312955,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":5.0029341437993216,"y":2.995454266770603},"nextControl":null,"holonomicAngle":-178.60254149056965,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
src/main/deploy/pathplanner/Rotate.path
+1
View File
@@ -0,0 +1 @@
{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":false}
src/main/deploy/pathplanner/generatedJSON/First Test Path.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/pathplanner/generatedJSON/Move Down.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/pathplanner/generatedJSON/Move Forward.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/pathplanner/generatedJSON/New Path.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/pathplanner/generatedJSON/Rotate.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/java/frc4388/robot/Constants.java
+4 -2
View File
@@ -27,8 +27,8 @@ public final class Constants {
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 4;
public static final double WHEEL_SPEED = 0.1;
public static final double WIDTH = 22;
public static final double HEIGHT = 22;
public static final double WIDTH = 15.27;
public static final double HEIGHT = 15.27;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
public static final double MAX_SPEED_FEET_PER_SEC = 20; // redundant constant?
@@ -102,5 +102,7 @@ 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 double LEFT_AXIS_DEADBAND = 0.1;
public static final double RIGHT_AXIS_DEADBAND = 0.6;
}
}
src/main/java/frc4388/robot/Main.java
+4 -1
View File
@@ -4,7 +4,9 @@
package frc4388.robot;
import edu.wpi.first.wpilibj.RobotBase;
import frc4388.utility.AnsiLogging;
/**
* Do NOT add any static variables to this class, or any initialization at all.
@@ -21,9 +23,10 @@ public final class Main {
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
AnsiLogging.systemInstall();
RobotBase.startRobot(Robot::new);
}
}
// hi ryan
// hi ryan
src/main/java/frc4388/robot/Robot.java
+20 -1
View File
@@ -6,8 +6,11 @@ package frc4388.robot;
import java.io.IOException;
import java.util.logging.Level;
import java.util.logging.Logger;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -22,6 +25,7 @@ import frc4388.utility.RobotTime;
* project.
*/
public class Robot extends TimedRobot {
private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
Command m_autonomousCommand;
private RobotTime m_robotTime = RobotTime.getInstance();
@@ -33,6 +37,17 @@ public class Robot extends TimedRobot {
*/
@Override
public void robotInit() {
if (org.fusesource.jansi.Ansi.isEnabled()) {
LOGGER.log(Level.ALL, "Logging Test 1/8");
LOGGER.log(Level.SEVERE, "Logging Test 2/8");
LOGGER.log(Level.WARNING, "Logging Test 3/8");
LOGGER.log(Level.INFO, "Logging Test 4/8");
LOGGER.log(Level.CONFIG, "Logging Test 5/8");
LOGGER.log(Level.FINE, "Logging Test 6/8");
LOGGER.log(Level.FINER, "Logging Test 7/8");
LOGGER.log(Level.FINEST, "Logging Test 8/8");
}
LOGGER.fine("robotInit()");
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
@@ -66,6 +81,7 @@ public class Robot extends TimedRobot {
*/
@Override
public void disabledInit() {
LOGGER.fine("disabledInit()");
m_robotTime.endMatchTime();
RobotLogger.getInstance().setEnabled(false);
}
@@ -82,6 +98,7 @@ public class Robot extends TimedRobot {
*/
@Override
public void autonomousInit() {
LOGGER.fine("autonomousInit()");
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
@@ -112,6 +129,7 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
LOGGER.fine("teleopInit()");
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
@@ -121,6 +139,7 @@ public class Robot extends TimedRobot {
}
m_robotTime.startMatchTime();
RobotLogger.getInstance().setEnabled(true);
DriverStation.silenceJoystickConnectionWarning(true);
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+108 -36
View File
@@ -5,19 +5,36 @@
package frc4388.robot;
import java.util.List;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
import edu.wpi.first.math.controller.HolonomicDriveController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
import edu.wpi.first.wpilibj.XboxController;
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;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.LEDPatterns;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController;
import frc4388.utility.controller.DeadbandedXboxController;
/**
* This class is where the bulk of the robot should be declared. Since
@@ -37,8 +54,8 @@ public class RobotContainer {
private final LED m_robotLED = new LED(m_robotMap.LEDController);
/* 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 XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -49,10 +66,10 @@ public class RobotContainer {
// drives the swerve drive with a two-axis input from the driver controller
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
XboxController.ClampJoystickAxis(
getDriverController().getLeftXAxis(),
getDriverController().getLeftYAxis()),
-getDriverController().getRightXAxis(),
getDriverController().getLeftX(),
getDriverController().getLeftY(),
getDriverController().getRightX(),
getDriverController().getRightY(),
true),
m_robotSwerveDrive));
@@ -68,45 +85,114 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
/* Driver Buttons */
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.m_gyro.reset());
.whenPressed(m_robotSwerveDrive.m_gyro::reset);
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
.whenPressed(() -> resetOdometry());
.whenPressed(() -> zeroOdometry(new Pose2d(0, 0, new Rotation2d(0))));
//.whenPressed(this::resetOdometry);
/* Operator Buttons */
// activates "Lit Mode"
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON)
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
}
public void configAuto(boolean pathplanner) {
}
public void configAuto() {
configAuto(true);
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// no auto
return new InstantCommand();
// https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
TrajectoryConfig config = new TrajectoryConfig(1.0, 1.0)
.setKinematics(m_robotSwerveDrive.m_kinematics);
Trajectory exTraj = TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 0, new Rotation2d(0)),
List.of(new Translation2d(0, 0)),
new Pose2d(1, 0, new Rotation2d(0)),
config);
Trajectory firstTestPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0);
Trajectory moveForward = PathPlanner.loadPath("Move Forward", 1.0, 1.0);
Trajectory rotate = PathPlanner.loadPath("Rotate", 1.0, 1.0);
Trajectory currentPath = moveForward; // change this to change auto
PIDController xController = new PIDController(10.0, 0.0, 0.0);
PIDController yController = new PIDController(1.3, 0.0, 0.0);
ProfiledPIDController thetaController = new ProfiledPIDController(
10.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, Math.PI));
thetaController.enableContinuousInput(-Math.PI, Math.PI);
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
currentPath,
m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics,
xController,
yController,
thetaController,
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive
);
PathPlannerTrajectory ppfirstTestPath = PathPlanner.loadPath("First Test Path", 4.0, 4.0);
PathPlannerTrajectory ppMoveForward = PathPlanner.loadPath("Move Forward", 1.0, 1.0);
PathPlannerTrajectory ppRotate = PathPlanner.loadPath("Rotate", 1.0, 1.0);
PathPlannerTrajectory ppCurrentPath = ppfirstTestPath; // change this to change auto
PPSwerveControllerCommand ppSwerveControllerCommand = new PPSwerveControllerCommand(
ppCurrentPath,
m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics,
xController,
yController,
thetaController,
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive
);
// return new SequentialCommandGroup(
// new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(currentPath.getInitialPose())),
// swerveControllerCommand,
// new InstantCommand(() -> m_robotSwerveDrive.stopModules())
// );
return new SequentialCommandGroup(
new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()),
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(ppCurrentPath.getInitialPose())),
ppSwerveControllerCommand,
new InstantCommand(() -> m_robotSwerveDrive.stopModules())
);
}
/**
* Add your docs here.
*/
public IHandController getDriverController() {
public XboxController getDriverController() {
return m_driverXbox;
}
@@ -114,27 +200,13 @@ public class RobotContainer {
return m_robotSwerveDrive.getOdometry();
}
public void resetOdometry() {
m_robotSwerveDrive.resetOdometry();
public void zeroOdometry(Pose2d pose) {
m_robotSwerveDrive.resetOdometry(pose);
}
/**
* Add your docs here.
*/
public IHandController getOperatorController() {
public XboxController getOperatorController() {
return m_operatorXbox;
}
/**
* Add your docs here.
*/
public Joystick getOperatorJoystick() {
return m_operatorXbox.getJoyStick();
}
/**
* Add your docs here.
*/
public Joystick getDriverJoystick() {
return m_driverXbox.getJoyStick();
}
}
src/main/java/frc4388/robot/RobotMap.java
+28 -28
View File
@@ -64,41 +64,41 @@ public class RobotMap {
rightBackSteerMotor.configFactoryDefault();
rightBackWheelMotor.configFactoryDefault();
// leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
leftFrontWheelMotor.setNeutralMode(NeutralMode.Coast);
leftFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
rightFrontWheelMotor.setNeutralMode(NeutralMode.Coast);
rightFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
leftBackSteerMotor.setNeutralMode(NeutralMode.Brake);
leftBackWheelMotor.setNeutralMode(NeutralMode.Coast);
leftBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
rightBackSteerMotor.setNeutralMode(NeutralMode.Brake);
rightBackWheelMotor.setNeutralMode(NeutralMode.Coast);
rightBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
src/main/java/frc4388/robot/subsystems/LED.java
+4 -4
View File
@@ -4,10 +4,10 @@
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import java.util.logging.Logger;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.LEDConstants;
import frc4388.utility.LEDPatterns;
@@ -27,7 +27,7 @@ public class LED extends SubsystemBase {
m_LEDController = LEDController;
setPattern(LEDConstants.DEFAULT_PATTERN);
updateLED();
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
Logger.getLogger(LED.class.getName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
}
@Override
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+71 -41
View File
@@ -4,9 +4,8 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -17,25 +16,15 @@ 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.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.RobotMap;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
import frc4388.utility.RobotLogger;
public class SwerveDrive extends SubsystemBase {
// private WPI_TalonFX m_leftFrontSteerMotor;
// private WPI_TalonFX m_leftFrontWheelMotor;
// private WPI_TalonFX m_rightFrontSteerMotor;
// private WPI_TalonFX m_rightFrontWheelMotor;
// private WPI_TalonFX m_leftBackSteerMotor;
// private WPI_TalonFX m_leftBackWheelMotor;
// private WPI_TalonFX m_rightBackSteerMotor;
// private WPI_TalonFX m_rightBackWheelMotor;
// private CANCoder m_leftFrontEncoder;
// private CANCoder m_rightFrontEncoder;
// private CANCoder m_leftBackEncoder;
// private CANCoder m_rightBackEncoder;
private SwerveModule m_leftFront;
private SwerveModule m_leftBack;
@@ -47,27 +36,26 @@ public class SwerveDrive extends SubsystemBase {
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth));
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth));
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
// setSwerveGains();
private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
public SwerveModule[] modules;
public WPI_PigeonIMU m_gyro;
protected FusionStatus fstatus = new FusionStatus();
/* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
below are robot specific, and should be tuned. */
private SwerveDrivePoseEstimator m_poseEstimator;
public SwerveDrivePoseEstimator m_poseEstimator;
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
private final Field2d m_field = new Field2d();
public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_PigeonIMU gyro) {
// m_leftFrontSteerMotor = leftFrontSteerMotor;
// m_leftFrontWheelMotor = leftFrontWheelMotor;
@@ -104,7 +92,8 @@ public class SwerveDrive extends SubsystemBase {
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(Units.degreesToRadians(0.01)),
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
m_gyro.reset();
m_gyro.reset();
SmartDashboard.putData("Field", m_field);
}
//https://github.com/ZachOrr/MK3-Swerve-Example
/**
@@ -115,25 +104,48 @@ public class SwerveDrive extends SubsystemBase {
* @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
public void driveWithInput(double[] speeds, double rot, boolean fieldRelative)
public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative)
{
if (speeds[0] == 0 && speeds[1] == 0 && rot == 0) ignoreAngles = true;
else ignoreAngles = false;
speeds[0] *= speeds[0] * speeds[0];
speeds[1] *= speeds[1] * speeds[1];
if (speedX == 0 && speedY == 0 && rot == 0) ignoreAngles = true;
else ignoreAngles = false;
Translation2d speed = new Translation2d(speedX, speedY);
double mag = speed.getNorm();
speed = speed.times(mag * speedAdjust);
double xSpeedMetersPerSecond = -speeds[0] * speedAdjust;
double ySpeedMetersPerSecond = speeds[1] * speedAdjust;
SwerveModuleState[] states =
m_kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
for (int i = 0; i < states.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = states[i];
module.setDesiredState(state, ignoreAngles);
double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY();
SwerveModuleState[] states =
m_kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
}
private Rotation2d rotTarget = new Rotation2d();
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
{
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
Translation2d speed = new Translation2d(leftX, leftY);
speed = speed.times(speed.getNorm() * speedAdjust);
if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1));
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY();
SwerveModuleState[] states =
m_kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
}
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state, false);
}
}
@@ -144,6 +156,17 @@ public class SwerveDrive extends SubsystemBase {
// m_gyro.setFusedHeadingToCompass();
// m_gyro.setYawToCompass();
RobotLogger.getInstance().put("poseMeters", m_poseEstimator.getEstimatedPosition());
SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees());
SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading});
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
super.periodic();
}
@@ -184,8 +207,8 @@ public class SwerveDrive extends SubsystemBase {
/**
* Resets the odometry of the robot to (x=0, y=0, theta=0).
*/
public void resetOdometry() {
m_poseEstimator.resetPosition(new Pose2d(0, 0, new Rotation2d(0)), m_gyro.getRotation2d());
public void resetOdometry(Pose2d pose) {
m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
}
/** Updates the field relative position of the robot. */
@@ -203,6 +226,13 @@ public class SwerveDrive extends SubsystemBase {
// Timer.getFPGATimestamp() - 0.1);
}
public void stopModules() {
modules[0].stop();
modules[1].stop();
modules[2].stop();
modules[3].stop();
}
public void highSpeed(boolean shift){
if (shift){
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST;
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+8 -3
View File
@@ -15,7 +15,6 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration;
import edu.wpi.first.math.geometry.Rotation2d;
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;
@@ -28,13 +27,14 @@ public class SwerveModule extends SubsystemBase {
private static double kEncoderTicksPerRotation = 4096;
private SwerveModuleState state;
private double canCoderFeedbackCoefficient;
/** Creates a new SwerveModule. */
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
this.canCoder = canCoder;
canCoderFeedbackCoefficient = canCoder.configGetFeedbackCoefficient();
TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
@@ -83,7 +83,7 @@ public class SwerveModule extends SubsystemBase {
// Find the new absolute position of the module based on the difference in rotation
double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation;
// Convert the CANCoder from it's position reading back to ticks
double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
double desiredTicks = currentTicks + deltaTicks;
if (!ignoreAngle){
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
@@ -104,4 +104,9 @@ public class SwerveModule extends SubsystemBase {
return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
}
public void stop() {
driveMotor.set(0);
angleMotor.set(0);
}
}
src/main/java/frc4388/utility/AnsiLogging.java
+107
View File
@@ -0,0 +1,107 @@
package frc4388.utility;
import static org.fusesource.jansi.Ansi.ansi;
import java.io.IOException;
import java.io.OutputStream;
import java.io.PrintStream;
import java.io.PrintWriter;
import java.io.StringWriter;
import java.time.ZoneId;
import java.time.ZonedDateTime;
import java.util.logging.ConsoleHandler;
import java.util.logging.Formatter;
import java.util.logging.Level;
import java.util.logging.LogManager;
import java.util.logging.LogRecord;
import java.util.logging.Logger;
import org.fusesource.jansi.Ansi;
import org.fusesource.jansi.Ansi.Attribute;
import org.fusesource.jansi.Ansi.Color;
import org.fusesource.jansi.AnsiConsole;
public class AnsiLogging extends ConsoleHandler {
public static void systemInstall() {
try {
// Configures java.util.logging.Logger to output additional colored information.
LogManager.getLogManager().updateConfiguration(key -> (o, n) -> {
switch (key) {
case ".level": return Level.ALL.getName();
case "handlers": return AnsiColorConsoleHandler.class.getName();
default: return n;
}
});
// Replaces standard output streams with org.fusesource.jansi.AnsiPrintStreams.
AnsiConsole.systemInstall();
// Replaces standard output stream with java.util.logging.Logger.
System.setOut(printStreamLogger(Logger.getGlobal(), Level.INFO));
// Replaces standard error output stream with java.util.logging.Logger.
System.setErr(printStreamLogger(Logger.getGlobal(), Level.SEVERE));
} catch (IOException exception) {
exception.printStackTrace(AnsiConsole.sysErr());
}
}
public static class AnsiColorConsoleHandler extends ConsoleHandler {
@Override
public void publish(LogRecord logRecord) {
AnsiConsole.err().print(getFormatter().format(logRecord));
AnsiConsole.err().flush();
}
@Override
public Formatter getFormatter() {
return formatter;
}
private static final Formatter formatter = new Formatter() {
@Override
public String format(LogRecord logRecord) {
ZonedDateTime zdt = ZonedDateTime.ofInstant(logRecord.getInstant(), ZoneId.systemDefault());
String source;
if (logRecord.getSourceClassName() != null && !logRecord.getSourceClassName().startsWith(AnsiLogging.class.getName())) {
source = logRecord.getSourceClassName();
if (logRecord.getSourceMethodName() != null) {
source += " " + logRecord.getSourceMethodName();
}
} else
source = logRecord.getLoggerName();
String message = formatMessage(logRecord);
String throwable = "";
if (logRecord.getThrown() != null) {
StringWriter sw = new StringWriter();
try (PrintWriter pw = new PrintWriter(sw)) {
pw.println();
logRecord.getThrown().printStackTrace(pw);
}
throwable = sw.toString();
}
Ansi ansi;
if (logRecord.getLevel() == Level.SEVERE) ansi = ansi().fgBright(Color.RED);
else if (logRecord.getLevel() == Level.WARNING) ansi = ansi().fgBright(Color.YELLOW);
else if (logRecord.getLevel() == Level.INFO) ansi = ansi().fg(Color.GREEN);
else if (logRecord.getLevel() == Level.CONFIG) ansi = ansi().fgBright(Color.BLUE);
else if (logRecord.getLevel() == Level.FINE) ansi = ansi().fg(Color.CYAN);
else if (logRecord.getLevel() == Level.FINER) ansi = ansi().fg(Color.MAGENTA);
else if (logRecord.getLevel() == Level.FINEST) ansi = ansi().fgBright(Color.BLACK);
else ansi = ansi().fg(Color.DEFAULT);
String format = ansi.bold().a(Attribute.UNDERLINE).a("%1$tb %1$td, %1$tY %1$tl:%1$tM:%1$tS %1$Tp %2$s %4$s:").boldOff().a(Attribute.UNDERLINE_OFF).a("%n%5$s%6$s").reset().a("%n").toString();
return String.format(format, zdt, source, logRecord.getLoggerName(), logRecord.getLevel().getLocalizedName(), message, throwable);
}
};
}
private static PrintStream printStreamLogger(Logger logger, Level level) {
return new PrintStream(new OutputStream() {
private final StringBuilder stringBuilder = new StringBuilder();
@Override
public final void write(int i) throws IOException {
if (i == '\n') {
logger.log(level, stringBuilder::toString);
stringBuilder.setLength(0);
} else
stringBuilder.appendCodePoint(i);
}
});
}
}
src/main/java/frc4388/utility/DataLogging.java
+12
View File
@@ -0,0 +1,12 @@
// 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 DataLogging {
public DataLogging() {
}
}
src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java
+20
View File
@@ -0,0 +1,20 @@
package frc4388.utility.controller;
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
import edu.wpi.first.math.geometry.Translation2d;
public class DeadbandedRawXboxController extends RawXboxController {
public DeadbandedRawXboxController(int port) { super(port); }
@Override public double getLeftX() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); }
@Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); }
@Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); }
@Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); }
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();
if (magnitude >= 1) return translation2d.div(magnitude);
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
return translation2d;
}
}
src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+21
View File
@@ -0,0 +1,21 @@
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 skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); }
@Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); }
@Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); }
@Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); }
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();
if (magnitude >= 1) return translation2d.div(magnitude);
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
return translation2d;
}
}
src/main/java/frc4388/utility/controller/IHandController.java
-23
View File
@@ -1,23 +0,0 @@
package frc4388.utility.controller;
/**
* Add your docs here.
*/
public interface IHandController {
public double getLeftXAxis();
public double getLeftYAxis();
public double getRightXAxis();
public double getRightYAxis();
public double getLeftTriggerAxis();
public double getRightTriggerAxis();
public int getDpadAngle();
public void updateInput();
}
src/main/java/frc4388/utility/controller/RawXboxController.java
+241
View File
@@ -0,0 +1,241 @@
package frc4388.utility.controller;
import java.nio.ByteBuffer;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.XboxController;
public class RawXboxController extends XboxController {
private final int m_port;
public RawXboxController(int port) {
super(port);
if (port < 0 || port >= DriverStation.kJoystickPorts)
throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
m_port = port;
}
private static class HALJoystickButtons {
public int m_buttons;
public byte m_count;
}
private static class HALJoystickAxes {
public float[] m_axes;
public short m_count;
HALJoystickAxes(int count) {
m_axes = new float[count];
}
}
private static class HALJoystickPOVs {
public short[] m_povs;
public short m_count;
HALJoystickPOVs(int count) {
m_povs = new short[count];
for (int i = 0; i < count; i++) {
m_povs[i] = -1;
}
}
}
private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
private static double m_nextMessageTime;
private HALJoystickAxes m_joystickAxes = new HALJoystickAxes(HAL.kMaxJoystickAxes);
private HALJoystickPOVs m_joystickPOVs = new HALJoystickPOVs(HAL.kMaxJoystickAxes);
private HALJoystickButtons m_joystickButtons = new HALJoystickButtons();
// Joystick button rising/falling edge flags
private int m_joystickButtonsPressed = 0;
private int m_joystickButtonsReleased = 0;
private static final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
@Override
public double getRawAxis(int axis) {
return getStickAxis(axis);
}
@Override
public boolean getRawButton(int button) {
return getStickButton((byte) button);
}
@Override
public boolean getRawButtonPressed(int button) {
return getStickButtonPressed((byte) button);
}
@Override
public boolean getRawButtonReleased(int button) {
return getStickButtonReleased(button);
}
@Override
public int getPOV(int pov) {
return getStickPOV(pov);
}
/**
* The state of one joystick button. Button indexes begin at 1.
*
* @param button The button index, beginning at 1.
* @return The state of the joystick button.
*/
public boolean getStickButton(final int button) {
if (button <= 0) {
reportJoystickUnpluggedError();
return false;
}
if (button <= m_joystickButtons.m_count) {
return (m_joystickButtons.m_buttons & 1 << (button - 1)) != 0;
}
reportJoystickUnpluggedWarning(button, m_port);
return false;
}
/**
* Whether one joystick button was pressed since the last check. Button indexes begin at 1.
*
* @param button The button index, beginning at 1.
* @return Whether the joystick button was pressed since the last check.
*/
public boolean getStickButtonPressed(final int button) {
getData();
if (button <= 0) {
reportJoystickUnpluggedError();
return false;
}
if (button <= m_joystickButtons.m_count) {
// If button was pressed, clear flag and return true
if ((m_joystickButtonsPressed & 1 << (button - 1)) != 0) {
m_joystickButtonsPressed &= ~(1 << (button - 1));
return true;
} else {
return false;
}
}
reportJoystickUnpluggedWarning(button, m_port);
return false;
}
/**
* Whether one joystick button was released since the last check. Button indexes begin at 1.
*
* @param button The button index, beginning at 1.
* @return Whether the joystick button was released since the last check.
*/
public boolean getStickButtonReleased(final int button) {
getData();
if (button <= 0) {
reportJoystickUnpluggedError();
return false;
}
if (button <= m_joystickButtons.m_count) {
// If button was released, clear flag and return true
if ((m_joystickButtonsReleased & 1 << (button - 1)) != 0) {
m_joystickButtonsReleased &= ~(1 << (button - 1));
return true;
} else {
return false;
}
}
reportJoystickUnpluggedWarning(button, m_port);
return false;
}
/**
* Get the value of the axis on a joystick. This depends on the mapping of the joystick connected to
* the specified port.
*
* @param axis The analog axis value to read from the joystick.
* @return The value of the axis on the joystick.
*/
public double getStickAxis(int axis) {
getData();
if (axis < 0 || axis >= HAL.kMaxJoystickAxes) {
throw new IllegalArgumentException("Joystick axis is out of range");
}
if (axis < m_joystickAxes.m_count) {
return m_joystickAxes.m_axes[axis];
}
reportJoystickUnpluggedWarning(axis, m_port);
return 0.0;
}
/**
* Get the state of a POV on the joystick.
*
* @param pov The POV to read.
* @return the angle of the POV in degrees, or -1 if the POV is not pressed.
*/
public int getStickPOV(int pov) {
getData();
if (pov < 0 || pov >= HAL.kMaxJoystickPOVs) {
throw new IllegalArgumentException("Joystick POV is out of range");
}
if (pov < m_joystickPOVs.m_count) {
return m_joystickPOVs.m_povs[pov];
}
reportJoystickUnpluggedWarning(pov, m_port);
return -1;
}
/**
* The state of the buttons on the joystick.
*
* @return The state of the buttons on the joystick.
*/
public int getStickButtons() {
getData();
return m_joystickButtons.m_buttons;
}
protected void getData() {
// Get the status of all of the joysticks
byte stick = (byte) m_port;
m_joystickAxes.m_count = HAL.getJoystickAxes(stick, m_joystickAxes.m_axes);
m_joystickPOVs.m_count = HAL.getJoystickPOVs(stick, m_joystickPOVs.m_povs);
m_joystickButtons.m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer);
m_joystickButtons.m_count = m_buttonCountBuffer.get(0);
}
/**
* Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
* the DS.
*/
private static void reportJoystickUnpluggedError(String message) {
double currentTime = Timer.getFPGATimestamp();
if (currentTime > m_nextMessageTime) {
DriverStation.reportError(message, false);
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
}
}
/**
* Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
* the DS.
*/
private static void reportJoystickUnpluggedWarning(String message) {
if (DriverStation.isFMSAttached() || !DriverStation.isJoystickConnectionWarningSilenced()) {
double currentTime = Timer.getFPGATimestamp();
if (currentTime > m_nextMessageTime) {
DriverStation.reportWarning(message, false);
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
}
}
}
private static void reportJoystickUnpluggedWarning(final int pov, final int stick) {
reportJoystickUnpluggedWarning("Joystick POV " + pov + " on port " + stick + " not available, check if controller is plugged in");
}
private static void reportJoystickUnpluggedError() {
reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
}
}
src/main/java/frc4388/utility/controller/XboxController.java
-234
View File
@@ -1,234 +0,0 @@
package frc4388.utility.controller;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* This is a wrapper for the WPILib Joystick class that represents an XBox
* controller.
* @author frc1675
*/
public class XboxController implements IHandController
{
public static final int LEFT_X_AXIS = 0;
public static final int LEFT_Y_AXIS = 1;
public static final int LEFT_TRIGGER_AXIS = 2;
public static final int RIGHT_TRIGGER_AXIS = 3;
public static final int RIGHT_X_AXIS = 4;
public static final int RIGHT_Y_AXIS = 5;
public static final int LEFT_RIGHT_DPAD_AXIS = 6;
public static final int TOP_BOTTOM_DPAD_AXIS = 6;
public static final int A_BUTTON = 1;
public static final int B_BUTTON = 2;
public static final int X_BUTTON = 3;
public static final int Y_BUTTON = 4;
public static final int LEFT_BUMPER_BUTTON = 5;
public static final int RIGHT_BUMPER_BUTTON = 6;
public static final int BACK_BUTTON = 7;
public static final int START_BUTTON = 8;
public static final int LEFT_JOYSTICK_BUTTON = 9;
public static final int RIGHT_JOYSTICK_BUTTON = 10;
private static final double LEFT_DPAD_TOLERANCE = -0.9;
private static final double RIGHT_DPAD_TOLERANCE = 0.9;
private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
private static final double TOP_DPAD_TOLERANCE = 0.9;
private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
private static final double DEADZONE = 0.2;
private Joystick m_stick;
public void updateInput() {}
public static double[] ClampJoystickAxis(double x, double y) {
double square_mag = x * x + y * y;
double[] ret = {x,y};
if (square_mag > 1.d) {
double mag = Math.sqrt(square_mag);
ret[0] = x / mag;
ret[1] = y / mag;
}
double [] to_smart_dashboard = {square_mag, x, y, ret[0], ret[1]};
//SmartDashboard.putNumberArray("Input", to_smart_dashboard);
return ret;
}
/**
* Add your docs here.
*/
public XboxController(int portNumber){
m_stick = new Joystick(portNumber);
}
/**
* Add your docs here.
*/
public Joystick getJoyStick() {
return m_stick;
}
/**
* Checks if the input falls within the deadzone.
* @param input from an axis on the controller
* @return true if input falls in deadzone, false if input falls outside deadzone
*/
public static boolean inDeadZone(double input){
return (Math.abs(input) < DEADZONE);
}
/**
* Updates an input to have a deadzone around the 0 position
* @param input from an axis on the controller
* @return updated input
*/
private double getAxisWithDeadZoneCheck(double input){
if(inDeadZone(input)){
return 0.0;
} else {
return input;
}
}
public boolean getAButton(){
return m_stick.getRawButton(A_BUTTON);
}
public boolean getXButton(){
return m_stick.getRawButton(X_BUTTON);
}
public boolean getBButton(){
return m_stick.getRawButton(B_BUTTON);
}
public boolean getYButton(){
return m_stick.getRawButton(Y_BUTTON);
}
public boolean getBackButton(){
return m_stick.getRawButton(BACK_BUTTON);
}
public boolean getStartButton(){
return m_stick.getRawButton(START_BUTTON);
}
public boolean getLeftBumperButton(){
return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
}
public boolean getRightBumperButton(){
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
}
public boolean getLeftJoystickButton(){
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
}
public boolean getRightJoystickButton(){
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
}
public double getLeftXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
}
public double getLeftYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
}
public double getRightXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
}
public double getRightYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
}
public double getLeftTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
}
public double getRightTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
}
/**
* Get the angle input from the dpad.
* @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
*/
public int getDpadAngle() {
return m_stick.getPOV(0);
}
public boolean getDPadLeft(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
}
public boolean getDPadRight(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
}
public boolean getDPadTop(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
}
public boolean getDPadBottom(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
}
public boolean getLeftTrigger(){
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
}
public boolean getRightTrigger(){
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
}
public boolean getRightAxisUpTrigger(){
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
}
public boolean getRightAxisDownTrigger(){
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
}
public boolean getRightAxisLeftTrigger(){
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
}
public boolean getRightAxisRightTrigger(){
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
}
public boolean getLeftAxisUpTrigger(){
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
}
public boolean getLeftAxisDownTrigger(){
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
}
public boolean getLeftAxisLeftTrigger(){
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
}
public boolean getLeftAxisRightTrigger(){
return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
}
}
src/main/java/frc4388/utility/controller/XboxControllerRaw.java
-104
View File
@@ -1,104 +0,0 @@
package frc4388.utility.controller;
import java.nio.ByteBuffer;
import edu.wpi.first.hal.HAL;
public class XboxControllerRaw implements IHandController {
public static final int LEFT_X_AXIS = 0;
public static final int LEFT_Y_AXIS = 1;
public static final int LEFT_TRIGGER_AXIS = 2;
public static final int RIGHT_TRIGGER_AXIS = 3;
public static final int RIGHT_X_AXIS = 4;
public static final int RIGHT_Y_AXIS = 5;
public static final int LEFT_RIGHT_DPAD_AXIS = 6;
public static final int TOP_BOTTOM_DPAD_AXIS = 6;
public static final int A_BUTTON = 0;
public static final int B_BUTTON = 1;
public static final int X_BUTTON = 2;
public static final int Y_BUTTON = 3;
public static final int LEFT_BUMPER_BUTTON = 4;
public static final int RIGHT_BUMPER_BUTTON = 5;
public static final int BACK_BUTTON = 6;
public static final int START_BUTTON = 7;
public static final int LEFT_JOYSTICK_BUTTON = 8;
public static final int RIGHT_JOYSTICK_BUTTON = 9;
private static final double DEADZONE = 0.08;
private int m_ID;
private int m_buttons = 0;
private int m_numButtons;
private ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
float[] m_axes = new float[HAL.kMaxJoystickAxes];
public XboxControllerRaw(int id) {
m_ID = id;
}
public void updateInput() {
HAL.getJoystickAxes((byte) m_ID, m_axes);
m_buttons = HAL.getJoystickButtons((byte) m_ID, m_buttonCountBuffer);
m_numButtons = m_buttonCountBuffer.get(0);
}
public int getNumButtons() {
return m_numButtons;
}
public boolean getButton(int index) {
return (m_buttons & 1 << index) > 0;
}
/**
* Checks if the input falls within the deadzone.
* @param input from an axis on the controller
* @return true if input falls in deadzone, false if input falls outside deadzone
*/
public static boolean inDeadZone(double input){
return (Math.abs(input) < DEADZONE);
}
/**
* Updates an input to have a deadzone around the 0 position
* @param input from an axis on the controller
* @return updated input
*/
private double getAxisWithDeadZoneCheck(double input){
if(inDeadZone(input)){
return 0.0;
} else {
return input;
}
}
public double getLeftXAxis() {
return getAxisWithDeadZoneCheck(m_axes[LEFT_X_AXIS]);
}
public double getLeftYAxis() {
return getAxisWithDeadZoneCheck(m_axes[LEFT_Y_AXIS]);
}
public double getRightXAxis() {
return getAxisWithDeadZoneCheck(m_axes[RIGHT_X_AXIS]);
}
public double getRightYAxis() {
return getAxisWithDeadZoneCheck(m_axes[RIGHT_Y_AXIS]);
}
public double getLeftTriggerAxis() {
return getAxisWithDeadZoneCheck(m_axes[LEFT_TRIGGER_AXIS]);
}
public double getRightTriggerAxis() {
return getAxisWithDeadZoneCheck(m_axes[RIGHT_TRIGGER_AXIS]);
}
public int getDpadAngle() {
return -1;
}
}
src/main/java/frc4388/utility/controller/XboxControllerRawButton.java
-18
View File
@@ -1,18 +0,0 @@
package frc4388.utility.controller;
import edu.wpi.first.wpilibj2.command.button.Button;
public class XboxControllerRawButton extends Button {
private final XboxControllerRaw m_controller;
private final int m_buttonNumber;
public XboxControllerRawButton(XboxControllerRaw controller, int buttonNumber) {
m_controller = controller;
m_buttonNumber = buttonNumber;
}
@Override
public boolean get() {
return m_controller.getButton(m_buttonNumber);
}
}
src/main/java/frc4388/utility/controller/XboxTriggerButton.java
-50
View File
@@ -1,50 +0,0 @@
package frc4388.utility.controller;
import edu.wpi.first.wpilibj2.command.button.Button;
/**
* 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 static final int RIGHT_TRIGGER = 0;
public static final int LEFT_TRIGGER = 1;
public static final int RIGHT_AXIS_UP_TRIGGER = 2;
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
public static final int LEFT_AXIS_UP_TRIGGER = 6;
public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
private XboxController m_controller;
private int m_trigger;
/**
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger
*/
public XboxTriggerButton(XboxController controller, int trigger) {
m_controller = controller;
m_trigger = trigger;
}
/** {@inheritDoc} */
@Override
public boolean get() {
switch (m_trigger) {
case RIGHT_TRIGGER: return m_controller.getRightTrigger();
case LEFT_TRIGGER: return m_controller.getLeftTrigger();
case RIGHT_AXIS_UP_TRIGGER: return m_controller.getRightAxisUpTrigger();
case RIGHT_AXIS_DOWN_TRIGGER: return m_controller.getRightAxisDownTrigger();
case RIGHT_AXIS_RIGHT_TRIGGER: return m_controller.getRightAxisRightTrigger();
case RIGHT_AXIS_LEFT_TRIGGER: return m_controller.getRightAxisLeftTrigger();
case LEFT_AXIS_UP_TRIGGER: return m_controller.getLeftAxisUpTrigger();
case LEFT_AXIS_DOWN_TRIGGER: return m_controller.getLeftAxisDownTrigger();
case LEFT_AXIS_RIGHT_TRIGGER: return m_controller.getLeftAxisRightTrigger();
case LEFT_AXIS_LEFT_TRIGGER: return m_controller.getLeftAxisLeftTrigger();
default: return false;
}
}
}
src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java
+12 -12
View File
@@ -5,7 +5,7 @@
package frc4388.robot.subsystems;
import static org.junit.Assert.assertEquals;
import static org.mockito.Mockito.mock;
// import static org.mockito.Mockito.mock;
import org.junit.Test;
@@ -20,37 +20,37 @@ public class LEDSubsystemTest {
@Test
public void testConstructor() {
// Arrange
Spark ledController = mock(Spark.class);
// Spark ledController = mock(Spark.class);
// Act
LED led = new LED(ledController);
// LED led = new LED(ledController);
// Assert
assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
// assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
}
@Test
public void testPatterns() {
// Arrange
Spark ledController = mock(Spark.class);
LED led = new LED(ledController);
// Spark ledController = mock(Spark.class);
// LED led = new LED(ledController);
// Act
led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
// led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
// Assert
assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
// assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
// Act
led.setPattern(LEDPatterns.BLUE_BREATH);
// led.setPattern(LEDPatterns.BLUE_BREATH);
// Assert
assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
// assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
// Act
led.setPattern(LEDPatterns.SOLID_BLACK);
// led.setPattern(LEDPatterns.SOLID_BLACK);
// Assert
assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
// assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
}
}