From 0252e6edf85c05f1eb9db69382014ad9ea9dee21 Mon Sep 17 00:00:00 2001 From: Moso Lee Date: Sun, 27 Feb 2022 13:47:57 -0500 Subject: [PATCH] LEFT_BUMPER is now changed to drive straight tankDrive --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/oi/DriverOI.java | 12 +++-- .../robot/subsystems/ChassisSubsystem.java | 54 ++++++++++++++----- 3 files changed, 50 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 16ba26d..4d1162a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -80,6 +80,7 @@ private Constants() {} public static final double DRIVE_GEAR_RATIO = 10.71; public static final double DRIVE_WHEEL_DIAMETER = 6; + public static final int GYRO_NOTUSED = -1; //PID Value public static final double DRIVETRAIN_KP=0.06; diff --git a/src/main/java/frc/robot/oi/DriverOI.java b/src/main/java/frc/robot/oi/DriverOI.java index 651fba5..de2c710 100755 --- a/src/main/java/frc/robot/oi/DriverOI.java +++ b/src/main/java/frc/robot/oi/DriverOI.java @@ -6,10 +6,11 @@ /*----------------------------------------------------------------------------*/ package frc.robot.oi; -//import edu.wpi.first.wpilibj.buttons.JoystickButton; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants; import frc.robot.RobotContainer; +import frc.robot.subsystems.ChassisSubsystem; /** * Driver OI Controls @@ -18,10 +19,15 @@ public class DriverOI extends OI { public DriverOI(int channel, RobotContainer robotContainer) { super(channel); - + a = new JoystickButton(joystick, Constants.A); y = new JoystickButton(joystick, Constants.Y); b = new JoystickButton(joystick, Constants.B); - a = new JoystickButton(joystick, Constants.A); + x = new JoystickButton(joystick, Constants.X); + + ChassisSubsystem chassisSubsystem = robotContainer.getChassisSubsystem(); + leftBumper = new JoystickButton(joystick, Constants.LEFT_BUMPER); + leftBumper.whenPressed(new InstantCommand(chassisSubsystem::startDriveStraight, chassisSubsystem)); + leftBumper.whenReleased(new InstantCommand(chassisSubsystem::endDriveStraight, chassisSubsystem)); } } diff --git a/src/main/java/frc/robot/subsystems/ChassisSubsystem.java b/src/main/java/frc/robot/subsystems/ChassisSubsystem.java index 8a12f08..9eb20e4 100644 --- a/src/main/java/frc/robot/subsystems/ChassisSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ChassisSubsystem.java @@ -8,6 +8,8 @@ import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.RelativeEncoder; + +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -25,9 +27,10 @@ public enum RobotSide { private CANSparkMax frontLeftMotor = new CANSparkMax(Constants.FRONT_LEFT_MOTOR, MotorType.kBrushless); private CANSparkMax frontRightMotor = new CANSparkMax(Constants.FRONT_RIGHT_MOTOR, MotorType.kBrushless); private CANSparkMax rearLeftMotor = new CANSparkMax(Constants.REAR_LEFT_MOTOR, MotorType.kBrushless); - private CANSparkMax rearRightMotor = new CANSparkMax(Constants.REAR_RIGHT_MOTOR, MotorType.kBrushless); + private CANSparkMax rearRightMotor = new CANSparkMax(Constants.REAR_RIGHT_MOTOR, MotorType.kBrushless); // The gyro sensor private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro(); + private final PIDController controller = new PIDController(Constants.DRIVETRAIN_KP, Constants.DRIVETRAIN_KI, Constants.DRIVETRAIN_KD); // Encoder declarations private RelativeEncoder frontLeftEncoder; @@ -36,7 +39,8 @@ public enum RobotSide { private RelativeEncoder rearRightEncoder; private DifferentialDrive driveTrain; - + private double m_gyroAngle = Constants.GYRO_NOTUSED; + //drive constants /** * The scaling factor between the joystick value and the speed controller @@ -51,7 +55,7 @@ public enum RobotSide { /** * The scale factor for crawl mode */ - private static final double crawl = 0.3; + private static final double crawl = 0.3; /** * The minimum (closest to 0) speed controller command for the right side of the drive train to start moving forward. Must be empirically derived. @@ -82,7 +86,7 @@ public enum RobotSide { //arcade drive constant private double turnGain = 0.75; - + /** * The direction which is "forward"; 1 represents the hatch side and -1 represents the cargo side. */ @@ -115,7 +119,8 @@ public ChassisSubsystem() { rearRightEncoder = rearRightMotor.getEncoder(); driveTrain = new DifferentialDrive(frontLeftMotor, frontRightMotor); - frontRightMotor.setInverted(true); + frontRightMotor.setInverted(true); + m_gyroAngle = Constants.GYRO_NOTUSED; } @Override @@ -128,7 +133,7 @@ public void periodic() { SmartDashboard.putNumber("Rear Right Motor RPM", rearRightEncoder.getVelocity()); SmartDashboard.putNumber("Left POS", frontLeftEncoder.getPosition()); SmartDashboard.putNumber("Right POS", frontRightEncoder.getPosition()); - SmartDashboard.putNumber("Gyro angle", m_gyro.getAngle()); + SmartDashboard.putNumber("Gyro angle", m_gyro.getAngle()); } /** @@ -149,6 +154,15 @@ public void arcadeDrive(double speed, double rotation) { driveTrain.arcadeDrive(speed, rotation); } + public void startDriveStraight() { + resetGyro(); + m_gyroAngle = m_gyro.getAngle(); + controller.setTolerance(1, 5); + } + public void endDriveStraight() { + m_gyroAngle = Constants.GYRO_NOTUSED; + } + /** * drive - set speed to chassis with the joystick input with a scale * @param driverOI @@ -156,12 +170,13 @@ public void arcadeDrive(double speed, double rotation) { */ public void drive(RobotContainer robotContainer, DriverOI driverOI, int scale) { speedMultiplier = driverOI.getJoystick().getRawButton(Constants.RIGHT_BUMPER) ? crawl : normal; - dir = driverOI.getJoystick().getRawButton(Constants.LEFT_BUMPER) ? Constants.OUT : Constants.IN; - + // LEFT_BUMPER is now changed to drive straight tankDrive. + //dir = driverOI.getJoystick().getRawButton(Constants.LEFT_BUMPER) ? Constants.OUT : Constants.IN; + RobotContainer.DriveTrainType driveTrainType = robotContainer.getDriveTrainType(); if (driveTrainType == RobotContainer.DriveTrainType.TANK) { - double rightVal; + double rightVal; double leftVal; if(dir == Constants.IN) { rightVal = getScaledValue(driverOI.getJoystick().getRawAxis(Constants.RIGHT_JOYSTICK_Y), scale, RobotSide.RIGHT); @@ -170,9 +185,21 @@ public void drive(RobotContainer robotContainer, DriverOI driverOI, int scale) { rightVal = getScaledValue(driverOI.getJoystick().getRawAxis(Constants.LEFT_JOYSTICK_Y), scale, RobotSide.RIGHT); leftVal = getScaledValue(driverOI.getJoystick().getRawAxis(Constants.RIGHT_JOYSTICK_Y), scale, RobotSide.LEFT); } - SmartDashboard.putNumber("Left Speed", leftVal); - SmartDashboard.putNumber("Right Speed", rightVal); - tankDrive(leftVal * speedMultiplier * dir, rightVal * speedMultiplier * dir); + if(m_gyroAngle != Constants.GYRO_NOTUSED) + { + var pidOutput = controller.calculate(getGyroAngle(), 0) / 10; + + var speed = leftVal * speedMultiplier * dir; + SmartDashboard.putNumber("Left Speed", speed+pidOutput); + SmartDashboard.putNumber("Right Speed", speed-pidOutput); + tankDrive(speed + pidOutput, speed - pidOutput); + } + else + { + SmartDashboard.putNumber("Left Speed", leftVal); + SmartDashboard.putNumber("Right Speed", rightVal); + tankDrive(leftVal * speedMultiplier * dir, rightVal * speedMultiplier * dir); + } } else if (driveTrainType == RobotContainer.DriveTrainType.ARCADE) { double speed = getScaledValue(driverOI.getJoystick().getRawAxis(Constants.LEFT_JOYSTICK_Y), scale, RobotSide.LEFT) * dir; double rotation = getScaledValue(driverOI.getJoystick().getRawAxis(Constants.LEFT_JOYSTICK_X), scale, RobotSide.RIGHT); @@ -184,7 +211,7 @@ public void drive(RobotContainer robotContainer, DriverOI driverOI, int scale) { /** * Find the scale factor based on the input value and scale on each side of the Robot's motors - * + * * @param val * @param scale * @param side @@ -247,7 +274,6 @@ public void resetEncoders() { public double getLeftDistanceInch() { return frontLeftEncoder.getPosition() / Constants.DRIVE_GEAR_RATIO * Math.PI * Constants.DRIVE_WHEEL_DIAMETER; - } public double getRightDistanceInch() {