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

three ball auto (hopefully)

Browse Source
This commit is contained in:
aarav18
2022-03-24 00:30:25 -06:00
parent e1c01041d4
commit 0e556e3d07
5 changed files with 98 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+58 -13
View File
@@ -51,6 +51,7 @@ 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;
@@ -64,6 +65,7 @@ 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;
@@ -359,28 +361,71 @@ public class RobotContainer {
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);
// // * 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.
// ! 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
// ! 134 inches: 1.0 input, 1 second
// ! 48 inches: 0.75, 1 second
// ? positive leftY went left, negative leftY went right.
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.
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.
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
// ! 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 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 ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake
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))
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.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))
// );
}
src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
+5 -1
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
@@ -45,7 +49,7 @@ public class DriveWithInputForTime extends CommandBase {
public void execute() {
System.out.println("RUNNING");
elapsed = System.currentTimeMillis() - start;
this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], false);
this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true);
}
// Called once the command ends or is interrupted.
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/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};
}
}