diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4af2056..89d2b77 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -30,7 +30,7 @@ public final class Constants { public static final int PIGEON_ID = 6; /* Drive Inversions */ - public static final boolean isRightMotorInverted = false; + public static final boolean isRightMotorInverted = true; public static final boolean isLeftMotorInverted = false; public static final boolean isRightArcadeInverted = false; public static final boolean isAuxPIDInverted = false; @@ -53,7 +53,7 @@ public final class Constants { /* PID Constants Drive*/ public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); 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.05, 0.0, 0, 0.55); + 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; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 1029754..4f894c9 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -105,7 +105,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - m_robotContainer.setDriveGearState(true); + //m_robotContainer.setDriveGearState(true); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); // This makes sure that the autonomous stops running when diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d2761a7..a51873d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -27,6 +27,7 @@ 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.DriveWithJoystick; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; @@ -39,6 +40,7 @@ import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; import frc4388.robot.commands.TrackTarget; +import frc4388.robot.commands.TurnDegrees; import frc4388.robot.commands.storageOutake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; @@ -109,11 +111,11 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new InstantCommand()); + .whenPressed(new TurnDegrees(90, m_robotDrive)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new InstantCommand()); + .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 6000)); // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) @@ -183,7 +185,7 @@ public class RobotContainer { //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); // return new InstantCommand(); - return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0); + return new DrivePositionMPAux(m_robotDrive, 12.0, 12.0, 2, 100.0); } TrajectoryConfig getTrajectoryConfig() { diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java index 0afb152..ea76f96 100644 --- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java +++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java @@ -29,12 +29,12 @@ public class DrivePositionMPAux extends CommandBase { * Creates a new DrivePositionMPAux. * * @param subsystem The drive subsystem - * @param cruiseVel The target velocity for the motors in units + * @param cruiseVel The target velocity for the motors in in/s * @param rampDist The distance before cruise velocity is reached in inches * @param rampRate The time to reach the cruise velocity in seconds * @param targetPos The target position */ - public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) { + public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10; @@ -86,7 +86,7 @@ public class DrivePositionMPAux extends CommandBase { @Override public boolean isFinished() { if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) { - return true; + //return true; } return false; } diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index f51621a..2811fe1 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -38,8 +38,8 @@ public class DriveWithJoystick extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double moveInput = -m_controller.getLeftYAxis(); - double steerInput = m_controller.getRightXAxis(); + double moveInput = m_controller.getRightXAxis(); + double steerInput = -m_controller.getLeftYAxis(); double moveOutput = 0; double steerOutput = 0; if (moveInput >= 0){ diff --git a/src/main/java/frc4388/robot/commands/TurnDegrees.java b/src/main/java/frc4388/robot/commands/TurnDegrees.java index 2b47050..52eb5dc 100644 --- a/src/main/java/frc4388/robot/commands/TurnDegrees.java +++ b/src/main/java/frc4388/robot/commands/TurnDegrees.java @@ -64,7 +64,7 @@ public class TurnDegrees extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) { + if ((Math.abs(m_drive.getTurnRate()) < 1) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) { return true; } return false; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index bbf917b..422d2c7 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -106,13 +106,6 @@ public class Drive extends SubsystemBase { m_pigeon.configFactoryDefault(); resetGyroYaw(); - /* flip input so forward becomes back, etc */ - m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted); - m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted); - m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted); - m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted); - m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted); - /* Config Open Loop Ramp so we don't make sudden output changes */ m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); @@ -120,10 +113,10 @@ public class Drive extends SubsystemBase { m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); /* Config Supply Current Limit (Use only for debugging) */ - m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); - m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); - m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); - m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); + //m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); + //m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); + //m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); + //m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); /* Config deadbands so that */ m_leftBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); @@ -252,6 +245,13 @@ public class Drive extends SubsystemBase { /* Set up Orchestra */ m_orchestra = new Orchestra(); + + /* flip input so forward becomes back, etc */ + m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted); + m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted); + m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted); + m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted); + //m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted); /* Set up music for drive train */ m_orchestra.addInstrument(m_leftBackMotor); @@ -322,7 +322,7 @@ public class Drive extends SubsystemBase { * using the Differential Drive class to manage the two inputs */ public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(steer, move); + m_driveTrain.arcadeDrive(move, steer); m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); } @@ -723,8 +723,8 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); - SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); - SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); + //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); + //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); @@ -756,9 +756,9 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent()); //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); + SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); @@ -766,7 +766,7 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Odometry Heading", getHeading()); //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); - //SmartDashboard.putNumber("Delta Time", m_deltaTime); + SmartDashboard.putNumber("Delta Time", m_deltaTimeMs); if (m_currentSong != m_songChooser.getSelected()){ m_currentSong = m_songChooser.getSelected();