From 4ce861bda158661d014242696002f8d9364449f2 Mon Sep 17 00:00:00 2001 From: Noah Date: Thu, 21 Nov 2024 13:31:56 +0000 Subject: [PATCH] Started implamenting SparkMaxs and PID --- src/main/java/frc/robot/RobotContainer.java | 16 ++-- .../frc/robot/constants/ClimberConstants.java | 6 ++ .../frc/robot/constants/HoodConstants.java | 5 ++ .../frc/robot/constants/IndexerConstants.java | 5 ++ .../robot/constants/IntakePivotConstants.java | 16 ++++ .../constants/IntakeRollersConstants.java | 5 ++ .../frc/robot/constants/SensorsConstants.java | 6 ++ .../frc/robot/constants/ShooterConstants.java | 5 ++ src/main/java/frc/robot/subsystems/Hood.java | 8 +- .../java/frc/robot/subsystems/Indexer.java | 6 -- .../frc/robot/subsystems/IntakePivot.java | 72 +++++++++++++----- .../frc/robot/subsystems/IntakeRollers.java | 14 ++-- .../java/frc/robot/subsystems/Shooter.java | 20 ++--- vendordeps/REVLib.json | 74 +++++++++++++++++++ 14 files changed, 203 insertions(+), 55 deletions(-) create mode 100644 src/main/java/frc/robot/constants/ClimberConstants.java create mode 100644 src/main/java/frc/robot/constants/HoodConstants.java create mode 100644 src/main/java/frc/robot/constants/IndexerConstants.java create mode 100644 src/main/java/frc/robot/constants/IntakePivotConstants.java create mode 100644 src/main/java/frc/robot/constants/IntakeRollersConstants.java create mode 100644 src/main/java/frc/robot/constants/SensorsConstants.java create mode 100644 src/main/java/frc/robot/constants/ShooterConstants.java create mode 100644 vendordeps/REVLib.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bd8042c..86727cc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -61,15 +61,19 @@ public class RobotContainer { shooter.setDefaultCommand(shooter.stopShooter()); //controller bindings// - driver.a().onTrue(ampUp()); - driver.a().onFalse(ampScore()); - driver.leftTrigger().onTrue(); + driver.a().whileTrue(ampUp()); + driver.a().whileFalse(ampScore()); + driver.leftTrigger().whileTrue(smartIntake()); + + operator.povUp().onTrue(hood.hoodUp()); + operator.povDown().whileTrue(climber.runClimber()); } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } + //methods for smart actions// public Command ampUp() { if (!sensors.getHoodBeamBreak()) { @@ -91,13 +95,13 @@ public class RobotContainer { } } - public Command smartIntake() { if (!sensors.getIndexerBeamBreak()) { return intakePivot.intakeDown() - .alongWith(null) + .alongWith(intakeRollers.runIntakeRollers(1)); } else { - + return intakePivot.intakeUp() + .alongWith(intakeRollers.stopIntakeRollers()); } } } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java new file mode 100644 index 0000000..ec3d45d --- /dev/null +++ b/src/main/java/frc/robot/constants/ClimberConstants.java @@ -0,0 +1,6 @@ +package frc.robot.constants; + +public class ClimberConstants { + public static final int kMotor1 = 0; + public static final int kMotor2 = 1; +} diff --git a/src/main/java/frc/robot/constants/HoodConstants.java b/src/main/java/frc/robot/constants/HoodConstants.java new file mode 100644 index 0000000..e9dd2f5 --- /dev/null +++ b/src/main/java/frc/robot/constants/HoodConstants.java @@ -0,0 +1,5 @@ +package frc.robot.constants; + +public class HoodConstants { + +} diff --git a/src/main/java/frc/robot/constants/IndexerConstants.java b/src/main/java/frc/robot/constants/IndexerConstants.java new file mode 100644 index 0000000..f023033 --- /dev/null +++ b/src/main/java/frc/robot/constants/IndexerConstants.java @@ -0,0 +1,5 @@ +package frc.robot.constants; + +public class IndexerConstants { + +} diff --git a/src/main/java/frc/robot/constants/IntakePivotConstants.java b/src/main/java/frc/robot/constants/IntakePivotConstants.java new file mode 100644 index 0000000..43119de --- /dev/null +++ b/src/main/java/frc/robot/constants/IntakePivotConstants.java @@ -0,0 +1,16 @@ +package frc.robot.constants; + +public class IntakePivotConstants { + public static final int kUpAngle = 90; + public static final int kDownAngle = 0; + + public static final int kFFS = 0; + public static final int kFFG = 0; + public static final int kFFV = 0; + + public static final int kPPivot = 0; + public static final int kIPivot = 0; + public static final int kDPivot = 0; + + public static final int kIntakePivotConversionFactor = 0; +} diff --git a/src/main/java/frc/robot/constants/IntakeRollersConstants.java b/src/main/java/frc/robot/constants/IntakeRollersConstants.java new file mode 100644 index 0000000..b76eb15 --- /dev/null +++ b/src/main/java/frc/robot/constants/IntakeRollersConstants.java @@ -0,0 +1,5 @@ +package frc.robot.constants; + +public class IntakeRollersConstants { + public static final int kIntakeRoller = 0; +} diff --git a/src/main/java/frc/robot/constants/SensorsConstants.java b/src/main/java/frc/robot/constants/SensorsConstants.java new file mode 100644 index 0000000..3140629 --- /dev/null +++ b/src/main/java/frc/robot/constants/SensorsConstants.java @@ -0,0 +1,6 @@ +package frc.robot.constants; + +public class SensorsConstants { + public static final int kHoodBeamBreak = 0; + public static final int kIndexerBeamBreak = 1; +} diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java new file mode 100644 index 0000000..4f59ab2 --- /dev/null +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -0,0 +1,5 @@ +package frc.robot.constants; + +public class ShooterConstants { + +} diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java index 9cc0467..47d8351 100644 --- a/src/main/java/frc/robot/subsystems/Hood.java +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -1,19 +1,21 @@ package frc.robot.subsystems; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.motorcontrol.VictorSP; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Hood extends SubsystemBase { - private VictorSP pivotMotor; + private CANSparkMax pivotMotor; private DigitalInput bottomLimitSwitch; private DigitalInput topLimitSwitch; public Hood() { - pivotMotor = new VictorSP(0); + pivotMotor = new CANSparkMax(0, MotorType.kBrushless); bottomLimitSwitch = new DigitalInput(0); topLimitSwitch = new DigitalInput(0); diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 99606b9..256a7e1 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -17,12 +17,6 @@ public class Indexer extends SubsystemBase { }); } - public Command reverseIndexer(double speed) { - return run(() -> { - motor1.set(speed); - }); - } - public Command stopIndexer() { return run(() -> { motor1.set(0); diff --git a/src/main/java/frc/robot/subsystems/IntakePivot.java b/src/main/java/frc/robot/subsystems/IntakePivot.java index 50424b8..0f36e8c 100644 --- a/src/main/java/frc/robot/subsystems/IntakePivot.java +++ b/src/main/java/frc/robot/subsystems/IntakePivot.java @@ -1,40 +1,74 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.motorcontrol.VictorSP; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.IntakePivotConstants; public class IntakePivot extends SubsystemBase { - private VictorSP pivotMotor; - - private DigitalInput topLimitSwitch; - private DigitalInput bottomLimitSwitch; + private CANSparkMax pivotMotor; + + private RelativeEncoder intakeEncoder; + + private PIDController intakePIDController; + private ArmFeedforward intakeFF; public IntakePivot() { - pivotMotor = new VictorSP(0); + pivotMotor = new CANSparkMax(0, MotorType.kBrushless); - topLimitSwitch = new DigitalInput(0); - bottomLimitSwitch = new DigitalInput(0); + intakeEncoder = pivotMotor.getEncoder(); + + intakeFF = new ArmFeedforward( + IntakePivotConstants.kFFS, + IntakePivotConstants.kFFG, + IntakePivotConstants.kFFV + ); + + intakePIDController = new PIDController( + IntakePivotConstants.kPPivot, + IntakePivotConstants.kIPivot, + IntakePivotConstants.kDPivot + ); } public Command intakeUp() { return run(() -> { - if (topLimitSwitch.get()) { - pivotMotor.set(0); - } else { - pivotMotor.set(0.5); - } + pivotMotor.setVoltage( + intakePIDController.calculate( + getIntakeAngle(), + IntakePivotConstants.kUpAngle + ) + intakeFF.calculate( + IntakePivotConstants.kUpAngle, + 0.0 + ) + ); }); } public Command intakeDown() { return run(() -> { - if (bottomLimitSwitch.get()) { - pivotMotor.set(0); - } else { - pivotMotor.set(-0.5); - } + pivotMotor.setVoltage( + intakePIDController.calculate( + getIntakeAngle(), + IntakePivotConstants.kDownAngle + ) + intakeFF.calculate( + IntakePivotConstants.kDownAngle, + 0.0 + ) + ); }); } + + public double getIntakeAngle(){ + return Units.rotationsToRadians( + intakeEncoder.getPosition()/ + IntakePivotConstants.kIntakePivotConversionFactor + ); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeRollers.java b/src/main/java/frc/robot/subsystems/IntakeRollers.java index 1dc7915..ad10b93 100644 --- a/src/main/java/frc/robot/subsystems/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/IntakeRollers.java @@ -1,14 +1,16 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.motorcontrol.VictorSP; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeRollers extends SubsystemBase { - private VictorSP motor1; + private CANSparkMax motor1; public IntakeRollers() { - motor1 = new VictorSP(0); + motor1 = new CANSparkMax(0, MotorType.kBrushless); } public Command runIntakeRollers(double speed) { @@ -17,12 +19,6 @@ public class IntakeRollers extends SubsystemBase { }); } - public Command reverseIntakeRollers(double speed) { - return run(() -> { - motor1.set(speed); - }); - } - public Command stopIntakeRollers() { return run(() -> { motor1.set(0); diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index fa41293..fcd97b3 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -1,18 +1,20 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.motorcontrol.VictorSP; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Shooter extends SubsystemBase { - private VictorSP motor1; - private VictorSP motor2; + private CANSparkMax motor1; + private CANSparkMax motor2; public Shooter() { - motor1 = new VictorSP(0); - motor2 = new VictorSP(0); + motor1 = new CANSparkMax(0, MotorType.kBrushless); + motor2 = new CANSparkMax(0, MotorType.kBrushless); - motor1.addFollower(motor2); + motor2.follow(motor1); motor2.setInverted(true); } @@ -22,12 +24,6 @@ public class Shooter extends SubsystemBase { }); } - public Command reverseShooter() { - return run(() -> { - motor1.set(-1); - }); - } - public Command stopShooter() { return run(() -> { motor1.set(0); diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..f85acd4 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.4", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.4" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.4", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.4", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.4", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file