diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5fb5bc2..5054477 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -55,8 +55,8 @@ public final class Constants { public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0); public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55); public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); - public static final int DRIVE_CRUISE_VELOCITY = 20000; - public static final int DRIVE_ACCELERATION = 7000; + public static final int DRIVE_CRUISE_VELOCITY = 25000; + public static final int DRIVE_ACCELERATION = 21000; public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5); public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1b5b04c..4576358 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,6 +30,8 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DrivePositionMPAux; import frc4388.robot.commands.DriveStraightAtVelocityPID; +import frc4388.robot.commands.DriveStraightToPositionMM; +import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; @@ -114,19 +116,19 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new TurnDegrees(90, m_robotDrive)); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, -192)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 6000)); + .whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 6000)); // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new InstantCommand()); + .whenPressed(new DrivePositionMPAux(m_robotDrive, 12.0, 3.0, 1, 36.0)); // X driver test button new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whenPressed(new InstantCommand()); + .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36)); /* Driver Buttons */ // sets solenoids into high gear @@ -192,25 +194,25 @@ public class RobotContainer { return new SequentialCommandGroup(new Wait(2, m_robotDrive), //add aim command //add shooter command - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0), - new ParallelCommandGroup( + new DriveStraightToPositionMM(m_robotDrive, 48.0), + /*new ParallelCommandGroup( new StorageIntakeGroup(m_robotIntake, m_robotStorage), - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)), + new DriveStraightToPositionMM(m_robotDrive, 36.0)), new ParallelCommandGroup( new StorageIntakeGroup(m_robotIntake, m_robotStorage), - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)), - new StorageIntakeGroup(m_robotIntake, m_robotStorage), + new DriveStraightToPositionMM(m_robotDrive, 36.0)), + new StorageIntakeGroup(m_robotIntake, m_robotStorage),*/ //add aim command //add shooter command //Below this would be the picking up additional balls outside of those in the trench directly behind us - new TurnDegrees(-150, m_robotDrive), - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 96.0), - new StorageIntakeGroup(m_robotIntake, m_robotStorage), - new TurnDegrees(75, m_robotDrive), - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 18.0), - new TurnDegrees(-45, m_robotDrive), - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.0)); + new TurnDegrees(m_robotDrive, -90), + new DriveStraightToPositionMM(m_robotDrive, 96.0));//, + //new StorageIntakeGroup(m_robotIntake, m_robotStorage), + //new TurnDegrees(m_robotDrive, 75), + //new DriveStraightToPositionMM(m_robotDrive, 18.0), + //new TurnDegrees(m_robotDrive, -45), + //new DriveStraightToPositionMM(m_robotDrive, 12.0)); } TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java index ea76f96..68d8390 100644 --- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java +++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java @@ -66,13 +66,13 @@ public class DrivePositionMPAux extends CommandBase { if (System.currentTimeMillis() - m_startTime < m_rampRate) { // Ramping m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs; - m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro); + m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro); } else if (m_targetPos - m_currentPos > m_rampDist) { // Cruising - m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro); + m_drive.runDriveVelocityPID(m_cruiseVel, m_targetGyro); } else { // Deramp PID - m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro); + m_drive.runDrivePositionPID(m_targetPos, m_targetGyro); } m_counter ++; } diff --git a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java index 14cc97e..c79ccbc 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java @@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase { @Override public void execute() { //System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); - m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro); + m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index 949a4bf..9d84087 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -58,7 +58,7 @@ public class DriveStraightToPositionPID extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 5){ + if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 10){ return true; } else { i++; diff --git a/src/main/java/frc4388/robot/commands/TurnDegrees.java b/src/main/java/frc4388/robot/commands/TurnDegrees.java index 52eb5dc..83630dd 100644 --- a/src/main/java/frc4388/robot/commands/TurnDegrees.java +++ b/src/main/java/frc4388/robot/commands/TurnDegrees.java @@ -24,7 +24,7 @@ public class TurnDegrees extends CommandBase { /** * Creates a new TurnDeg. */ - public TurnDegrees(double targetAngle, Drive subsystem) { + public TurnDegrees(Drive subsystem, double targetAngle) { // Use addRequirements() here to declare subsystem dependencies. m_targetAngle = targetAngle;