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' into experimental

Browse Source
This commit is contained in:
nathanrsxtn
2022-04-07 17:01:15 -06:00
parent 1ca28810f1 06193efd83
commit 7f5d9318ec
8 changed files with 189 additions and 259 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -2
View File
@@ -35,6 +35,7 @@ import frc4388.utility.LEDPatterns;
public final class Constants {
public static final double TICKS_PER_ROTATION_FX = 2048;
public static final double LOOP_TIME = 0.02;
public static final String DRIVE_CAN_NAME = "rio";
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 2.3;
@@ -167,9 +168,10 @@ public final class Constants {
public static final double STORAGE_SPEED = 1.0;//0.9;
}
public static final class LEDConstants {
public static final int LED_SPARK_ID = 0;
public static final int LED_SPARK_ID = 3;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.SOLID_RED;
public static final LEDPatterns SHOOTING_PATTERN = LEDPatterns.SOLID_GREEN;
}
public static final class ClimberConstants {
src/main/java/frc4388/robot/Main.java
+1
View File
@@ -35,3 +35,4 @@ public final class Main {
}
// hi ryan -aarav
// hi ryan -quinn
src/main/java/frc4388/robot/Robot.java
+2 -3
View File
@@ -14,9 +14,11 @@ import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
import frc4388.utility.RobotTime;
import frc4388.utility.Vector2D;
@@ -35,8 +37,6 @@ public class Robot extends TimedRobot {
private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer;
// private SendableChooser<Command> autoChooser = new SendableChooser<Command>();
// private double current;
// private static DesmosServer desmosServer = new DesmosServer(8000);
@@ -77,7 +77,6 @@ public class Robot extends TimedRobot {
// desmosServer.start();
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
ExtenderIntakeGroup.setDirectionToOut();
// DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0});
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+155 -240
View File
@@ -32,6 +32,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
@@ -56,6 +57,7 @@ import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Extender;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Serializer;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.subsystems.SwerveDrive;
@@ -88,7 +90,7 @@ public class RobotContainer {
public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor);
public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor);
// private final LED m_robotLED = new LED(m_robotMap.LEDController); // ! no LED makes aarav sad
private final LED m_robotLED = new LED(m_robotMap.LEDController); // ! no LED makes aarav sad
public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
@@ -122,7 +124,8 @@ public class RobotContainer {
private enum DriveMode { ON, OFF };
private DriveMode currentDriveMode = DriveMode.ON;
private SendableChooser<SequentialCommandGroup> quickAutoChooser = new SendableChooser<>();
// private SendableChooser<SequentialCommandGroup> quickAutoChooser = new SendableChooser<>();
public SendableChooser<Command> autoChooser = new SendableChooser<Command>();
/**
* SmartDash
@@ -147,85 +150,12 @@ public class RobotContainer {
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
// 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.
// var justShoot = new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
// makeTheWeirdGroup(), // * shoot
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // * stop running storage
// // ! SHOOT FIRST BALL, THEN DRIVE OFF LINE (HOPEFULLY)
// var offTheLine = new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
// makeTheWeirdGroup(), // * shoot
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
// 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}, 0.25))); // * drive off line
// // ! TWO BALL AUTO (HOPEFULLY)
// var twoBall = new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
// makeTheWeirdGroup(), // * 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 376),
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target
// makeTheWeirdGroup(), // * shoot
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)))); // * stop running storage
// // ! THREE BALL AUTO (HOPEFULLY)
// var threeBall = new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
// makeTheWeirdGroup(), // * 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_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target
// makeTheWeirdGroup(), // * 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[] {0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y}, 0.5d),
// 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 InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
// new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, secondBallPosition.toDoubleArray()), // * aim to target
// makeTheWeirdGroup(), // * shoot
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
// )));
// autoChoices = Map.of(
// "justShoot", justShoot,
// "offTheLine", offTheLine,
// "twoBall", twoBall,
// "threeBall", threeBall
// );
// SmartDashboard.putData(quickAutoChooser);
autoChooser.addOption("DriveOffLineAndShoot", driveOffLineAndShoot);
autoChooser.addOption("OneBallAuto", oneBallAuto);
autoChooser.setDefaultOption("TwoBallAuto", twoBallAuto);
autoChooser.addOption("ThreeBallAuto", threeBallAuto);
SmartDashboard.putData("AutoChooser", autoChooser);
configureButtonBindings();
/* Default Commands */
@@ -355,7 +285,7 @@ public class RobotContainer {
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0));
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); // * aim with turret to target);
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED)); // * aim with turret to target);
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
@@ -500,171 +430,156 @@ public class RobotContainer {
return seqCG;
}
// ! 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.
// * 4. try new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive);
// ? 1.0 input, 1 second: 134 inches
// ? 0.75 input, 1 second: 48 inches
// ! POSITIVE Y IS LEFT, POSITIVE X IS BACKWARDS
double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
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
// ! ball positions are "unit tested"
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.
private SequentialCommandGroup shoot(double storageRunTime) {
return new SequentialCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new ParallelCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), storageRunTime, true)
)
);
}
private SequentialCommandGroup shoot(double storageRunTime, double timeout) {
return new SequentialCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withTimeout(timeout),
new ParallelCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).until(TrackTarget::isTargetNotLocked),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), storageRunTime, true)
)
);
}
SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new ParallelCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0, true)
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
SequentialCommandGroup weirdAutoShootingGroup2 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new ParallelCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.3, true)
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
SequentialCommandGroup weirdAutoShootingGroup3 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new ParallelCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 4.0, true)
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
Command resetGyro = new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive);
Command brakeDrive = new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive);
private Command brakeStorage(double storageRunTime) {
return new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotIntake), storageRunTime, true);
}
SequentialCommandGroup extendThenAimTurret() {
return new SequentialCommandGroup(
new ExtenderIntakeGroup(m_robotIntake, m_robotExtender),
new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true) // TODO: optimize time
);
}
ParallelDeadlineGroup idleDrumUntilShootingFirstBall() {
return new ParallelDeadlineGroup(
extendThenAimTurret(),
new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom)
);
}
private ParallelDeadlineGroup intakeWithPath1(double intakeRunTime) {
return new ParallelDeadlineGroup(
new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), intakeRunTime, true), // TODO: optimize time
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
buildAuto(3.0, 3.0, "JMove1") // TODO: make faster?
);
}
private ParallelDeadlineGroup intakeWithPath2(double intakeRunTime) {
return new ParallelDeadlineGroup(
new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), intakeRunTime, true), // TODO: optimize time
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
buildAuto(3.0, 3.0, "JMove2") // TODO: make faster?
);
}
ParallelCommandGroup intakeWithPathAndTrackTarget = new ParallelCommandGroup(intakeWithPath1(3.0), weirdAutoShootingGroup2);
// ParallelCommandGroup intakeWithPath2AndTrackTarget = new ParallelCommandGroup(intakeWithPath2, weirdAutoShootingGroup3);
ParallelDeadlineGroup intakeWithPath2AndIdleShooterAndAimTurret = new ParallelDeadlineGroup(
intakeWithPath2(4.2),
new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom),
new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true)
);
// ! DRIVE OFF LINE AND SHOOT (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE TARGET)
SequentialCommandGroup driveOffLineAndShoot = new SequentialCommandGroup(
resetGyro,
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (5.0 * 12) / distancePerSecond), // * go backwards five feet
brakeDrive,
shoot(1.0),
brakeStorage(0.1)
);
// ! ONE BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE TARGET)
SequentialCommandGroup oneBallAuto = new SequentialCommandGroup(
shoot(1.0),
brakeStorage(0.1)
);
// ! TWO BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE FIRST BALL OUTSIDE THE TARMAC, BUMPERS FLUSH WITH THE EDGE)
SequentialCommandGroup twoBallAuto = new SequentialCommandGroup(
idleDrumUntilShootingFirstBall(),
shoot(1.0, 4.0), // TODO: optimize time
brakeStorage(0.1),
intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget
shoot(5.0), // TODO: optimize time
brakeStorage(0.1)
);
// ! THREE BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE FIRST BALL OUTSIDE THE TARMAC, BUMPERS FLUSH WITH THE EDGE)
SequentialCommandGroup threeBallAuto = new SequentialCommandGroup(
idleDrumUntilShootingFirstBall(),
shoot(1.0), // TODO: optimize time
brakeStorage(0.1),
intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget
shoot(2.3), // TODO: optimize time
brakeStorage(0.1),
intakeWithPath2AndIdleShooterAndAimTurret,
shoot(4.0), // TODO: optimize time
brakeStorage(0.1)
);
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// ! 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.
// * 4. try new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive);
// ? 1.0 input, 1 second: 134 inches
// ? 0.75 input, 1 second: 48 inches
// ! POSITIVE Y IS LEFT, POSITIVE X IS BACKWARDS
double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
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
// ! ball positions are "unit tested"
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, true).withName("runStorage: 1.0s")
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
SequentialCommandGroup weirdAutoShootingGroup2 = 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), 2.3, true).withName("runStorage: 1.0s")
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
SequentialCommandGroup weirdAutoShootingGroup3 = 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), 4.0, true).withName("runStorage: 1.0s")
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
// ! DRIVE BACKWARDS AND SHOOT (HOPEFULLY)
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (5.0 * 12) / distancePerSecond),//0.269), // * go backwards three feet
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
// weirdAutoShootingGroup, // * shoot
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5)); // * stop running storage
// ! TWO BALL AUTO (HOPEFULLY)
// return new SequentialCommandGroup( 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[] {-1.0, 0.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 376),
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target
// weirdAutoShootingGroup, // * shoot
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5)))); // * stop running storage
// ! DRIVE OFF LINE, THEN SHOOT BALL (HOPEFULLY)
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-0.5, 0, 0.0, 0.0}, 1.0), // * drive out of tarmac
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0, 0.0, 0.0}, 1.0), // * drive out of tarmac
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.5, 0.0, 0.0}, 1.0), // * drive out of tarmac
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, -0.5, 0.0, 0.0}, 1.0), // * drive out of tarmac
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive)); // * brake
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target
// weirdAutoShootingGroup, // * shoot
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
// );
// ! THREE BALL AUTO (HOPEFULLY)
// return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * 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_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * 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[] {0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y}, 0.5d),
// 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 InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
// 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.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 RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0))
// );
// ! PathPlanner Testing
ParallelDeadlineGroup intakeWithPath1 = new ParallelDeadlineGroup(new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), 3.0, true),
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
buildAuto(3.0, 3.0, "JMove1"));
ParallelDeadlineGroup intakeWithPath2 = new ParallelDeadlineGroup(new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), 5.0, true),
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
buildAuto(3.0, 3.0, "JMove2"));
ParallelCommandGroup extendWhileTurretIsAiming = new ParallelCommandGroup(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
ParallelCommandGroup intakeWithPathAndTrackTarget = new ParallelCommandGroup(intakeWithPath1.withName("intakeWithPath"), weirdAutoShootingGroup2.withName("weirdAutoShooting"));
// ParallelCommandGroup intakeWithPath2AndTrackTarget = new ParallelCommandGroup(intakeWithPath2, weirdAutoShootingGroup3);
ParallelDeadlineGroup intakeWithPath2AndAimTurret = new ParallelDeadlineGroup(intakeWithPath2.withName("intakeWithPath"), new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("idle turret"), new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true).withName("rotateShooter: -120: 0.7s"));
SequentialCommandGroup extendThenAimTurret = new SequentialCommandGroup(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true));
ParallelDeadlineGroup idleDrumUntilShootingFirstBall = new ParallelDeadlineGroup(extendThenAimTurret, new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("idle turret"));
// return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Move Forward"));
return new SequentialCommandGroup(idleDrumUntilShootingFirstBall,
//new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true),
// extendWhileTurretIsAiming,//new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target
weirdAutoShootingGroup.withName("weirdAutoShooting"), // * shoot
// extendWhileTurretIsAiming,
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true).withName("runStorage: 0.1s"),
// new ExtenderIntakeGroup(m_robotIntake, m_robotExtender),
intakeWithPathAndTrackTarget.withName("intakeWithPathAndTrackTarget"),
// intakeWithPath,
// weirdAutoShootingGroup2,
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true)); // * aim with turret to target); // * shoot
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true).withName("runStorage: 0.1s"),
// intakeWithPath2,
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 1.0, true),
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true),
// intakeWithPath2AndTrackTarget,
intakeWithPath2AndAimTurret.withName("intakeWithPath2AndAimTurret"),
weirdAutoShootingGroup3.withName("weirdAutoShooting"),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true).withName("runStorage: 0.1s"));
// return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Diamond"));
return autoChooser.getSelected();
}
public void switchControlMode() {
src/main/java/frc4388/robot/RobotMap.java
+2 -2
View File
@@ -44,9 +44,9 @@ public class RobotMap {
}
/* LED Subsystem */
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
// void configureLEDMotorControllers() {}
void configureLEDMotorControllers() {}
/* Swerve Subsystem */
src/main/java/frc4388/robot/commands/RunCommandForTime.java
-3
View File
@@ -56,7 +56,6 @@ public class RunCommandForTime extends CommandBase {
// 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();
}
@@ -71,8 +70,6 @@ public class RunCommandForTime extends CommandBase {
@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
+5 -4
View File
@@ -7,6 +7,7 @@ package frc4388.robot.commands.ShooterCommands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.VisionOdometry;
@@ -21,10 +22,10 @@ public class Seek extends SequentialCommandGroup {
* @author Aarav Shah
* @blame Aarav Shah (thomas did this)
*/
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) {
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, LED leds) {
// 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(turret, drum, hood, visionOdometry));
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry, leds));
}
/** Seeks.
@@ -32,9 +33,9 @@ public class Seek extends SequentialCommandGroup {
* @author Aarav Shah
* @blame Aarav Shah (thomas did this)
*/
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo) {
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, LED leds, 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));
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, fakeOdo, false, false), new TrackTarget(turret, drum, hood, visionOdometry, leds));
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+19 -4
View File
@@ -17,14 +17,17 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.RobotContainer;
import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.Vision;
import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.LEDPatterns;
import frc4388.utility.Vector2D;
import frc4388.utility.desmos.DesmosServer;
@@ -36,6 +39,7 @@ public class TrackTarget extends CommandBase {
VisionOdometry m_visionOdometry;
BoomBoom m_boomBoom;
Hood m_hood;
LED m_leds;
boolean isAuto;
@@ -44,7 +48,10 @@ public class TrackTarget extends CommandBase {
ArrayList<Point> points = new ArrayList<>();
private boolean targetLocked = false;
private static boolean targetLocked = false;
public static boolean isTargetNotLocked() {
return !targetLocked;
}
private double velocityTolerance = 300.0;
private double hoodTolerance = 5.0;
@@ -57,11 +64,12 @@ public class TrackTarget extends CommandBase {
long startTime;
private double timeTolerance;
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) {
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, LED leds, boolean isAuto) {
m_turret = turret;
m_boomBoom = boomBoom;
m_hood = hood;
m_visionOdometry = visionOdometry;
m_leds = leds;
this.isAuto = isAuto;
this.timeTolerance = 1000;
@@ -71,8 +79,8 @@ public class TrackTarget extends CommandBase {
SmartDashboard.putBoolean("Target Locked", false);
}
public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
this(turret, boomBoom, hood, visionOdometry, false);
public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, LED leds) {
this(turret, boomBoom, hood, visionOdometry, leds, false);
}
// Called when the command is initially scheduled.
@@ -134,6 +142,13 @@ public class TrackTarget extends CommandBase {
SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition);
SmartDashboard.putNumber("Vel Target Track", velocity);
SmartDashboard.putBoolean("Target Locked", targetLocked);
if (targetLocked){
m_leds.setPattern(LEDConstants.SHOOTING_PATTERN);
}
else{
m_leds.setPattern(LEDConstants.DEFAULT_PATTERN);
}
}
public ArrayList<Point> filterPoints(ArrayList<Point> points) {