mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal
This commit is contained in:
66945
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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}]}
|
||||
@@ -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;
|
||||
|
||||
@@ -213,8 +213,7 @@ public class Robot extends TimedRobot {
|
||||
* This function is called periodically during autonomous.
|
||||
*/
|
||||
@Override
|
||||
public void autonomousPeriodic() {
|
||||
}
|
||||
public void autonomousPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
|
||||
@@ -46,6 +46,8 @@ 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.Trajectory.State;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
@@ -57,8 +59,10 @@ import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.NotifierCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.subsystems.Claws;
|
||||
@@ -71,7 +75,8 @@ import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
|
||||
import frc4388.robot.commands.ClimberCommands.RunClaw;
|
||||
import frc4388.robot.commands.ClimberCommands.RunClimberPath;
|
||||
// import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
|
||||
import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
|
||||
import frc4388.robot.commands.DriveCommands.RotateUntilTarget;
|
||||
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
|
||||
import frc4388.robot.commands.ShooterCommands.AimToCenter;
|
||||
import frc4388.robot.commands.ShooterCommands.Seek;
|
||||
@@ -97,6 +102,7 @@ import frc4388.utility.PathPlannerUtil;
|
||||
import frc4388.utility.Vector2D;
|
||||
import frc4388.utility.PathPlannerUtil.Path.Waypoint;
|
||||
import frc4388.utility.controller.ButtonBox;
|
||||
import frc4388.utility.controller.DeadbandedRawXboxController;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
|
||||
/**
|
||||
@@ -128,8 +134,8 @@ public class RobotContainer {
|
||||
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
|
||||
|
||||
// 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);
|
||||
|
||||
// Autonomous
|
||||
@@ -218,6 +224,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))
|
||||
);
|
||||
|
||||
// autoInit();
|
||||
// recordInit();
|
||||
}
|
||||
@@ -267,8 +278,8 @@ public class RobotContainer {
|
||||
.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));
|
||||
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)
|
||||
@@ -281,11 +292,11 @@ public class RobotContainer {
|
||||
|
||||
|
||||
//! 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)
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
|
||||
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
|
||||
.whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
|
||||
@@ -366,8 +377,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,
|
||||
@@ -378,12 +392,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;
|
||||
@@ -416,6 +431,13 @@ public class RobotContainer {
|
||||
// }).withName("No Autonomous Path");
|
||||
// }
|
||||
|
||||
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
||||
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
||||
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
||||
// thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
loadPath("Move Forward");
|
||||
|
||||
// ! this will run each of the specified PathPlanner paths in sequence.
|
||||
// * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
|
||||
|
||||
@@ -424,8 +446,30 @@ 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)
|
||||
));
|
||||
|
||||
// TODO: we should test TrackTarget timing with my RunCommandForTime thing at some point, same with DriveWithInput timing
|
||||
|
||||
// * 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() {
|
||||
@@ -558,7 +602,7 @@ public class RobotContainer {
|
||||
}
|
||||
|
||||
private void saveRecording() {
|
||||
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
|
||||
// ! IMPORTANT: Had to chown the pathplanner folder in order to save autos.
|
||||
File outputFile = PATHPLANNER_DIRECTORY
|
||||
.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
|
||||
LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
|
||||
@@ -579,8 +623,7 @@ public class RobotContainer {
|
||||
public void recordPeriodic() {
|
||||
Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
|
||||
Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
|
||||
// FIXME: Chassis speeds are created from joystick inputs and do not reflect
|
||||
// actual robot velocity.
|
||||
// * FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity
|
||||
Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0],
|
||||
m_robotSwerveDrive.getChassisSpeeds()[1]);
|
||||
Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
|
||||
|
||||
@@ -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 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() {
|
||||
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() {
|
||||
return (elapsed >= duration);
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,78 @@
|
||||
// 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() {
|
||||
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) {
|
||||
return (this.elapsed >= this.duration);
|
||||
} else {
|
||||
return (this.command.isFinished() && (this.elapsed >= this.duration));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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,33 @@ 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 = filterPoints(points);
|
||||
Point average = VisionOdometry.averagePoint(points);
|
||||
|
||||
double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
|
||||
output *= 2;
|
||||
|
||||
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,27 +129,30 @@ public class TrackTarget extends CommandBase {
|
||||
} catch (Exception e){
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
// run storage
|
||||
|
||||
}
|
||||
|
||||
public ArrayList<Point> filterPoints(ArrayList<Point> points) {
|
||||
Point average = VisionOdometry.averagePoint(points);
|
||||
// public ArrayList<Point> filterPoints(ArrayList<Point> points) {
|
||||
// Point average = VisionOdometry.averagePoint(points);
|
||||
|
||||
HashMap<Point, Double> pointDistances = new HashMap<>();
|
||||
double distanceSum = 0;
|
||||
// HashMap<Point, Double> pointDistances = new HashMap<>();
|
||||
// double distanceSum = 0;
|
||||
|
||||
for(Point point : points) {
|
||||
Vector2D difference = new Vector2D(point);
|
||||
difference.subtract(new Vector2D(average));
|
||||
// for(Point point : points) {
|
||||
// Vector2D difference = new Vector2D(point);
|
||||
// difference.subtract(new Vector2D(average));
|
||||
|
||||
double mag = difference.magnitude();
|
||||
distanceSum += mag;
|
||||
// double mag = difference.magnitude();
|
||||
// distanceSum += mag;
|
||||
|
||||
pointDistances.put(point, mag);
|
||||
}
|
||||
// pointDistances.put(point, mag);
|
||||
// }
|
||||
|
||||
final double averageDist = distanceSum / points.size();
|
||||
return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
|
||||
}
|
||||
// final double averageDist = distanceSum / points.size();
|
||||
// return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
|
||||
// }
|
||||
|
||||
public final double getDistance(double averageY) {
|
||||
double y_rot = averageY / VisionConstants.LIME_VIXELS;
|
||||
@@ -142,7 +164,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 +176,22 @@ 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() {
|
||||
return false;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -158,6 +158,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() {
|
||||
|
||||
@@ -228,10 +235,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());
|
||||
}
|
||||
|
||||
|
||||
@@ -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() {
|
||||
|
||||
|
||||
@@ -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();
|
||||
|
||||
|
||||
Reference in New Issue
Block a user