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

Merge branch 'cleanup' of https://github.com/Team4388/2022NoWayHome into cleanup

Browse Source
This commit is contained in:
nathanrsxtn
2022-03-24 06:54:18 -06:00
parent d11949bcae 0e556e3d07
commit 5f537548cd
9 changed files with 128 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
-3
View File
@@ -64,9 +64,6 @@ public class Robot extends TimedRobot {
LOGGER.log(Level.FINE, "Logging Test 6/8");
LOGGER.log(Level.FINER, "Logging Test 7/8");
LOGGER.log(Level.FINEST, "Logging Test 8/8");
Errors.log().run(() -> {
throw new Throwable("Exception Test");
});
// var path =
// PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move
src/main/java/frc4388/robot/RobotContainer.java
+67 -55
View File
@@ -40,6 +40,7 @@ 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.NotifierCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -47,8 +48,10 @@ import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.PathRecorder;
import frc4388.robot.commands.RunCommandForTime;
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;
@@ -62,8 +65,10 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.PathPlannerUtil;
import frc4388.utility.Vector2D;
import frc4388.utility.controller.ButtonBox;
import frc4388.utility.controller.DeadbandedRawXboxController;
import frc4388.utility.controller.DeadbandedXboxController;
/**
* This class is where the bulk of the robot should be declared. Since
@@ -96,8 +101,8 @@ public class RobotContainer {
/* Autonomous */
private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive);
// Controllers
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 static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
public static boolean softLimits = true;
@@ -354,67 +359,74 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// if (loadedPathTrajectory != null) {
// PIDController xController = SwerveDriveConstants.X_CONTROLLER;
// PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
// ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
// thetaController.enableContinuousInput(-Math.PI, Math.PI);
// PathPlannerState initialState = loadedPathTrajectory.getInitialState();
// Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
// return new SequentialCommandGroup(
// new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
// new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)),
// new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry,
// m_robotSwerveDrive.m_kinematics, xController, yController, thetaController,
// m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
// new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
// } else {
// LOGGER.severe("No auto selected.");
// return new RunCommand(() -> {
// }).withName("No Autonomous Path");
// }
// ! ways to not coast
// // * 1. try zero joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.0, 0.0, false), m_robotSwerveDrive);
// * 2. try opposite joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive);
// * 3a. try permanently setting drive motors to brake, not coast, in RobotMap.java, and ask the driver how it feels.
// * 3b. try to only set the drive motors to brake if in auto mode.
// ! this will run each of the specified PathPlanner paths in sequence.
// * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
// ! 1.0 input, 1 second: 134 inches
// ! 0.75 input, 1 second: 48 inches
// ? positive leftY went left, negative leftY went right?
// TODO: if line 372 is true, switch joystick inputs accordingly
// ! this will run each of the specified PathPlanner paths in sequence, while simultaneously running the intake throughout all the paths.
// * return new ParallelCommandGroup(buildAuto(null,
// * 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"));
double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
// * assume turret is already pointed towards target.
double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0
double offset = 10.0; // * distance (in inches) from ball that we actually want to stop
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub.
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.
SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(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)
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
// ! THREE BALL AUTO (HOPEFULLY)
return new SequentialCommandGroup( new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), // * aim with turret to target
weirdAutoShootingGroup, // * shoot
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake
new ParallelCommandGroup( new RunCommand(() -> m_robotIntake.runAtOutput(1.0), m_robotIntake), // * run intake all throughout path
new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball
new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 363)
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), // * aim with turret to target
weirdAutoShootingGroup, // * shoot
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y, true), m_robotSwerveDrive), // * rotate so intake points towards second ball
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-firstToSecond.unit().x, -firstToSecond.unit().y, 0.0, 0.0}, (firstToSecond.magnitude() - offset) / distancePerSecond), // * drive to second ball
new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(firstToSecond.unit().x, firstToSecond.unit().y, 0.0, 0.0, true)), // * brake (see line 363)
new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, secondBallPosition.toDoubleArray()), // * aim to target
weirdAutoShootingGroup, // * shoot
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
)));
// 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 DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, 1.0),
// new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive),
// 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 RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
// ));
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0))
// 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
+4
View File
@@ -27,6 +27,10 @@ public class DriveWithInputForTime extends CommandBase {
public DriveWithInputForTime(SwerveDrive swerve, double[] inputs, double duration) {
// Use addRequirements() here to declare subsystem dependencies.
if (inputs.length != 4) {
throw new IllegalArgumentException();
}
this.swerve = swerve;
this.inputs = inputs;
this.duration = duration * 1000; // ! convert seconds to milliseconds, duh
src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java
+11
View File
@@ -26,4 +26,15 @@ public class Seek extends SequentialCommandGroup {
// addCommands(new FooCommand(), new BarCommand());
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry));
}
/** Seeks.
* Seeking -> Sought
* @author Aarav Shah
* @blame Aarav Shah (thomas did this)
*/
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, fakeOdo, false, false), new TrackTarget(turret, drum, hood, visionOdometry));
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+20 -5
View File
@@ -4,8 +4,11 @@
package frc4388.robot.commands.ShooterCommands;
import java.util.Objects;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants;
@@ -60,6 +63,8 @@ public class Shoot extends CommandBase {
private boolean endsWithLimelight;
private double[] fakeOdo;
/**
* Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
*
@@ -94,13 +99,23 @@ public class Shoot extends CommandBase {
isAimedInTolerance = false;
this.inverted = 1;
this.fakeOdo = null;
addRequirements(this.swerve, this.drum, this.turret, this.hood, this.visionOdometry);
}
// public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) {
// this(swerve, drum, turret, hood, false);
// }
public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo, boolean toShoot, boolean endsWithLime) {
this(swerve, drum, turret, hood, visionOdometry, toShoot, endsWithLime);
if (fakeOdo.length != 2) {
throw new IllegalArgumentException();
}
this.fakeOdo = fakeOdo;
}
public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo) {
this(swerve, drum, turret, hood, visionOdometry, false, false);
}
/**
* Updates error for custom PID.
@@ -127,8 +142,8 @@ public class Shoot extends CommandBase {
timerStarted = false;
startTime = 0;
this.odoX = -this.swerve.getOdometry().getY(); // 3.2766
this.odoY = -this.swerve.getOdometry().getX(); // -0.9398
this.odoX = Objects.requireNonNullElse(Units.inchesToMeters(this.fakeOdo[0]), -this.swerve.getOdometry().getY());
this.odoY = Objects.requireNonNullElse(Units.inchesToMeters(this.fakeOdo[1]), -this.swerve.getOdometry().getX());
this.distance = Math.hypot(odoX, odoY);
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+16 -17
View File
@@ -79,15 +79,14 @@ public class TrackTarget extends CommandBase {
velocity = 0;
hoodPosition = 0;
m_visionOdometry.setDriverMode(false);
m_visionOdometry.setLEDs(true);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
try {
m_visionOdometry.setDriverMode(false);
m_visionOdometry.setLEDs(true);
points = m_visionOdometry.getTargetPoints();
// points = getFakePoints();
//// points = filterPoints(points);
@@ -114,8 +113,8 @@ public class TrackTarget extends CommandBase {
velocity = m_boomBoom.getVelocity(regressedDistance);
hoodPosition = m_boomBoom.getHood(regressedDistance);
m_boomBoom.runDrumShooterVelocityPID(velocity);
m_hood.runAngleAdjustPID(hoodPosition);
m_boomBoom.runDrumShooterVelocityPID(velocity + 20);
m_hood.runAngleAdjustPID(hoodPosition + 20);
double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity();
double currentHood = this.m_hood.getEncoderPosition();
@@ -195,18 +194,18 @@ public class TrackTarget extends CommandBase {
// 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;
//// }
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 false;
}
}
src/main/java/frc4388/robot/subsystems/Claws.java
+5 -2
View File
@@ -30,12 +30,14 @@ public class Claws extends SubsystemBase {
private double m_rightOffset;
private boolean m_open;
private boolean clawsOpen;
public static enum ClawType {LEFT, RIGHT}
public Claws(Servo leftClaw, Servo rightClaw) {
m_leftClaw = leftClaw;
m_rightClaw = rightClaw;
m_open = false;
clawsOpen = false;
}
/**
@@ -46,11 +48,11 @@ public class Claws extends SubsystemBase {
if(open) {
m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 900);
m_rightClaw.setRaw(ClawConstants.TOP_LIMIT + 100);
SmartDashboard.putBoolean("Claws Closed", false);
clawsOpen = false;
} else {
m_leftClaw.setRaw(ClawConstants.TOP_LIMIT - 400);
m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 400);
SmartDashboard.putBoolean("Claws Closed", true);
clawsOpen = true;
}
}
@@ -62,5 +64,6 @@ public class Claws extends SubsystemBase {
@Override
public void periodic() {
SmartDashboard.putBoolean("Claws Closed", clawsOpen);
}
}
src/main/java/frc4388/robot/subsystems/Hood.java
+1 -1
View File
@@ -64,7 +64,7 @@ public class Hood extends SubsystemBase {
// SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition());
// * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
runVelocityRamping();
// runVelocityRamping();
}
public void runVelocityRamping() {
src/main/java/frc4388/utility/Vector2D.java
+4
View File
@@ -155,4 +155,8 @@ public class Vector2D extends Vector2d {
public String toString() {
return "<" + this.x + ", " + this.y + ">";
}
public double[] toDoubleArray() {
return new double[] {this.x, this.y};
}
}