From 0df215f0c4d10488e065957c0242df788536d8f5 Mon Sep 17 00:00:00 2001 From: drorKeren Date: Tue, 7 Jan 2025 20:54:51 +0200 Subject: [PATCH 1/3] added manually and angle based pivot --- src/main/java/frc/robot/Robot.java | 1 + .../frc/robot/commands/PivotMoveToAngle.java | 45 ++++++++ .../robot/commands/PivotMoveWithJoystick.java | 49 ++++++++ src/main/java/frc/robot/subsystems/Pivot.java | 105 ++++++++++++++++++ vendordeps/REVLib-2025.0.0.json | 74 ++++++++++++ 5 files changed, 274 insertions(+) create mode 100644 src/main/java/frc/robot/commands/PivotMoveToAngle.java create mode 100644 src/main/java/frc/robot/commands/PivotMoveWithJoystick.java create mode 100644 src/main/java/frc/robot/subsystems/Pivot.java create mode 100644 vendordeps/REVLib-2025.0.0.json diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 47d3d5a..dc135c9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.Pivot; /** * The methods in this class are called automatically corresponding to each mode, as described in diff --git a/src/main/java/frc/robot/commands/PivotMoveToAngle.java b/src/main/java/frc/robot/commands/PivotMoveToAngle.java new file mode 100644 index 0000000..f1f11a6 --- /dev/null +++ b/src/main/java/frc/robot/commands/PivotMoveToAngle.java @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Pivot; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class PivotMoveToAngle extends Command { + public Pivot myPivot; + private double myAngle; + + public PivotMoveToAngle(double myAngle) + { + this.myAngle = myAngle; + myPivot = Pivot.GetInstance(); + addRequirements(myPivot); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() + { + myPivot.movePivotToAngle(myAngle); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) + { + myPivot.StopMotor(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return myPivot.isAtAngle(myAngle); + } +} diff --git a/src/main/java/frc/robot/commands/PivotMoveWithJoystick.java b/src/main/java/frc/robot/commands/PivotMoveWithJoystick.java new file mode 100644 index 0000000..eebcbd3 --- /dev/null +++ b/src/main/java/frc/robot/commands/PivotMoveWithJoystick.java @@ -0,0 +1,49 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Pivot; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class PivotMoveWithJoystick extends Command { + public Pivot myPivot; + + private DoubleSupplier _double_Supplier; + + public PivotMoveWithJoystick(DoubleSupplier Double_Supplier) + { + _double_Supplier = Double_Supplier; + + myPivot = Pivot.GetInstance(); + addRequirements(myPivot); + } + + // 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() + { + myPivot.SetPower(_double_Supplier.getAsDouble()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) + { + myPivot.StopMotor(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java new file mode 100644 index 0000000..78d19df --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -0,0 +1,105 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; + +public class Pivot extends SubsystemBase { + private SparkFlex motor; + private SparkFlexConfig motorConfig; + private SparkClosedLoopController closedLoopController; + private RelativeEncoder encoder; + + private static Pivot instance = null; + + public double tolerance = 2.0; + + private Pivot() + { + motor = new SparkFlex(1, MotorType.kBrushless); + closedLoopController = motor.getClosedLoopController(); + encoder = motor.getEncoder(); + + motorConfig.encoder.positionConversionFactor(360); + + motorConfig = new SparkFlexConfig(); + + motorConfig.idleMode(IdleMode.kBrake); + + motorConfig.closedLoop + .feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder) + .p(0.4) + .i(0) + .d(0) + .outputRange(-1, 1); + + motorConfig.closedLoop.maxMotion + .maxVelocity(1000) + .maxAcceleration(1000) + .allowedClosedLoopError(1); + + motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + + encoder.setPosition(motor.getAbsoluteEncoder().getPosition()); + } + + public static Pivot GetInstance() + { + if (instance == null) + { + instance = new Pivot(); + } + + return instance; + } + + /*** + * The function moves the motor to the wanted angle + * + * @param angle - the angle to move the motor to + */ + public void movePivotToAngle(double angle) + { + closedLoopController.setReference(angle, ControlType.kMAXMotionPositionControl,ClosedLoopSlot.kSlot0); + } + + public void SetPower(double power) + { + motor.set(power); + } + + public void StopMotor() + { + motor.stopMotor(); + } + + /*** + * The function checks if the motor has reached the wanted angle + * + * @param angle - The angle we want the motor to move to + * @return if the motor has reached the wanted angle + */ + public boolean isAtAngle(double angle) + { + return encoder.getPosition() <= angle + tolerance && encoder.getPosition() >= angle - tolerance; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/vendordeps/REVLib-2025.0.0.json b/vendordeps/REVLib-2025.0.0.json new file mode 100644 index 0000000..cde6011 --- /dev/null +++ b/vendordeps/REVLib-2025.0.0.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib-2025.0.0.json", + "name": "REVLib", + "version": "2025.0.0", + "frcYear": "2025", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2025.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2025.0.0", + "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": "2025.0.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From 808570b071071b1088214a6e73c84c67c7e96a3f Mon Sep 17 00:00:00 2001 From: drorKeren Date: Thu, 9 Jan 2025 18:35:38 +0200 Subject: [PATCH 2/3] Added constants and fixed func names --- src/main/java/frc/robot/Constants.java | 15 +++++++++++ .../frc/robot/commands/PivotMoveToAngle.java | 6 ++--- .../robot/commands/PivotMoveWithJoystick.java | 12 ++++----- src/main/java/frc/robot/subsystems/Pivot.java | 27 +++++++++---------- 4 files changed, 37 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c50ba05..950a218 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,5 +15,20 @@ public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; + } + + + public static final double completeAngle = 360; + + // Variables used by the pivot + public static final int pivotMotorID = 1; + public static final double pivotTolerance = 2.0; + + public static final double pivotP = 0.4; + public static final double pivotI = 0.0; + public static final double pivotD = 0.0; + + public static final int pivotMaxVelocity = 1000; + public static final int pivotMaxAccel = 1000; } diff --git a/src/main/java/frc/robot/commands/PivotMoveToAngle.java b/src/main/java/frc/robot/commands/PivotMoveToAngle.java index f1f11a6..f757416 100644 --- a/src/main/java/frc/robot/commands/PivotMoveToAngle.java +++ b/src/main/java/frc/robot/commands/PivotMoveToAngle.java @@ -9,10 +9,10 @@ /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class PivotMoveToAngle extends Command { - public Pivot myPivot; + private Pivot myPivot; private double myAngle; - public PivotMoveToAngle(double myAngle) + private PivotMoveToAngle(double myAngle) { this.myAngle = myAngle; myPivot = Pivot.GetInstance(); @@ -34,7 +34,7 @@ public void execute() {} @Override public void end(boolean interrupted) { - myPivot.StopMotor(); + myPivot.stopMotor(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/PivotMoveWithJoystick.java b/src/main/java/frc/robot/commands/PivotMoveWithJoystick.java index eebcbd3..6d8c64e 100644 --- a/src/main/java/frc/robot/commands/PivotMoveWithJoystick.java +++ b/src/main/java/frc/robot/commands/PivotMoveWithJoystick.java @@ -11,13 +11,13 @@ /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class PivotMoveWithJoystick extends Command { - public Pivot myPivot; + private Pivot myPivot; - private DoubleSupplier _double_Supplier; + private DoubleSupplier doubleSupplier; - public PivotMoveWithJoystick(DoubleSupplier Double_Supplier) + private PivotMoveWithJoystick(DoubleSupplier myDoubleSupplier) { - _double_Supplier = Double_Supplier; + doubleSupplier = myDoubleSupplier; myPivot = Pivot.GetInstance(); addRequirements(myPivot); @@ -31,14 +31,14 @@ public void initialize() {} @Override public void execute() { - myPivot.SetPower(_double_Supplier.getAsDouble()); + myPivot.setPower(doubleSupplier.getAsDouble()); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - myPivot.StopMotor(); + myPivot.stopMotor(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 78d19df..4809084 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -5,6 +5,7 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -26,15 +27,13 @@ public class Pivot extends SubsystemBase { private static Pivot instance = null; - public double tolerance = 2.0; - private Pivot() { - motor = new SparkFlex(1, MotorType.kBrushless); + motor = new SparkFlex(Constants.pivotMotorID, MotorType.kBrushless); closedLoopController = motor.getClosedLoopController(); encoder = motor.getEncoder(); - motorConfig.encoder.positionConversionFactor(360); + motorConfig.encoder.positionConversionFactor(Constants.completeAngle); motorConfig = new SparkFlexConfig(); @@ -42,14 +41,14 @@ private Pivot() motorConfig.closedLoop .feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder) - .p(0.4) - .i(0) - .d(0) + .p(Constants.pivotP) + .i(Constants.pivotI) + .d(Constants.pivotD) .outputRange(-1, 1); motorConfig.closedLoop.maxMotion - .maxVelocity(1000) - .maxAcceleration(1000) + .maxVelocity(Constants.pivotMaxVelocity) + .maxAcceleration(Constants.pivotMaxAccel) .allowedClosedLoopError(1); motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); @@ -68,7 +67,7 @@ public static Pivot GetInstance() } /*** - * The function moves the motor to the wanted angle + * The function moves the pivot to the wanted angle * * @param angle - the angle to move the motor to */ @@ -77,25 +76,25 @@ public void movePivotToAngle(double angle) closedLoopController.setReference(angle, ControlType.kMAXMotionPositionControl,ClosedLoopSlot.kSlot0); } - public void SetPower(double power) + public void setPower(double power) { motor.set(power); } - public void StopMotor() + public void stopMotor() { motor.stopMotor(); } /*** - * The function checks if the motor has reached the wanted angle + * The function checks if the pivot has reached the wanted angle * * @param angle - The angle we want the motor to move to * @return if the motor has reached the wanted angle */ public boolean isAtAngle(double angle) { - return encoder.getPosition() <= angle + tolerance && encoder.getPosition() >= angle - tolerance; + return encoder.getPosition() <= angle + Constants.pivotTolerance && encoder.getPosition() >= angle - Constants.pivotTolerance; } @Override From d98db84fddce80e788f6d83105ff61d276c2a40c Mon Sep 17 00:00:00 2001 From: drorKeren Date: Thu, 9 Jan 2025 19:43:09 +0200 Subject: [PATCH 3/3] changed GetInstance to lower case --- src/main/java/frc/robot/commands/PivotMoveToAngle.java | 2 +- src/main/java/frc/robot/commands/PivotMoveWithJoystick.java | 2 +- src/main/java/frc/robot/subsystems/Pivot.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/PivotMoveToAngle.java b/src/main/java/frc/robot/commands/PivotMoveToAngle.java index f757416..48a428f 100644 --- a/src/main/java/frc/robot/commands/PivotMoveToAngle.java +++ b/src/main/java/frc/robot/commands/PivotMoveToAngle.java @@ -15,7 +15,7 @@ public class PivotMoveToAngle extends Command { private PivotMoveToAngle(double myAngle) { this.myAngle = myAngle; - myPivot = Pivot.GetInstance(); + myPivot = Pivot.getInstance(); addRequirements(myPivot); } diff --git a/src/main/java/frc/robot/commands/PivotMoveWithJoystick.java b/src/main/java/frc/robot/commands/PivotMoveWithJoystick.java index 6d8c64e..d7a0014 100644 --- a/src/main/java/frc/robot/commands/PivotMoveWithJoystick.java +++ b/src/main/java/frc/robot/commands/PivotMoveWithJoystick.java @@ -19,7 +19,7 @@ private PivotMoveWithJoystick(DoubleSupplier myDoubleSupplier) { doubleSupplier = myDoubleSupplier; - myPivot = Pivot.GetInstance(); + myPivot = Pivot.getInstance(); addRequirements(myPivot); } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 4809084..f82fa26 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -56,7 +56,7 @@ private Pivot() encoder.setPosition(motor.getAbsoluteEncoder().getPosition()); } - public static Pivot GetInstance() + public static Pivot getInstance() { if (instance == null) {