From cfee098d057e63662465da2a4b6c5627479cbdcd Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 3 Feb 2020 16:40:39 -0700 Subject: [PATCH] Revert "Reverted Drive code to master" This reverts commit 4dd70bf1fe4fe9156c4883f7b248151f8fe50f42. --- src/main/java/frc4388/robot/Constants.java | 18 +++- .../java/frc4388/robot/RobotContainer.java | 12 +++ .../robot/commands/DriveAtVelocityPID.java | 52 +++++++++ .../robot/commands/DriveToDistanceMM.java | 57 ++++++++++ .../robot/commands/DriveToDistancePID.java | 60 +++++++++++ .../java/frc4388/robot/subsystems/Drive.java | 101 +++++++++++++++++- 6 files changed, 294 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java create mode 100644 src/main/java/frc4388/robot/commands/DriveToDistanceMM.java create mode 100644 src/main/java/frc4388/robot/commands/DriveToDistancePID.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e0c6f24..c5de68c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -25,7 +25,12 @@ public final class Constants { public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; public static final int PIGEON_ID = 6; - + /* PID Constants Drive*/ + public static final int DRIVE_SLOT_IDX = 0; + public static final int DRIVE_PID_LOOP_IDX = 0; + public static final int DRIVE_TIMEOUT_MS = 30; + public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); + public static final double ENCODER_TICKS_PER_REV = 2048; } @@ -33,7 +38,18 @@ public final class Constants { public static final int INTAKE_SPARK_ID = 1; } + public static final class ElevatorConstants { + public static final int TALON_1 = 7; + public static final int TALON_2 = 8; + /* PID Constants Elevator */ + public static final int ELEVATOR_SLOT_IDX = 0; + public static final int ELEVATOR_PID_LOOP_IDX = 1; + public static final int ELEVATOR_TIMEOUT_MS = 30; + public static final Gains ELEVATOR_GAINS = new Gains(0.1, 0.0, 0.0, 0.2, 0, 1.0); + + public static final double ENCODER_TICKS_PER_REV = 2048; + } public static final class ShooterConstants { public static final int SHOOTER_FALCON_ID = 8; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fea891a..9c5eb38 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,6 +15,9 @@ 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.DriveToDistancePID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; @@ -78,6 +81,15 @@ public class RobotContainer { .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) + .whenPressed(new DriveToDistancePID(m_robotDrive, 5000)); + + new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) + .whenPressed(new DriveToDistanceMM(m_robotDrive, 5000)); + + new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 8200)); diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java new file mode 100644 index 0000000..3d3e96c --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* 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 edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Drive; + +public class DriveAtVelocityPID extends CommandBase { + Drive m_drive; + double m_targetVel; + double m_leftTarget; + double m_rightTarget; + /** + * Creates a new DriveAtVelocityPID. + */ + public DriveAtVelocityPID(Drive subsystem, double targetVel) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_targetVel = targetVel; + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_leftTarget = m_targetVel; + m_rightTarget = -m_targetVel; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.runDriveVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.runDriveVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java new file mode 100644 index 0000000..df7ff44 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* 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 edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Drive; + +public class DriveToDistanceMM extends CommandBase { + Drive m_drive; + double m_distance; + double m_leftTarget; + double m_rightTarget; + + /** + * Creates a new DriveToDistancePID. + */ + public DriveToDistanceMM(Drive subsystem, double distance) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_distance = distance; + addRequirements(m_drive); + } + + // 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); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.runDriveMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.runDriveMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){ + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java new file mode 100644 index 0000000..79b27e9 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java @@ -0,0 +1,60 @@ +/*----------------------------------------------------------------------------*/ +/* 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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Drive; + +public class DriveToDistancePID extends CommandBase { + Drive m_drive; + double m_distance; + double m_leftTarget; + double m_rightTarget; + + /** + * Creates a new DriveToDistancePID. + */ + public DriveToDistancePID(Drive subsystem, double distance) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_distance = distance; + addRequirements(m_drive); + } + + // 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("Left Target", m_leftTarget); + SmartDashboard.putNumber("Right Target", m_rightTarget); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.runDrivePositionPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.runDrivePositionPID(m_drive.m_rightFrontMotor, m_rightTarget); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){ + 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 ef20818..9ccb252 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -34,6 +34,8 @@ public class Drive extends SubsystemBase { public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + public static Gains m_driveGains = DriveConstants.DRIVE_GAINS; + /** * Add your docs here. */ @@ -51,17 +53,63 @@ public class Drive extends SubsystemBase { m_rightBackMotor.follow(m_rightFrontMotor); /* set neutral mode */ - setDriveTrainNeutralMode(NeutralMode.Coast); + setDriveTrainNeutralMode(NeutralMode.Brake); m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); - m_rightFrontMotor.setInverted(true); - m_driveTrain.setRightSideInverted(true); + m_rightFrontMotor.setInverted(false); m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); + + /* Motor Encoders */ + //m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); + + /*m_leftFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); + + m_leftFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);*/ + + setDriveTrainGains(); + + m_leftFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS); + + m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); + + /* Smart Dashboard Initial Values */ + SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); + SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); + SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + + SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition()); + + SmartDashboard.putNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); + SmartDashboard.putNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); + SmartDashboard.putNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); + SmartDashboard.putNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + + int closedLoopTimeMs = 1; + m_leftFrontMotor.configClosedLoopPeriod(0, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configClosedLoopPeriod(1, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + + } @Override @@ -70,9 +118,23 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + + SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); + SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); + + m_driveGains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); + m_driveGains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); + m_driveGains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); + m_driveGains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + + setDriveTrainGains(); + } catch (Exception e) { - System.err.println("The programming team has failed successfully in the Drive Subsystem in Periodic."); - e.printStackTrace(System.err); + + System.err.println("The programming team has failed successfully in the Drive Subsystem."); + } } @@ -87,6 +149,23 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.setNeutralMode(mode); } + /** + * Add your docs here. + */ + public void setDriveTrainGains(){ + m_leftFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); + m_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); + m_rightFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + } + /** * Add your docs here. */ @@ -94,6 +173,18 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } + public void runDrivePositionPID(WPI_TalonFX talon, double targetPos) { + talon.set(TalonFXControlMode.Position, targetPos); + } + + public void runDriveVelocityPID(WPI_TalonFX talon, double targetVel) { + talon.set(TalonFXControlMode.Velocity, targetVel); + } + + public void runDriveMotionMagicPID(WPI_TalonFX talon, double targetPos){ + talon.set(TalonFXControlMode.MotionMagic, targetPos); + } + public double getGyroYaw() { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr);