From 2eb0bf806a76635812cfdac0926a603e7336cfda Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Jan 2020 08:29:34 -0800 Subject: [PATCH 01/35] Created Elevator Subsystem Initialized motors --- .../frc4388/robot/subsystems/Elevator.java | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 src/main/java/frc4388/robot/subsystems/Elevator.java diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java new file mode 100644 index 0000000..19712e8 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Elevator extends SubsystemBase { + + public WPI_TalonSRX m_talon1; + public WPI_TalonSRX m_talon2; + + /** + * Creates a new Elevator. + */ + public Elevator() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From ca31832c6197f9e659915b74144bda56837ab2f6 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Jan 2020 10:30:22 -0800 Subject: [PATCH 02/35] Added Elevator Subystem Added method to make it move --- .../java/frc4388/robot/RobotContainer.java | 6 +++++ .../frc4388/robot/subsystems/Elevator.java | 24 ++++++++++++++++--- 2 files changed, 27 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 202f647..eb93394 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -20,6 +20,7 @@ import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Elevator; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.utility.LEDPatterns; @@ -38,6 +39,7 @@ public class RobotContainer { private final Drive m_robotDrive = new Drive(); private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); + private final Elevator m_robotElevator = new Elevator(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -56,6 +58,10 @@ public class RobotContainer { getDriverController().getRightXAxis()), m_robotDrive)); // drives motor with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); + // moves elevator with one-axis input from the driver controller + m_robotElevator.setDefaultCommand(new RunCommand(() -> m_robotElevator.moveElevator( + getOperatorController().getLeftYAxis()), m_robotElevator + )); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 19712e8..4af5ef1 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -7,24 +7,42 @@ package frc4388.robot.subsystems; +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.Constants.ElevatorConstants; public class Elevator extends SubsystemBase { - - public WPI_TalonSRX m_talon1; - public WPI_TalonSRX m_talon2; + + public WPI_TalonSRX m_talon1 = new WPI_TalonSRX(ElevatorConstants.TALON_1); + public WPI_TalonSRX m_talon2 = new WPI_TalonSRX(ElevatorConstants.TALON_2); /** * Creates a new Elevator. */ public Elevator() { + m_talon1.configFactoryDefault(); + m_talon2.configFactoryDefault(); + m_talon2.follow(m_talon1); + + m_talon1.setNeutralMode(NeutralMode.Brake); + m_talon2.setNeutralMode(NeutralMode.Brake); + + m_talon1.setInverted(false); + m_talon2.setInverted(false); + m_talon1.setInverted(InvertType.FollowMaster); + m_talon2.setInverted(InvertType.FollowMaster); } @Override public void periodic() { // This method will be called once per scheduler run } + public void moveElevator(double speed) { + m_talon1.set(speed); + m_talon2.set(speed); + } } From d4be141676055a545e1e6209804be0b4326e6d48 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Jan 2020 10:30:57 -0800 Subject: [PATCH 03/35] Changed Constants --- src/main/java/frc4388/robot/Constants.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 466ff74..b3409f0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -38,6 +38,11 @@ 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; + } + public static final class LEDConstants { public static final int LED_SPARK_ID = 0; From 101cfb5d10f496315ce7bfaff4ba003cfed626dc Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Jan 2020 14:29:30 -0800 Subject: [PATCH 04/35] 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); + } } From d9bd213ff29377c5b2dc21a2b4190e0812758e21 Mon Sep 17 00:00:00 2001 From: Kyra <101209@psdschools.org> Date: Sat, 18 Jan 2020 15:28:48 -0800 Subject: [PATCH 05/35] Create Shooter Subsystem and PID stuff Created shooter subsystem, set up command for shooter to run, set up PID constants, set up gains method in shooter. --- src/main/java/frc4388/robot/Constants.java | 12 ++++ .../java/frc4388/robot/RobotContainer.java | 11 +++- .../java/frc4388/robot/subsystems/Drive.java | 26 ++++----- .../frc4388/robot/subsystems/Elevator.java | 20 ++++--- .../frc4388/robot/subsystems/Shooter.java | 57 +++++++++++++++++++ 5 files changed, 102 insertions(+), 24 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Shooter.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2714b3b..9dbda6a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -51,6 +51,18 @@ public final class Constants { public static final double ENCODER_TICKS_PER_REV = 2048; } + public static final class ShooterConstants { + public static final int SHOOTER_FALCON_ID = 8; + + /* PID Constants Shooter */ + 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.2, 0.0, 0.0, 0.2, 0, 1.0); + + public static final double ENCODER_TICKS_PER_REV = 2048; + } + public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 25f9528..abdfb35 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -24,6 +24,7 @@ import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Elevator; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Shooter; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -41,6 +42,7 @@ public class RobotContainer { private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); private final Elevator m_robotElevator = new Elevator(); + private final Shooter m_robotShooter = new Shooter(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -57,12 +59,15 @@ public class RobotContainer { m_robotDrive.setDefaultCommand(new RunCommand(() -> m_robotDrive.driveWithInput( getDriverController().getLeftYAxis(), getDriverController().getRightXAxis()), m_robotDrive)); - // drives motor with input from triggers on the opperator controller + + // drives motor with input from triggers on the operator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); + // moves elevator with one-axis input from the driver controller m_robotElevator.setDefaultCommand(new RunCommand(() -> m_robotElevator.moveElevator( getOperatorController().getLeftYAxis()), m_robotElevator )); + // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); } @@ -85,7 +90,6 @@ public class RobotContainer { .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whenPressed(new DriveToDistancePID(m_robotDrive, 5000)); @@ -100,6 +104,9 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive)); + + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + .whileHeld(() -> m_robotShooter.runDrumShooter(1), m_robotShooter); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 833f132..df272f9 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -34,7 +34,7 @@ public class Drive extends SubsystemBase { public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - public static Gains m_gains = DriveConstants.DRIVE_GAINS; + public static Gains m_driveGains = DriveConstants.DRIVE_GAINS; /** * Add your docs here. @@ -121,10 +121,10 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - m_gains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); - m_gains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); - m_gains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); - m_gains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + 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(); @@ -151,16 +151,16 @@ public class Drive extends SubsystemBase { */ public void setDriveTrainGains(){ m_leftFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); - m_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + 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_gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + 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); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 4423466..eec7909 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -21,7 +21,7 @@ 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; + public static Gains m_elevatorGains = ElevatorConstants.ELEVATOR_GAINS; /** * Creates a new Elevator. */ @@ -41,6 +41,8 @@ public class Elevator extends SubsystemBase { m_talon1.setInverted(InvertType.FollowMaster); m_talon2.setInverted(InvertType.FollowMaster); + setElevatorGains(); + 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); @@ -59,16 +61,16 @@ public class Elevator extends SubsystemBase { } 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_talon1.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.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); + m_talon2.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); } public void runElevatorPositionPID(WPI_TalonSRX talon, double targetPos) { talon.set(ControlMode.Position, targetPos); diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java new file mode 100644 index 0000000..eb1b6d3 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Shooter.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.subsystems; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Gains; +import frc4388.robot.Constants.ShooterConstants; + +public class Shooter extends SubsystemBase { + + WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); + + public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; + + /** + * Creates a new Shooter. + */ + public Shooter() { + m_shooterFalcon.configFactoryDefault(); + + m_shooterFalcon.setNeutralMode(NeutralMode.Coast); + + m_shooterFalcon.setInverted(false); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Runs drum shooter motor. + * @param speed + */ + public void runDrumShooter(double speed) { + m_shooterFalcon.set(speed); + } + + /** + * Configures gains for shooter PID. + */ + public void setShooterGains() { + m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); + m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + } +} From de4265be232a8524c6902b8b2718856f4c4eecbd Mon Sep 17 00:00:00 2001 From: aarav18 <57469559+aarav18@users.noreply.github.com> Date: Sun, 19 Jan 2020 00:28:21 -0700 Subject: [PATCH 06/35] Added Shooter Subsystem PID Stuff Initialized gains and added velocity PID method. --- .../java/frc4388/robot/subsystems/Shooter.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index eb1b6d3..934cbae 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -29,6 +29,14 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.setNeutralMode(NeutralMode.Coast); m_shooterFalcon.setInverted(false); + + setShooterGains(); + + m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + + int closedLoopTimeMs = 1; + m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); } @Override @@ -38,7 +46,7 @@ public class Shooter extends SubsystemBase { /** * Runs drum shooter motor. - * @param speed + * @param speed Speed to set the motor at */ public void runDrumShooter(double speed) { m_shooterFalcon.set(speed); @@ -54,4 +62,12 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS); } + /** + * Runs drum shooter velocity PID. + * @param falcon Motor to use + * @param targetVel Target velocity to run motor at + */ + public void runDrumShooterVelocityPID(WPI_TalonFX falcon, double targetVel) { + falcon.set(TalonFXControlMode.Velocity, targetVel); + } } From a96cd7a3e2189695446895be06591a63b9097ffb Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 20 Jan 2020 08:59:20 -0800 Subject: [PATCH 07/35] Finished up Velocity Control PID Shooter PID --- .../java/frc4388/robot/RobotContainer.java | 6 ++- .../commands/ShooterVelocityControlPID.java | 47 +++++++++++++++++++ .../frc4388/robot/subsystems/Shooter.java | 3 +- 3 files changed, 54 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index abdfb35..d4902b2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -20,6 +20,7 @@ import frc4388.robot.commands.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; import frc4388.robot.commands.RunIntakeWithTriggers; +import frc4388.robot.commands.ShooterVelocityControlPID; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Elevator; import frc4388.robot.subsystems.Intake; @@ -99,13 +100,16 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 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)); - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) .whileHeld(() -> m_robotShooter.runDrumShooter(1), m_robotShooter); } diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java new file mode 100644 index 0000000..dcc0fb3 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Shooter; + +public class ShooterVelocityControlPID extends CommandBase { + Shooter m_shooter; + double m_targetVel; + /** + * Creates a new ShooterVelocityControlPID. + */ + public ShooterVelocityControlPID(Shooter subsystem, double targetVel) { + // Use addRequirements() here to declare subsystem dependencies. + m_shooter = subsystem; + m_targetVel = targetVel; + addRequirements(m_shooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_shooter.runDrumShooterVelocityPID(m_shooter.m_shooterFalcon, m_targetVel); + } + + // 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/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 934cbae..8e7cd9c 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -8,6 +8,7 @@ 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 edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -16,7 +17,7 @@ import frc4388.robot.Constants.ShooterConstants; public class Shooter extends SubsystemBase { - WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); + public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; From db5c15a452143a55f64a23e284e69a2fed03018b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 20 Jan 2020 15:22:44 -0700 Subject: [PATCH 08/35] Tested Shooter, Fine tuned it --- src/main/java/frc4388/robot/RobotContainer.java | 8 ++++---- .../robot/commands/ShooterVelocityControlPID.java | 2 +- src/main/java/frc4388/robot/subsystems/Shooter.java | 13 ++++++++++--- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d4902b2..9c45f3e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -101,16 +101,16 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 2000)); + .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(2300), m_robotShooter)); 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)); + /*new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) + .whenPressed(new InstantCommand(null, m_robotDrive));*/ new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - .whileHeld(() -> m_robotShooter.runDrumShooter(1), m_robotShooter); + .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooter(0.2), m_robotShooter)); } /** diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index dcc0fb3..520e501 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -31,7 +31,7 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.runDrumShooterVelocityPID(m_shooter.m_shooterFalcon, m_targetVel); + m_shooter.runDrumShooterVelocityPID(m_targetVel); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 8e7cd9c..24b7461 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -11,14 +11,18 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +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); + private double m_targetVel = 2300; + public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; /** @@ -29,7 +33,7 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.setNeutralMode(NeutralMode.Coast); - m_shooterFalcon.setInverted(false); + m_shooterFalcon.setInverted(true); setShooterGains(); @@ -38,11 +42,14 @@ public class Shooter extends SubsystemBase { int closedLoopTimeMs = 1; m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + + SmartDashboard.putNumber("Shooter Velocity", m_targetVel); } @Override public void periodic() { // This method will be called once per scheduler run + m_targetVel = SmartDashboard.getNumber("Shooter Velocity", m_targetVel); } /** @@ -68,7 +75,7 @@ public class Shooter extends SubsystemBase { * @param falcon Motor to use * @param targetVel Target velocity to run motor at */ - public void runDrumShooterVelocityPID(WPI_TalonFX falcon, double targetVel) { - falcon.set(TalonFXControlMode.Velocity, targetVel); + public void runDrumShooterVelocityPID(double targetVel) { + m_shooterFalcon.set(TalonFXControlMode.Velocity, m_targetVel*ShooterConstants.ENCODER_TICKS_PER_REV/600); } } From 7f96cecc4d95c2dbae2873f4b6b034c37fffcdb9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 21 Jan 2020 16:45:29 -0700 Subject: [PATCH 09/35] Spin up before engaging shooter pid --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../frc4388/robot/commands/ShooterVelocityControlPID.java | 6 +++++- src/main/java/frc4388/robot/subsystems/Shooter.java | 5 +++-- 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9dbda6a..835dff1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -46,7 +46,7 @@ public final class Constants { 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 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; } @@ -58,7 +58,7 @@ 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.2, 0.0, 0.0, 0.2, 0, 1.0); + public static final Gains SHOOTER_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9c45f3e..99b7d52 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -101,7 +101,7 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(2300), m_robotShooter)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 2300)); new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new DistanceElevatorPID(m_robotElevator, 20000)); diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 520e501..4c7b058 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -31,7 +31,11 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.runDrumShooterVelocityPID(m_targetVel); + if (m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity() < 1000){ + m_shooter.runDrumShooter(0.5); + } else { + m_shooter.runDrumShooterVelocityPID(m_targetVel); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 24b7461..9452170 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -43,13 +43,14 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); - SmartDashboard.putNumber("Shooter Velocity", m_targetVel); + SmartDashboard.putNumber("Shooter Velocity Target", m_targetVel); } @Override public void periodic() { // This method will be called once per scheduler run - m_targetVel = SmartDashboard.getNumber("Shooter Velocity", m_targetVel); + m_targetVel = SmartDashboard.getNumber("Shooter Velocity Target", m_targetVel); + SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV); } /** From 29593564c2bd14a63f2b88bf5db178d29403eb72 Mon Sep 17 00:00:00 2001 From: RishabhCowlagi <59751356+RishabhCowlagi@users.noreply.github.com> Date: Tue, 28 Jan 2020 17:07:49 -0800 Subject: [PATCH 10/35] Add Basic Storage Code --- src/main/java/frc4388/robot/Constants.java | 10 ++- .../frc4388/robot/subsystems/Storage.java | 48 +++++++++++++ vendordeps/REVRobotics.json | 70 +++++++++++++++++++ 3 files changed, 127 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Storage.java create mode 100644 vendordeps/REVRobotics.json diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 793ce5c..eb408fa 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,7 +29,15 @@ public final class Constants { public static final class IntakeConstants { public static final int INTAKE_SPARK_ID = 1; } - + public static final class StorageConstants { + public static final int STORAGE_TALON_ID = 9; + public static final int BEAM_SENSOR_DIO_0 = 0; + public static final int BEAM_SENSOR_DIO_1 = 1; + public static final int BEAM_SENSOR_DIO_2 = 2; + public static final int BEAM_SENSOR_DIO_3 = 3; + public static final int BEAM_SENSOR_DIO_4 = 0; + public static final int BEAM_SENSOR_DIO_5 = 0; + } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java new file mode 100644 index 0000000..7b85a0d --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems; + +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc4388.robot.Constants.StorageConstants; + +public class Storage extends SubsystemBase { + private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_TALON_ID, MotorType.kBrushless); + private DigitalInput[] m_beamSensors = new DigitalInput[6]; + /** + * Creates a new Storage. + */ + public Storage() { + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0); + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1); + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2); + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_3); + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_4); + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_5); + } + + @Override + public void periodic() { + // NO + } + + /** + * Runs storage motor + * @param input the voltage to run motor at + */ + public void runStorage(double input) { + m_storageMotor.set(input); + boolean beam_on = m_beamSensors[0].get(); + } +} diff --git a/vendordeps/REVRobotics.json b/vendordeps/REVRobotics.json new file mode 100644 index 0000000..63380b6 --- /dev/null +++ b/vendordeps/REVRobotics.json @@ -0,0 +1,70 @@ +{ + "fileName": "REVRobotics.json", + "name": "REVRobotics", + "version": "1.5.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "http://www.revrobotics.com/content/sw/max/sdk/maven/" + ], + "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-java", + "version": "1.5.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.5.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-cpp", + "version": "1.5.1", + "libName": "SparkMax", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.5.1", + "libName": "SparkMaxDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + } + ] +} \ No newline at end of file From 66893b5890d928a4aeee831aa46c2a27402a5fd1 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Tue, 28 Jan 2020 18:56:09 -0800 Subject: [PATCH 11/35] Added climber subsystem --- src/main/java/frc4388/robot/Constants.java | 4 ++ .../java/frc4388/robot/RobotContainer.java | 5 ++ .../commands/RunClimberWithTriggers.java | 63 +++++++++++++++++ .../frc4388/robot/subsystems/Climber.java | 46 ++++++++++++ vendordeps/Phoenix.json | 30 ++++---- vendordeps/REVRobotics.json | 70 +++++++++++++++++++ 6 files changed, 203 insertions(+), 15 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java create mode 100644 src/main/java/frc4388/robot/subsystems/Climber.java create mode 100644 vendordeps/REVRobotics.json diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 793ce5c..da1a079 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,6 +29,10 @@ public final class Constants { public static final class IntakeConstants { public static final int INTAKE_SPARK_ID = 1; } + + public static final class ClimberConstants { + public static final int CLIMBER_SPARK_ID = 10; + } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d52b23e..82188ef 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,7 +16,9 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DriveWithJoystick; +import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunIntakeWithTriggers; +import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -36,6 +38,7 @@ public class RobotContainer { private final Drive m_robotDrive = new Drive(); private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); + private final Climber m_robotClimber = new Climber(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -52,6 +55,8 @@ public class RobotContainer { m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); // drives motor with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); + // drives motor with input from triggers on the opperator controller + m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); } diff --git a/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java b/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java new file mode 100644 index 0000000..9ab4285 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Climber; +import frc4388.utility.controller.IHandController; + +public class RunClimberWithTriggers extends CommandBase { + private Climber m_climber; + private IHandController m_controller; + + /** + * Uses input from opperator triggers to control climber motor + * @param subsystem the climber subsystem + * @param controller the driver controller + */ + public RunClimberWithTriggers(Climber subsystem, IHandController controller) { + m_climber = subsystem; + m_controller = controller; + addRequirements(m_climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double rightTrigger = m_controller.getRightTriggerAxis(); + double leftTrigger = m_controller.getLeftTriggerAxis(); + double output = 0; + if (rightTrigger < .5) { + if(rightTrigger > leftTrigger) { + output = rightTrigger; + } + if (leftTrigger > rightTrigger) { + output = -leftTrigger; + } + } else { + output = rightTrigger; + } + m_climber.runClimber(output); + } + + // 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/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java new file mode 100644 index 0000000..de0b618 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems; + +import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ClimberConstants; + +public class Climber extends SubsystemBase { + CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless); + CANDigitalInput m_forwardLimit, m_reverseLimit; + /** + * Creates a new Climber. + */ + public Climber() { + m_climberMotor.restoreFactoryDefaults(); + + m_forwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_reverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + + m_forwardLimit.enableLimitSwitch(true); + m_reverseLimit.enableLimitSwitch(true); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Runs climber motor + * @param input the voltage to run motor at + */ + public void runClimber(double input) { + m_climberMotor.set(input); + } +} diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index f8d42a4..a633555 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.17.3", + "version": "5.17.4", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "http://devsite.ctr-electronics.com/maven/release/" @@ -11,19 +11,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.17.3" + "version": "5.17.4" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.17.3" + "version": "5.17.4" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -47,7 +47,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -69,7 +69,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -83,7 +83,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": false, @@ -97,7 +97,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": false, @@ -111,7 +111,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": false, @@ -125,7 +125,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixDiagnostics", "headerClassifier": "headers", "sharedLibrary": false, @@ -139,7 +139,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixCanutils", "headerClassifier": "headers", "sharedLibrary": false, @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixPlatform", "headerClassifier": "headers", "sharedLibrary": false, @@ -165,7 +165,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixCore", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVRobotics.json b/vendordeps/REVRobotics.json new file mode 100644 index 0000000..63380b6 --- /dev/null +++ b/vendordeps/REVRobotics.json @@ -0,0 +1,70 @@ +{ + "fileName": "REVRobotics.json", + "name": "REVRobotics", + "version": "1.5.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "http://www.revrobotics.com/content/sw/max/sdk/maven/" + ], + "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-java", + "version": "1.5.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.5.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-cpp", + "version": "1.5.1", + "libName": "SparkMax", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.5.1", + "libName": "SparkMaxDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + } + ] +} \ No newline at end of file From 42db77fe6d596ced6bc5e1303090f716a7a150ef Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 31 Jan 2020 21:39:51 -0700 Subject: [PATCH 12/35] Fix Shooter PID --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 13 +--- .../robot/commands/DistanceElevatorPID.java | 56 ------------- .../commands/ShooterVelocityControlPID.java | 5 +- .../frc4388/robot/subsystems/Elevator.java | 78 ------------------- .../frc4388/robot/subsystems/Shooter.java | 13 +++- 6 files changed, 16 insertions(+), 151 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/DistanceElevatorPID.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Elevator.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 835dff1..4ae4474 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -58,7 +58,7 @@ 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.1, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.0542, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 86be53e..e9527ae 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.DistanceElevatorPID; import frc4388.robot.commands.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; @@ -23,7 +22,6 @@ import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Elevator; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Shooter; @@ -43,7 +41,6 @@ public class RobotContainer { private final Drive m_robotDrive = new Drive(); private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); - private final Elevator m_robotElevator = new Elevator(); private final Shooter m_robotShooter = new Shooter(); /* Controllers */ @@ -61,12 +58,9 @@ public class RobotContainer { m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); // drives motor with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); - // moves elevator with one-axis input from the driver controller - m_robotElevator.setDefaultCommand(new RunCommand(() -> m_robotElevator.moveElevator( - getOperatorController().getLeftYAxis()), m_robotElevator - )); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter)); } /** @@ -97,10 +91,7 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 2300)); - - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new DistanceElevatorPID(m_robotElevator, 20000)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 3200)); /*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 deleted file mode 100644 index 10644ca..0000000 --- a/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java +++ /dev/null @@ -1,56 +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 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/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 4c7b058..dbd6603 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -31,10 +31,11 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity() < 1000){ + if (/*m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity() < 1000*/false){ m_shooter.runDrumShooter(0.5); + System.err.println("Less than 1000: " + m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); } else { - m_shooter.runDrumShooterVelocityPID(m_targetVel); + m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); } } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java deleted file mode 100644 index eec7909..0000000 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ /dev/null @@ -1,78 +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.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 { - - 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_elevatorGains = 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); - m_talon2.setNeutralMode(NeutralMode.Brake); - - m_talon1.setInverted(false); - m_talon2.setInverted(false); - m_talon1.setInverted(InvertType.FollowMaster); - m_talon2.setInverted(InvertType.FollowMaster); - - setElevatorGains(); - - 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 - public void periodic() { - // This method will be called once per scheduler run - } - public void moveElevator(double speed) { - 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_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.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_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - } - public void runElevatorPositionPID(WPI_TalonSRX talon, double targetPos) { - talon.set(ControlMode.Position, targetPos); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 9452170..5fe805a 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -25,6 +25,7 @@ public class Shooter extends SubsystemBase { public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; + double velP; /** * Creates a new Shooter. */ @@ -76,7 +77,13 @@ public class Shooter extends SubsystemBase { * @param falcon Motor to use * @param targetVel Target velocity to run motor at */ - public void runDrumShooterVelocityPID(double targetVel) { - m_shooterFalcon.set(TalonFXControlMode.Velocity, m_targetVel*ShooterConstants.ENCODER_TICKS_PER_REV/600); + public void runDrumShooterVelocityPID(double targetVel, double actualVel) { + velP = actualVel/targetVel; + if(velP < 0.1){ + velP = 0.1; + } + double runSpeed = velP*(1-velP); + System.err.println(runSpeed); + m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed/*ShooterConstants.ENCODER_TICKS_PER_REV/600*/); } -} +} \ No newline at end of file From 8e7e5218125370745c2b23bcd96419e7d169b613 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 31 Jan 2020 21:43:46 -0700 Subject: [PATCH 13/35] Update build.gradle --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 68eb59bdc687fc45d1932062f0fd90c7fca46844 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 31 Jan 2020 22:02:39 -0700 Subject: [PATCH 14/35] Fix the thing and that other thing --- .../frc4388/robot/commands/ShooterVelocityControlPID.java | 8 ++------ src/main/java/frc4388/robot/subsystems/Shooter.java | 2 +- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index dbd6603..799163b 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -31,12 +31,8 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (/*m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity() < 1000*/false){ - m_shooter.runDrumShooter(0.5); - System.err.println("Less than 1000: " + m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); - } else { - m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); - } + System.err.println(m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); + m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 5fe805a..6cfd3be 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -83,7 +83,7 @@ public class Shooter extends SubsystemBase { velP = 0.1; } double runSpeed = velP*(1-velP); - System.err.println(runSpeed); + //System.err.println(runSpeed); m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed/*ShooterConstants.ENCODER_TICKS_PER_REV/600*/); } } \ No newline at end of file From 43a454f9ded01eb4ee37234e5a8cac205184ea5a Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 1 Feb 2020 12:40:39 -0700 Subject: [PATCH 15/35] Runup --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- .../robot/commands/ShooterVelocityControlPID.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Shooter.java | 11 +++++------ 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e9527ae..153c296 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -60,7 +60,7 @@ public class RobotContainer { m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter)); + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.2), m_robotShooter)); } /** @@ -91,7 +91,7 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 3200)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 13200)); /*new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive));*/ diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 799163b..67b5cb1 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -31,8 +31,8 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - System.err.println(m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); - m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); + System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); + m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 6cfd3be..fdbc39e 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -79,11 +79,10 @@ public class Shooter extends SubsystemBase { */ public void runDrumShooterVelocityPID(double targetVel, double actualVel) { velP = actualVel/targetVel; - if(velP < 0.1){ - velP = 0.1; - } - double runSpeed = velP*(1-velP); - //System.err.println(runSpeed); - m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed/*ShooterConstants.ENCODER_TICKS_PER_REV/600*/); + double runSpeed = actualVel + (20000*velP); + if (runSpeed > targetVel) {runSpeed = targetVel;} + System.err.println(velP + " " + runSpeed); + SmartDashboard.putNumber("shoot", actualVel); + m_shooterFalcon.set(TalonFXControlMode.Velocity, runSpeed); } } \ No newline at end of file From 4dd70bf1fe4fe9156c4883f7b248151f8fe50f42 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 1 Feb 2020 13:38:23 -0700 Subject: [PATCH 16/35] Reverted Drive code to master --- 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, 6 insertions(+), 294 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java delete mode 100644 src/main/java/frc4388/robot/commands/DriveToDistanceMM.java delete 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 4ae4474..0e904eb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -25,12 +25,7 @@ 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; } @@ -38,18 +33,7 @@ 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 153c296..532edaa 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,9 +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.DriveToDistancePID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; @@ -81,15 +78,6 @@ 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, 13200)); 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 3d3e96c..0000000 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ /dev/null @@ -1,52 +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 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 deleted file mode 100644 index df7ff44..0000000 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ /dev/null @@ -1,57 +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 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 deleted file mode 100644 index 79b27e9..0000000 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ /dev/null @@ -1,60 +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 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 9ccb252..ef20818 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -34,8 +34,6 @@ 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. */ @@ -53,63 +51,17 @@ public class Drive extends SubsystemBase { m_rightBackMotor.follow(m_rightFrontMotor); /* set neutral mode */ - setDriveTrainNeutralMode(NeutralMode.Brake); + setDriveTrainNeutralMode(NeutralMode.Coast); 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(false); + m_rightFrontMotor.setInverted(true); + m_driveTrain.setRightSideInverted(true); 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 @@ -118,23 +70,9 @@ 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."); - + System.err.println("The programming team has failed successfully in the Drive Subsystem in Periodic."); + e.printStackTrace(System.err); } } @@ -149,23 +87,6 @@ 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. */ @@ -173,18 +94,6 @@ 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); From 7bb3b8b18d947dbe0eb234bdb3f0654aaf04a56e Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 1 Feb 2020 14:21:23 -0700 Subject: [PATCH 17/35] Set idle speed --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 153c296..5815b1a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -60,7 +60,7 @@ public class RobotContainer { m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.2), m_robotShooter)); + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter)); } /** From 0f5a6ef09ff1ee553e37392f6f1d065dcb9a656a Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 1 Feb 2020 16:07:59 -0700 Subject: [PATCH 18/35] PID tuning P and F --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../java/frc4388/robot/subsystems/Shooter.java | 16 ++++++++-------- 3 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 0e904eb..e0c6f24 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -42,7 +42,7 @@ 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.0, 0.0, 0.0, 0.0542, 0, 1.0); + public static final Gains SHOOTER_GAINS = new Gains(0.13, 0.0, 0.0, 0.053, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0025383..fea891a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -79,7 +79,7 @@ public class RobotContainer { .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 13200)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 8200)); /*new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive));*/ diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index fdbc39e..3c937b2 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -21,8 +21,6 @@ public class Shooter extends SubsystemBase { public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); - private double m_targetVel = 2300; - public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; double velP; @@ -43,14 +41,11 @@ public class Shooter extends SubsystemBase { int closedLoopTimeMs = 1; m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); - - SmartDashboard.putNumber("Shooter Velocity Target", m_targetVel); } @Override public void periodic() { // This method will be called once per scheduler run - m_targetVel = SmartDashboard.getNumber("Shooter Velocity Target", m_targetVel); SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV); } @@ -79,10 +74,15 @@ public class Shooter extends SubsystemBase { */ public void runDrumShooterVelocityPID(double targetVel, double actualVel) { velP = actualVel/targetVel; - double runSpeed = actualVel + (20000*velP); + double runSpeed = actualVel + (12000*velP); if (runSpeed > targetVel) {runSpeed = targetVel;} - System.err.println(velP + " " + runSpeed); SmartDashboard.putNumber("shoot", actualVel); - m_shooterFalcon.set(TalonFXControlMode.Velocity, runSpeed); + + if (actualVel < targetVel - 7000){ + m_shooterFalcon.set(TalonFXControlMode.Velocity, runSpeed); + } + else{ + m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); + } } } \ No newline at end of file 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 19/35] 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); From 9d8f047bedada579f1dd01f8aaf1e77a8e524ad3 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 4 Feb 2020 18:09:40 -0700 Subject: [PATCH 20/35] Fix Shooter --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../java/frc4388/robot/subsystems/Shooter.java | 16 +++++++++------- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c5de68c..469fcb8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -58,7 +58,7 @@ 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.13, 0.0, 0.0, 0.053, 0, 1.0); + public static final Gains SHOOTER_GAINS = new Gains(0.425, 0.0005, 13, 0.05, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9c5eb38..8831be9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -91,7 +91,7 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 8200)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); /*new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive));*/ diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 3c937b2..2e42c11 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -35,6 +35,8 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.setInverted(true); setShooterGains(); + + m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); @@ -73,16 +75,16 @@ public class Shooter extends SubsystemBase { * @param targetVel Target velocity to run motor at */ public void runDrumShooterVelocityPID(double targetVel, double actualVel) { - velP = actualVel/targetVel; - double runSpeed = actualVel + (12000*velP); - if (runSpeed > targetVel) {runSpeed = targetVel;} + velP = actualVel/targetVel; //Percent of target + double runSpeed = actualVel + (12000*velP); //Ramp up equation + //if (runSpeed > targetVel) {runSpeed = targetVel;} SmartDashboard.putNumber("shoot", actualVel); - - if (actualVel < targetVel - 7000){ - m_shooterFalcon.set(TalonFXControlMode.Velocity, runSpeed); + runSpeed = runSpeed/targetVel; //Convert to percent + if (actualVel < targetVel - 1000){ + m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed); } else{ - m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); + m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } } } \ No newline at end of file From c0ead80055877053bfa8f08af4499c552eae5e08 Mon Sep 17 00:00:00 2001 From: KyraRivera <59713772+KyraRivera@users.noreply.github.com> Date: Thu, 6 Feb 2020 19:51:17 -0700 Subject: [PATCH 21/35] Added Leveler --- src/main/java/frc4388/robot/Constants.java | 3 ++ .../java/frc4388/robot/RobotContainer.java | 5 ++ .../commands/RunLevelerWithJoystick.java | 53 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Intake.java | 1 + .../frc4388/robot/subsystems/Leveler.java | 44 +++++++++++++++ 5 files changed, 106 insertions(+) create mode 100644 src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java create mode 100644 src/main/java/frc4388/robot/subsystems/Leveler.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 793ce5c..916ada6 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,6 +29,9 @@ public final class Constants { public static final class IntakeConstants { public static final int INTAKE_SPARK_ID = 1; } + public static final class LevelerConstants { + public static final int LEVELER_TALON_ID = 9; + } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4131d4b..e2fddb7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -17,9 +17,11 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; +import frc4388.robot.commands.RunLevelerWithJoystick; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Leveler; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -36,6 +38,7 @@ public class RobotContainer { private final Drive m_robotDrive = new Drive(); private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); + private final Leveler m_robotLeveler = new Leveler(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -54,6 +57,8 @@ public class RobotContainer { m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + // drives the robot with an axis input from the driver controller + m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); } /** diff --git a/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java b/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java new file mode 100644 index 0000000..0b91068 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Leveler; +import frc4388.utility.controller.IHandController; + +public class RunLevelerWithJoystick extends CommandBase { + private Leveler m_leveler; + private IHandController m_controller; + + /** + * Creates a new RunLevelerWithJoystick to control the leveler with an Xbox controller. + * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the + * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in + * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + */ + public RunLevelerWithJoystick(Leveler subsystem, IHandController controller) { + m_leveler = subsystem; + m_controller = controller; + addRequirements(m_leveler); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double input = m_controller.getLeftXAxis(); + m_leveler.runLeveler(input); + } + + // 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/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index ea2ba9c..a1c1a4c 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -18,6 +18,7 @@ public class Intake extends SubsystemBase { * Creates a new Intake. */ public Intake() { + m_intakeMotor.setInverted(false); } diff --git a/src/main/java/frc4388/robot/subsystems/Leveler.java b/src/main/java/frc4388/robot/subsystems/Leveler.java new file mode 100644 index 0000000..613a8cb --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Leveler.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.wpilibj.Talon; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.LevelerConstants; + +public class Leveler extends SubsystemBase { + WPI_TalonSRX m_levelerMotor = new WPI_TalonSRX(LevelerConstants.LEVELER_TALON_ID); + + /** + * Creates a new Leveler. + */ + public Leveler() { + m_levelerMotor.configFactoryDefault(); + + m_levelerMotor.setNeutralMode(NeutralMode.Brake); + + m_levelerMotor.setInverted(false); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Runs intake motor + * @param input the percent output to run motor at + */ + public void runLeveler(double input) { + m_levelerMotor.set(input); + } +} From f5106f42329bbed41006eaee2cf3c78f6b545485 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 6 Feb 2020 19:57:25 -0700 Subject: [PATCH 22/35] Added command for storage to run --- .../java/frc4388/robot/RobotContainer.java | 5 +++++ .../frc4388/robot/subsystems/Storage.java | 20 +++++++++++++------ 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d52b23e..d54a017 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -20,6 +20,7 @@ import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Storage; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -36,6 +37,7 @@ public class RobotContainer { private final Drive m_robotDrive = new Drive(); private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); + private final Storage m_robotStorage = new Storage(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -54,6 +56,9 @@ public class RobotContainer { m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + // runs storage motor at 50 percent + m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.5), m_robotStorage)); + } /** diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 7b85a0d..31634ca 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -8,6 +8,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -18,18 +19,18 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.StorageConstants; public class Storage extends SubsystemBase { - private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_TALON_ID, MotorType.kBrushless); + private WPI_TalonSRX m_storageMotor = new WPI_TalonSRX(StorageConstants.STORAGE_TALON_ID); private DigitalInput[] m_beamSensors = new DigitalInput[6]; /** * Creates a new Storage. */ public Storage() { m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0); - m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1); - m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2); - m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_3); - m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_4); - m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_5); + m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1); + m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2); + m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_3); + m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_4); + m_beamSensors[5] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_5); } @Override @@ -44,5 +45,12 @@ public class Storage extends SubsystemBase { public void runStorage(double input) { m_storageMotor.set(input); boolean beam_on = m_beamSensors[0].get(); + + if (beam_on) { + System.err.println("Beam on"); + } else { + System.err.println("Beam off"); + } + } } From 8cbd39ebe8deff37f11e301a2031b6a9ab1259cf Mon Sep 17 00:00:00 2001 From: keenandbuckley Date: Sat, 8 Feb 2020 13:01:25 -0700 Subject: [PATCH 23/35] Added double solenoid and method to shift gears Has not been tested yet, waiting for solenoids to arrive. --- .../java/frc4388/robot/RobotContainer.java | 8 ++++++++ .../java/frc4388/robot/subsystems/Drive.java | 18 ++++++++++++++++++ 2 files changed, 26 insertions(+) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d9c82a3..0ad14ab 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -83,6 +83,14 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); + // sets solenoids into high gear + new JoystickButton(getDriverJoystick(), XboxController.RIGHT_TRIGGER_AXIS) + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + + // sets solenoids into low gear + new JoystickButton(getDriverJoystick(), XboxController.LEFT_TRIGGER_AXIS) + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); + //new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) // .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000, 0), m_robotDrive)); // interrupts any running command diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index ee725ad..52f3fa6 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -20,6 +20,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; +import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -49,6 +50,8 @@ public class Drive extends SubsystemBase { public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS; public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS; + public DoubleSolenoid speedShift; + /** * Add your docs here. */ @@ -61,6 +64,8 @@ public class Drive extends SubsystemBase { m_pigeon.configFactoryDefault(); resetGyroYaw(); + speedShift = new DoubleSolenoid(1,0,1); + /* set back motors as followers */ m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); @@ -414,4 +419,17 @@ public class Drive extends SubsystemBase { m_pigeon.setYaw(0); m_pigeon.setAccumZAngle(0); } + + /** + * Set to high or low gear based on boolean state, true = high, false = low + * @param state Chooses between high or low gear + */ + public void setShiftState(boolean state) { + if (state == true) { + speedShift.set(DoubleSolenoid.Value.kForward); + } + if (state == false) { + speedShift.set(DoubleSolenoid.Value.kReverse); + } + } } From 1f37fe9f4a8d67120e90ec106d81c6593b878cb2 Mon Sep 17 00:00:00 2001 From: RishabhCowlagi <59751356+RishabhCowlagi@users.noreply.github.com> Date: Sat, 8 Feb 2020 12:02:07 -0800 Subject: [PATCH 24/35] Update Motor Type --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/subsystems/Storage.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index eb408fa..25912fc 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 INTAKE_SPARK_ID = 1; } public static final class StorageConstants { - public static final int STORAGE_TALON_ID = 9; + public static final int STORAGE_CAN_ID = 10; public static final int BEAM_SENSOR_DIO_0 = 0; public static final int BEAM_SENSOR_DIO_1 = 1; public static final int BEAM_SENSOR_DIO_2 = 2; diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 31634ca..a7d1bfb 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -19,7 +19,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.StorageConstants; public class Storage extends SubsystemBase { - private WPI_TalonSRX m_storageMotor = new WPI_TalonSRX(StorageConstants.STORAGE_TALON_ID); + private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); private DigitalInput[] m_beamSensors = new DigitalInput[6]; /** * Creates a new Storage. From 33f974d6d90f9103365f7362fc6d6fb9b4f0dba3 Mon Sep 17 00:00:00 2001 From: keenandbuckley Date: Sat, 8 Feb 2020 14:05:12 -0700 Subject: [PATCH 25/35] Changed IDs to make storage subsystem work --- src/main/java/frc4388/robot/Constants.java | 6 +++--- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 25912fc..db3c0f2 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -30,13 +30,13 @@ public final class Constants { public static final int INTAKE_SPARK_ID = 1; } public static final class StorageConstants { - public static final int STORAGE_CAN_ID = 10; + public static final int STORAGE_CAN_ID = 9; public static final int BEAM_SENSOR_DIO_0 = 0; public static final int BEAM_SENSOR_DIO_1 = 1; public static final int BEAM_SENSOR_DIO_2 = 2; public static final int BEAM_SENSOR_DIO_3 = 3; - public static final int BEAM_SENSOR_DIO_4 = 0; - public static final int BEAM_SENSOR_DIO_5 = 0; + public static final int BEAM_SENSOR_DIO_4 = 4; + public static final int BEAM_SENSOR_DIO_5 = 5; } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d54a017..c75e34c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -57,7 +57,7 @@ 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 storage motor at 50 percent - m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.5), m_robotStorage)); + m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); } From 5ab2fce79bb2e4e4d7c4633e85d532c214726b16 Mon Sep 17 00:00:00 2001 From: KyraRivera <101209@psdschools.org> Date: Sat, 8 Feb 2020 14:49:16 -0700 Subject: [PATCH 26/35] Set up leveler motor so that it can be run. --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/subsystems/Leveler.java | 11 ++++++----- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e9c899c..e788406 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -70,11 +70,11 @@ public final class Constants { } public static final class LevelerConstants { - public static final int LEVELER_TALON_ID = 9; + public static final int LEVELER_CAN_ID = 9; } public static final class StorageConstants { - public static final int STORAGE_CAN_ID = 9; + public static final int STORAGE_CAN_ID = -1; public static final int BEAM_SENSOR_DIO_0 = 0; public static final int BEAM_SENSOR_DIO_1 = 1; public static final int BEAM_SENSOR_DIO_2 = 2; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e7ae628..cf5b717 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -60,7 +60,7 @@ public class RobotContainer { m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - // drives the robot with an axis input from the driver controller + // drives the leveler with an axis input from the driver controller 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)); diff --git a/src/main/java/frc4388/robot/subsystems/Leveler.java b/src/main/java/frc4388/robot/subsystems/Leveler.java index 613a8cb..02df406 100644 --- a/src/main/java/frc4388/robot/subsystems/Leveler.java +++ b/src/main/java/frc4388/robot/subsystems/Leveler.java @@ -10,22 +10,23 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.Talon; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.LevelerConstants; public class Leveler extends SubsystemBase { - WPI_TalonSRX m_levelerMotor = new WPI_TalonSRX(LevelerConstants.LEVELER_TALON_ID); + CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless); /** * Creates a new Leveler. */ public Leveler() { - m_levelerMotor.configFactoryDefault(); - - m_levelerMotor.setNeutralMode(NeutralMode.Brake); - + m_levelerMotor.restoreFactoryDefaults(); + m_levelerMotor.setIdleMode(IdleMode.kCoast); m_levelerMotor.setInverted(false); } From 0015b078cbcf45f0380e24931ecce996a1cd0df0 Mon Sep 17 00:00:00 2001 From: KyraRivera <101209@psdschools.org> Date: Sat, 8 Feb 2020 15:11:23 -0700 Subject: [PATCH 27/35] Turned off limit switches so climber would run. --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/subsystems/Climber.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 50bbe8a..4ca2bc8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -61,7 +61,7 @@ public class RobotContainer { m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); // drives motor with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); - // drives motor with input from triggers on the opperator controller + // drives climber with input from triggers on the opperator controller m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index de0b618..6b35036 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -27,8 +27,8 @@ public class Climber extends SubsystemBase { m_forwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); m_reverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); - m_forwardLimit.enableLimitSwitch(true); - m_reverseLimit.enableLimitSwitch(true); + m_forwardLimit.enableLimitSwitch(false); + m_reverseLimit.enableLimitSwitch(false); } @Override From 92feb32651727881c1df41dc80706c09c5497ce3 Mon Sep 17 00:00:00 2001 From: KyraRivera <101209@psdschools.org> Date: Sat, 8 Feb 2020 15:23:42 -0700 Subject: [PATCH 28/35] Reconfigured the motor to get intake to run. --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/subsystems/Intake.java | 5 ++++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e788406..1904d8d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -66,11 +66,11 @@ public final class Constants { } public static final class IntakeConstants { - public static final int INTAKE_SPARK_ID = 1; + public static final int INTAKE_SPARK_ID = 9; } public static final class LevelerConstants { - public static final int LEVELER_CAN_ID = 9; + public static final int LEVELER_CAN_ID = -1; } public static final class StorageConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index cf5b717..5387b64 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -56,7 +56,7 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); - // drives motor with input from triggers on the opperator controller + // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index a1c1a4c..09d1827 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -7,12 +7,15 @@ package frc4388.robot.subsystems; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.IntakeConstants; public class Intake extends SubsystemBase { - Spark m_intakeMotor = new Spark(IntakeConstants.INTAKE_SPARK_ID); + CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); /** * Creates a new Intake. From 9aeac07431f05023d6e386ddfe9ed86a4baf0350 Mon Sep 17 00:00:00 2001 From: KyraRivera <101209@psdschools.org> Date: Sat, 8 Feb 2020 15:58:11 -0700 Subject: [PATCH 29/35] Added RunExtenderOutIn command and defined extender motor --- src/main/java/frc4388/robot/Constants.java | 1 + .../robot/commands/RunExtenderOutIn.java | 59 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Intake.java | 28 ++++++++- 3 files changed, 86 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/RunExtenderOutIn.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c3640d2..2840cdb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -67,6 +67,7 @@ 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 class ClimberConstants { diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java new file mode 100644 index 0000000..d5a2e29 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Intake; +import frc4388.utility.controller.IHandController; + +public class RunExtenderOutIn extends CommandBase { + private Intake m_intake; + private boolean isOut = false; + + /** + * Uses input from opperator triggers to control intake motor. + * The right trigger will run the intake in and the left trigger will run it out. + * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + * @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the + * {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in + * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + */ + public RunExtenderOutIn(Intake subsystem, IHandController controller) { + // Use addRequirements() here to declare subsystem dependencies. + m_intake = subsystem; + addRequirements(m_intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + isOut = !isOut; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (isOut){ + m_intake.runExtender(0.3); + } else { + m_intake.runExtender(-0.3); + } + + } + + // 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/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 09d1827..62bb30a 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -7,7 +7,10 @@ package frc4388.robot.subsystems; +import com.revrobotics.CANDigitalInput; import com.revrobotics.CANSparkMax; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.Spark; @@ -16,13 +19,26 @@ import frc4388.robot.Constants.IntakeConstants; public class Intake extends SubsystemBase { CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); - + CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); + CANDigitalInput m_extenderForwardLimit; + CANDigitalInput m_extenderReverseLimit; + /** * Creates a new Intake. */ public Intake() { - + m_intakeMotor.restoreFactoryDefaults(); + m_extenderMotor.restoreFactoryDefaults(); + + m_intakeMotor.setIdleMode(IdleMode.kCoast); + m_extenderMotor.setIdleMode(IdleMode.kBrake); m_intakeMotor.setInverted(false); + m_extenderMotor.setInverted(false); + + m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_extenderForwardLimit.enableLimitSwitch(false); + m_extenderReverseLimit.enableLimitSwitch(false); } @Override @@ -37,4 +53,12 @@ public class Intake extends SubsystemBase { public void runIntake(double input) { m_intakeMotor.set(input); } + + /** + * Runs extender motor + * @param input the percent output to run motor at + */ + public void runExtender(double input) { + m_extenderMotor.set(input); + } } From 827aca7fce5ddf4821e000d92fd118295070ad10 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 8 Feb 2020 16:10:21 -0700 Subject: [PATCH 30/35] Small edits to make it work --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Drive.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0ad14ab..c4cc293 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -84,11 +84,11 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); // sets solenoids into high gear - new JoystickButton(getDriverJoystick(), XboxController.RIGHT_TRIGGER_AXIS) + new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); // sets solenoids into low gear - new JoystickButton(getDriverJoystick(), XboxController.LEFT_TRIGGER_AXIS) + new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); //new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 52f3fa6..07ba57a 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -64,7 +64,7 @@ public class Drive extends SubsystemBase { m_pigeon.configFactoryDefault(); resetGyroYaw(); - speedShift = new DoubleSolenoid(1,0,1); + speedShift = new DoubleSolenoid(7,0,1); /* set back motors as followers */ m_leftBackMotor.follow(m_leftFrontMotor); From 674523687939b011f3aa651914dae69bfa44354b Mon Sep 17 00:00:00 2001 From: KyraRivera <59713772+KyraRivera@users.noreply.github.com> Date: Mon, 10 Feb 2020 17:37:26 -0700 Subject: [PATCH 31/35] Added an intake Extender command to be tested. --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++++ .../java/frc4388/robot/commands/RunExtenderOutIn.java | 11 +++++++---- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6accd17..8ecddc4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc4388.robot.Constants.*; import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunClimberWithTriggers; +import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; @@ -87,6 +88,9 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + .whenPressed(new RunExtenderOutIn(m_robotIntake)); /* PID Test Command */ // runs velocity PID while driving straight diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java index d5a2e29..c9e6de9 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -14,16 +14,14 @@ import frc4388.utility.controller.IHandController; public class RunExtenderOutIn extends CommandBase { private Intake m_intake; private boolean isOut = false; + private long startTime; /** * Uses input from opperator triggers to control intake motor. * The right trigger will run the intake in and the left trigger will run it out. * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - * @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the - * {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in - * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} */ - public RunExtenderOutIn(Intake subsystem, IHandController controller) { + public RunExtenderOutIn(Intake subsystem) { // Use addRequirements() here to declare subsystem dependencies. m_intake = subsystem; addRequirements(m_intake); @@ -33,6 +31,7 @@ public class RunExtenderOutIn extends CommandBase { @Override public void initialize() { isOut = !isOut; + startTime = System.currentTimeMillis(); } // Called every time the scheduler runs while the command is scheduled. @@ -49,11 +48,15 @@ public class RunExtenderOutIn extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + m_intake.runExtender(0.0); } // Returns true when the command should end. @Override public boolean isFinished() { + if (startTime + 3000 < System.currentTimeMillis()) { + return true; + } return false; } } From 80a0198965d70ee00670cd51de0f1fc7d8dce5a4 Mon Sep 17 00:00:00 2001 From: KyraRivera <59713772+KyraRivera@users.noreply.github.com> Date: Mon, 10 Feb 2020 17:50:52 -0700 Subject: [PATCH 32/35] Updated comments to be accurate --- src/main/java/frc4388/robot/commands/RunExtenderOutIn.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java index c9e6de9..b0bb140 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -17,8 +17,8 @@ public class RunExtenderOutIn extends CommandBase { private long startTime; /** - * Uses input from opperator triggers to control intake motor. - * The right trigger will run the intake in and the left trigger will run it out. + * Uses input from opperator to run the extender motor. + * The left bumper will run the extender in and out. * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} */ public RunExtenderOutIn(Intake subsystem) { From 6fbbca937c18083b65208419d238f4eb0101ff6d Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 10 Feb 2020 18:02:37 -0700 Subject: [PATCH 33/35] fix keenans dumb merge --- .../java/frc4388/robot/RobotContainer.java | 3 - .../robot/commands/DriveAtVelocityPID.java | 52 ---------------- .../robot/commands/DriveToDistanceMM.java | 60 ------------------- .../robot/commands/DriveToDistancePID.java | 60 ------------------- .../java/frc4388/robot/subsystems/Drive.java | 17 ------ .../frc4388/robot/subsystems/Shooter.java | 8 +-- 6 files changed, 4 insertions(+), 196 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java delete mode 100644 src/main/java/frc4388/robot/commands/DriveToDistanceMM.java delete mode 100644 src/main/java/frc4388/robot/commands/DriveToDistancePID.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bfbbac0..355708b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,9 +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.DriveToDistancePID; import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunClimberWithTriggers; 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 3d3e96c..0000000 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ /dev/null @@ -1,52 +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 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 deleted file mode 100644 index 625e522..0000000 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ /dev/null @@ -1,60 +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 edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -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. - * @param subsystem drive subsystem - * @param distance distance to travel in inches - */ - public DriveToDistanceMM(Drive subsystem, double distance) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_distance = distance * DriveConstants.TICKS_PER_INCH; - 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.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget); - //m_drive.runMotionMagicPID(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 deleted file mode 100644 index 79b27e9..0000000 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ /dev/null @@ -1,60 +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 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 26ad512..35db61c 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -318,23 +318,6 @@ public class Drive extends SubsystemBase { } } - /** - * 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. */ diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 2e42c11..7c90fb3 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -64,10 +64,10 @@ public class Shooter extends SubsystemBase { */ public void setShooterGains() { m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); - m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kF, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kP, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); } /** * Runs drum shooter velocity PID. From 82ebcef450096df08e69d5d4a1c10e7cbf90fcf9 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 10 Feb 2020 19:21:10 -0700 Subject: [PATCH 34/35] Tuning and Comments --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/subsystems/Shooter.java | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d7b76af..ef8356a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -76,7 +76,7 @@ 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.425, 0.0005, 13, 0.05, 0, 1.0); + 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; } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 7c90fb3..169e36f 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -25,7 +25,7 @@ public class Shooter extends SubsystemBase { double velP; /** - * Creates a new Shooter. + * Creates a new Shooter subsystem. */ public Shooter() { m_shooterFalcon.configFactoryDefault(); @@ -80,10 +80,10 @@ public class Shooter extends SubsystemBase { //if (runSpeed > targetVel) {runSpeed = targetVel;} SmartDashboard.putNumber("shoot", actualVel); runSpeed = runSpeed/targetVel; //Convert to percent - if (actualVel < targetVel - 1000){ + if (actualVel < targetVel - 1000){ //PID Based on ramp up amount m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed); } - else{ + else{ //PID Based on targetVel m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } } From 1b63e2a63d3617e2a5759332166ed9ad44594711 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 11 Feb 2020 02:56:55 +0000 Subject: [PATCH 35/35] Update RobotContainer.java --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bdff586..285d467 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -101,7 +101,7 @@ public class RobotContainer { .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new RunExtenderOutIn(m_robotIntake));