mirror of
https://github.com/Team4388/2023WayOfTheRobot.git
synced 2026-06-09 00:37:59 -06:00
Merge branch 'master' into arm
This commit is contained in:
Aarav Shah
@@ -23,49 +23,54 @@ import frc4388.utility.LEDPatterns;
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final class SwerveDriveConstants {
|
||||
|
||||
public static final double ROTATION_SPEED = -0.7;
|
||||
|
||||
public static final class IDs {
|
||||
public static final int LEFT_FRONT_WHEEL_ID = -1; // TODO: find the actual ID
|
||||
public static final int RIGHT_FRONT_WHEEL_ID = -1; // TODO: find the actual ID
|
||||
public static final int LEFT_BACK_WHEEL_ID = -1; // TODO: find the actual ID
|
||||
public static final int RIGHT_BACK_STEER_ID = -1; // TODO: find the actual ID
|
||||
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 LEFT_FRONT_STEER_ID = -1; // TODO: find the actual ID
|
||||
public static final int RIGHT_FRONT_STEER_ID = -1; // TODO: find the actual ID
|
||||
public static final int LEFT_BACK_STEER_ID = -1; // TODO: find the actual ID
|
||||
public static final int RIGHT_BACK_WHEEL_ID = -1; // TODO: find the actual ID
|
||||
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_FRONT_ENCODER_ID = -1; // TODO: find the actual ID
|
||||
public static final int RIGHT_FRONT_ENCODER_ID = -1; // TODO: find the actual ID
|
||||
public static final int LEFT_BACK_ENCODER_ID = -1; // TODO: find the actual ID
|
||||
public static final int RIGHT_BACK_ENCODER_ID = -1; // TODO: find the actual ID
|
||||
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(1.0, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
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 PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
|
||||
public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
|
||||
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(0.0, 0.0, 0.0,
|
||||
new TrapezoidProfile.Constraints(0.0, 0.0)
|
||||
);
|
||||
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 = -1; // TODO: find the actual value
|
||||
public static final double PATH_MAX_ACC = -1; // TODO: find the actual value
|
||||
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 = 11.0; // TODO: find the actual value
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2.0; // TODO: find the actual value
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 5.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 MOTOR_REV_PER_WHEEL_REV = -1; // TODO: find the actual value
|
||||
public static final double TICKS_PER_MOTOR_REV = 2048;
|
||||
public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value
|
||||
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;
|
||||
@@ -81,18 +86,14 @@ public final class Constants {
|
||||
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 LEFT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value
|
||||
public static final double RIGHT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value
|
||||
public static final double LEFT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value
|
||||
public static final double RIGHT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value
|
||||
}
|
||||
|
||||
public static final double MAX_SPEED_FEET_PER_SECOND = -1; // 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 = -1; // TODO: find the actual value
|
||||
public static final double HEIGHT = -1; // TODO: find the actual value
|
||||
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;
|
||||
|
||||
@@ -102,7 +103,13 @@ public final class Constants {
|
||||
}
|
||||
|
||||
public static final class GyroConstants {
|
||||
public static final int ID = -1; // TODO: find the actual ID
|
||||
public static final int ID = 14; // TODO: find the actual ID
|
||||
}
|
||||
|
||||
public static final class ArmConstants {
|
||||
public static final int MIN_ARM_LEN = 0;
|
||||
public static final int MAX_ARM_LEN = 1;
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
}
|
||||
|
||||
public static final class ArmConstants {
|
||||
@@ -125,9 +132,9 @@ public final class Constants {
|
||||
|
||||
public static final double OFFSET = 0;
|
||||
}
|
||||
|
||||
|
||||
public static final class LEDConstants {
|
||||
public static final int LED_SPARK_ID = 0;
|
||||
// public static final int LED_SPARK_ID = 0;
|
||||
|
||||
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
|
||||
}
|
||||
@@ -135,5 +142,8 @@ public final class Constants {
|
||||
public static final class OIConstants {
|
||||
public static final int XBOX_DRIVER_ID = 0;
|
||||
public static final int XBOX_OPERATOR_ID = 1;
|
||||
public static final double LEFT_AXIS_DEADBAND = 0.1;
|
||||
public static final double RIGHT_AXIS_DEADBAND = 0.6;
|
||||
public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing
|
||||
}
|
||||
}
|
||||
|
||||
@@ -7,19 +7,24 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
|
||||
import com.ctre.phoenix.sensors.WPI_Pigeon2;
|
||||
import java.lang.System;
|
||||
import java.lang.reflect.Array;
|
||||
import java.util.Arrays;
|
||||
import java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.io.PrintWriter;
|
||||
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.commands.AutoBalance;
|
||||
import frc4388.utility.RobotGyro;
|
||||
import frc4388.utility.RobotTime;
|
||||
|
||||
import frc4388.robot.subsystems.Location;
|
||||
import frc4388.robot.subsystems.Apriltags.Tag;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the
|
||||
* functions corresponding to each mode, as described in the TimedRobot
|
||||
@@ -33,34 +38,8 @@ public class Robot extends TimedRobot {
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
private RobotContainer m_robotContainer;
|
||||
|
||||
public static class MicroBot extends SubsystemBase {
|
||||
public WPI_Pigeon2 pigeon = new WPI_Pigeon2(14);
|
||||
public RobotGyro gyro = new RobotGyro(pigeon);
|
||||
private Location location = new Location();
|
||||
|
||||
public TalonSRX frontLeft = new TalonSRX(2);
|
||||
public TalonSRX backLeft = new TalonSRX(3);
|
||||
public TalonSRX backRight = new TalonSRX(5);
|
||||
public TalonSRX frontRight = new TalonSRX(4);
|
||||
|
||||
public MicroBot() {
|
||||
frontRight.configFactoryDefault();
|
||||
frontLeft.configFactoryDefault();
|
||||
backLeft.configFactoryDefault();
|
||||
backRight.configFactoryDefault();
|
||||
|
||||
frontLeft.setInverted(true);
|
||||
backLeft.setInverted(true);
|
||||
}
|
||||
|
||||
public void setOutput(double output) {
|
||||
frontRight.set(ControlMode.PercentOutput, output);
|
||||
frontLeft.set(ControlMode.PercentOutput, output);
|
||||
backLeft.set(ControlMode.PercentOutput, output);
|
||||
backRight.set(ControlMode.PercentOutput, output);
|
||||
}
|
||||
}
|
||||
|
||||
private MicroBot bot = new MicroBot();
|
||||
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be
|
||||
@@ -72,7 +51,7 @@ public class Robot extends TimedRobot {
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
|
||||
bot.setDefaultCommand(new AutoBalance(bot));
|
||||
SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -91,6 +70,13 @@ public class Robot extends TimedRobot {
|
||||
// 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();
|
||||
|
||||
final Tag pos = location.getPosRot();
|
||||
if (pos != null) {
|
||||
SmartDashboard.putNumber("x position", pos.x);
|
||||
}
|
||||
|
||||
//ystem.out.print(apriltagPos[0]);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -112,15 +98,9 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
m_robotContainer.m_robotSwerveDrive.resetGyro();
|
||||
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();
|
||||
@@ -137,6 +117,8 @@ public class Robot extends TimedRobot {
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
m_robotContainer.m_robotSwerveDrive.resetGyro();
|
||||
|
||||
// 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
|
||||
@@ -144,9 +126,8 @@ public class Robot extends TimedRobot {
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
m_robotContainer.m_robotSwerveDrive.resetGyro();
|
||||
m_robotTime.startMatchTime();
|
||||
|
||||
m_robotContainer.gyroRef.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -154,9 +135,6 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle());
|
||||
SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch());
|
||||
SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -7,18 +7,43 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import org.opencv.objdetect.HOGDescriptor;
|
||||
|
||||
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.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.Trajectory.State;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
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.Arm;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants;
|
||||
import frc4388.robot.commands.AutoBalance;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.RobotGyro;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
import frc4388.robot.commands.JoystickPlayback;
|
||||
import frc4388.robot.commands.JoystickRecorder;
|
||||
import frc4388.robot.commands.PlaybackChooser;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
|
||||
/**
|
||||
@@ -30,43 +55,69 @@ import frc4388.utility.controller.XboxController;
|
||||
*/
|
||||
public class RobotContainer {
|
||||
/* RobotMap */
|
||||
private final RobotMap m_robotMap = new RobotMap();
|
||||
public final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro);
|
||||
private final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
|
||||
private final LED m_robotLED = new LED(m_robotMap.LEDController);
|
||||
|
||||
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
|
||||
m_robotMap.rightFront,
|
||||
m_robotMap.leftBack,
|
||||
m_robotMap.rightBack,
|
||||
m_robotMap.gyro);
|
||||
|
||||
/* Controllers */
|
||||
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
|
||||
public RobotGyro gyroRef = m_robotMap.gyro;
|
||||
/* Autos */
|
||||
public SendableChooser<Command> chooser = new SendableChooser<>();
|
||||
|
||||
private Command noAuto = new InstantCommand();
|
||||
|
||||
// private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
|
||||
|
||||
// private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt");
|
||||
// private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
|
||||
|
||||
// private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1);
|
||||
// private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
|
||||
|
||||
// private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
|
||||
|
||||
private PlaybackChooser playbackChooser;
|
||||
/**
|
||||
* 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));
|
||||
// * Default Commands
|
||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
|
||||
getDeadbandedDriverController().getRight(),
|
||||
true);
|
||||
}, m_robotSwerveDrive)
|
||||
.withName("SwerveDrive DefaultCommand"));
|
||||
|
||||
// * Auto Commands
|
||||
// chooser.setDefaultOption("NoAuto", noAuto);
|
||||
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
|
||||
getDriverController().getLeftYAxis(),
|
||||
-getDriverController().getRightXAxis(), false), m_robotSwerveDrive)
|
||||
);
|
||||
// chooser.addOption("Blue1Path", blue1Path);
|
||||
// chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
|
||||
// chooser.addOption("Red1Path", red1Path);
|
||||
// chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
|
||||
|
||||
// chooser.addOption("Taxi", taxi);
|
||||
|
||||
m_robotArm.setDefaultCommand(new RunCommand(() -> m_robotArm.armSetLength(
|
||||
getOperatorController().getLeftYAxis()), m_robotArm)
|
||||
);
|
||||
playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
|
||||
.addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
|
||||
.buildDisplay();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be
|
||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||
@@ -74,20 +125,27 @@ public class RobotContainer {
|
||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
// * Driver Buttons
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
|
||||
|
||||
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> gyroRef.reset()));
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
|
||||
// // .onFalse()
|
||||
|
||||
/* Operator Buttons */
|
||||
// activates "Lit Mode"
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
.whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED))
|
||||
.whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED));
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
|
||||
|
||||
//New interupt button
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
|
||||
.onTrue(new InstantCommand());
|
||||
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());
|
||||
|
||||
// * Operator Buttons
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -96,35 +154,16 @@ public class RobotContainer {
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// no auto
|
||||
return new InstantCommand();
|
||||
// return chooser.getSelected();
|
||||
return playbackChooser.getCommand();
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public IHandController getDriverController() {
|
||||
return m_driverXbox;
|
||||
public DeadbandedXboxController getDeadbandedDriverController() {
|
||||
return this.m_driverXbox;
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public IHandController getOperatorController() {
|
||||
return m_operatorXbox;
|
||||
public DeadbandedXboxController getDeadbandedOperatorController() {
|
||||
return this.m_operatorXbox;
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getOperatorJoystick() {
|
||||
return m_operatorXbox.getJoyStick();
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getDriverJoystick() {
|
||||
return m_driverXbox.getJoyStick();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -9,10 +9,10 @@ package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.WPI_Pigeon2;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import frc4388.robot.Constants.ArmConstants;
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
@@ -40,7 +40,7 @@ public class RobotMap {
|
||||
}
|
||||
|
||||
/* LED Subsystem */
|
||||
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
|
||||
void configureLEDMotorControllers() {
|
||||
|
||||
@@ -115,19 +115,18 @@ public class RobotMap {
|
||||
|
||||
rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// config magnet offset
|
||||
leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET);
|
||||
rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
leftBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
// set neutral mode
|
||||
leftFrontSteer.setNeutralMode(NeutralMode.Brake);
|
||||
rightFrontSteer.setNeutralMode(NeutralMode.Brake);
|
||||
leftBackSteer.setNeutralMode(NeutralMode.Brake);
|
||||
rightBackSteer.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
// initialize SwerveModules
|
||||
this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET);
|
||||
this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET);
|
||||
this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
|
||||
this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
|
||||
this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
|
||||
this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
|
||||
}
|
||||
|
||||
// arm stuff
|
||||
|
||||
@@ -5,37 +5,44 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import frc4388.robot.Robot;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
// NOTE: Consider using this command inline, rather than writing a subclass. For more
|
||||
// information, see:
|
||||
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
|
||||
public class AutoBalance extends PelvicInflammatoryDisease {
|
||||
Robot.MicroBot bot;
|
||||
RobotGyro gyro;
|
||||
SwerveDrive drive;
|
||||
|
||||
/** Creates a new AutoBalanceTF2. */
|
||||
public AutoBalance(Robot.MicroBot bot) {
|
||||
super(.7, .1, 15, 0);
|
||||
addRequirements(bot);
|
||||
this.bot = bot;
|
||||
public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
|
||||
super(0.6, 0, 0, 0);
|
||||
|
||||
this.gyro = gyro;
|
||||
this.drive = drive;
|
||||
|
||||
addRequirements(drive);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
return bot.gyro.getPitch();
|
||||
var pitch = gyro.getRoll();
|
||||
SmartDashboard.putNumber("pitch", pitch);
|
||||
return pitch;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
double out2 = MathUtil.clamp(output / 40, -.5, .5);
|
||||
if (Math.abs(bot.gyro.getPitch()) < 3) out2 = 0;
|
||||
bot.setOutput(out2);
|
||||
|
||||
if (Math.abs(getError()) < 3) out2 = 0;
|
||||
drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
super.initialize();
|
||||
this.bot.gyro.reset();
|
||||
// this.gyro.reset();
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
|
||||
@@ -0,0 +1,150 @@
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,2
|
||||
0.0,0.0,0.0,0.0,18
|
||||
0.0,0.0,0.0,0.0,31
|
||||
0.0,0.0,0.0,0.0,61
|
||||
0.0,0.0,0.0,0.0,72
|
||||
0.0,0.0,0.0,0.0,85
|
||||
0.0,0.0,0.0,0.0,102
|
||||
0.0,0.0,0.0,0.0,116
|
||||
0.0,0.0,0.0,0.0,163
|
||||
0.0,0.0,0.0,0.0,175
|
||||
0.0,0.0,0.0,0.0,188
|
||||
0.0,0.0,0.0,0.0,205
|
||||
0.0,0.0,0.0,0.0,217
|
||||
0.0,0.0,0.0,0.0,229
|
||||
0.0,0.0,0.0,0.0,241
|
||||
0.0,0.0,0.0,0.0,261
|
||||
0.0,0.0,0.0,0.0,281
|
||||
0.0,0.0,0.0,0.0,302
|
||||
0.0,0.0,0.0,0.0,322
|
||||
0.0,0.0,0.0,0.0,341
|
||||
0.0,0.0,0.0,0.0,361
|
||||
0.0,-0.1328125,0.0,0.0,397
|
||||
0.0,-0.1328125,0.0,0.0,412
|
||||
0.0,-0.1484375,0.0,0.0,425
|
||||
0.0,-0.1796875,0.0,0.0,441
|
||||
0.0,-0.1875,0.0,0.0,461
|
||||
0.0,-0.1953125,0.0,0.0,481
|
||||
0.0,-0.234375,0.0,0.0,502
|
||||
0.0,-0.2890625,0.0,0.0,521
|
||||
0.0,-0.3125,0.0,0.0,541
|
||||
0.0,-0.328125,0.0,0.0,561
|
||||
0.0,-0.3671875,0.0,0.0,582
|
||||
0.0,-0.390625,0.0,0.0,601
|
||||
0.0,-0.4609375,0.0,0.0,642
|
||||
0.0,-0.4765625,0.0,0.0,653
|
||||
0.0,-0.4765625,0.0,0.0,666
|
||||
0.0,-0.4765625,0.0,0.0,681
|
||||
0.0,-0.4765625,0.0,0.0,702
|
||||
0.0,-0.4765625,0.0,0.0,721
|
||||
0.0,-0.4765625,0.0,0.0,741
|
||||
0.0,-0.4765625,0.0,0.0,762
|
||||
0.0,-0.4765625,0.0,0.0,782
|
||||
0.0,-0.484375,0.0,0.0,803
|
||||
0.0,-0.484375,0.0,0.0,821
|
||||
0.0,-0.4921875,0.0,0.0,842
|
||||
0.0,-0.5078125,0.0,0.0,878
|
||||
0.0,-0.5234375,0.0,0.0,893
|
||||
0.0,-0.5234375,0.0,0.0,906
|
||||
0.0,-0.53125,0.0,0.0,922
|
||||
0.0,-0.53125,0.0,0.0,942
|
||||
0.0,-0.53125,0.0,0.0,962
|
||||
0.0,-0.53125,0.0,0.0,982
|
||||
0.0,-0.5390625,0.0,0.0,1001
|
||||
0.0,-0.5390625,0.0,0.0,1022
|
||||
0.0,-0.546875,0.0,0.0,1042
|
||||
0.0,-0.546875,0.0,0.0,1061
|
||||
0.0,-0.546875,0.0,0.0,1082
|
||||
0.0,-0.546875,0.0,0.0,1164
|
||||
0.0,-0.546875,0.0,0.0,1176
|
||||
0.0,-0.546875,0.0,0.0,1190
|
||||
0.0,-0.546875,0.0,0.0,1202
|
||||
0.0,-0.546875,0.0,0.0,1219
|
||||
0.0,-0.546875,0.0,0.0,1230
|
||||
0.0,-0.546875,0.0,0.0,1244
|
||||
0.0,-0.546875,0.0,0.0,1258
|
||||
0.0,-0.546875,0.0,0.0,1268
|
||||
0.0,-0.546875,0.0,0.0,1281
|
||||
0.0,-0.546875,0.0,0.0,1301
|
||||
0.0,-0.546875,0.0,0.0,1321
|
||||
0.0,-0.546875,0.0,0.0,1363
|
||||
0.0,-0.546875,0.0,0.0,1376
|
||||
0.0,-0.546875,0.0,0.0,1388
|
||||
0.0,-0.546875,0.0,0.0,1402
|
||||
0.0,-0.546875,0.0,0.0,1422
|
||||
0.0,-0.546875,0.0,0.0,1441
|
||||
0.0,-0.546875,0.0,0.0,1461
|
||||
0.0,-0.546875,0.0,0.0,1482
|
||||
0.0,-0.546875,0.0,0.0,1502
|
||||
0.0,-0.546875,0.0,0.0,1521
|
||||
0.0,-0.546875,0.0,0.0,1542
|
||||
0.0,-0.546875,0.0,0.0,1562
|
||||
0.0,-0.546875,0.0,0.0,1598
|
||||
0.0,-0.546875,0.0,0.0,1609
|
||||
0.0,-0.546875,0.0,0.0,1621
|
||||
0.0,-0.546875,0.0,0.0,1642
|
||||
0.0,-0.546875,0.0,0.0,1661
|
||||
0.0,-0.546875,0.0,0.0,1682
|
||||
0.0,-0.546875,0.0,0.0,1702
|
||||
0.0,-0.546875,0.0,0.0,1722
|
||||
0.0,-0.546875,0.0,0.0,1742
|
||||
0.0,-0.546875,0.0,0.0,1762
|
||||
0.0,-0.546875,0.0,0.0,1781
|
||||
0.0,-0.5390625,0.0,0.0,1801
|
||||
0.0,-0.5390625,0.0,0.0,1835
|
||||
0.0,-0.5390625,0.0,0.0,1847
|
||||
0.0,-0.5390625,0.0,0.0,1861
|
||||
0.0,-0.5390625,0.0,0.0,1882
|
||||
0.0,-0.5390625,0.0,0.0,1901
|
||||
0.0,-0.5390625,0.0,0.0,1921
|
||||
0.0,-0.5390625,0.0,0.0,1941
|
||||
0.0,-0.5390625,0.0,0.0,1962
|
||||
0.0,-0.5390625,0.0,0.0,1983
|
||||
0.0,-0.5390625,0.0,0.0,2001
|
||||
0.0,-0.5390625,0.0,0.0,2022
|
||||
0.0,-0.5390625,0.0,0.0,2042
|
||||
0.0,-0.5390625,0.0,0.0,2085
|
||||
0.0,-0.5390625,0.0,0.0,2097
|
||||
0.0,-0.5390625,0.0,0.0,2113
|
||||
0.0,-0.5390625,0.0,0.0,2124
|
||||
0.0,-0.5390625,0.0,0.0,2141
|
||||
0.0,-0.5390625,0.0,0.0,2161
|
||||
0.0,-0.5390625,0.0,0.0,2181
|
||||
0.0,-0.5390625,0.0,0.0,2201
|
||||
0.0,-0.5390625,0.0,0.0,2221
|
||||
0.0,-0.5390625,0.0,0.0,2241
|
||||
0.0,-0.5390625,0.0,0.0,2262
|
||||
0.0,-0.5390625,0.0,0.0,2283
|
||||
0.0,-0.2265625,0.0,0.0,2377
|
||||
0.0,-0.2265625,0.0,0.0,2390
|
||||
0.0,0.0,0.0,0.0,2402
|
||||
0.0,0.0,0.0,0.0,2417
|
||||
0.0,0.0,0.0,0.0,2428
|
||||
0.0,0.0,0.0,0.0,2440
|
||||
0.0,0.0,0.0,0.0,2451
|
||||
0.0,0.0,0.0,0.0,2463
|
||||
0.0,0.0,0.0,0.0,2475
|
||||
0.0,0.0,0.0,0.0,2493
|
||||
0.0,0.0,0.0,0.0,2505
|
||||
0.0,0.0,0.0,0.0,2521
|
||||
0.0,0.0,0.0,0.0,2558
|
||||
0.0,0.0,0.0,0.0,2576
|
||||
0.0,0.0,0.0,0.0,2592
|
||||
0.0,0.0,0.0,0.0,2603
|
||||
0.0,0.0,0.0,0.0,2621
|
||||
0.0,0.0,0.0,0.0,2641
|
||||
0.0,0.0,0.0,0.0,2661
|
||||
0.0,0.0,0.0,0.0,2681
|
||||
0.0,0.0,0.0,0.0,2702
|
||||
0.0,0.0,0.0,0.0,2721
|
||||
0.0,0.0,0.0,0.0,2742
|
||||
0.0,0.0,0.0,0.0,2762
|
||||
0.0,0.0,0.0,0.0,2805
|
||||
0.0,0.0,0.0,0.0,2814
|
||||
0.0,0.0,0.0,0.0,2825
|
||||
0.0,0.0,0.0,0.0,2841
|
||||
0.0,0.0,0.0,0.0,2861
|
||||
0.0,0.0,0.0,0.0,2882
|
||||
0.0,0.0,0.0,0.0,2901
|
||||
0.0,0.0,0.0,0.0,2922
|
||||
@@ -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;
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -11,7 +11,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
|
||||
protected Gains gains;
|
||||
private double output = 0;
|
||||
|
||||
/** Creates a new PelvicInflamitoryDisease. */
|
||||
/** Creates a new PelvicInflammatoryDisease. */
|
||||
public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) {
|
||||
gains = new Gains(kp, ki, kd, kf, 0);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,90 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
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.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(() -> {}))
|
||||
.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);
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -7,7 +7,6 @@ import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.CANCoderConfiguration;
|
||||
|
||||
import frc4388.robot.Constants.ArmConstants;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
package frc4388.robot.subsystems;
|
||||
import edu.wpi.first.wpilibj.PWM;
|
||||
|
||||
public class Claw {
|
||||
private PWM m_clawMotor;
|
||||
private boolean m_open = false;
|
||||
|
||||
// Opens claw
|
||||
public Claw(PWM m_clawMotor) {
|
||||
this.m_clawMotor = m_clawMotor;
|
||||
setClaw(true);
|
||||
}
|
||||
|
||||
public void setClaw(boolean open) {
|
||||
// Open claw
|
||||
m_open = open;
|
||||
m_clawMotor.setRaw(open ? 0 : 2000);
|
||||
}
|
||||
|
||||
public boolean isClawOpen() {
|
||||
return m_open;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import frc4388.robot.subsystems.Apriltags.Tag;
|
||||
|
||||
public class Location {
|
||||
final Apriltags apriltag = new Apriltags();
|
||||
|
||||
private boolean isLimelight = false;
|
||||
private boolean isApriltag = false;
|
||||
|
||||
//Determines which source to get pos and rot from and also resets
|
||||
private void reoderPrio(){
|
||||
isLimelight = false; //If limelight gets position and if within a certain range of poles
|
||||
isApriltag = apriltag.isAprilTag();
|
||||
}
|
||||
|
||||
public Tag getPosRot() {
|
||||
reoderPrio();
|
||||
if(isApriltag){
|
||||
return apriltag.getTagPosRot();
|
||||
} else if (isLimelight) {
|
||||
return null;
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
}
|
||||
@@ -4,15 +4,16 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
|
||||
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 edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
@@ -25,27 +26,23 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
private SwerveModule[] modules;
|
||||
|
||||
private RobotGyro gyro;
|
||||
|
||||
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 SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
|
||||
|
||||
private SwerveDriveOdometry odometry = new SwerveDriveOdometry(
|
||||
kinematics,
|
||||
gyro.getRotation2d(),
|
||||
new SwerveModulePosition[] {
|
||||
leftFront.getPosition(),
|
||||
rightFront.getPosition(),
|
||||
leftBack.getPosition(),
|
||||
rightBack.getPosition()
|
||||
}
|
||||
);
|
||||
private RobotGyro gyro;
|
||||
|
||||
// private SwerveDriveOdometry odometry;
|
||||
|
||||
private SwerveDrivePoseEstimator poseEstimator;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
|
||||
|
||||
public Rotation2d rotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
|
||||
@@ -53,23 +50,55 @@ public class SwerveDrive extends SubsystemBase {
|
||||
this.rightFront = rightFront;
|
||||
this.leftBack = leftBack;
|
||||
this.rightBack = rightBack;
|
||||
|
||||
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
|
||||
|
||||
|
||||
this.gyro = gyro;
|
||||
}
|
||||
|
||||
public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
||||
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
// this.odometry = new SwerveDriveOdometry(
|
||||
// kinematics,
|
||||
// gyro.getRotation2d(),
|
||||
// new SwerveModulePosition[] {
|
||||
// leftFront.getPosition(),
|
||||
// rightFront.getPosition(),
|
||||
// leftBack.getPosition(),
|
||||
// rightBack.getPosition()
|
||||
// },
|
||||
// getOdometry()
|
||||
// );
|
||||
|
||||
SwerveModuleState[] states = kinematics.toSwerveModuleStates(
|
||||
fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)
|
||||
this.poseEstimator = new SwerveDrivePoseEstimator(
|
||||
kinematics,
|
||||
gyro.getRotation2d(),
|
||||
new SwerveModulePosition[] {
|
||||
leftFront.getPosition(),
|
||||
rightFront.getPosition(),
|
||||
leftBack.getPosition(),
|
||||
rightBack.getPosition()
|
||||
},
|
||||
new Pose2d(0,0, new Rotation2d(0))
|
||||
);
|
||||
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
||||
setModuleStates(states);
|
||||
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
|
||||
}
|
||||
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (fieldRelative) {
|
||||
if (rightStick.getNorm() > 0.1) {
|
||||
rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1));
|
||||
}
|
||||
|
||||
double rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
|
||||
|
||||
// Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
|
||||
// Convert field-relative speeds to robot-relative speeds.
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * 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));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -77,6 +106,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
* @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];
|
||||
@@ -84,11 +114,33 @@ public class SwerveDrive extends SubsystemBase {
|
||||
}
|
||||
}
|
||||
|
||||
public double getGyroAngle() {
|
||||
return gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
gyro.reset();
|
||||
// setOdometry(getOdometry());
|
||||
rotTarget = new Rotation2d(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the odometry of the SwerveDrive.
|
||||
*/
|
||||
public void updateOdometry() {
|
||||
odometry.update(
|
||||
// public void updateOdometry() {
|
||||
// odometry.update(
|
||||
// gyro.getRotation2d(),
|
||||
// new SwerveModulePosition[] {
|
||||
// leftFront.getPosition(),
|
||||
// rightFront.getPosition(),
|
||||
// leftBack.getPosition(),
|
||||
// rightBack.getPosition()
|
||||
// }
|
||||
// );
|
||||
// }
|
||||
|
||||
public void updatePoseEstimator() {
|
||||
poseEstimator.update(
|
||||
gyro.getRotation2d(),
|
||||
new SwerveModulePosition[] {
|
||||
leftFront.getPosition(),
|
||||
@@ -103,16 +155,33 @@ public class SwerveDrive extends SubsystemBase {
|
||||
* Gets the odometry of the SwerveDrive.
|
||||
* @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
return odometry.getPoseMeters();
|
||||
// public Pose2d getOdometry() {
|
||||
// return odometry.getPoseMeters();
|
||||
// }
|
||||
|
||||
public Pose2d getPoseEstimator() {
|
||||
return poseEstimator.getEstimatedPosition();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the odometry of the SwerveDrive.
|
||||
* @param pose Pose to set the odometry to.
|
||||
*/
|
||||
public void setOdometry(Pose2d pose) {
|
||||
odometry.resetPosition(
|
||||
// public void setOdometry(Pose2d pose) {
|
||||
// odometry.resetPosition(
|
||||
// gyro.getRotation2d(),
|
||||
// new SwerveModulePosition[] {
|
||||
// leftFront.getPosition(),
|
||||
// rightFront.getPosition(),
|
||||
// leftBack.getPosition(),
|
||||
// rightBack.getPosition()
|
||||
// },
|
||||
// pose
|
||||
// );
|
||||
// }
|
||||
|
||||
public void setPoseEstimator(Pose2d pose) {
|
||||
poseEstimator.resetPosition(
|
||||
gyro.getRotation2d(),
|
||||
new SwerveModulePosition[] {
|
||||
leftFront.getPosition(),
|
||||
@@ -124,27 +193,41 @@ public class SwerveDrive extends SubsystemBase {
|
||||
);
|
||||
}
|
||||
|
||||
public void resetPoseEstimator() {
|
||||
setPoseEstimator(new Pose2d());
|
||||
}
|
||||
|
||||
public void stopModules() {
|
||||
for (SwerveModule module : this.modules) {
|
||||
module.stop();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry of the SwerveDrive to 0.
|
||||
* *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions.
|
||||
* *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions.
|
||||
*/
|
||||
public void resetOdometry() {
|
||||
odometry.resetPosition(
|
||||
gyro.getRotation2d(),
|
||||
new SwerveModulePosition[] {
|
||||
leftFront.getPosition(),
|
||||
rightFront.getPosition(),
|
||||
leftBack.getPosition(),
|
||||
rightBack.getPosition()
|
||||
},
|
||||
new Pose2d()
|
||||
);
|
||||
// public void resetOdometry() {
|
||||
// setOdometry(new Pose2d());
|
||||
// }
|
||||
|
||||
public SwerveDriveKinematics getKinematics() {
|
||||
return this.kinematics;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
updateOdometry();
|
||||
|
||||
// updateOdometry();
|
||||
updatePoseEstimator();
|
||||
|
||||
// SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX()));
|
||||
// SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY()));
|
||||
// SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees());
|
||||
|
||||
// SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
|
||||
// SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -10,7 +10,6 @@ import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.CANCoderConfiguration;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
@@ -23,15 +22,15 @@ import frc4388.utility.Gains;
|
||||
public class SwerveModule extends SubsystemBase {
|
||||
private WPI_TalonFX driveMotor;
|
||||
private WPI_TalonFX angleMotor;
|
||||
private CANCoder canCoder;
|
||||
private CANCoder encoder;
|
||||
|
||||
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
|
||||
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
|
||||
this.driveMotor = driveMotor;
|
||||
this.angleMotor = angleMotor;
|
||||
this.canCoder = canCoder;
|
||||
this.encoder = encoder;
|
||||
|
||||
TalonFXConfiguration angleConfig = new TalonFXConfiguration();
|
||||
angleConfig.slot0.kP = swerveGains.kP;
|
||||
@@ -39,14 +38,15 @@ public class SwerveModule extends SubsystemBase {
|
||||
angleConfig.slot0.kD = swerveGains.kD;
|
||||
|
||||
// use the CANcoder as the remote sensor for the primary TalonFX PID
|
||||
angleConfig.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
|
||||
angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
|
||||
angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
|
||||
angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
|
||||
angleMotor.configAllSettings(angleConfig);
|
||||
|
||||
CANCoderConfiguration canCoderConfig = new CANCoderConfiguration();
|
||||
canCoderConfig.magnetOffsetDegrees = offset;
|
||||
canCoder.configAllSettings(canCoderConfig);
|
||||
encoder.configMagnetOffset(offset);
|
||||
|
||||
driveMotor.setSelectedSensorPosition(0);
|
||||
driveMotor.config_kP(0, 0.2);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -70,7 +70,7 @@ public class SwerveModule extends SubsystemBase {
|
||||
* @return the CANcoder of the SwerveModule
|
||||
*/
|
||||
public CANCoder getEncoder() {
|
||||
return this.canCoder;
|
||||
return this.encoder;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -78,8 +78,20 @@ public class SwerveModule extends SubsystemBase {
|
||||
* @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(canCoder.getAbsolutePosition());
|
||||
// * 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() {
|
||||
@@ -126,15 +138,17 @@ public class SwerveModule extends SubsystemBase {
|
||||
double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
|
||||
|
||||
// convert the CANCoder from its position reading to ticks
|
||||
double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
|
||||
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
|
||||
|
||||
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
|
||||
|
||||
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
|
||||
|
||||
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
||||
}
|
||||
|
||||
public void reset() {
|
||||
canCoder.setPositionToAbsolute();
|
||||
public void reset(double position) {
|
||||
encoder.setPositionToAbsolute();
|
||||
}
|
||||
|
||||
public double getCurrent() {
|
||||
@@ -144,4 +158,4 @@ public class SwerveModule extends SubsystemBase {
|
||||
public double getVoltage() {
|
||||
return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
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 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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -54,6 +54,12 @@ public class Gains {
|
||||
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.
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
|
||||
public class DeadbandedXboxController extends XboxController {
|
||||
public DeadbandedXboxController(int port) { super(port); }
|
||||
|
||||
@Override public double getLeftX() { return getLeft().getX(); }
|
||||
@Override public double getLeftY() { return getLeft().getY(); }
|
||||
@Override public double getRightX() { return getRight().getX(); }
|
||||
@Override public double getRightY() { return getRight().getY(); }
|
||||
|
||||
public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
|
||||
public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
|
||||
|
||||
public static Translation2d skewToDeadzonedCircle(double x, double y) {
|
||||
Translation2d translation2d = new Translation2d(x, y);
|
||||
double magnitude = translation2d.getNorm();
|
||||
if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude);
|
||||
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
|
||||
return translation2d;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user