diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9b77168..5981c2f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; -import frc4388.robot.commands.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveStraightAtVelocityPID; @@ -68,9 +67,8 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - // test command to spin the robot while pressing A on the driver controller new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whileHeld(() -> m_robotDrive.driveWithInput(0, 1), m_robotDrive); + .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36)); /* Operator Buttons */ // activates "Lit Mode" @@ -80,7 +78,7 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 1000)) + .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500)) .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java deleted file mode 100644 index 7d241b6..0000000 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ /dev/null @@ -1,72 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands; - -import com.ctre.phoenix.motorcontrol.FollowerType; -import com.ctre.phoenix.motorcontrol.NeutralMode; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; - -public class DriveAtVelocityPID extends CommandBase { - Drive m_drive; - double m_targetGyro; - double m_targetVel; - double m_leftTarget; - double m_rightTarget; - double m_copiedTargetVel; - /** - * Creates a new DriveAtVelocityPID. - * @param subsystem drive subsystem - * @param distance target velocity in inches/second - */ - public DriveAtVelocityPID(Drive subsystem, double targetVel) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_targetVel = targetVel * DriveConstants.TICKS_PER_INCH/DriveConstants.SECONDS_TO_TICK_TIME; - m_copiedTargetVel = targetVel; - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_leftTarget = m_targetVel; - m_rightTarget = m_targetVel; - m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); - m_drive.setDriveTrainNeutralMode(NeutralMode.Coast); - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_drive.runVelocityPID(-20000, m_targetGyro); - - SmartDashboard.putNumber("Input Target Velocity", m_copiedTargetVel); - SmartDashboard.putNumber("Output Target Velocity", m_targetVel); - - SmartDashboard.putNumber("Target Angle", m_targetGyro); - //m_drive.runVelocityPID(m_leftTarget); - //m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_drive.setDriveTrainNeutralMode(NeutralMode.Coast); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java index 8179bed..e965036 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java @@ -35,7 +35,7 @@ public class DriveStraightAtVelocityPID extends CommandBase { @Override public void execute() { System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); - m_drive.runVelocityPID(m_targetVel, m_targetGyro); + m_drive.runDriveStraightVelocityPID(-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 03e27d4..a07846f 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -14,38 +14,33 @@ import frc4388.robot.subsystems.Drive; public class DriveStraightToPositionPID extends CommandBase { Drive m_drive; - double m_distance; - double m_leftTarget; - double m_rightTarget; + double m_targetPos; + double m_targetGyro; /** * Creates a new DriveToDistancePID. * @param subsystem drive subsystem - * @param distance distance to travel in inches + * @param targetPos distance to travel in inches */ - public DriveStraightToPositionPID(Drive subsystem, double distance) { + public DriveStraightToPositionPID(Drive subsystem, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_distance = distance * DriveConstants.TICKS_PER_INCH; + m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH; addRequirements(m_drive); - SmartDashboard.putNumber("Distance Target Inches", distance); + SmartDashboard.putNumber("Distance Target Inches", targetPos); } // Called when the command is initially scheduled. @Override public void initialize() { - m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; - m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance); - SmartDashboard.putNumber("Distance Target Ticks", m_distance); - SmartDashboard.putNumber("Left Target Ticks", m_leftTarget); - SmartDashboard.putNumber("Right Target Ticks", m_rightTarget); + m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - //m_drive.runPositionPID(m_drive.m_leftFrontMotor, m_leftTarget); - //m_drive.runPositionPID(m_drive.m_rightFrontMotor, m_rightTarget); + System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + m_drive.runDriveStraightPositionPID(m_targetPos, m_targetGyro); } // Called once the command ends or is interrupted. @@ -56,7 +51,7 @@ public class DriveStraightToPositionPID extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){ + if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_targetPos) < 500){ return true; } else { return false; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 70f425d..6a79518 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -93,7 +93,14 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.kP, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.kI, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); /* Setup Sensors for WPI_TalonFXs */ m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); @@ -313,21 +320,25 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } - public void runPositionPID(WPI_TalonFX talon, double targetPos) { - talon.selectProfileSlot(DriveConstants.SLOT_DISTANCE , DriveConstants.PID_PRIMARY); - talon.set(TalonFXControlMode.Position, targetPos); + public void runDriveStraightPositionPID(double targetPos, double targetGyro) { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + + targetPos *= 2; + m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro); + m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); m_driveTrain.feedWatchdog(); } - long last = 0; - public void runVelocityPID(double targetVel, double targetGyro) { + public void runDriveStraightVelocityPID(double targetVel, double targetGyro) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + targetVel *= 2; m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro); - //m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + m_driveTrain.feedWatchdog(); } @@ -345,7 +356,7 @@ public class Drive extends SubsystemBase { */ public void runTurningPID(double targetAngle){ double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV; - runVelocityPID(0, targetGyro); + runDriveStraightVelocityPID(0, targetGyro); } public double getGyroYaw() {