From bf8f4759fa4e3b5c17b1b41cd9dcfb9d45148d44 Mon Sep 17 00:00:00 2001 From: koolfyn Date: Sat, 20 Jan 2024 13:06:20 -0500 Subject: [PATCH 1/4] intakeretractor feat? --- README.md | 2 +- build.gradle | 18 +-- .../org/robolancers321/BuildConstants.java | 18 +++ .../subsystems/IntakeRetractor.java | 109 ++++++++++++++++++ .../commands/retractor/retractIntake.java | 9 ++ vendordeps/REVLib.json | 74 ++++++++++++ 6 files changed, 220 insertions(+), 10 deletions(-) create mode 100644 src/main/java/org/robolancers321/BuildConstants.java create mode 100644 src/main/java/org/robolancers321/subsystems/IntakeRetractor.java create mode 100644 src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java create mode 100644 vendordeps/REVLib.json diff --git a/README.md b/README.md index ecf7e73..2212a7a 100644 --- a/README.md +++ b/README.md @@ -1 +1 @@ -## WIP \ No newline at end of file +## WIP diff --git a/build.gradle b/build.gradle index 1f93411..5b90b01 100644 --- a/build.gradle +++ b/build.gradle @@ -156,12 +156,12 @@ spotless { } } -project.compileJava.dependsOn(createVersionFile) -gversion { - srcDir = "src/main/java/" - classPackage = "org.robolancers321" - className = "BuildConstants" - dateFormat = "yyyy-MM-dd HH:mm:ss z" - timeZone = "America/New_York" // Use preferred time zone - indent = " " -} +// project.compileJava.dependsOn(createVersionFile) +// // gversion { +// // srcDir = "src/main/java/" +// // classPackage = "org.robolancers321" +// // className = "BuildConstants" +// // dateFormat = "yyyy-MM-dd HH:mm:ss z" +// // timeZone = "America/New_York" // Use preferred time zone +// // indent = " " +// // } diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java new file mode 100644 index 0000000..f5162d2 --- /dev/null +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -0,0 +1,18 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321; + +/** Automatically generated file containing build version information. */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "321-Crescendo-2024"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 4; + public static final String GIT_SHA = "2de603cd5ac29e8182034c832bc185d3a6f14393"; + public static final String GIT_DATE = "2024-01-08 20:28:49 EST"; + public static final String GIT_BRANCH = "main"; + public static final String BUILD_DATE = "2024-01-10 17:47:24 EST"; + public static final long BUILD_UNIX_TIME = 1704926844560L; + public static final int DIRTY = 1; + + private BuildConstants() {} +} diff --git a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java new file mode 100644 index 0000000..a79a6e4 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java @@ -0,0 +1,109 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems; +import edu.wpi.first.math.proto.Controller; +import edu.wpi.first.wpilibj.DigitalInput; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; + +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class IntakeRetractor extends SubsystemBase { + + public CANSparkMax retractorMotor; + private DigitalInput retractorLimitSwitch; + private DigitalInput retractorBeamBreak; + private Trigger retractorTrigger; + SparkPIDController retractorPIDController; + AbsoluteEncoder retractorEncoder1; + RelativeEncoder retractorEncoder2; + + public double referencePosition = 0; + public double retractPosition = 90; + + public static final double kP = 0.000; + public static final double kI = 0.000; + public static final double kD = 0.000; + public static final double kFF = 0.0001; + public static final double kErrorThreshold = 2.0; + + public static final int kPort = 0; + public static final int kRetractorLimitSwitch = 0; + private static final Type kDutyCycle = null; + + + public IntakeRetractor() { + this.retractorMotor = new CANSparkMax(kPort, MotorType.kBrushless); + this.retractorEncoder1 = retractorMotor.getAbsoluteEncoder(kDutyCycle); + this.retractorEncoder2 = retractorMotor.getEncoder(); + retractorPIDController = retractorMotor.getPIDController(); + retractorLimitSwitch = new DigitalInput(0); + retractorBeamBreak = new DigitalInput(0); + + this.retractorEncoder1.setPositionConversionFactor(1.0); + + retractorPIDController.setD(kD); + retractorPIDController.setP(kP); + retractorPIDController.setI(kI); + + this.retractorMotor.setSmartCurrentLimit(20); + + + // this.retractorLimitSwitch = new DigitalInput(kRetractorLimitSwitch); + // this.retractorTrigger = new Trigger(this::limitSwitchTriggered); + // this.retractorTrigger.whileTrue(new InstantCommand(() -> { + // this.resetEncoder(); + // }, this)); + } + + @Override + public void periodic() { + + retractorPIDController.setD(kD); + retractorPIDController.setP(kP); + retractorPIDController.setI(kI); + retractorPIDController.setFF(kFF); + + retractorMotor.getOutputCurrent(); + retractorMotor.set(100); + + // pivotcontrolleroutput = controller.calculate(stuff) + // pivotMotor.set(Output) + + } + +// public boolean limitSwitchTriggered() { +// return !this.retractorLimitSwitch.get(); +// } + + +public void setRetractorPosition() { + retractorEncoder2.setPosition(retractPosition); +} + +public double getRetractPosition() { + return retractorEncoder2.getPosition(); +} + +public boolean isRetractorAtCorrectPos(double position) { + return Math.abs(this.getRetractPosition() - position) < kErrorThreshold; +} + + +public void resetEncoder() { + retractorEncoder2.setPosition(0); +} + + public boolean isBeamBroken() { + return !this.retractorBeamBreak.get(); + } + +} diff --git a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java new file mode 100644 index 0000000..2df6665 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java @@ -0,0 +1,9 @@ +package org.robolancers321.subsystems.commands.retractor; + +import edu.wpi.first.wpilibj2.command.Command; +import org.robolancers321.subsystems.IntakeRetractor; + +public class retractIntake extends Command { + public IntakeRetractor intakeRetractor; + +} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..0f3520e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.0", + "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.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "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.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": "2024.2.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 c279ee0ba899d80fd3fb6014938537d48d90cc27 Mon Sep 17 00:00:00 2001 From: koolfyn Date: Sat, 20 Jan 2024 13:18:23 -0500 Subject: [PATCH 2/4] intakeretractor: feat?? --- .../subsystems/IntakeRetractor.java | 27 ++++++++++--------- .../commands/retractor/retractIntake.java | 23 ++++++++++++++++ 2 files changed, 38 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java index a79a6e4..bdcd164 100644 --- a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java +++ b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java @@ -26,8 +26,8 @@ public class IntakeRetractor extends SubsystemBase { AbsoluteEncoder retractorEncoder1; RelativeEncoder retractorEncoder2; - public double referencePosition = 0; - public double retractPosition = 90; + public double extendedPosition = 90; + public double retractedPosition = 0; public static final double kP = 0.000; public static final double kI = 0.000; @@ -36,6 +36,7 @@ public class IntakeRetractor extends SubsystemBase { public static final double kErrorThreshold = 2.0; public static final int kPort = 0; + public static final int kRetractorBeamBreakPort = 0; public static final int kRetractorLimitSwitch = 0; private static final Type kDutyCycle = null; @@ -45,9 +46,9 @@ public IntakeRetractor() { this.retractorEncoder1 = retractorMotor.getAbsoluteEncoder(kDutyCycle); this.retractorEncoder2 = retractorMotor.getEncoder(); retractorPIDController = retractorMotor.getPIDController(); + this.retractorBeamBreak = new DigitalInput(kRetractorBeamBreakPort); retractorLimitSwitch = new DigitalInput(0); - retractorBeamBreak = new DigitalInput(0); - + this.retractorEncoder1.setPositionConversionFactor(1.0); retractorPIDController.setD(kD); @@ -84,26 +85,28 @@ public void periodic() { // return !this.retractorLimitSwitch.get(); // } +public void retractedPosition() { + retractorEncoder2.setPosition(retractedPosition); +} -public void setRetractorPosition() { - retractorEncoder2.setPosition(retractPosition); +public void extendRetractor() { + retractorEncoder2.setPosition(extendedPosition); } public double getRetractPosition() { return retractorEncoder2.getPosition(); } -public boolean isRetractorAtCorrectPos(double position) { +public boolean RetractorAtCorrectPos(double position) { return Math.abs(this.getRetractPosition() - position) < kErrorThreshold; -} - +} public void resetEncoder() { retractorEncoder2.setPosition(0); } - public boolean isBeamBroken() { - return !this.retractorBeamBreak.get(); - } +public boolean beamBroken() { + return !this.retractorBeamBreak.get(); +} } diff --git a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java index 2df6665..299a2f0 100644 --- a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java +++ b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java @@ -5,5 +5,28 @@ public class retractIntake extends Command { public IntakeRetractor intakeRetractor; + + public retractIntake() { + addRequirements(this.intakeRetractor); + } + @Override + public void initialize() { + intakeRetractor.resetEncoder(); + } + + @Override + public void execute() { + intakeRetractor.extendRetractor(); + } + + @Override + public boolean isFinished() { + return intakeRetractor.beamBroken(); + } + + @Override + public void end(boolean interrupted) { + intakeRetractor.retractedPosition(); + } } From 995665ccda84593c46c7ec4fa5e5c4dce1bafcff Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Sat, 20 Jan 2024 14:34:38 -0500 Subject: [PATCH 3/4] major refactoring: singleton pattern, changed a lot of names and modifiers, added abstractions for configuration, and added framework for feedforward control and tuning. next steps: small changes where indicated and implement feedforward control, motion profiling, and tuning --- .../commands/intake/RetractIntake.java | 28 +++ .../subsystems/IntakeRetractor.java | 112 ---------- .../commands/retractor/retractIntake.java | 32 --- .../subsystems/intake/IntakeRetractor.java | 204 ++++++++++++++++++ 4 files changed, 232 insertions(+), 144 deletions(-) create mode 100644 src/main/java/org/robolancers321/commands/intake/RetractIntake.java delete mode 100644 src/main/java/org/robolancers321/subsystems/IntakeRetractor.java delete mode 100644 src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java create mode 100644 src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java diff --git a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java new file mode 100644 index 0000000..a68d09d --- /dev/null +++ b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java @@ -0,0 +1,28 @@ +package org.robolancers321.commands.intake; + +import org.robolancers321.subsystems.intake.IntakeRetractor; +import org.robolancers321.subsystems.intake.IntakeRetractor.RetractorSetpoint; + +import edu.wpi.first.wpilibj2.command.Command; + +public class RetractIntake extends Command { + private IntakeRetractor intakeRetractor; + + public RetractIntake() { + this.intakeRetractor = IntakeRetractor.getInstance(); + + addRequirements(this.intakeRetractor); + } + + @Override + public void initialize() { + this.intakeRetractor.setSetpoint(RetractorSetpoint.kRetracted); + } + + @Override + public boolean isFinished() { + // TODO: encoder vs limit switch? limit switch MAY be more accurate but is also subject to physical damage + + return this.intakeRetractor.isAtSetpoint(); + } +} diff --git a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java deleted file mode 100644 index bdcd164..0000000 --- a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java +++ /dev/null @@ -1,112 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.subsystems; -import edu.wpi.first.math.proto.Controller; -import edu.wpi.first.wpilibj.DigitalInput; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkAbsoluteEncoder.Type; -import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkLowLevel.MotorType; - -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -public class IntakeRetractor extends SubsystemBase { - - public CANSparkMax retractorMotor; - private DigitalInput retractorLimitSwitch; - private DigitalInput retractorBeamBreak; - private Trigger retractorTrigger; - SparkPIDController retractorPIDController; - AbsoluteEncoder retractorEncoder1; - RelativeEncoder retractorEncoder2; - - public double extendedPosition = 90; - public double retractedPosition = 0; - - public static final double kP = 0.000; - public static final double kI = 0.000; - public static final double kD = 0.000; - public static final double kFF = 0.0001; - public static final double kErrorThreshold = 2.0; - - public static final int kPort = 0; - public static final int kRetractorBeamBreakPort = 0; - public static final int kRetractorLimitSwitch = 0; - private static final Type kDutyCycle = null; - - - public IntakeRetractor() { - this.retractorMotor = new CANSparkMax(kPort, MotorType.kBrushless); - this.retractorEncoder1 = retractorMotor.getAbsoluteEncoder(kDutyCycle); - this.retractorEncoder2 = retractorMotor.getEncoder(); - retractorPIDController = retractorMotor.getPIDController(); - this.retractorBeamBreak = new DigitalInput(kRetractorBeamBreakPort); - retractorLimitSwitch = new DigitalInput(0); - - this.retractorEncoder1.setPositionConversionFactor(1.0); - - retractorPIDController.setD(kD); - retractorPIDController.setP(kP); - retractorPIDController.setI(kI); - - this.retractorMotor.setSmartCurrentLimit(20); - - - // this.retractorLimitSwitch = new DigitalInput(kRetractorLimitSwitch); - // this.retractorTrigger = new Trigger(this::limitSwitchTriggered); - // this.retractorTrigger.whileTrue(new InstantCommand(() -> { - // this.resetEncoder(); - // }, this)); - } - - @Override - public void periodic() { - - retractorPIDController.setD(kD); - retractorPIDController.setP(kP); - retractorPIDController.setI(kI); - retractorPIDController.setFF(kFF); - - retractorMotor.getOutputCurrent(); - retractorMotor.set(100); - - // pivotcontrolleroutput = controller.calculate(stuff) - // pivotMotor.set(Output) - - } - -// public boolean limitSwitchTriggered() { -// return !this.retractorLimitSwitch.get(); -// } - -public void retractedPosition() { - retractorEncoder2.setPosition(retractedPosition); -} - -public void extendRetractor() { - retractorEncoder2.setPosition(extendedPosition); -} - -public double getRetractPosition() { - return retractorEncoder2.getPosition(); -} - -public boolean RetractorAtCorrectPos(double position) { - return Math.abs(this.getRetractPosition() - position) < kErrorThreshold; -} - -public void resetEncoder() { - retractorEncoder2.setPosition(0); -} - -public boolean beamBroken() { - return !this.retractorBeamBreak.get(); -} - -} diff --git a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java deleted file mode 100644 index 299a2f0..0000000 --- a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.robolancers321.subsystems.commands.retractor; - -import edu.wpi.first.wpilibj2.command.Command; -import org.robolancers321.subsystems.IntakeRetractor; - -public class retractIntake extends Command { - public IntakeRetractor intakeRetractor; - - public retractIntake() { - addRequirements(this.intakeRetractor); - } - - @Override - public void initialize() { - intakeRetractor.resetEncoder(); - } - - @Override - public void execute() { - intakeRetractor.extendRetractor(); - } - - @Override - public boolean isFinished() { - return intakeRetractor.beamBroken(); - } - - @Override - public void end(boolean interrupted) { - intakeRetractor.retractedPosition(); - } -} diff --git a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java new file mode 100644 index 0000000..0243641 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java @@ -0,0 +1,204 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.intake; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class IntakeRetractor extends SubsystemBase { + /* + * Singleton + */ + + private static IntakeRetractor instance = null; + + public static IntakeRetractor getInstance(){ + if (instance == null) instance = new IntakeRetractor(); + + return instance; + } + + /* + * Constants + */ + + private static final int kMotorPort = 0; + private static final int kLimitSwitchPort = 0; + private static final int kBeamBreakPort = 0; + + // TODO: put constants for motor and encoder configuration here + + private static final double kP = 0.000; + private static final double kI = 0.000; + private static final double kD = 0.000; + + // TODO: feed forward controller using wpilib ArmFeedforward class + + private static final double kErrorThreshold = 2.0; + + public static enum RetractorSetpoint { + kRetracted(0), + kMating(0), + kIntake(0); + + private double angle; + + private RetractorSetpoint(double angle){ + this.angle = angle; + } + + public double getAngle(){ + return this.angle; + } + } + + /* + * Implementation + */ + + private CANSparkMax motor; + private AbsoluteEncoder absoluteEncoder; + private RelativeEncoder relativeEncoder; + private SparkPIDController feedbackController; + + private DigitalInput limitSwitch; + private DigitalInput beamBreak; + + private RetractorSetpoint setpoint; + + private IntakeRetractor() { + this.motor = new CANSparkMax(kMotorPort, MotorType.kBrushless); + this.absoluteEncoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); + this.relativeEncoder = this.motor.getEncoder(); + this.feedbackController = this.motor.getPIDController(); + + this.limitSwitch = new DigitalInput(kLimitSwitchPort); + this.beamBreak = new DigitalInput(kBeamBreakPort); + + this.configureMotor(); + this.configureEncoder(); + this.configureController(); + + this.configureLimitSwitchResponder(); + } + + private void configureMotor(){ + /* + * TODO + * + * using constants set: + * motor inversion, current limit (30?) + * + * without constants set: + * idle mode (brake), voltage compensation (12) + */ + } + + private void configureEncoder(){ + /* + * TODO + * + * set position conversion rate (store this rate as a constant), set encoder inversion + * + * this should probably be done for both encoders just in case + */ + } + + private void configureController(){ + this.feedbackController.setD(kD); + this.feedbackController.setP(kP); + this.feedbackController.setI(kI); + + // TODO: also configure feed forward controller once it it written + } + + private void configureLimitSwitchResponder(){ + new Trigger(this::isLimitSwitchTriggered).whileTrue(new RunCommand(this::resetEncoder)); + } + + public double getPosition() { + return this.absoluteEncoder.getPosition(); + + // if using relative encoder + // return this.relativeEncoder.getPosition(); + } + + public boolean isLimitSwitchTriggered() { + // TODO: is this negation correct? + + return !this.limitSwitch.get(); + } + + public boolean isBeamBroken(){ + // TODO: is this negation correct? + return !this.beamBreak.get(); + } + + // TODO: this method is totally fine but may be scrapped when we move to a motion profiling implementation + public boolean isAtPosition(double position) { + return Math.abs(this.getPosition() - position) < kErrorThreshold; + } + + public boolean isAtSetpoint(){ + return this.isAtPosition(this.setpoint.getAngle()); + } + + public void resetEncoder(double position){ + this.relativeEncoder.setPosition(position); + + // TODO: does it make sense to implement this for the mag offset of the absolute encoder? + } + + public void resetEncoder(){ + this.resetEncoder(RetractorSetpoint.kRetracted.getAngle()); + } + + public void setSetpoint(RetractorSetpoint setpoint){ + this.setpoint = setpoint; + } + + private void useController(){ + /* + * TODO + * + * this method will be called periodically to set the motor voltage based on feedback and feedforward controllers + * + * add the outputs of the feedback controller (pid) and feedforward controller (once it is written) and supply them to the motor via set() + * + * good examples of this code are this years shooter pivot and last years arm + */ + } + + private void doSendables(){ + // TODO: use smart dashboard to log the position, limit switch status, beam break status, etc. + } + + @Override + public void periodic() { + useController(); + doSendables(); + } + + public void initTuning(){ + SmartDashboard.putNumber("retractor kp", SmartDashboard.getNumber("retractor kp", kP)); + + // TODO: ^ do this for kI, kD, and any constants for the feedforward controller + } + + public void tune(){ + double tunedP = SmartDashboard.getNumber("retractor kp", kP); + + this.feedbackController.setP(tunedP); + + // TODO: ^ do this for kI, kD, and any constants for the feedforward controller + } +} From 7f7b8e373147f598df049fe120e153253030a457 Mon Sep 17 00:00:00 2001 From: koolfyn Date: Wed, 24 Jan 2024 17:08:45 -0500 Subject: [PATCH 4/4] dababy --- .../commands/intake/RetractIntake.java | 2 - .../subsystems/intake/IntakeRetractor.java | 105 ++++++++++-------- 2 files changed, 59 insertions(+), 48 deletions(-) diff --git a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java index a68d09d..394c4ef 100644 --- a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java +++ b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java @@ -21,8 +21,6 @@ public void initialize() { @Override public boolean isFinished() { - // TODO: encoder vs limit switch? limit switch MAY be more accurate but is also subject to physical damage - return this.intakeRetractor.isAtSetpoint(); } } diff --git a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java index 0243641..eeb1d5c 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java @@ -1,15 +1,17 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystems.intake; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAbsoluteEncoder.Type; -import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -35,14 +37,20 @@ public static IntakeRetractor getInstance(){ private static final int kLimitSwitchPort = 0; private static final int kBeamBreakPort = 0; - // TODO: put constants for motor and encoder configuration here + private static final boolean kInvertMotor = false; + private static final int kCurrentLimit = 20; private static final double kP = 0.000; private static final double kI = 0.000; private static final double kD = 0.000; - - // TODO: feed forward controller using wpilib ArmFeedforward class + private static final double kS = 0.000; + private static final double kV = 0.000; + private static final double kA = 0.000; + private static final double kG = 0.000; + private static final double kMaxVelocityDeg = 0.0; + private static final double kMaxAccelerationDeg = 0.0; + private static final double kErrorThreshold = 2.0; public static enum RetractorSetpoint { @@ -68,7 +76,8 @@ public double getAngle(){ private CANSparkMax motor; private AbsoluteEncoder absoluteEncoder; private RelativeEncoder relativeEncoder; - private SparkPIDController feedbackController; + private ProfiledPIDController feedbackController; + private ArmFeedforward feedforwardController; private DigitalInput limitSwitch; private DigitalInput beamBreak; @@ -79,31 +88,31 @@ private IntakeRetractor() { this.motor = new CANSparkMax(kMotorPort, MotorType.kBrushless); this.absoluteEncoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); this.relativeEncoder = this.motor.getEncoder(); - this.feedbackController = this.motor.getPIDController(); + this.feedbackController = new ProfiledPIDController(kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocityDeg, kMaxAccelerationDeg)); this.limitSwitch = new DigitalInput(kLimitSwitchPort); this.beamBreak = new DigitalInput(kBeamBreakPort); this.configureMotor(); this.configureEncoder(); - this.configureController(); - + this.configureController(); this.configureLimitSwitchResponder(); } private void configureMotor(){ - /* - * TODO - * - * using constants set: - * motor inversion, current limit (30?) - * - * without constants set: - * idle mode (brake), voltage compensation (12) - */ + this.motor.setInverted(kInvertMotor); + this.motor.setIdleMode(CANSparkMax.IdleMode.kBrake); + this.motor.setSmartCurrentLimit(kCurrentLimit); + this.motor.enableVoltageCompensation(12); + } private void configureEncoder(){ + this.relativeEncoder.setInverted(kInvertMotor); + this.absoluteEncoder.setInverted(kInvertMotor); + this.relativeEncoder.setPositionConversionFactor(360); + this.absoluteEncoder.setPositionConversionFactor(360); + /* * TODO * @@ -114,11 +123,11 @@ private void configureEncoder(){ } private void configureController(){ - this.feedbackController.setD(kD); + this.feedforwardController = new ArmFeedforward(kS, kG, kV); + this.feedbackController.setP(kP); this.feedbackController.setI(kI); - - // TODO: also configure feed forward controller once it it written + this.feedbackController.setD(kD); } private void configureLimitSwitchResponder(){ @@ -133,13 +142,10 @@ public double getPosition() { } public boolean isLimitSwitchTriggered() { - // TODO: is this negation correct? - return !this.limitSwitch.get(); } public boolean isBeamBroken(){ - // TODO: is this negation correct? return !this.beamBreak.get(); } @@ -154,8 +160,6 @@ public boolean isAtSetpoint(){ public void resetEncoder(double position){ this.relativeEncoder.setPosition(position); - - // TODO: does it make sense to implement this for the mag offset of the absolute encoder? } public void resetEncoder(){ @@ -167,20 +171,20 @@ public void setSetpoint(RetractorSetpoint setpoint){ } private void useController(){ - /* - * TODO - * - * this method will be called periodically to set the motor voltage based on feedback and feedforward controllers - * - * add the outputs of the feedback controller (pid) and feedforward controller (once it is written) and supply them to the motor via set() - * - * good examples of this code are this years shooter pivot and last years arm - */ + State setpointState = this.feedbackController.getSetpoint(); + + double feedforwardOutput = this.feedforwardController.calculate(setpointState.position, setpointState.velocity); + double feedbackOutput = this.feedbackController.calculate(this.getPosition()); + + double controllerOutput = feedforwardOutput + feedbackOutput; + this.motor.setVoltage(controllerOutput); } private void doSendables(){ - // TODO: use smart dashboard to log the position, limit switch status, beam break status, etc. - } + SmartDashboard.putNumber("current position", relativeEncoder.getPosition()); + SmartDashboard.putBoolean("note detected", this.isBeamBroken()); + SmartDashboard.putBoolean("Retractor at position(deg)", this.isAtSetpoint()); + } @Override public void periodic() { @@ -189,16 +193,25 @@ public void periodic() { } public void initTuning(){ - SmartDashboard.putNumber("retractor kp", SmartDashboard.getNumber("retractor kp", kP)); - - // TODO: ^ do this for kI, kD, and any constants for the feedforward controller + SmartDashboard.putNumber("retractor kP", SmartDashboard.getNumber("retractor kP", kP)); + SmartDashboard.putNumber("retractor kI", SmartDashboard.getNumber("retractor kI", kI)); + SmartDashboard.putNumber("retractor kD", SmartDashboard.getNumber("retractor kD", kD)); + SmartDashboard.putNumber("retractor kS", SmartDashboard.getNumber("retractor kS", kS)); + SmartDashboard.putNumber("retractor kV", SmartDashboard.getNumber("retractor kV", kV)); + SmartDashboard.putNumber("retractor kA", SmartDashboard.getNumber("retractor kA", kA)); + SmartDashboard.putNumber("retractor kG", SmartDashboard.getNumber("retractor kG", kG)); } - public void tune(){ + public void tune(){ double tunedP = SmartDashboard.getNumber("retractor kp", kP); + double tunedI = SmartDashboard.getNumber("retractor kI", kI); + double tunedD = SmartDashboard.getNumber("retractor kD", kD); + + this.feedforwardController = new ArmFeedforward(kS, kG, kV); this.feedbackController.setP(tunedP); + this.feedbackController.setI(tunedI); + this.feedbackController.setD(tunedD); - // TODO: ^ do this for kI, kD, and any constants for the feedforward controller } }