From 101cfb5d10f496315ce7bfaff4ba003cfed626dc Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Jan 2020 14:29:30 -0800 Subject: [PATCH] Finished Elevator Robot Challenge --- src/main/java/frc4388/robot/Constants.java | 8 +++ .../java/frc4388/robot/RobotContainer.java | 4 ++ .../robot/commands/DistanceElevatorPID.java | 56 +++++++++++++++++++ .../robot/commands/DriveAtVelocityPID.java | 4 +- .../robot/commands/DriveToDistanceMM.java | 4 +- .../robot/commands/DriveToDistancePID.java | 4 +- .../java/frc4388/robot/subsystems/Drive.java | 6 +- .../frc4388/robot/subsystems/Elevator.java | 28 ++++++++++ 8 files changed, 105 insertions(+), 9 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/DistanceElevatorPID.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b3409f0..2714b3b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -41,6 +41,14 @@ public final class Constants { 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.2, 0.0, 0.0, 0.2, 0, 1.0); + + public static final double ENCODER_TICKS_PER_REV = 2048; } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index eb93394..25f9528 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,6 +15,7 @@ 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.DistanceElevatorPID; import frc4388.robot.commands.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; @@ -94,6 +95,9 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + .whenPressed(new DistanceElevatorPID(m_robotElevator, 20000)); + new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive)); } diff --git a/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java b/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java new file mode 100644 index 0000000..10644ca --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Elevator; + +public class DistanceElevatorPID extends CommandBase { + Elevator m_elevator; + double m_distance; + double m_target1; + double m_target2; + /** + * Creates a new DistanceElevatorPID. + */ + public DistanceElevatorPID(Elevator subsystem, double distance) { + // Use addRequirements() here to declare subsystem dependencies. + m_elevator = subsystem; + m_distance = distance; + addRequirements(m_elevator); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_target1 = m_elevator.m_talon1.getActiveTrajectoryPosition() + m_distance; + m_target2 = m_elevator.m_talon2.getActiveTrajectoryPosition() + m_distance; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_elevator.runElevatorPositionPID(m_elevator.m_talon1, m_target1); + m_elevator.runElevatorPositionPID(m_elevator.m_talon2, m_target2); + } + + // 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_elevator.m_talon1.getActiveTrajectoryPosition() - m_target1) < 100) { + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java index 70e54dd..3d3e96c 100644 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -35,8 +35,8 @@ public class DriveAtVelocityPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.runVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget); + 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. diff --git a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java index 0faf431..df7ff44 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java @@ -36,8 +36,8 @@ public class DriveToDistanceMM extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget); + 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. diff --git a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java index 3fe0ea0..79b27e9 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java @@ -39,8 +39,8 @@ public class DriveToDistancePID extends CommandBase { // 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); + 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. diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 2ad83d9..833f132 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -170,15 +170,15 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } - public void runPositionPID(WPI_TalonFX talon, double targetPos) { + public void runDrivePositionPID(WPI_TalonFX talon, double targetPos) { talon.set(TalonFXControlMode.Position, targetPos); } - public void runVelocityPID(WPI_TalonFX talon, double targetVel) { + public void runDriveVelocityPID(WPI_TalonFX talon, double targetVel) { talon.set(TalonFXControlMode.Velocity, targetVel); } - public void runMotionMagicPID(WPI_TalonFX talon, double targetPos){ + public void runDriveMotionMagicPID(WPI_TalonFX talon, double targetPos){ talon.set(TalonFXControlMode.MotionMagic, targetPos); } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 4af5ef1..4423466 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -7,11 +7,13 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Gains; import frc4388.robot.Constants.ElevatorConstants; public class Elevator extends SubsystemBase { @@ -19,13 +21,16 @@ public class Elevator extends SubsystemBase { public WPI_TalonSRX m_talon1 = new WPI_TalonSRX(ElevatorConstants.TALON_1); public WPI_TalonSRX m_talon2 = new WPI_TalonSRX(ElevatorConstants.TALON_2); + public static Gains m_gains = ElevatorConstants.ELEVATOR_GAINS; /** * Creates a new Elevator. */ public Elevator() { + //resets motors m_talon1.configFactoryDefault(); m_talon2.configFactoryDefault(); + //config following settings m_talon2.follow(m_talon1); m_talon1.setNeutralMode(NeutralMode.Brake); @@ -35,6 +40,13 @@ public class Elevator extends SubsystemBase { m_talon2.setInverted(false); m_talon1.setInverted(InvertType.FollowMaster); m_talon2.setInverted(InvertType.FollowMaster); + + m_talon1.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + + int closedLoopTimeMs = 1; + m_talon1.configClosedLoopPeriod(0, closedLoopTimeMs, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.configClosedLoopPeriod(1, closedLoopTimeMs, ElevatorConstants.ELEVATOR_TIMEOUT_MS); } @Override @@ -45,4 +57,20 @@ public class Elevator extends SubsystemBase { m_talon1.set(speed); m_talon2.set(speed); } + public void setElevatorGains(){ + m_talon1.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX); + m_talon1.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + + m_talon2.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX); + m_talon2.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + } + public void runElevatorPositionPID(WPI_TalonSRX talon, double targetPos) { + talon.set(ControlMode.Position, targetPos); + } }