diff --git a/.Glass/glass.json b/.Glass/glass.json new file mode 100644 index 0000000..c966180 --- /dev/null +++ b/.Glass/glass.json @@ -0,0 +1,23 @@ +{ + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Drivetrain": { + "open": true + }, + "open": true + } + }, + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Autonomous": "String Chooser" + } + }, + "NetworkTables Info": { + "visible": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "694" + } +} diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index f04c0cc..6330dd5 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -10,6 +10,7 @@ import com.stuypulse.robot.commands.drivetrain.DrivetrainDrive; import com.stuypulse.robot.commands.drivetrain.DrivetrainDriveForever; import com.stuypulse.robot.commands.launcher.LaunchPrepare; +import com.stuypulse.robot.commands.launcher.LauncherLaunch; import com.stuypulse.robot.commands.launcher.LauncherIntakeNote; import com.stuypulse.robot.commands.launcher.LauncherStop; import com.stuypulse.robot.commands.odometry.OdometryRealign; @@ -24,6 +25,7 @@ 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.WaitCommand; public class RobotContainer { @@ -33,7 +35,7 @@ public class RobotContainer { // Subsystem public final AbstractDrivetrain drivetrain = AbstractDrivetrain.getInstance(); - // public final Launcher launcher = Launcher.getInstance(); + public final Launcher launcher = Launcher.getInstance(); //public final AbstractOdometry odometry = AbstractOdometry.getInstance(); //public final AbstractVision vision = AbstractVision.getInstance(); @@ -66,23 +68,17 @@ private void configureButtonBindings() { } private void configureDriverBindings() { - operator.getLeftBumper() - .onTrue(new LauncherIntakeNote()) + driver.getRightBumper() + .whileTrue(new LauncherIntakeNote()) .onFalse(new LauncherStop()); - operator.getRightBumper() + driver.getBottomButton() .onTrue(new LaunchPrepare()) - .whileTrue(new LauncherIntakeNote()) + .whileTrue(new WaitCommand(0.5).andThen(new LauncherLaunch())) .onFalse(new LauncherStop()); } private void configureOperatorBindings() { - // odometry - //driver.getDPadUp().onTrue(new OdometryRealign(Rotation2d.fromDegrees(180))); - //driver.getDPadLeft().onTrue(new OdometryRealign(Rotation2d.fromDegrees(-90))); - //driver.getDPadDown().onTrue(new OdometryRealign(Rotation2d.fromDegrees(0))); - //driver.getDPadRight().onTrue(new OdometryRealign(Rotation2d.fromDegrees(90))); - driver.getLeftButton().onTrue(new DrivetrainDriveForever(2)); } diff --git a/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDrive.java b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDrive.java index b2d2483..c4e5a6d 100644 --- a/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDrive.java @@ -19,18 +19,17 @@ public class DrivetrainDrive extends Command { public DrivetrainDrive(Gamepad driver) { this.drivetrain = AbstractDrivetrain.getInstance(); - this.speed = IStream.create( - () -> driver.getRightY() - driver.getLeftY()) + this.speed = IStream.create(() -> driver.getRightTrigger() - driver.getLeftTrigger()) .filtered( x -> SLMath.deadband(x, 0), x -> SLMath.spow(x, 2) ); - this.angle = IStream.create( - () -> driver.getRightX() - driver.getLeftX()) + this.angle = IStream.create(() -> driver.getLeftX()) .filtered( x -> SLMath.deadband(x, 0), - x -> SLMath.spow(x, 2) + x -> SLMath.spow(x, 2), + x -> -x ); addRequirements(drivetrain); @@ -39,6 +38,6 @@ public DrivetrainDrive(Gamepad driver) { @Override public void execute() { - drivetrain.arcadeDrive(speed.get(), angle.get()); + drivetrain.curvatureDrive(speed.get(), angle.get(), true); } } diff --git a/src/main/java/com/stuypulse/robot/commands/launcher/LaunchNote.java b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherLaunch.java similarity index 80% rename from src/main/java/com/stuypulse/robot/commands/launcher/LaunchNote.java rename to src/main/java/com/stuypulse/robot/commands/launcher/LauncherLaunch.java index 351bff1..bb1b12d 100644 --- a/src/main/java/com/stuypulse/robot/commands/launcher/LaunchNote.java +++ b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherLaunch.java @@ -4,10 +4,10 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; -public class LaunchNote extends InstantCommand { +public class LauncherLaunch extends InstantCommand { Launcher launcher; - public LaunchNote() { + public LauncherLaunch() { launcher = Launcher.getInstance(); addRequirements(launcher); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/AbstractDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/AbstractDrivetrain.java index 7bc74f5..2a094d0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/AbstractDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/AbstractDrivetrain.java @@ -29,10 +29,11 @@ public static AbstractDrivetrain getInstance() { public abstract void tankDriveVolts(double leftVolts, double rightVolts); public abstract void arcadeDrive(double speed, double angle); - // public abstract void curvatureDrive(double speed, double angle, boolean isQuickTurn); + public abstract void curvatureDrive(double speed, double angle, boolean isQuickTurn); + public abstract void stop(); - public abstract void periodicChild(); + public void periodicChild() {} @Override public void periodic() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java index 59f6616..cc6caec 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java @@ -77,7 +77,7 @@ public Angle getAngle() { //********** Drive Methods **********// public void tankDriveVolts(double leftVolts, double rightVolts) { leftFront.setVoltage(leftVolts); - rightFront.setVoltage(-rightVolts); + rightFront.setVoltage(rightVolts); drivetrain.feed(); }