From 5bb1dbc397324d95af0772f00e913ffe6dd8f5b1 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Tue, 11 Feb 2020 16:09:31 -0800 Subject: [PATCH 1/5] Added Neo PID loops to Shooter Subsystem for angle adjustment and rotating the shooter --- src/main/java/frc4388/robot/Constants.java | 5 ++ .../java/frc4388/robot/RobotContainer.java | 7 +++ .../frc4388/robot/subsystems/Shooter.java | 49 ++++++++++++++++++- 3 files changed, 60 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 97d4f30..12e8dd0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -71,7 +71,10 @@ public final class Constants { } public static final class ShooterConstants { + /* Motor IDs */ public static final int SHOOTER_FALCON_ID = 8; + public static final int SHOOTER_ANGLE_ADJUST_ID = -1; + public static final int SHOOTER_ROTATE_ID = -2; /* PID Constants Shooter */ public static final int SHOOTER_SLOT_IDX = 0; @@ -80,6 +83,8 @@ public final class Constants { public static final Gains SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; + public static final double NEO_UNITS_PER_REV = 42; + public static final double DEGREES_PER_ROT = 360; } public static final class ClimberConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 37a847d..6f6e29f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -134,6 +134,13 @@ public class RobotContainer { // interrupts any running command new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); + + /* TEST shooter rotate PIDs */ + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(360, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(360, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + /* TEST for both commands above */ } /** diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 169e36f..7d2b87f 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -10,19 +10,32 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.revrobotics.CANEncoder; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; -import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.ShooterConstants; public class Shooter extends SubsystemBase { public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); + public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; + // Configure PID Controllers + CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); + CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); + + CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); + CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); + double velP; /** * Creates a new Shooter subsystem. @@ -87,4 +100,38 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } } + + /* Angle Adjustment PID Control */ + public void runAngleAdjustPID(double targetAngle, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + { + // Set PID Coefficients + m_angleAdjustPIDController.setP(kP); + m_angleAdjustPIDController.setI(kI); + m_angleAdjustPIDController.setD(kD); + m_angleAdjustPIDController.setIZone(kIz); + m_angleAdjustPIDController.setFF(kF); + m_angleAdjustPIDController.setOutputRange(kminOutput, kmaxOutput); + + // Convert input angle in degrees to rotations of the motor + targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; + + m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); + } + + /* Rotate Shooter PID Control */ + public void runshooterRotatePID(double targetAngle, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + { + // Set PID Coefficients + m_shooterRotatePIDController.setP(kP); + m_shooterRotatePIDController.setI(kI); + m_shooterRotatePIDController.setD(kD); + m_shooterRotatePIDController.setIZone(kIz); + m_shooterRotatePIDController.setFF(kF); + m_shooterRotatePIDController.setOutputRange(kminOutput, kmaxOutput); + + // Convert input angle in degrees to rotations of the motor + targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; + + m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition); + } } \ No newline at end of file From 668b57a4f8a69886889f937fdac1102c706de157 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Thu, 13 Feb 2020 15:31:13 -0800 Subject: [PATCH 2/5] commit --- src/main/java/frc4388/robot/Constants.java | 10 +++++----- src/main/java/frc4388/robot/RobotContainer.java | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 12e8dd0..2d8ad15 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -66,15 +66,15 @@ public final class Constants { } public static final class IntakeConstants { - public static final int INTAKE_SPARK_ID = 9; - public static final int EXTENDER_SPARK_ID = 10; + public static final int INTAKE_SPARK_ID = -9; + public static final int EXTENDER_SPARK_ID = -10; } public static final class ShooterConstants { /* Motor IDs */ public static final int SHOOTER_FALCON_ID = 8; - public static final int SHOOTER_ANGLE_ADJUST_ID = -1; - public static final int SHOOTER_ROTATE_ID = -2; + public static final int SHOOTER_ANGLE_ADJUST_ID = 9; + public static final int SHOOTER_ROTATE_ID = 10; /* PID Constants Shooter */ public static final int SHOOTER_SLOT_IDX = 0; @@ -88,7 +88,7 @@ public final class Constants { } public static final class ClimberConstants { - public static final int CLIMBER_SPARK_ID = 10; + public static final int CLIMBER_SPARK_ID = -1; } public static final class LevelerConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6f6e29f..d6c8a4d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -76,11 +76,11 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the drum shooter in idle mode - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter)); + // m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter)); // drives the leveler with an axis input from the driver controller - m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); + // m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); // runs storage motor at 50 percent - m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + // m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); } /** @@ -136,8 +136,8 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); /* TEST shooter rotate PIDs */ - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(360, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(1, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(360, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); /* TEST for both commands above */ From 29cdf74ea12b52a182914ed21b878a0c9c79e051 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Thu, 13 Feb 2020 18:57:11 -0800 Subject: [PATCH 3/5] PIDs fully added and tested for Neos When robot is enabled once after robot code is deplyed, the program works. But, after the robot is disabled and re-enabled, the neo attempts to stay at one position and PID loops will no longer run because the Neo is focused on one position. There were also several commands taken out when testing (ie. the idle shooter command) --- src/main/java/frc4388/robot/RobotContainer.java | 4 +++- .../java/frc4388/robot/subsystems/Shooter.java | 16 ++++++++++++++++ 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d6c8a4d..1c6f0d8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -137,9 +137,11 @@ public class RobotContainer { /* TEST shooter rotate PIDs */ new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(1, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(360, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(360, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(720, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); /* TEST for both commands above */ } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 7d2b87f..527bfa4 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -41,6 +41,10 @@ public class Shooter extends SubsystemBase { * Creates a new Shooter subsystem. */ public Shooter() { + //Testing purposes reseting gyros + resetGyroAngleAdj(); + resetGyroShooterRotate(); + m_shooterFalcon.configFactoryDefault(); m_shooterFalcon.setNeutralMode(NeutralMode.Coast); @@ -134,4 +138,16 @@ public class Shooter extends SubsystemBase { m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition); } + + /* For Testing Purposes, reseting gyro for angle adjuster */ + public void resetGyroAngleAdj() + { + m_angleEncoder.setPosition(0); + } + + /* For Testing Purposes, reseting gyro for shooter rotation */ + public void resetGyroShooterRotate() + { + m_shooterRotateEncoder.setPosition(0); + } } \ No newline at end of file From 6f8c90751ca8f605214d478eb8a5aed299fc6e2c Mon Sep 17 00:00:00 2001 From: mayabartels Date: Thu, 13 Feb 2020 19:12:08 -0800 Subject: [PATCH 4/5] yeet --- src/main/java/frc4388/robot/Constants.java | 4 ++- .../java/frc4388/robot/RobotContainer.java | 9 ++++-- .../frc4388/robot/subsystems/Shooter.java | 30 +++++++++---------- 3 files changed, 24 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2d8ad15..62e9d20 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -80,8 +80,10 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); + public static final double SHOOTER_TURRET_MIN = -1.0; public static final double ENCODER_TICKS_PER_REV = 2048; public static final double NEO_UNITS_PER_REV = 42; public static final double DEGREES_PER_ROT = 360; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1c6f0d8..cc1e52b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -137,11 +137,14 @@ public class RobotContainer { /* TEST shooter rotate PIDs */ new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(360, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(360))); + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(360, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(360))); + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(720, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(720))); + /* TEST for both commands above */ } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 527bfa4..a41a19b 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -27,7 +27,7 @@ public class Shooter extends SubsystemBase { public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); - public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; + public static Gains m_shooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; // Configure PID Controllers CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); @@ -106,15 +106,15 @@ public class Shooter extends SubsystemBase { } /* Angle Adjustment PID Control */ - public void runAngleAdjustPID(double targetAngle, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + public void runAngleAdjustPID(double targetAngle) { // Set PID Coefficients - m_angleAdjustPIDController.setP(kP); - m_angleAdjustPIDController.setI(kI); - m_angleAdjustPIDController.setD(kD); - m_angleAdjustPIDController.setIZone(kIz); - m_angleAdjustPIDController.setFF(kF); - m_angleAdjustPIDController.setOutputRange(kminOutput, kmaxOutput); + m_angleAdjustPIDController.setP(m_shooterGains.m_kP); + m_angleAdjustPIDController.setI(m_shooterGains.m_kI); + m_angleAdjustPIDController.setD(m_shooterGains.m_kD); + m_angleAdjustPIDController.setIZone(m_shooterGains.m_kIzone); + m_angleAdjustPIDController.setFF(m_shooterGains.m_kF); + m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterGains.m_kPeakOutput); // Convert input angle in degrees to rotations of the motor targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; @@ -123,15 +123,15 @@ public class Shooter extends SubsystemBase { } /* Rotate Shooter PID Control */ - public void runshooterRotatePID(double targetAngle, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + public void runshooterRotatePID(double targetAngle) { // Set PID Coefficients - m_shooterRotatePIDController.setP(kP); - m_shooterRotatePIDController.setI(kI); - m_shooterRotatePIDController.setD(kD); - m_shooterRotatePIDController.setIZone(kIz); - m_shooterRotatePIDController.setFF(kF); - m_shooterRotatePIDController.setOutputRange(kminOutput, kmaxOutput); + m_shooterRotatePIDController.setP(m_shooterGains.m_kP); + m_shooterRotatePIDController.setI(m_shooterGains.m_kI); + m_shooterRotatePIDController.setD(m_shooterGains.m_kD); + m_shooterRotatePIDController.setFF(m_shooterGains.m_kF); + m_shooterRotatePIDController.setIZone(m_shooterGains.m_kIzone); + m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterGains.m_kPeakOutput); // Convert input angle in degrees to rotations of the motor targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; From 2f7660b938f619f9973c23a120abcd4a706e2a80 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 15 Feb 2020 13:02:10 -0700 Subject: [PATCH 5/5] Update RobotContainer.java --- src/main/java/frc4388/robot/RobotContainer.java | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index cc1e52b..f82ffd8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -134,18 +134,6 @@ public class RobotContainer { // interrupts any running command new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); - - /* TEST shooter rotate PIDs */ - new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(360))); - - new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(360))); - - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(720))); - - /* TEST for both commands above */ } /**