From e96ccc527af57e008deeabc2b736d6be955b0ea5 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 16:44:05 -0700 Subject: [PATCH] more work done (getting closer) --- build.gradle | 2 +- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 6 ++--- .../robot/commands/DriveAtVelocityPID.java | 5 +++- .../java/frc4388/robot/subsystems/Drive.java | 27 +++++++------------ 5 files changed, 19 insertions(+), 23 deletions(-) diff --git a/build.gradle b/build.gradle index e00a044..e0194f3 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2020.1.2" + id "edu.wpi.first.GradleRIO" version "2020.2.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7608c30..4c96e86 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 DRIVE_TIMEOUT_MS = 30; public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.3); - public static final Gains DRIVE_TURNING_GAINS = new Gains(0.01, 0.0, 0.0, 0.0, 0, 0.3); + public static final Gains DRIVE_TURNING_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.3); public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); public static final int DRIVE_CRUISE_VELOCITY = 15000; public static final int DRIVE_ACCELERATION = 6000; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5d2b182..ba1ef76 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -79,16 +79,16 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whileHeld(new DriveAtVelocityPID(m_robotDrive, 20)); + .whenPressed(new DriveAtVelocityPID(m_robotDrive, 0)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive)); + .whileHeld(new RunCommand(() -> m_robotDrive.driveWithInput(0, 0.3), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) - .whenPressed(new InstantCommand(() -> System.err.print("interrupt"), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotDrive.getCurrentCommand().cancel(), m_robotDrive)); } /** diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java index a1e7501..68fd295 100644 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -8,6 +8,7 @@ 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; @@ -40,12 +41,13 @@ public class DriveAtVelocityPID extends CommandBase { 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(m_targetVel, 0); + m_drive.runVelocityPID(m_targetVel, m_targetGyro+1000); SmartDashboard.putNumber("Input Target Velocity", m_copiedTargetVel); SmartDashboard.putNumber("Output Target Velocity", m_targetVel); @@ -58,6 +60,7 @@ public class DriveAtVelocityPID extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + m_drive.setDriveTrainNeutralMode(NeutralMode.Brake); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 6fcf8b3..272bc54 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -98,19 +98,19 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */ - m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorSum, + m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); /* Scale Feedback by 0.5 to half the sum of Distance */ - m_rightFrontMotor.configSelectedFeedbackCoefficient( 0.5, // Coefficient + m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient DriveConstants.PID_PRIMARY, // PID Slot of Source DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference, + m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS); @@ -133,10 +133,6 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.setInverted(false); m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); - m_rightFrontMotor.setSensorPhase(false); - m_leftFrontMotor.setSensorPhase(false); - /*m_rightBackMotor.setSensorPhase(true); - m_leftBackMotor.setSensorPhase(true);*/ /* Set status frame periods to ensure we don't have stale data */ m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS); @@ -174,10 +170,10 @@ public class Drive extends SubsystemBase { /* PID */ Gains gains = m_chooser.getSelected(); - SmartDashboard.putNumber("P Value Drive", gains.kP); - SmartDashboard.putNumber("I Value Drive", gains.kI); - SmartDashboard.putNumber("D Value Drive", gains.kD); - SmartDashboard.putNumber("F Value Drive", gains.kF); + Shuffleboard.getTab("PID").add("P Value Drive", gains.kP); + Shuffleboard.getTab("PID").add("I Value Drive", gains.kI); + Shuffleboard.getTab("PID").add("D Value Drive", gains.kD); + Shuffleboard.getTab("PID").add("F Value Drive", gains.kF); /** * Max out the peak output (for all modes). @@ -310,7 +306,7 @@ public class Drive extends SubsystemBase { * Add your docs here. */ public void driveWithInput(double move, double steer){ - m_leftFrontMotor.setInverted(false); + m_rightFrontMotor.setInverted(false); m_driveTrain.arcadeDrive(move, steer); } @@ -321,13 +317,10 @@ public class Drive extends SubsystemBase { public void runVelocityPID(double targetVel, double targetGyro) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - //m_leftFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - //m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetVelLeft, DemandType.AuxPID, 0); - //m_leftFrontMotor.setInverted(true); - m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro); + m_rightFrontMotor.setInverted(false); + m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, targetVel, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - //m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, 0); } public void runMotionMagicPID(WPI_TalonFX talon, double targetPos){