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

Initial commit

Browse Source
This commit is contained in:
C4llSqin
2024-01-08 11:38:08 -07:00
committed by GitHub
commit d861e109fc
62 changed files with 4618 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/example.txt
+3
View File
@@ -0,0 +1,3 @@
Files placed in this directory will be deployed to the RoboRIO into the
'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function
to get a proper path relative to the deploy directory.
src/main/java/frc4388/robot/Constants.java
+152
View File
@@ -0,0 +1,152 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import frc4388.utility.LEDPatterns;
import frc4388.utility.Gains;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be
* declared globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
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 double ROTATION_SPEED = MAX_ROT_SPEED;
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
public static final double CORRECTION_MIN = 10;
public static final double CORRECTION_MAX = 50;
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 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 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 class AutoConstants {
public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
}
public static final class Conversions {
public static final 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 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 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
// dimensions
public static final double WIDTH = 18.5;
public static final double HEIGHT = 18.5;
public static final double HALF_WIDTH = WIDTH / 2.d;
public static final double HALF_HEIGHT = HEIGHT / 2.d;
// misc
public static final int TIMEOUT_MS = 30;
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class VisionConstants {
public static final 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 int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class LEDConstants {
public static final int LED_SPARK_ID = 0;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
}
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
}
}
src/main/java/frc4388/robot/Main.java
+29
View File
@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all.
* Unless you know what you are doing, do not modify this file except to
* change the parameter class to the startRobot call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}
src/main/java/frc4388/robot/Robot.java
+131
View File
@@ -0,0 +1,131 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc4388.utility.DeferredBlock;
import frc4388.utility.RobotTime;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the TimedRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
Command m_autonomousCommand;
private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer;
/**
* 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();
}
/**
* 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.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
m_robotTime.updateTimes();
// 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
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
* You can use it to reset any subsystem information you want to clear when
* the robot is disabled.
*/
@Override
public void disabledInit() {
m_robotTime.endMatchTime();
}
@Override
public void disabledPeriodic() {
}
@Override
public void disabledExit() {
DeferredBlock.execute();
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
m_robotTime.startMatchTime();
}
/**
* This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {
}
@Override
public void 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
// this line or comment it out.
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
m_robotTime.startMatchTime();
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
}
src/main/java/frc4388/robot/RobotContainer.java
+119
View File
@@ -0,0 +1,119 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot;
import edu.wpi.first.wpilibj.Joystick;
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;
/**
* This class is where the bulk of the robot should be declared. Since
* Command-based is a "declarative" paradigm, very little robot logic should
* actually be handled in the {@link Robot} periodic methods (other than the
* scheduler calls). Instead, the structure of the robot (including subsystems,
* commands, and button mappings) should be declared here.
*/
public class RobotContainer {
/* RobotMap */
private final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
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);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
configureButtonBindings();
/* Default Commands */
// drives the robot with a two-axis input from the driver controller
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
}
/**
* Use this method to define your button->command mappings. Buttons can be
* created by instantiating a {@link GenericHID} or one of its subclasses
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
/* Driver 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());
/* 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));
}
/**
* 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();
}
/**
* Add your docs here.
*/
public IHandController getDriverController() {
return m_driverXbox;
}
/**
* Add your docs here.
*/
public IHandController 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
+40
View File
@@ -0,0 +1,40 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot;
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 edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import frc4388.robot.Constants.LEDConstants;
import frc4388.utility.RobotGyro;
/**
* Defines and holds all I/O objects on the Roborio. This is useful for unit
* testing and modularization.
*/
public class RobotMap {
public RobotMap() {
configureLEDMotorControllers();
configureDriveMotorControllers();
}
/* LED Subsystem */
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
void configureLEDMotorControllers() {
}
void configureDriveMotorControllers() {
}
}
src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java
+91
View File
@@ -0,0 +1,91 @@
package frc4388.robot.commands.Autos;
import java.io.File;
import java.util.ArrayList;
import java.util.HashMap;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc4388.robot.commands.Swerve.JoystickPlayback;
import frc4388.robot.subsystems.SwerveDrive;
public class PlaybackChooser {
private final ArrayList<SendableChooser<Command>> m_choosers = new ArrayList<>();
private SendableChooser<Command> m_playback = null;
private final ArrayList<ComplexWidget> m_widgets = new ArrayList<>();
private final HashMap<String, Command> m_commandPool = new HashMap<>();
private final File m_dir = new File("/home/lvuser/autos/");
private int m_cmdNum = 0;
private final SwerveDrive m_swerve;
// commands
private Command m_noAuto = new InstantCommand();
public PlaybackChooser(SwerveDrive swerve, Object... pool) {
m_swerve = swerve;
}
public PlaybackChooser addOption(String name, Command option) {
m_commandPool.put(name, option);
return this;
}
public PlaybackChooser buildDisplay() {
for (int i = 0; i < 10; i++) {
appendCommand();
}
m_playback = m_choosers.get(0);
nextChooser();
Shuffleboard.getTab("Auto Chooser")
.add("Add Sequence", new InstantCommand(() -> nextChooser()))
.withPosition(4, 0);
return this;
}
// This will be bound to a button for the time being
public void appendCommand() {
var chooser = new SendableChooser<Command>();
chooser.setDefaultOption("No Auto", m_noAuto);
m_choosers.add(chooser);
ComplexWidget widget = Shuffleboard.getTab("Auto Chooser")
.add("Command: " + m_choosers.size(), chooser)
.withSize(4, 1)
.withPosition(0, m_choosers.size() - 1)
.withWidget(BuiltInWidgets.kSplitButtonChooser);
m_widgets.add(widget);
}
public void nextChooser() {
SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
for (String auto : m_dir.list()) {
chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
}
for (var cmd_name : m_commandPool.keySet()) {
chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
}
}
public Command getCommand() {
Command command = m_playback.getSelected();
command = command == null ? m_noAuto : command.asProxy();
Command[] commands = new Command[m_cmdNum - 1];
for (int i = 0; i < m_cmdNum - 1; i++) {
Command command2 = m_choosers.get(i + 1).getSelected();
command2 = command2 == null ? m_noAuto : command2.asProxy();
commands[i] = command2.asProxy();
}
return command.andThen(commands);
}
}
src/main/java/frc4388/robot/commands/Autos/Taxi.txt
+225
View File
@@ -0,0 +1,225 @@
0.0,0.0,0.0,0.0,0
0.0,0.0,0.0,0.0,0
0.0,0.0,0.0,0.0,12
0.0,0.0,0.0,0.0,26
0.0,0.0,0.0,0.0,37
0.0,0.0,0.0,0.0,50
0.0,0.0,0.0,0.0,62
0.0,0.0,0.0,0.0,73
0.0,0.0,0.0,0.0,88
0.0,0.0,0.0,0.0,103
0.0,0.0,0.0,0.0,116
0.0,0.0,0.0,0.0,160
0.0,0.0,0.0,0.0,173
0.0,0.0,0.0,0.0,185
0.0,0.0,0.0,0.0,200
0.0,0.0,0.0,0.0,211
0.0,0.0,0.0,0.0,223
0.0,0.0,0.0,0.0,235
0.0,0.0,0.0,0.0,247
0.0,0.0,0.0,0.0,263
0.0,0.0,0.0,0.0,283
0.0,0.0,0.0,0.0,303
0.0,-0.109375,0.0,0.0,323
0.0,-0.1484375,0.0,0.0,343
0.0,-0.2109375,0.0,0.0,363
0.0,-0.3671875,0.0,0.0,398
0.0,-0.4140625,0.0,0.0,411
0.0,-0.4765625,0.0,0.0,425
0.0,-0.5078125,0.0,0.0,443
0.0,-0.5078125,0.0,0.0,463
0.0,-0.53125,0.0,0.0,483
0.0,-0.5546875,0.0,0.0,504
0.0,-0.5625,0.0,0.0,523
0.0,-0.5625,0.0,0.0,544
0.0,-0.5703125,0.0,0.0,563
0.0,-0.5859375,0.0,0.0,584
0.0,-0.5859375,0.0,0.0,603
0.0,-0.5859375,0.0,0.0,640
0.0,-0.59375,0.0,0.0,657
0.0,-0.6015625,0.0,0.0,672
0.0,-0.6015625,0.0,0.0,685
0.0,-0.6015625,0.0,0.0,703
0.0,-0.6015625,0.0,0.0,723
0.0,-0.6015625,0.0,0.0,743
0.0,-0.6015625,0.0,0.0,763
0.0,-0.6015625,0.0,0.0,783
0.0,-0.6015625,0.0,0.0,803
0.0,-0.6015625,0.0,0.0,823
0.0,-0.6015625,0.0,0.0,844
0.0,-0.6015625,0.0,0.0,878
0.0,-0.6015625,0.0,0.0,893
0.0,-0.6015625,0.0,0.0,907
0.0,-0.6015625,0.0,0.0,924
0.0,-0.609375,0.0,0.0,943
0.0,-0.609375,0.0,0.0,963
0.0,-0.609375,0.0,0.0,983
0.0,-0.609375,0.0,0.0,1004
0.0,-0.609375,0.0,0.0,1023
0.0,-0.609375,0.0,0.0,1043
0.0,-0.609375,0.0,0.0,1064
0.0,-0.609375,0.0,0.0,1083
0.0,-0.609375,0.0,0.0,1156
0.0,-0.609375,0.0,0.0,1172
0.0,-0.609375,0.0,0.0,1185
0.0,-0.609375,0.0,0.0,1200
0.0,-0.609375,0.0,0.0,1215
0.0,-0.609375,0.0,0.0,1225
0.0,-0.609375,0.0,0.0,1236
0.0,-0.609375,0.0,0.0,1249
0.0,-0.609375,0.0,0.0,1263
0.0,-0.609375,0.0,0.0,1283
0.0,-0.609375,0.0,0.0,1303
0.0,-0.609375,0.0,0.0,1323
0.0,-0.609375,0.0,0.0,1363
0.0,-0.6015625,0.0,0.0,1376
0.0,-0.6015625,0.0,0.0,1394
0.0,-0.6015625,0.0,0.0,1405
0.0,-0.6015625,0.0,0.0,1423
0.0,-0.6015625,0.0,0.0,1443
0.0,-0.6015625,0.0,0.0,1463
0.0,-0.6015625,0.0,0.0,1483
0.0,-0.6015625,0.0,0.0,1503
0.0,-0.6015625,0.0,0.0,1523
0.0,-0.6015625,0.0,0.0,1543
0.0,-0.6015625,0.0,0.0,1563
0.0,-0.6015625,0.0,0.0,1597
0.0,-0.6015625,0.0,0.0,1608
0.0,-0.6015625,0.0,0.0,1624
0.0,-0.6015625,0.0,0.0,1643
0.0,-0.6015625,0.0,0.0,1664
0.0,-0.5859375,0.0,0.0,1683
0.0,-0.5859375,0.0,0.0,1703
0.0,-0.5625,0.0,0.0,1723
0.0,-0.5625,0.0,0.0,1743
0.0,-0.5625,0.0,0.0,1763
0.0,-0.5625,0.0,0.0,1783
0.0,-0.5625,0.0,0.0,1803
0.0,-0.5625,0.0,0.0,1843
0.0,-0.5625,0.0,0.0,1855
0.0,-0.5625,0.0,0.0,1868
0.0,-0.5625,0.0,0.0,1883
0.0,-0.5625,0.0,0.0,1903
0.0,-0.5625,0.0,0.0,1923
0.0,-0.5625,0.0,0.0,1943
0.0,-0.5625,0.0,0.0,1963
0.0,-0.5625,0.0,0.0,1983
0.0,-0.5625,0.0,0.0,2003
0.0,-0.5625,0.0,0.0,2024
0.0,-0.5625,0.0,0.0,2043
0.0,-0.5625,0.0,0.0,2081
0.0,-0.5625,0.0,0.0,2093
0.0,-0.5625,0.0,0.0,2105
0.0,-0.5625,0.0,0.0,2123
0.0,-0.5625,0.0,0.0,2143
0.0,-0.5625,0.0,0.0,2163
0.0,-0.5625,0.0,0.0,2183
0.0,-0.5625,0.0,0.0,2203
0.0,-0.5625,0.0,0.0,2223
0.0,-0.5625,0.0,0.0,2243
0.0,-0.5625,0.0,0.0,2263
0.0,-0.5625,0.0,0.0,2283
0.0,-0.5625,0.0,0.0,2366
0.0,-0.5625,0.0,0.0,2377
0.0,-0.5625,0.0,0.0,2394
0.0,-0.5703125,0.0,0.0,2405
0.0,-0.5703125,0.0,0.0,2418
0.0,-0.5703125,0.0,0.0,2431
0.0,-0.5703125,0.0,0.0,2444
0.0,-0.5703125,0.0,0.0,2458
0.0,-0.5703125,0.0,0.0,2470
0.0,-0.5703125,0.0,0.0,2485
0.0,-0.5703125,0.0,0.0,2503
0.0,-0.5703125,0.0,0.0,2523
0.0,-0.5703125,0.0,0.0,2563
0.0,-0.5703125,0.0,0.0,2577
0.0,-0.5703125,0.0,0.0,2591
0.0,-0.5703125,0.0,0.0,2608
0.0,-0.5703125,0.0,0.0,2624
0.0,-0.5703125,0.0,0.0,2643
0.0,-0.5703125,0.0,0.0,2677
0.0,-0.5703125,0.0,0.0,2698
0.0,-0.5703125,0.0,0.0,2711
0.0,-0.5703125,0.0,0.0,2725
0.0,-0.5703125,0.0,0.0,2743
0.0,-0.5703125,0.0,0.0,2764
0.0,-0.5703125,0.0,0.0,2810
0.0,-0.5703125,0.0,0.0,2820
0.0,-0.5703125,0.0,0.0,2833
0.0,-0.5703125,0.0,0.0,2845
0.0,-0.5703125,0.0,0.0,2863
0.0,-0.5703125,0.0,0.0,2883
0.0,-0.5703125,0.0,0.0,2904
0.0,-0.5703125,0.0,0.0,2924
0.0,-0.5703125,0.0,0.0,2943
0.0,-0.5703125,0.0,0.0,2963
0.0,-0.5703125,0.0,0.0,2983
0.0,-0.5703125,0.0,0.0,3003
0.0,-0.5703125,0.0,0.0,3033
0.0,-0.5703125,0.0,0.0,3050
0.0,-0.5703125,0.0,0.0,3065
0.0,-0.5703125,0.0,0.0,3083
0.0,-0.5703125,0.0,0.0,3103
0.0,-0.5703125,0.0,0.0,3123
0.0,-0.5703125,0.0,0.0,3144
0.0,-0.5703125,0.0,0.0,3164
0.0,-0.5703125,0.0,0.0,3184
0.0,-0.5703125,0.0,0.0,3203
0.0,-0.5703125,0.0,0.0,3223
0.0,-0.5703125,0.0,0.0,3243
0.0,-0.5703125,0.0,0.0,3272
0.0,-0.5703125,0.0,0.0,3289
0.0,-0.5703125,0.0,0.0,3303
0.0,-0.5703125,0.0,0.0,3323
0.0,-0.5703125,0.0,0.0,3343
0.0,-0.5703125,0.0,0.0,3363
0.0,-0.5703125,0.0,0.0,3383
0.0,-0.5703125,0.0,0.0,3403
0.0,-0.5703125,0.0,0.0,3423
0.0,-0.5703125,0.0,0.0,3443
0.0,-0.5703125,0.0,0.0,3463
0.0,-0.5703125,0.0,0.0,3483
0.0,-0.5703125,0.0,0.0,3566
0.0,-0.5703125,0.0,0.0,3578
0.0,-0.5703125,0.0,0.0,3596
0.0,-0.5703125,0.0,0.0,3610
0.0,-0.5703125,0.0,0.0,3623
0.0,-0.5703125,0.0,0.0,3640
0.0,-0.5703125,0.0,0.0,3651
0.0,-0.5703125,0.0,0.0,3663
0.0,-0.5703125,0.0,0.0,3678
0.0,-0.5703125,0.0,0.0,3691
0.0,-0.5703125,0.0,0.0,3706
0.0,-0.5703125,0.0,0.0,3723
0.0,-0.5703125,0.0,0.0,3766
0.0,-0.5703125,0.0,0.0,3778
0.0,-0.5703125,0.0,0.0,3792
0.0,-0.5703125,0.0,0.0,3807
0.0,-0.5703125,0.0,0.0,3823
0.0,-0.5703125,0.0,0.0,3843
0.0,-0.53125,0.0,0.0,3863
0.0,-0.53125,0.0,0.0,3884
0.0,-0.421875,0.0,0.0,3904
0.0,0.0,0.0,0.0,3924
0.0,0.0,0.0,0.0,3944
0.0,0.0,0.0,0.0,3963
0.0,0.0,0.0,0.0,3999
0.0,0.0,0.0,0.0,4010
0.0,0.0,0.0,0.0,4025
0.0,0.0,0.0,0.0,4043
0.0,0.0,0.0,0.0,4063
0.0,0.0,0.0,0.0,4083
0.0,0.0,0.0,0.0,4103
0.0,0.0,0.0,0.0,4123
0.0,0.0,0.0,0.0,4143
0.0,0.0,0.0,0.0,4163
0.0,0.0,0.0,0.0,4183
0.0,0.0,0.0,0.0,4203
0.0,0.0,0.0,0.0,4236
0.0,0.0,0.0,0.0,4247
0.0,0.0,0.0,0.0,4264
0.0,0.0,0.0,0.0,4284
0.0,0.0,0.0,0.0,4304
0.0,0.0,0.0,0.0,4324
0.0,0.0,0.0,0.0,4343
0.0,0.0,0.0,0.0,4363
src/main/java/frc4388/robot/commands/PID.java
+60
View File
@@ -0,0 +1,60 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.utility.Gains;
public abstract class PID extends CommandBase {
protected Gains gains;
private double output = 0;
private double tolerance = 0;
/** Creates a new PelvicInflammatoryDisease. */
public PID(double kp, double ki, double kd, double kf, double tolerance) {
gains = new Gains(kp, ki, kd, kf, 0);
this.tolerance = tolerance;
}
public PID(Gains gains, double tolerance) {
this.gains = gains;
this.tolerance = tolerance;
}
/** produces the error from the setpoint */
public abstract double getError();
/** figure it out bitch */
public abstract void runWithOutput(double output);
// Called when the command is initially scheduled.
@Override
public final void initialize() {
output = 0;
}
private double prevError, cumError = 0;
// Called every time the scheduler runs while the command is scheduled.
@Override
public final void execute() {
double error = getError();
cumError += error * .02; // 20 ms
double delta = error - prevError;
output = error * gains.kP;
output += cumError * gains.kI;
output += delta * gains.kD;
output += gains.kF;
runWithOutput(output);
}
// Returns true when the command should end.
@Override
public final boolean isFinished() {
return Math.abs(getError()) < tolerance;
}
}
src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java
+141
View File
@@ -0,0 +1,141 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.Swerve;
import java.io.File;
import java.io.FileNotFoundException;
import java.util.ArrayList;
import java.util.Scanner;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickPlayback extends CommandBase {
private final SwerveDrive swerve;
private String filename;
private int mult = 1;
private Scanner input;
private final ArrayList<TimedOutput> outputs = new ArrayList<>();
private int counter = 0;
private long startTime = 0;
private long playbackTime = 0;
private int lastIndex;
private boolean m_finished = false; // ! find a better way
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
this.swerve = swerve;
this.filename = filename;
this.mult = mult;
addRequirements(this.swerve);
}
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename) {
this(swerve, filename, 1);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
outputs.clear();
m_finished = false;
startTime = System.currentTimeMillis();
playbackTime = 0;
lastIndex = 0;
try {
input = new Scanner(new File("/home/lvuser/autos/" + filename));
String line = "";
while (input.hasNextLine()) {
line = input.nextLine();
if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
continue;
}
String[] values = line.split(",");
System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
var out = new TimedOutput();
out.leftX = Double.parseDouble(values[0]) * mult;
out.leftY = Double.parseDouble(values[1]);
out.rightX = Double.parseDouble(values[2]);
out.rightY = Double.parseDouble(values[3]);
out.timedOffset = Long.parseLong(values[4]);
outputs.add(out);
}
input.close();
} catch (FileNotFoundException e) {
e.printStackTrace();
}
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (counter == 0) {
startTime = System.currentTimeMillis();
playbackTime = 0;
} else {
playbackTime = System.currentTimeMillis() - startTime;
}
// skip to reasonable time frame
// too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
{
int i = lastIndex == 0 ? 1 : lastIndex;
while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
i++;
}
if (i >= outputs.size()) {
m_finished = true; // ! kind of a hack
return;
}
lastIndex = i;
}
TimedOutput lastOut = outputs.get(lastIndex - 1);
TimedOutput out = outputs.get(lastIndex);
double deltaTime = out.timedOffset - lastOut.timedOffset;
double playbackDelta = playbackTime - lastOut.timedOffset;
double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime);
double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime);
double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime);
double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
// this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY),
// new Translation2d(out.rightX, out.rightY),
// true);
this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
new Translation2d(lerpRX, lerpRY),
true);
counter++;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
input.close();
swerve.stopModules();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return m_finished;
}
}
src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
+97
View File
@@ -0,0 +1,97 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.Swerve;
import java.io.File;
import java.io.IOException;
import java.io.PrintWriter;
import java.util.ArrayList;
import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickRecorder extends CommandBase {
public final SwerveDrive swerve;
public final Supplier<Double> leftX;
public final Supplier<Double> leftY;
public final Supplier<Double> rightX;
public final Supplier<Double> rightY;
private String filename;
public final ArrayList<TimedOutput> outputs = new ArrayList<>();
private long startTime = -1;
/** Creates a new JoystickRecorder. */
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY,
Supplier<Double> rightX, Supplier<Double> rightY,
String filename)
{
this.swerve = swerve;
this.leftX = leftX;
this.leftY = leftY;
this.rightX = rightX;
this.rightY = rightY;
this.filename = filename;
addRequirements(this.swerve);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
outputs.clear();
this.startTime = System.currentTimeMillis();
outputs.add(new TimedOutput());
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
var inputs = new TimedOutput();
inputs.leftX = leftX.get();
inputs.leftY = leftY.get();
inputs.rightX = rightX.get();
inputs.rightY = rightY.get();
inputs.timedOffset = System.currentTimeMillis() - startTime;
outputs.add(inputs);
swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
new Translation2d(inputs.rightX, inputs.rightY),
true);
System.out.println("RECORDING");
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
File output = new File("/home/lvuser/autos/" + filename);
try (PrintWriter writer = new PrintWriter(output)) {
for (var input : outputs) {
writer.println( input.leftX + "," + input.leftY + "," +
input.rightX + "," + input.rightY + "," +
input.timedOffset);
}
writer.close();
} catch (IOException e) {
e.printStackTrace();
}
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java
+35
View File
@@ -0,0 +1,35 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.Swerve;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.commands.PID;
import frc4388.robot.subsystems.SwerveDrive;
public class RotateToAngle extends PID {
SwerveDrive drive;
double targetAngle;
/** Creates a new RotateToAngle. */
public RotateToAngle(SwerveDrive drive, double targetAngle) {
super(0.3, 0.0, 0.0, 0.0, 1);
this.drive = drive;
this.targetAngle = targetAngle;
addRequirements(drive);
}
@Override
public double getError() {
return targetAngle - drive.getGyroAngle();
}
@Override
public void runWithOutput(double output) {
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);
}
}
src/main/java/frc4388/robot/subsystems/Apriltags.java
+36
View File
@@ -0,0 +1,36 @@
package frc4388.robot.subsystems;
//import edu.wpi.first.apriltag.AprilTag;
//import edu.wpi.first.math.geometry.Pose3d;
//import edu.wpi.first.math.geometry.Rotation3d;
//import edu.wpi.first.networktables.NetworkTable;
//import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
public class Apriltags {
public static class Tag {
public boolean visible = true;
public double x, y, z = 0;
public double ry, rp, rr = 0;
}
public Tag getTagPosRot() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
final Tag tag = new Tag();
tag.visible = isAprilTag();
tag.x = tagTable.getEntry("TagPosX").getDouble(0);
tag.y = tagTable.getEntry("TagPosY").getDouble(0);
tag.z = tagTable.getEntry("TagPosZ").getDouble(0);
tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
return tag;
}
public boolean isAprilTag() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
return tagTable.getEntry("IsTag").getBoolean(false);
}
}
src/main/java/frc4388/robot/subsystems/DiffDrive.java
+85
View File
@@ -0,0 +1,85 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.utility.RobotGyro;
import frc4388.utility.RobotTime;
/**
* Add your docs here.
*/
public class DiffDrive extends SubsystemBase {
// Put methods for controlling this subsystem
// here. Call these from Commands.
private RobotTime m_robotTime = RobotTime.getInstance();
private WPI_TalonFX m_leftFrontMotor;
private WPI_TalonFX m_rightFrontMotor;
private WPI_TalonFX m_leftBackMotor;
private WPI_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) {
m_leftFrontMotor = leftFrontMotor;
m_rightFrontMotor = rightFrontMotor;
m_leftBackMotor = leftBackMotor;
m_rightBackMotor = rightBackMotor;
m_driveTrain = driveTrain;
m_gyro = gyro;
}
@Override
public void periodic() {
m_gyro.updatePigeonDeltas();
if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
updateSmartDashboard();
}
}
/**
* Add your docs here.
*/
public void driveWithInput(double move, double steer) {
m_driveTrain.arcadeDrive(move, steer);
}
/**
* Add your docs here.
*/
public void tankDriveWithInput(double leftMove, double rightMove) {
m_leftFrontMotor.set(leftMove);
m_rightFrontMotor.set(rightMove);
}
/**
* Add your docs here.
*/
private void updateSmartDashboard() {
// Examples of the functionality of RobotGyro
SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
//SmartDashboard.putData(m_gyro);
}
}
src/main/java/frc4388/robot/subsystems/LED.java
+63
View File
@@ -0,0 +1,63 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.LEDConstants;
import frc4388.utility.LEDPatterns;
/**
* Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
* Driver
*/
public class LED extends SubsystemBase {
private LEDPatterns m_currentPattern;
private Spark m_LEDController;
/**
* Add your docs here.
*/
public LED(Spark LEDController){
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.");
}
@Override
public void periodic(){
SmartDashboard.putNumber("LED", m_currentPattern.getValue());
}
/**
* Add your docs here.
*/
public void updateLED(){
m_LEDController.set(m_currentPattern.getValue());
}
/**
* Add your docs here.
*/
public void setPattern(LEDPatterns pattern){
m_currentPattern = pattern;
m_LEDController.set(m_currentPattern.getValue());
}
/**
* Add your docs here.
* @return
*/
public LEDPatterns getPattern() {
return m_currentPattern;
}
}
src/main/java/frc4388/robot/subsystems/Limelight.java
+165
View File
@@ -0,0 +1,165 @@
// 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
+195
View File
@@ -0,0 +1,195 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.subsystems;
import edu.wpi.first.math.geometry.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.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.RobotGyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class SwerveDrive extends SubsystemBase {
private SwerveModule leftFront;
private SwerveModule rightFront;
private SwerveModule leftBack;
private SwerveModule rightBack;
private SwerveModule[] modules;
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
private RobotGyro gyro;
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public double rotTarget = 0.0;
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
this.leftFront = leftFront;
this.rightFront = rightFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
this.gyro = gyro;
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
boolean stopped = false;
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
double rot = 0;
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);
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
}
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.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);
}
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
/**
* Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state);
}
}
public boolean rotateToTarget(double angle) {
double currentAngle = getGyroAngle();
double error = angle - currentAngle;
driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
if (Math.abs(angle - getGyroAngle()) < 5.0) {
return true;
}
return false;
}
public double getGyroAngle() {
return gyro.getAngle();
}
public void resetGyro() {
gyro.reset();
rotTarget = 0.0;
}
public void stopModules() {
for (SwerveModule module : this.modules) {
module.stop();
}
}
public SwerveDriveKinematics getKinematics() {
return this.kinematics;
}
@Override
public void periodic() {
// This method will be called once per scheduler run\
SmartDashboard.putNumber("Gyro", getGyroAngle());
}
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;
}
}
public void setToSlow() {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
}
public void setToFast() {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
}
public void setToTurbo() {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
}
public void 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;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
} else {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
}
}
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+161
View File
@@ -0,0 +1,161 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.subsystems;
import com.ctre.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 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.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;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
/** Creates a new SwerveModule. */
public SwerveModule(WPI_TalonFX driveMotor, WPI_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;
// 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.setSelectedSensorPosition(0);
driveMotor.config_kP(0, 0.2);
}
/**
* Get the drive motor of the SwerveModule
* @return the drive motor of the SwerveModule
*/
public WPI_TalonFX getDriveMotor() {
return this.driveMotor;
}
/**
* Get the angle motor of the SwerveModule
* @return the angle motor of the SwerveModule
*/
public WPI_TalonFX getAngleMotor() {
return this.angleMotor;
}
/**
* Get the CANcoder of the SwerveModule
* @return the CANcoder of the SwerveModule
*/
public CANCoder getEncoder() {
return this.encoder;
}
/**
* Get the angle of a SwerveModule as a Rotation2d
* @return the angle of a SwerveModule as a Rotation2d
*/
public Rotation2d getAngle() {
// * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
}
public double getAngularVel() {
return this.angleMotor.getSelectedSensorVelocity();
}
public double getDrivePos() {
return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
}
public double getDriveVel() {
return this.driveMotor.getSelectedSensorVelocity(0);
}
public void stop() {
driveMotor.set(0);
angleMotor.set(0);
}
public void rotateToAngle(double angle) {
angleMotor.set(TalonFXControlMode.Position, angle);
}
/**
* Get state of swerve module
* @return speed in m/s and angle in degrees
*/
public SwerveModuleState getState() {
return new SwerveModuleState(
Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
getAngle()
);
}
/**
* Returns the current position of the SwerveModule
* @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
*/
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
}
/**
* Set the speed and rotation of the SwerveModule from a SwerveModuleState object
* @param desiredState a SwerveModuleState representing the desired new state of the module
*/
public void setDesiredState(SwerveModuleState desiredState) {
Rotation2d currentRotation = this.getAngle();
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
// calculate the difference between our current rotational position and our new rotational position
Rotation2d rotationDelta = state.angle.minus(currentRotation);
// 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;
// convert the CANCoder from its position reading to ticks
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
}
public void reset(double position) {
encoder.setPositionToAbsolute();
}
public double getCurrent() {
return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
}
public double getVoltage() {
return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
}
}
src/main/java/frc4388/robot/subsystems/Vision.java
+38
View File
@@ -0,0 +1,38 @@
package frc4388.robot.subsystems;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
public class Vision {
private final NetworkTableEntry m_isTags;
private final NetworkTableEntry m_xPoses;
private final NetworkTableEntry m_yPoses;
private final NetworkTableEntry m_zPoses;
public Vision() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
m_isTags = tagTable.getEntry("IsTag");
m_xPoses = tagTable.getEntry("TagPosX");
m_yPoses = tagTable.getEntry("TagPosY");
m_zPoses = tagTable.getEntry("TagPosZ");
}
public AprilTag[] getAprilTags() {
if (!m_isTags.getBoolean(false)) return new AprilTag[0];
double xarr[] = m_xPoses.getDoubleArray(new double[] {});
double yarr[] = m_yPoses.getDoubleArray(new double[] {});
double zarr[] = m_zPoses.getDoubleArray(new double[] {});
AprilTag tags[] = new AprilTag[xarr.length];
for (int i = 0; i < tags.length; i++) {
tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
}
return tags;
}
}
src/main/java/frc4388/utility/AprilTag.java
+13
View File
@@ -0,0 +1,13 @@
package frc4388.utility;
// This is a seperate class in case I want to encode rotation or other
// information about the tag
public class AprilTag {
public final double x, y, z;
public AprilTag(double _x, double _y, double _z) {
x = _x;
y = _y;
z = _z;
}
}
src/main/java/frc4388/utility/DeferredBlock.java
+23
View File
@@ -0,0 +1,23 @@
package frc4388.utility;
import java.util.ArrayList;
public class DeferredBlock {
private static ArrayList<Runnable> m_blocks = new ArrayList<>();
private static boolean m_hasRun = false;
public DeferredBlock(Runnable block) {
m_blocks.add(block);
}
public static void execute() {
if (m_hasRun) return;
for (Runnable block : m_blocks) {
block.run();
}
m_blocks.clear(); // for garbage collection
m_hasRun = true;
}
}
src/main/java/frc4388/utility/Gains.java
+83
View File
@@ -0,0 +1,83 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.utility;
/** Add your docs here. */
public class Gains {
public double kP;
public double kI;
public double kD;
public double kF;
public int kIZone;
public double kPeakOutput;
public double kMaxOutput;
public double kMinOutput;
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIZone The zone of the I value.
* @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
*/
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
this.kF = kF;
this.kIZone = kIZone;
this.kPeakOutput = kPeakOutput;
this.kMaxOutput = kPeakOutput;
this.kMinOutput = -kPeakOutput;
}
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIZone The zone of the I value.
*/
public Gains(double kP, double kI, double kD, double kF, int kIZone) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
this.kF = kF;
this.kIZone = kIZone;
this.kPeakOutput = 1.0;
this.kMaxOutput = 1.0;
this.kMinOutput = -1.0;
}
public Gains(double kP, double kI, double kD) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
}
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIZone The zone of the I value.
* @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
* @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
*/
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
this.kF = kF;
this.kIZone = kIZone;
this.kMaxOutput = kMaxOutput;
this.kMinOutput = kMinOutput;
this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
}
}
src/main/java/frc4388/utility/LEDPatterns.java
+45
View File
@@ -0,0 +1,45 @@
package frc4388.utility;
/**
* Add your docs here.
*/
public enum LEDPatterns {
/* PALLETTE PATTERNS */
RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f),
PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f),
PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f),
RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f),
RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f),
RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f),
BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f),
GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
/* COLOR 1 PATTERNS */
C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f),
C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f),
/* COLOR 2 PATTERNS */
C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f),
C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f),
/* COLOR 1 AND 2 PATTERNS */
C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
C1C2_WAVES(0.53f), C1C2_SINELON(0.55f),
/* SOLID COLORS */
SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f),
SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f),
SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f),
SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f),
SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f);
/* GETTERS/SETTERS */
private final float id;
LEDPatterns(float id) {
this.id = id;
}
public float getValue() {
return id;
}
}
src/main/java/frc4388/utility/RobotGyro.java
+200
View File
@@ -0,0 +1,200 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.utility;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.kauailabs.navx.frc.AHRS;
// import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.math.MathUtil;
/**
* Gyro class that allows for interchangeable use between a pigeon and a navX
*/
public class RobotGyro implements Gyro {
private RobotTime m_robotTime = RobotTime.getInstance();
private WPI_Pigeon2 m_pigeon = null;
private AHRS m_navX = null;
public boolean m_isGyroAPigeon; //true if pigeon, false if navX
private double m_lastPigeonAngle;
private double m_deltaPigeonAngle;
private double pitchZero = 0;
private double rollZero = 0;
/**
* Creates a Gyro based on a pigeon
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(WPI_Pigeon2 gyro) {
m_pigeon = gyro;
m_isGyroAPigeon = true;
}
/**
* Creates a Gyro based on a navX
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(AHRS gyro){
m_navX = gyro;
m_isGyroAPigeon = false;
}
/**
* Resets yaw, pitch, and roll.
*/
public void resetZeroValues() {
if (!m_isGyroAPigeon) return;
pitchZero = m_pigeon.getPitch();
rollZero = m_pigeon.getRoll();
}
/**
* Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
* that the getRate() method for a navX will likely be much more accurate than for a pigeon.
*/
public void updatePigeonDeltas() {
double currentPigeonAngle = getAngle();
m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
m_lastPigeonAngle = currentPigeonAngle;
}
/**
* <p>NavX:
* <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
* the center value as the Accumulator center value for subsequent measurements. It's important to
* make sure that the robot is not moving while the centering calculations are in progress, this
* is typically done when the robot is first turned on while it's sitting at rest before the
* competition starts.
*
* <p>Pigeon:
* <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
* into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
* has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
* make sure that the robot is not moving while the tempurature calculations are in progress, this
* 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();
}
}
@Override
public void reset() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(0);
} else {
m_navX.reset();
}
}
/**
* Get Yaw, Pitch, and Roll data.
*
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
* Yaw is within [-368,640, +368,640] degrees.
* Pitch is within [-90,+90] degrees.
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
}
@Override
public double getAngle() {
if (m_isGyroAPigeon) {
return getPigeonAngles()[0];
} else {
return m_navX.getAngle();
}
}
public double getYaw() {
return this.getAngle();
}
/**
* Gets an absolute heading of the robot
* @return heading from -180 to 180 degrees
*/
public double getHeading() {
return getHeading(getAngle());
}
/**
* Gets an absolute heading of the robot
* @return heading from -180 to 180 degrees
*/
public double getHeading(double angle) {
return Math.IEEEremainder(angle, 360);
}
/**
* Returns the current pitch value (in degrees, from -90 to 90)
* reported by the sensor. Pitch is a measure of rotation around
* the Y Axis.
* @return The current pitch value in degrees (-90 to 90).
*/
public double getPitch() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
} else {
return MathUtil.clamp(m_navX.getPitch(), -90, 90);
}
}
/**
* Returns the current roll value (in degrees, from -90 to 90)
* reported by the sensor. Roll is a measure of rotation around
* the X Axis.
* @return The current roll value in degrees (-90 to 90).
*/
public double getRoll() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
} else {
return MathUtil.clamp(m_navX.getRoll(), -90, 90);
}
}
@Override
public double getRate() {
if (m_isGyroAPigeon) {
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
} else {
return m_navX.getRate();
}
}
public WPI_Pigeon2 getPigeon(){
return m_pigeon;
}
public AHRS getNavX(){
return m_navX;
}
@Override
public void close() throws Exception {
}
}
src/main/java/frc4388/utility/RobotTime.java
+79
View File
@@ -0,0 +1,79 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.utility;
/**
* <p>Keeps track of Robot times like time passed, delta time, etc
* <p>All times are in milliseconds
*/
public class RobotTime {
private long m_currTime = System.currentTimeMillis();
public long m_deltaTime = 0;
private long m_startRobotTime = m_currTime;
public long m_robotTime = 0;
public long m_lastRobotTime = 0;
private long m_startMatchTime = 0;
public long m_matchTime = 0;
public long m_lastMatchTime = 0;
public long m_frameNumber = 0;
/**
* Private constructor prevents other classes from instantiating
*/
private RobotTime(){}
private static RobotTime instance = null;
/**
* Gets the instance of Robot Time. If there is no instance running one will be created.
* @return instance of Robot Time
*/
public static RobotTime getInstance() {
if (instance == null) {
instance = new RobotTime();
}
return instance;
}
/**
* Call this once per periodic loop.
*/
public void updateTimes() {
m_lastRobotTime = m_robotTime;
m_lastMatchTime = m_matchTime;
m_currTime = System.currentTimeMillis();
m_robotTime = m_currTime - m_startRobotTime;
m_deltaTime = m_robotTime - m_lastRobotTime;
m_frameNumber++;
if (m_startMatchTime != 0) {
m_matchTime = m_currTime - m_startMatchTime;
}
}
/**
* Call this in both the auto and periodic inits
*/
public void startMatchTime() {
if (m_startMatchTime == 0) {
m_startMatchTime = m_currTime;
}
}
/**
* Call this in disabled init
*/
public void endMatchTime() {
m_startMatchTime = 0;
m_matchTime = 0;
}
}
src/main/java/frc4388/utility/RobotUnits.java
+27
View File
@@ -0,0 +1,27 @@
// 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;
/** Aarav's good units class (better than WPILib)
* @author Aarav Shah */
public class RobotUnits {
// constants
// angle conversions
public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;}
public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;}
// falcon conversions
public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
// distance conversions
public static double metersToFeet(final double meters) {return meters * 3.28084;}
public static double feetToMeters(final double feet) {return feet / 3.28084;}
}
src/main/java/frc4388/utility/UtilityStructs.java
+12
View File
@@ -0,0 +1,12 @@
package frc4388.utility;
public class UtilityStructs {
public static class TimedOutput {
public double leftX = 0.0;
public double leftY = 0.0;
public double rightX = 0.0;
public double rightY = 0.0;
public long timedOffset = 0;
}
}
src/main/java/frc4388/utility/controller/IHandController.java
+21
View File
@@ -0,0 +1,21 @@
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();
}
src/main/java/frc4388/utility/controller/XboxController.java
+218
View File
@@ -0,0 +1,218 @@
package frc4388.utility.controller;
import edu.wpi.first.wpilibj.Joystick;
/**
* 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.1;
private Joystick m_stick;
/**
* 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
*/
private 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/XboxTriggerButton.java
+68
View File
@@ -0,0 +1,68 @@
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() {
if (m_trigger == RIGHT_TRIGGER) {
return m_controller.getRightTrigger();
}
else if (m_trigger == LEFT_TRIGGER) {
return m_controller.getLeftTrigger();
}
else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
return m_controller.getRightAxisUpTrigger();
}
else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
return m_controller.getRightAxisDownTrigger();
}
else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
return m_controller.getRightAxisRightTrigger();
}
else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
return m_controller.getRightAxisLeftTrigger();
}
else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
return m_controller.getLeftAxisUpTrigger();
}
else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
return m_controller.getLeftAxisDownTrigger();
}
else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
return m_controller.getLeftAxisRightTrigger();
}
else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
return m_controller.getLeftAxisLeftTrigger();
}
return false;
}
}