mirror of
https://github.com/Team4388/2023WayOfTheRobot.git
synced 2026-06-09 00:37:59 -06:00
Merge branch 'master' into vision-april-lime
This commit is contained in:
Aarav Shah
@@ -47,18 +47,17 @@ public final class Constants {
|
||||
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 {
|
||||
@@ -89,7 +88,8 @@ public final class Constants {
|
||||
public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
|
||||
}
|
||||
|
||||
public static final double MAX_SPEED_FEET_PER_SECOND = 10; // 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;
|
||||
|
||||
@@ -10,8 +10,13 @@ package frc4388.robot;
|
||||
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 frc4388.utility.RobotTime;
|
||||
@@ -45,6 +50,8 @@ public class Robot extends TimedRobot {
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
|
||||
SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -94,6 +101,7 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
m_robotContainer.m_robotSwerveDrive.resetGyro();
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
// schedule the autonomous command (example)
|
||||
@@ -112,6 +120,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
|
||||
@@ -119,6 +129,7 @@ public class Robot extends TimedRobot {
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
m_robotContainer.m_robotSwerveDrive.resetGyro();
|
||||
m_robotTime.startMatchTime();
|
||||
}
|
||||
|
||||
|
||||
@@ -7,15 +7,42 @@
|
||||
|
||||
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.Constants.SwerveDriveConstants.AutoConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants;
|
||||
import frc4388.robot.commands.AutoBalance;
|
||||
import frc4388.robot.commands.DriveWithInput;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
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,31 +57,60 @@ public class RobotContainer {
|
||||
public final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro);
|
||||
// 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);
|
||||
|
||||
/* 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 */
|
||||
|
||||
m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive,
|
||||
() -> getDriverController().getLeftXAxis(),
|
||||
() -> getDriverController().getLeftYAxis(),
|
||||
() -> getDriverController().getRightXAxis(),
|
||||
false));
|
||||
// * Default Commands
|
||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
|
||||
getDeadbandedDriverController().getRight(),
|
||||
true);
|
||||
}, m_robotSwerveDrive)
|
||||
.withName("SwerveDrive DefaultCommand"));
|
||||
|
||||
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
|
||||
// * Auto Commands
|
||||
// chooser.setDefaultOption("NoAuto", noAuto);
|
||||
|
||||
|
||||
// chooser.addOption("Blue1Path", blue1Path);
|
||||
// chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
|
||||
|
||||
// chooser.addOption("Red1Path", red1Path);
|
||||
// chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
|
||||
|
||||
// chooser.addOption("Taxi", taxi);
|
||||
|
||||
playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
|
||||
.addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
|
||||
.buildDisplay();
|
||||
}
|
||||
|
||||
|
||||
@@ -65,20 +121,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(() -> m_robotMap.gyro.reset()));
|
||||
// .onFalse()
|
||||
|
||||
/* Operator Buttons */
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
|
||||
// // .onFalse()
|
||||
|
||||
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
|
||||
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
|
||||
|
||||
// interrupt 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
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -87,35 +150,16 @@ public class RobotContainer {
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -25,7 +25,7 @@ public class AutoBalance extends PelvicInflammatoryDisease {
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
var pitch = gyro.getPitch();
|
||||
var pitch = gyro.getRoll();
|
||||
SmartDashboard.putNumber("pitch", pitch);
|
||||
return pitch;
|
||||
}
|
||||
@@ -33,8 +33,9 @@ public class AutoBalance extends PelvicInflammatoryDisease {
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
double out2 = MathUtil.clamp(output / 40, -.5, .5);
|
||||
if (Math.abs(gyro.getPitch()) < 3) out2 = 0;
|
||||
drive.drive(out2, 0, 0, false);
|
||||
|
||||
if (Math.abs(getError()) < 3) out2 = 0;
|
||||
drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
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.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.WidgetType;
|
||||
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 frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class PlaybackChooser {
|
||||
private final ArrayList<SendableChooser<Command>> m_choosers = new ArrayList<>();
|
||||
private SendableChooser<Command> m_playback = null;
|
||||
private final HashMap<String, Command> m_commandPool = new HashMap<>();
|
||||
|
||||
private final File m_dir = new File("/home/lvuser/autos/");
|
||||
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() {
|
||||
appendCommand();
|
||||
m_playback = m_choosers.get(0);
|
||||
|
||||
Shuffleboard.getTab("Auto Chooser")
|
||||
.add("Add Sequence", new InstantCommand(() -> appendCommand()))
|
||||
.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);
|
||||
for (String auto : m_dir.list()) {
|
||||
m_playback.addOption(auto, new JoystickPlayback(m_swerve, auto));
|
||||
}
|
||||
for (var cmd_name : m_commandPool.keySet()) {
|
||||
chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
|
||||
}
|
||||
|
||||
m_choosers.add(chooser);
|
||||
Shuffleboard.getTab("Auto Chooser")
|
||||
.add("Command: " + m_choosers.size(), chooser)
|
||||
.withSize(4, 1)
|
||||
.withPosition(0, m_choosers.size() - 1)
|
||||
.withWidget(BuiltInWidgets.kSplitButtonChooser);
|
||||
}
|
||||
|
||||
public Command getCommand() {
|
||||
Command command = m_playback.getSelected();
|
||||
command = command == null ? m_noAuto : command.asProxy();
|
||||
|
||||
Command[] commands = new Command[m_choosers.size() - 1];
|
||||
for (int i = 0; i < m_choosers.size()-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
|
||||
@@ -4,6 +4,9 @@
|
||||
|
||||
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;
|
||||
@@ -26,18 +29,13 @@ public class SwerveDrive extends SubsystemBase {
|
||||
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 SwerveDrive(
|
||||
// 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
|
||||
|
||||
@@ -47,18 +45,55 @@ public class SwerveDrive extends SubsystemBase {
|
||||
this.rightFront = rightFront;
|
||||
this.leftBack = leftBack;
|
||||
this.rightBack = rightBack;
|
||||
|
||||
this.gyro = gyro;
|
||||
|
||||
// this.odometry = new SwerveDriveOdometry(
|
||||
// kinematics,
|
||||
// gyro.getRotation2d(),
|
||||
// new SwerveModulePosition[] {
|
||||
// leftFront.getPosition(),
|
||||
// rightFront.getPosition(),
|
||||
// leftBack.getPosition(),
|
||||
// rightBack.getPosition()
|
||||
// },
|
||||
// getOdometry()
|
||||
// );
|
||||
|
||||
this.poseEstimator = new SwerveDrivePoseEstimator(
|
||||
kinematics,
|
||||
gyro.getRotation2d(),
|
||||
new SwerveModulePosition[] {
|
||||
leftFront.getPosition(),
|
||||
rightFront.getPosition(),
|
||||
leftBack.getPosition(),
|
||||
rightBack.getPosition()
|
||||
},
|
||||
new Pose2d(0,0, new Rotation2d(0))
|
||||
);
|
||||
|
||||
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
|
||||
}
|
||||
|
||||
// WPILib swerve drive example
|
||||
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
||||
// SwerveModuleState[] states = kinematics.toSwerveModuleStates(
|
||||
// fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d())
|
||||
// : new ChassisSpeeds(xSpeed, ySpeed, rot)
|
||||
// );
|
||||
SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
|
||||
setModuleStates(states);
|
||||
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));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -74,6 +109,16 @@ 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.
|
||||
*/
|
||||
@@ -89,6 +134,18 @@ public class SwerveDrive extends SubsystemBase {
|
||||
// );
|
||||
// }
|
||||
|
||||
public void updatePoseEstimator() {
|
||||
poseEstimator.update(
|
||||
gyro.getRotation2d(),
|
||||
new SwerveModulePosition[] {
|
||||
leftFront.getPosition(),
|
||||
rightFront.getPosition(),
|
||||
leftBack.getPosition(),
|
||||
rightBack.getPosition()
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the odometry of the SwerveDrive.
|
||||
* @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
|
||||
@@ -97,6 +154,10 @@ public class SwerveDrive extends SubsystemBase {
|
||||
// return odometry.getPoseMeters();
|
||||
// }
|
||||
|
||||
public Pose2d getPoseEstimator() {
|
||||
return poseEstimator.getEstimatedPosition();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the odometry of the SwerveDrive.
|
||||
* @param pose Pose to set the odometry to.
|
||||
@@ -114,21 +175,35 @@ public class SwerveDrive extends SubsystemBase {
|
||||
// );
|
||||
// }
|
||||
|
||||
public void setPoseEstimator(Pose2d pose) {
|
||||
poseEstimator.resetPosition(
|
||||
gyro.getRotation2d(),
|
||||
new SwerveModulePosition[] {
|
||||
leftFront.getPosition(),
|
||||
rightFront.getPosition(),
|
||||
leftBack.getPosition(),
|
||||
rightBack.getPosition()
|
||||
},
|
||||
pose
|
||||
);
|
||||
}
|
||||
|
||||
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.
|
||||
*/
|
||||
// public void resetOdometry() {
|
||||
// odometry.resetPosition(
|
||||
// gyro.getRotation2d(),
|
||||
// new SwerveModulePosition[] {
|
||||
// leftFront.getPosition(),
|
||||
// rightFront.getPosition(),
|
||||
// leftBack.getPosition(),
|
||||
// rightBack.getPosition()
|
||||
// },
|
||||
// new Pose2d()
|
||||
// );
|
||||
// setOdometry(new Pose2d());
|
||||
// }
|
||||
|
||||
public SwerveDriveKinematics getKinematics() {
|
||||
@@ -138,7 +213,16 @@ public class SwerveDrive extends SubsystemBase {
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
|
||||
// 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());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -124,7 +124,10 @@ public class SwerveModule extends SubsystemBase {
|
||||
|
||||
// convert the CANCoder from its position reading to ticks
|
||||
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
|
||||
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
|
||||
|
||||
if (!ignoreAngle) {
|
||||
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
|
||||
}
|
||||
|
||||
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user