diff --git a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java index decbc90..ab9d276 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java @@ -19,6 +19,8 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.drive.Drive; @@ -70,6 +72,10 @@ public static Command joystickDrive( linearMagnitude = linearMagnitude * linearMagnitude; omega = Math.copySign(omega * omega, omega); + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + // Calcaulate new linear velocity Translation2d linearVelocity = new Pose2d(new Translation2d(), linearDirection) @@ -82,7 +88,9 @@ public static Command joystickDrive( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega * drive.getMaxAngularSpeedRadPerSec(), - drive.getRotation())); + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); }, drive); } diff --git a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java index 33cc1a4..ff439c0 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java +++ b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java @@ -1,4 +1,4 @@ -// Copyright 2021-2024 FRC 6328, FRC 5829 +// Copyright 2021-2024 FRC 5829 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -24,7 +24,6 @@ public class IntakeShooterControls { private static final double DEADBAND = 0.3; - private static final double MAXINCHESPERSECOND = 3; private IntakeShooterControls() {} @@ -42,8 +41,10 @@ public static Command intakeShooterDrive( if (rightBumperSupplier.getAsBoolean()) { flywheel.runVelocity(-Constants.FlywheelConstants.shootingVelocity); - double flywheelRPM = -flywheel.getVelocityRPMBottom(); - double targetRPM = Constants.FlywheelConstants.shootingVelocity * 0.9; + double flywheelRPM = flywheel.getVelocityRPMBottom(); + double targetRPM = + Constants.FlywheelConstants.shootingVelocity + + 2000; // TODO find actual max velocity if (flywheelRPM > targetRPM) { intake.runPercentSpeed(1);