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

Merge remote-tracking branch 'origin/testRoboReveal' into cleanup

Browse Source
This commit is contained in:
nathanrsxtn
2022-03-22 17:48:41 -06:00
parent 393e5b8bb0 0b2dcc2425
commit cb31146e02
19 changed files with 764 additions and 213 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/L Path.path
+1 -55
View File
@@ -1,55 +1 @@
{
"waypoints": [
{
"anchorPoint": {
"x": 2.0,
"y": 2.0
},
"prevControl": null,
"nextControl": {
"x": 3.0,
"y": 2.0
},
"holonomicAngle": 0.0,
"isReversal": false,
"velOverride": null,
"isLocked": false
},
{
"anchorPoint": {
"x": 5.0,
"y": 2.0
},
"prevControl": {
"x": 4.000000000000002,
"y": 2.0
},
"nextControl": {
"x": 5.004218182347978,
"y": 2.0
},
"holonomicAngle": 0.0,
"isReversal": false,
"velOverride": null,
"isLocked": false
},
{
"anchorPoint": {
"x": 5.0,
"y": 5.0
},
"prevControl": {
"x": 4.991875448297241,
"y": 5.0
},
"nextControl": null,
"holonomicAngle": 0.0,
"isReversal": false,
"velOverride": null,
"isLocked": false
}
],
"maxVelocity": 1.0,
"maxAcceleration": 1.0,
"isReversed": null
}
{"waypoints":[{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":null,"nextControl":{"x":5.0,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.000000000000002,"y":2.0},"nextControl":{"x":5.004218182347978,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":4.991875448297241,"y":3.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
src/main/deploy/pathplanner/Move Forward.path
+1 -1
View File
@@ -1 +1 @@
{"waypoints":[{"anchorPoint":{"x":3.8761638155965237,"y":2.097178183811552},"prevControl":null,"nextControl":{"x":3.891027717524718,"y":2.097178183811552},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.989837747689554,"y":2.097178183811552},"prevControl":{"x":4.984427304969435,"y":2.097178183811552},"nextControl":{"x":4.984427304969435,"y":2.097178183811552},"holonomicAngle":91.52752544221289,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.8761638155965237,"y":2.140568077269722},"prevControl":{"x":3.3359301436282043,"y":2.140568077269722},"nextControl":null,"holonomicAngle":178.2100893917539,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
{"waypoints":[{"anchorPoint":{"x":3.0,"y":2.0},"prevControl":null,"nextControl":{"x":3.0148639019281944,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.45976632803168,"y":2.0},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
src/main/deploy/pathplanner/Rotate.path
+1 -1
View File
@@ -1 +1 @@
{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":false}
{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":0.05,"maxAcceleration":0.05,"isReversed":false}
src/main/deploy/pathplanner/Test Forward.path
+1
View File
@@ -0,0 +1 @@
{"waypoints":[{"anchorPoint":{"x":0.0,"y":0.0},"prevControl":null,"nextControl":{"x":1.0,"y":0.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":0.0},"prevControl":{"x":3.0,"y":0.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
src/main/java/frc4388/robot/Constants.java
+16 -16
View File
@@ -46,18 +46,18 @@ public final class Constants {
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant?
// IDs
public static final int LEFT_FRONT_STEER_CAN_ID = 2; // *
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // *
public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // *
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // *
public static final int LEFT_BACK_STEER_CAN_ID = 6; // *
public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // *
public static final int RIGHT_BACK_STEER_CAN_ID = 8; // *
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // *
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // *
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // *
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; // *
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // *
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
public static final int LEFT_BACK_STEER_CAN_ID = 6;
public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
public static final int RIGHT_BACK_STEER_CAN_ID = 8;
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
public static final int GYRO_ID = 14;
// offsets are in degrees
@@ -85,8 +85,8 @@ public final class Constants {
// swerve auto constants
public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0.1, 0.3,
new TrapezoidProfile.Constraints(Math.PI, Math.PI));
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(2.0, 0, 0.01,//0.1, 0.3,
new TrapezoidProfile.Constraints(Math.PI, Math.PI/2));
public static final boolean PATH_RECORD_VELOCITY = true;
public static final double PATH_MAX_VELOCITY = 5.0;
public static final double PATH_MAX_ACCELERATION = 5.0;
@@ -326,8 +326,8 @@ public final class Constants {
public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?)
public static final double H_FOV = 59.6;
public static final double V_FOV = 45.7;
public static final double LIME_HIXELS = 920;
public static final double LIME_VIXELS = 720;
public static final double LIME_HIXELS = 640;
public static final double LIME_VIXELS = 480;
public static final double TURRET_kP = 0.1;
public static final double POINTS_THRESHOLD = 400;
src/main/java/frc4388/robot/Robot.java
+3 -4
View File
@@ -46,7 +46,7 @@ public class Robot extends TimedRobot {
private double current;
private static DesmosServer desmosServer = new DesmosServer(8000);
// private static DesmosServer desmosServer = new DesmosServer(8000);
public static Alliance alliance;
@@ -121,7 +121,7 @@ public class Robot extends TimedRobot {
}
});
desmosServer.start();
// desmosServer.start();
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
// DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0});
}
@@ -214,8 +214,7 @@ public class Robot extends TimedRobot {
* This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {
}
public void autonomousPeriodic() {}
@Override
public void teleopInit() {
src/main/java/frc4388/robot/RobotContainer.java
+174 -107
View File
@@ -4,24 +4,42 @@
package frc4388.robot;
import java.io.File;
import java.io.IOException;
import java.io.StringWriter;
import java.nio.file.FileSystems;
import java.nio.file.StandardWatchEventKinds;
import java.nio.file.WatchEvent;
import java.nio.file.WatchKey;
import java.time.ZonedDateTime;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.List;
import java.util.Objects;
import java.util.Optional;
import java.util.logging.Logger;
import java.util.stream.Collectors;
import com.diffplug.common.base.Errors;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
import edu.wpi.first.math.Pair;
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.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.XboxController;
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.PrintCommand;
import edu.wpi.first.wpilibj2.command.NotifierCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -29,9 +47,8 @@ import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.PathRecorder;
// import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
import frc4388.robot.commands.ShooterCommands.Shoot;
import frc4388.robot.commands.ShooterCommands.TrackTarget;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Claws;
@@ -44,8 +61,9 @@ import frc4388.robot.subsystems.Storage;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.PathPlannerUtil;
import frc4388.utility.controller.ButtonBox;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.DeadbandedRawXboxController;
/**
* This class is where the bulk of the robot should be declared. Since
@@ -78,8 +96,8 @@ public class RobotContainer {
/* Autonomous */
private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive);
// Controllers
private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final static XboxController m_driverXbox = new DeadbandedRawXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedRawXboxController(OIConstants.XBOX_OPERATOR_ID);
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
public static boolean softLimits = true;
@@ -153,9 +171,11 @@ public class RobotContainer {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); }
}, m_robotClimber));
m_robotBoomBoom.setDefaultCommand(
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
);
// autoInit();
// recordInit();
}
/**
@@ -165,104 +185,117 @@ public class RobotContainer {
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Iterate over all Xbox controller buttons.
for (XboxController.Button binding : XboxController.Button.values()) {
/* ------------------------------------ Driver ------------------------------------ */
JoystickButton button = new JoystickButton(getDriverController(), binding.value);
if (binding == XboxController.Button.kLeftBumper)
/* Left Bumper > Shift Down */ button.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
else if (binding == XboxController.Button.kRightBumper)
/* Right Bumper > Shift Up */ button.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
else if (binding == XboxController.Button.kLeftStick)
/* Left Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kRightStick)
/* Right Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kA)
/* A > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kB)
/* B > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kX)
/* X > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kY)
/* Y > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kBack)
/* Back > Calibrate Odometry */ button.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
else if (binding == XboxController.Button.kStart)
/* Start > Calibrate Odometry */ button.whenPressed(m_robotSwerveDrive::resetGyro);
//! Driver Buttons
/* ------------------------------------ Operator ------------------------------------ */
button = new JoystickButton(getDriverController(), binding.value);
if (binding == XboxController.Button.kLeftBumper)
/* Left Bumper > Storage In */
button.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
else if (binding == XboxController.Button.kRightBumper)
/* Right Bumper > Storage Out */
button.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
else if (binding == XboxController.Button.kLeftStick)
/* Left Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kRightStick)
/* Right Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kA)
/* A > Spit Out Ball */
button.whileHeld(new RunCommand(m_robotTurret::gotoMidpoint, m_robotTurret))
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)));
else if (binding == XboxController.Button.kB)
/* B > Toggle Claws */ button.whenPressed(new InstantCommand(m_robotClaws::toggleClaws, m_robotClaws));
else if (binding == XboxController.Button.kX)
/* X > TEST BUTTON */ button.whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
///* X > Toggles extender in and out */ button.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
else if (binding == XboxController.Button.kY)
/* Y > TEST BUTTON */ button.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
///* Y > Full aim command */ button.whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry));
else if (binding == XboxController.Button.kBack)
/* Back > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kStart)
/* Start > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
}
// Iterate over all button box buttons.
for (ButtonBox.Button binding : ButtonBox.Button.values()) {
/* ---------------------------------- Button Box ---------------------------------- */
JoystickButton button = new JoystickButton(getButtonBox(), binding.value);
if (binding == ButtonBox.Button.kRightSwitch)
/* Right Switch > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == ButtonBox.Button.kMiddleSwitch)
/* Middle Switch > Climber and Shooter mode switching */
button.whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER))
.whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
else if (binding == ButtonBox.Button.kLeftSwitch)
/* Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender) */
button.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
// Start > Calibrate Odometry
new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
.whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
// Start > Calibrate Odometry
new JoystickButton(getDriverController(), XboxController.Button.kStart.value)
.whenPressed(m_robotSwerveDrive::resetGyro);
.whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
else if (binding == ButtonBox.Button.kRightButton)
/* Right Button > Extender Out */
button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
else if (binding == ButtonBox.Button.kLeftButton)
/* Left Button > Extender In */
button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
}
// Left Bumper > Shift Down
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
// Right Bumper > Shift Up
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
//! Operator Buttons
// Right Bumper > Storage Out
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// Left Bumper > Storage In
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// B > Toggle claws
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
.whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
// X > Toggles extender in and out
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
// A > Spit Out Ball
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
.whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret))
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)));
// Y > Full aim command
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry));
//! Test Buttons
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, false));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
//! Button Box Buttons
// Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender)
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
// Middle Switch > Climber and Shooter mode switching
new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
.whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
.whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER));
// Left Button > Extender In
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
// Left Button > Extender Out
new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
}
/**
@@ -288,8 +321,11 @@ public class RobotContainer {
if (inputs[i] instanceof String) {
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
PathPlannerState initState = (PathPlannerState) traj.sample(0);
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
PathPlannerState initState = traj.getInitialState();
Pose2d initPose = new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation);
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive));
commands.add(new PPSwerveControllerCommand(
traj,
m_robotSwerveDrive::getOdometry,
@@ -300,12 +336,13 @@ public class RobotContainer {
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive));
}
if (inputs[i] instanceof Command) {
commands.add((Command) inputs[i]);
}
}
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
commands.add(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, true), m_robotSwerveDrive));
Command[] ret = new Command[commands.size()];
ret = commands.toArray(ret);
return ret;
@@ -346,8 +383,38 @@ public class RobotContainer {
// * null,
// * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")),
// * new RunCommand(() -> m_robotIntake.runAtOutput(0.5))));
// return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset(), m_robotSwerveDrive),
// // new InstantCommand(() -> this.resetOdometry(new Pose2d())),
// new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive),
// "Diamond"));
return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), "Move Forward")); // test command
// * assume turret is already pointed towards target.
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
// new ParallelRaceGroup(
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
// new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
// ));
// return new RunCommandForTime(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, true), m_robotSwerveDrive), 1.0, true);
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.3, 0.0, 0.0}, 1.0));//,
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
// new ParallelCommandGroup(
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0)
//));
// * aim with RotateUntilTarget
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
// new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5),
// new ParallelRaceGroup(
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
// new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
// ));
}
public static XboxController getDriverController() {
src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
+63
View File
@@ -0,0 +1,63 @@
// 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.DriveCommands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
public class DriveWithInputForTime extends CommandBase {
// subsystems
SwerveDrive swerve;
double[] inputs;
// timing
long start;
long elapsed;
double duration;
/**
* DriveWithInput for a specified amount of time.
* @param inputs Inputs used in DriveWithInput (xspeed, yspeed, xrot, yrot).
* @param time Time to DriveWithInput for, in seconds.
*/
public DriveWithInputForTime(SwerveDrive swerve, double[] inputs, double duration) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerve = swerve;
this.inputs = inputs;
this.duration = duration * 1000; // ! convert seconds to milliseconds, duh
addRequirements(this.swerve);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
start = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
System.out.println("RUNNING");
elapsed = System.currentTimeMillis() - start;
this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
System.out.println("Duration: " + duration);
System.out.println("Elapsed: " + elapsed);
return ((double) elapsed >= duration);
}
}
src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java
+59
View File
@@ -0,0 +1,59 @@
// 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.DriveCommands;
import javax.swing.plaf.basic.BasicTreeUI.TreeCancelEditingAction;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.VisionObscuredException;
public class RotateUntilTarget extends CommandBase {
// subsystems
SwerveDrive swerve;
VisionOdometry visionOdometry;
double rotateSpeed;
/** Creates a new RotateUntilTarget. */
public RotateUntilTarget(SwerveDrive swerve, VisionOdometry visionOdometry, double rotateSpeed) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerve = swerve;
this.visionOdometry = visionOdometry;
this.rotateSpeed = rotateSpeed;
addRequirements(this.swerve, this.visionOdometry);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
this.visionOdometry.setLEDs(true);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
this.swerve.driveWithInput(0.0, 0.0, rotateSpeed, true);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
try { this.visionOdometry.getTargetPoints(); } catch (VisionObscuredException voe) { return false; }
return true;
// return this.visionOdometry.m_camera.getLatestResult().hasTargets();
}
}
src/main/java/frc4388/robot/commands/PlaybackDriveInput.java
+156
View File
@@ -0,0 +1,156 @@
// 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.util.Comparator;
import java.util.Map;
import java.util.Optional;
import java.util.function.Function;
import java.util.logging.Level;
import java.util.logging.Logger;
import java.util.regex.Pattern;
import java.util.stream.IntStream;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.CSV;
public class PlaybackDriveInput extends CommandBase {
private static final Logger LOGGER = Logger.getLogger(PlaybackDriveInput.class.getSimpleName());
public static class DriveInputEntry {
public double millis, leftAxisX, leftAxisY, rightAxisX;
}
private DriveInputEntry[] driveInputEntries;
private SwerveDrive swerve;
private long startTime;
/** Creates a new PlaybackDriveInput. */
public PlaybackDriveInput(SwerveDrive swerve, String autoName) {
this.swerve = swerve;
try {
// This is a helper class that allows us to read a CSV file into a Java array.
CSV<DriveInputEntry> csv = new CSV<>(DriveInputEntry::new) {
// This is a regular expression that removes all parentheses from the header of the CSV file.
private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
/**
* Removes the parentheses from the CSV header
*
* @param header The header to be sanitized.
* @return The headerSanitizer method is overriding the headerSanitizer method in the parent class.
* The parentheses.matcher(header) is matching the header with the parentheses regular
* expression. The replaceAll method is replacing all of the parentheses with an empty
* string. The super.headerSanitizer(parentheses.matcher(header).replaceAll("")) is calling
* the parent sanitizer.
*/
@Override
protected String headerSanitizer(final String header) {
return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
}
};
// This is reading the CSV file into a Java array.
driveInputEntries = csv.read(new File(Filesystem.getDeployDirectory(), autoName + ".csv").toPath());
// This is a running a helper method that is logging the contents of the table to the console on a new thread.
// ! new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(driveInputEntries, RobotBase.isSimulation()))).start();
} catch (final IOException exception) {
DriveInputEntry dummyEntry = new DriveInputEntry();
dummyEntry.millis = 0.0;
dummyEntry.leftAxisX = 0.0;
dummyEntry.leftAxisY = 0.0;
dummyEntry.rightAxisX = 0.0;
driveInputEntries = new DriveInputEntry[] { dummyEntry };
LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception);
}
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
startTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
long currentTime = System.currentTimeMillis() - startTime;
double leftAxisX = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.leftAxisX).doubleValue();
double leftAxisY = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.leftAxisY).doubleValue();
double rightAxisX = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.rightAxisX).doubleValue();
swerve.driveWithInput(leftAxisX, leftAxisY, rightAxisX, true);
}
/**
* Using the given lookup value (x) and lookup getter function, locates the nearest entries in the
* given table to be used as the lower (x0) and upper (x1) bounds for interpolation. Returns the
* interpolation (y) between the two values (y0 and y1) accquired by applying the target getter
* function to the lower and upper bounds entries.
*
* @param table An array of entries to search through.
* @param lookupValue The value to lookup in the table.
* @param lookupGetter A function that takes an entry from the table and returns .
* @param targetGetter A function that takes an E and returns a Number.
* @return The interpolated value.
*/
private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
final E closestRecord = closestEntry.getValue();
final int closestRecordIndex = closestEntry.getKey();
final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
}
/**
* If the value is in the table, return the entry. Otherwise, return the entry with the closest
* value
*
* @param table The array of values to search.
* @param value The value to search for.
* @param valueGetter A function that takes an element of the table and returns a Number to compare
* with the given value.
* @param exactMatch If true, the lookup will only return a match if the value is exactly equal to
* the value of the entry. If false, the lookup will return a match with a value closest to
* the given value.
* @return The entry with the closest value to the given value.
*/
private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
}
/**
* Given a value x, and two values x0 and x1, and two values y0 and y1, return a value y that is a
* linear interpolation of the two values y0 and y1
*
* @param x The value to interpolate.
* @param x0 the x coordinate of the lower bound to interpolate to
* @param y0 The value at x0.
* @param x1 the x-coordinate of the upper bound to interpolate to
* @param y1 The value at x1.
* @return The interpolation between y0 and y1 at x.
*/
private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/RecordDriveInput.java
+107
View File
@@ -0,0 +1,107 @@
// 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.FileOutputStream;
import java.io.PrintWriter;
import java.util.HashMap;
import java.util.function.Supplier;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
public class RecordDriveInput extends CommandBase {
private HashMap<Long, double[]> timedInput;
private SwerveDrive swerve;
private Supplier<Double> leftAxisXSupplier;
private Supplier<Double> leftAxisYSupplier;
private Supplier<Double> rightAxisXSupplier;
private String autoName;
private long startTime;
private long count;
private int nthRecord;
/** Creates a new RecordDriveInput. */
public RecordDriveInput(
SwerveDrive swerve,
Supplier<Double> leftAxisXSupplier,
Supplier<Double> leftAxisYSupplier,
Supplier<Double> rightAxisXSupplier,
String autoName,
int nthRecord)
{
this.swerve = swerve;
this.leftAxisXSupplier = leftAxisXSupplier;
this.leftAxisYSupplier = leftAxisYSupplier;
this.rightAxisXSupplier = rightAxisXSupplier;
this.autoName = autoName;
this.nthRecord = nthRecord;
timedInput = new HashMap<>();
addRequirements(this.swerve);
}
public RecordDriveInput(
SwerveDrive swerve,
Supplier<Double> leftAxisXSupplier,
Supplier<Double> leftAxisYSupplier,
Supplier<Double> rightAxisXSupplier,
String autoName)
{
this(swerve, leftAxisXSupplier, leftAxisYSupplier, rightAxisXSupplier, autoName, 1);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
startTime = System.currentTimeMillis();
count = 0;
timedInput.put((long) 0, new double[] {0, 0, 0});
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
long timeFromStart = System.currentTimeMillis() - startTime;
double[] input = {leftAxisXSupplier.get(), leftAxisYSupplier.get(), rightAxisXSupplier.get()};
if(count % (long) nthRecord == 0)
timedInput.put(timeFromStart, input);
swerve.driveWithInput(input[0], input[1], input[2], true);
count++;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
File csvOutput = new File(autoName + ".csv");
try(PrintWriter writer = new PrintWriter(csvOutput)) {
writer.println("millis,leftx,lefty,rightx");
for(long millis : timedInput.keySet())
writer.println(millis + "," + timedInput.get(millis)[0] + "," + timedInput.get(millis)[1] + "," + timedInput.get(millis)[2]);
//// writer.println(( + 1) + ",0,0,0");
writer.close();
} catch(Exception e) {
e.printStackTrace();
}
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/RunCommandForTime.java
+81
View File
@@ -0,0 +1,81 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.Subsystem;
public class RunCommandForTime extends CommandBase {
// command
Command command;
// timing
long start;
long elapsed;
double duration;
// override
boolean override;
/**
* Runs given command for given time.
* @param command Command to run.
* @param duration Time to run for, in seconds.
* @param override If true: end command when time ends, even if the command isn't finished. If false: end command when it finished and time has ended.
*/
public RunCommandForTime(Command command, double duration, boolean override) {
// Use addRequirements() here to declare subsystem dependencies.
this.command = command;
this.duration = duration * 1000; // ! convert seconds to milliseconds, duh
this.override = override;
addRequirements(this.command.getRequirements().toArray(Subsystem[]::new));
}
/**
* Runs given command for given time.
* @param command Command to run.
* @param duration Time to run for, in seconds.
*/
public RunCommandForTime(Command command, double duration) {
this(command, duration, false);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
this.start = System.currentTimeMillis();
this.command.initialize();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
System.out.println("RUNNING");
this.elapsed = System.currentTimeMillis() - this.start;
this.command.execute();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
this.command.end(interrupted);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (this.override) {
System.out.println("Duration: " + duration);
System.out.println("Elapsed: " + elapsed);
return (this.elapsed >= this.duration);
} else {
return (this.command.isFinished() && (this.elapsed >= this.duration));
}
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java
+1 -1
View File
@@ -24,6 +24,6 @@ public class Seek extends SequentialCommandGroup {
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(swerve, turret, drum, hood, visionOdometry));
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry));
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+66 -18
View File
@@ -35,6 +35,8 @@ public class TrackTarget extends CommandBase {
BoomBoom m_boomBoom;
Hood m_hood;
boolean isAuto;
static double velocity;
static double hoodPosition;
@@ -46,19 +48,35 @@ public class TrackTarget extends CommandBase {
boolean isExecuted = false;
public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
m_swerve = swerve;
// timing
boolean isAimed;
boolean timerStarted;
long startTime;
private double timeTolerance;
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) {
m_turret = turret;
m_boomBoom = boomBoom;
m_hood = hood;
m_visionOdometry = visionOdometry;
this.isAuto = isAuto;
this.timeTolerance = 1000;
addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry);
}
public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
this(turret, boomBoom, hood, visionOdometry, false);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
timerStarted = false;
startTime = 0;
velocity = 0;
hoodPosition = 0;
}
@@ -67,32 +85,34 @@ public class TrackTarget extends CommandBase {
@Override
public void execute() {
try {
m_visionOdometry.setDriverMode(false);
m_visionOdometry.setLEDs(true);
points = m_visionOdometry.getTargetPoints();
points = filterPoints(points);
// points = getFakePoints();
//// points = filterPoints(points);
Point average = VisionOdometry.averagePoint(points);
double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
output *= 2;
output *= 2.0;
m_turret.runTurretWithInput(output);
double position = m_turret.m_boomBoomRotateEncoder.getPosition();
// double position = m_turret.m_boomBoomRotateEncoder.getPosition();
if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
else
m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
true);
// if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
// Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
// m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
// else
// m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
// RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
// true);
double regressedDistance = getDistance(average.y);
// ! add 30 to the distance to get in target. May need to be adjusted
velocity = m_boomBoom.getVelocity(regressedDistance + 30);
hoodPosition = m_boomBoom.getHood(regressedDistance + 30);
// ! no longer a +30 lol -aarav
velocity = m_boomBoom.getVelocity(regressedDistance);
hoodPosition = m_boomBoom.getHood(regressedDistance);
m_boomBoom.runDrumShooterVelocityPID(velocity);
m_hood.runAngleAdjustPID(hoodPosition);
@@ -110,6 +130,9 @@ public class TrackTarget extends CommandBase {
} catch (Exception e){
e.printStackTrace();
}
// run storage
}
public ArrayList<Point> filterPoints(ArrayList<Point> points) {
@@ -129,7 +152,18 @@ public class TrackTarget extends CommandBase {
}
final double averageDist = distanceSum / points.size();
return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 1.3 * averageDist).collect(Collectors.toList());
}
public final ArrayList<Point> getFakePoints() {
ArrayList<Point> fakePoints = new ArrayList<>();
for(int i = 0; i < 10; i++) {
Point p = new Point((Math.random() * 20) - 10 + (VisionConstants.LIME_HIXELS/2), (Math.random() * 20) - 10 + (VisionConstants.LIME_VIXELS/2));
fakePoints.add(p);
}
return fakePoints;
}
public final double getDistance(double averageY) {
@@ -142,7 +176,8 @@ public class TrackTarget extends CommandBase {
double regressedDistance = distanceRegression(distance);
regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS;
SmartDashboard.putNumber("Distance from Lime 123", distance);
SmartDashboard.putNumber("Regressed Distance from Lime 123", regressedDistance);
return regressedDistance;
}
@@ -153,12 +188,25 @@ public class TrackTarget extends CommandBase {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_visionOdometry.setLEDs(false);
m_visionOdometry.setDriverMode(true);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
//// if (this.isAuto) {
//// if (targetLocked& !timerStarted) {
//// timerStarted = true;
//// startTime = System.currentTimeMillis();
//// }
//// return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
//// } else {
//// return false;
//// }
// // return isExecuted && Math.abs(output) < .1;
//// }
return false;
// return isExecuted && Math.abs(output) < .1;
}
}
src/main/java/frc4388/robot/commands/StorageCommands/Drum.java
View File
src/main/java/frc4388/robot/subsystems/BoomBoom.java
+6 -6
View File
@@ -94,9 +94,9 @@ public class BoomBoom extends SubsystemBase {
public void periodic() {
// This method will be called once per scheduler run
// speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0);
SmartDashboard.putNumber("Shooter Current", getCurrent());
SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage());
SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
// SmartDashboard.putNumber("Shooter Current", getCurrent());
// SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage());
// SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
}
public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
@@ -111,9 +111,9 @@ public class BoomBoom extends SubsystemBase {
public void runDrumShooter(double speed) {
// m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2);
m_shooterFalconLeft.set(speed);
SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent());
SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent());
// SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
// SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent());
// SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent());
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+17 -4
View File
@@ -156,6 +156,13 @@ public class SwerveDrive extends SubsystemBase {
// modules[0].setDesiredState(desiredStates[0], false);
}
public void setModuleRotationsToAngle(double angle) {
for (int i = 0; i < modules.length; i++) {
SwerveModule module = modules[i];
module.rotateToAngle(angle);
}
}
@Override
public void periodic() {
@@ -226,10 +233,16 @@ public class SwerveDrive extends SubsystemBase {
* Updates the field relative position of the robot.
*/
public void updateOdometry() {
m_poseEstimator.update( getRegGyro(),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),
Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI/2));
Rotation2d actual = new Rotation2d(-1 * m_gyro.getRotation2d().getRadians());
SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
m_poseEstimator.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),
modules[3].getState());
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+4
View File
@@ -182,6 +182,10 @@ public class SwerveModule extends SubsystemBase {
angleMotor.set(0);
}
public void rotateToAngle(double angle) {
this.angleMotor.set(TalonFXControlMode.Position, angle);
}
@Override
public void periodic() {
src/main/java/frc4388/robot/subsystems/VisionOdometry.java
+7
View File
@@ -47,6 +47,9 @@ public class VisionOdometry extends SubsystemBase {
m_camera = new PhotonCamera(VisionConstants.NAME);
m_drive = drive;
m_shooter = shooter;
setLEDs(false);
setDriverMode(true);
}
/** Gets the vision points from the limelight
@@ -93,6 +96,10 @@ public class VisionOdometry extends SubsystemBase {
m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
public void setDriverMode(boolean driverMode) {
m_camera.setDriverMode(driverMode);
}
public Point getTargetOffset() throws VisionObscuredException {
ArrayList<Point> screenPoints = getTargetPoints();