From 5b6b33c1f8ec004e3883ac7311808c7d61173d82 Mon Sep 17 00:00:00 2001 From: Ryan Bauroth <25bauroth@da.org> Date: Fri, 8 Sep 2023 15:28:10 -0400 Subject: [PATCH 1/9] started manipulator. Still need to do IO part --- .../subsystems/manipulator/Manipulator.java | 36 +++++++++++++++++++ .../subsystems/manipulator/ManipulatorIO.java | 30 ++++++++++++++++ 2 files changed, 66 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/manipulator/Manipulator.java create mode 100644 src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java diff --git a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java new file mode 100644 index 0000000..82d34d9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java @@ -0,0 +1,36 @@ +package frc.robot.subsystems.manipulator; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.filter.Debouncer; +import org.littletonrobotics.junction.Logger; + +public class Manipulator extends SubsystemBase { + public ManipulatorIO io; + public ManipulatorIO.ManipulatorIOInputs inputs; + + public Manipulator(int motorId){ + CANSparkMax motor = new CANSparkMax(motorId, CANSparkMaxLowLevel.MotorType.kBrushless); + motor.restoreFactoryDefaults(); + motor.setIdleMode(CANSparkMax.IdleMode.kBrake); + motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 10); + motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 20); + /*Debouncer motorIdleDebouncer = new Debouncer(0.075); + double lastPercent = 0.0; + CANSparkMax.IdleMode lastIdleMode = CANSparkMax.IdleMode.kBrake;*/ + //TODO figure out if this is important + } + + public void init() { + //TODO add default command for manipulator + } + + + @Override + public void periodic() { + io.updateInputs(inputs); + //TODO make this work idk io stuff + } + +} diff --git a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java new file mode 100644 index 0000000..8c2f2f6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.manipulator; + +import frc.robot.subsystems.flywheel.FlywheelIO; +import org.littletonrobotics.junction.AutoLog; + +public interface ManipulatorIO { + @AutoLog + public static class ManipulatorIOInputs { + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double[] currentAmps = new double[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ManipulatorIO.ManipulatorIOInputs inputs) { + } + + /** Run closed loop at the specified velocity. */ + public default void setVelocity(double velocityRadPerSec, double ffVolts) { + } + + /** Stop in open loop. */ + public default void stop() { + } + + /** Set velocity PID constants. */ + public default void configurePID(double kP, double kI, double kD) { + } +} From 9cfc59a8405d374c4dd32e1b0fd76dd9e14048cc Mon Sep 17 00:00:00 2001 From: Ryan Bauroth <25bauroth@da.org> Date: Fri, 8 Sep 2023 16:01:52 -0400 Subject: [PATCH 2/9] finished io part probably --- .../subsystems/manipulator/Manipulator.java | 3 +- .../manipulator/ManipulatorIOSparkMax.java | 63 +++++++++++++++++++ 2 files changed, 64 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java diff --git a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java index 82d34d9..abc5b3f 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java @@ -8,7 +8,7 @@ public class Manipulator extends SubsystemBase { public ManipulatorIO io; - public ManipulatorIO.ManipulatorIOInputs inputs; + public ManipulatorIO.ManipulatorIOInputs inputs; //TODO figure out how inputs is set public Manipulator(int motorId){ CANSparkMax motor = new CANSparkMax(motorId, CANSparkMaxLowLevel.MotorType.kBrushless); @@ -30,7 +30,6 @@ public void init() { @Override public void periodic() { io.updateInputs(inputs); - //TODO make this work idk io stuff } } diff --git a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java new file mode 100644 index 0000000..db2e73a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java @@ -0,0 +1,63 @@ +package frc.robot.subsystems.manipulator; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxPIDController; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.subsystems.flywheel.FlywheelIO; + +public class ManipulatorIOSparkMax implements ManipulatorIO { + public static final double GEAR_RATIO = 1; + public static final int deviceId = 6; //TODO fill this in + private final CANSparkMax motor; + private final RelativeEncoder encoder; + private final SparkMaxPIDController pid; + + public ManipulatorIOSparkMax(){ + motor = new CANSparkMax(deviceId, CANSparkMaxLowLevel.MotorType.kBrushless); + + encoder = motor.getEncoder(); + pid = motor.getPIDController(); + + motor.restoreFactoryDefaults(); + + motor.setInverted(false); + + motor.enableVoltageCompensation(12.0); + motor.setSmartCurrentLimit(30); + + motor.burnFlash(); + } + + @Override + public void updateInputs(ManipulatorIOInputs inputs) { + inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / GEAR_RATIO); + inputs.velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( + encoder.getVelocity() / GEAR_RATIO); + inputs.appliedVolts = motor.getAppliedOutput() * RobotController.getBatteryVoltage(); + inputs.currentAmps = new double[] {motor.getOutputCurrent()}; + } + + @Override + public void setVelocity(double velocityRadPerSec, double ffVolts) { + pid.setReference( + Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) + * GEAR_RATIO, + CANSparkMax.ControlType.kVelocity, 0, ffVolts, SparkMaxPIDController.ArbFFUnits.kVoltage); + } + + @Override + public void stop() { + motor.stopMotor(); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + pid.setP(kP, 0); + pid.setI(kI, 0); + pid.setD(kD, 0); + pid.setFF(0, 0); + } +} From 03aeb66fa4cfe22b480fa28283f46444e5c5164e Mon Sep 17 00:00:00 2001 From: Ryan Bauroth <25bauroth@da.org> Date: Fri, 8 Sep 2023 18:43:57 -0400 Subject: [PATCH 3/9] hypothetically added a SetManipulatorSpeed command --- .../manipulator/SetManipulatorSpeed.java | 21 +++++++++++++++++++ .../subsystems/manipulator/Manipulator.java | 18 ++++++++-------- .../subsystems/manipulator/ManipulatorIO.java | 2 +- 3 files changed, 31 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java diff --git a/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java b/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java new file mode 100644 index 0000000..ef7ed0a --- /dev/null +++ b/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java @@ -0,0 +1,21 @@ +package frc.robot.commands.manipulator; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.manipulator.Manipulator; + +public class SetManipulatorSpeed extends CommandBase { + Manipulator manipulator; + double speed; + public SetManipulatorSpeed(Manipulator manipulator, double speed){ + this.manipulator = manipulator; + this.speed = speed; + } + + private void init() { + addRequirements(manipulator); + } + + public void execute() { + manipulator.inputs.velocityRadPerSec = speed; + } +} diff --git a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java index abc5b3f..f5f39af 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java @@ -3,27 +3,27 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.math.filter.Debouncer; -import org.littletonrobotics.junction.Logger; +import frc.robot.commands.manipulator.SetManipulatorSpeed; public class Manipulator extends SubsystemBase { public ManipulatorIO io; - public ManipulatorIO.ManipulatorIOInputs inputs; //TODO figure out how inputs is set + public ManipulatorIO.ManipulatorIOInputs inputs; + public int motorId; - public Manipulator(int motorId){ + public Manipulator(ManipulatorIO io, int motorId){ + this.io = io; + this.motorId = motorId; + + //TODO put this in constructor or init @everett CANSparkMax motor = new CANSparkMax(motorId, CANSparkMaxLowLevel.MotorType.kBrushless); motor.restoreFactoryDefaults(); motor.setIdleMode(CANSparkMax.IdleMode.kBrake); motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 10); motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 20); - /*Debouncer motorIdleDebouncer = new Debouncer(0.075); - double lastPercent = 0.0; - CANSparkMax.IdleMode lastIdleMode = CANSparkMax.IdleMode.kBrake;*/ - //TODO figure out if this is important } public void init() { - //TODO add default command for manipulator + setDefaultCommand(new SetManipulatorSpeed(this, 0.05)); } diff --git a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java index 8c2f2f6..7ae58af 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java +++ b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java @@ -9,7 +9,7 @@ public static class ManipulatorIOInputs { public double positionRad = 0.0; public double velocityRadPerSec = 0.0; public double appliedVolts = 0.0; - public double[] currentAmps = new double[] {}; + public double[] currentAmps = new double[] {0.0}; } /** Updates the set of loggable inputs. */ From 90d715c0efd062f2b46987b73f07ed7e0b60e6d8 Mon Sep 17 00:00:00 2001 From: Ryan Bauroth <25bauroth@da.org> Date: Sat, 9 Sep 2023 12:33:02 -0400 Subject: [PATCH 4/9] formatting --- src/main/java/frc/robot/subsystems/manipulator/Manipulator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java index f5f39af..1ae0f9f 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java @@ -9,7 +9,7 @@ public class Manipulator extends SubsystemBase { public ManipulatorIO io; public ManipulatorIO.ManipulatorIOInputs inputs; public int motorId; - + public Manipulator(ManipulatorIO io, int motorId){ this.io = io; this.motorId = motorId; From 28ca954be68139097018167f31e73ecb633a40ff Mon Sep 17 00:00:00 2001 From: Ryan Bauroth <25bauroth@da.org> Date: Sat, 9 Sep 2023 12:34:06 -0400 Subject: [PATCH 5/9] added another TODO --- .../java/frc/robot/subsystems/manipulator/Manipulator.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java index 1ae0f9f..50c29eb 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java @@ -9,7 +9,7 @@ public class Manipulator extends SubsystemBase { public ManipulatorIO io; public ManipulatorIO.ManipulatorIOInputs inputs; public int motorId; - + public Manipulator(ManipulatorIO io, int motorId){ this.io = io; this.motorId = motorId; @@ -24,6 +24,7 @@ public Manipulator(ManipulatorIO io, int motorId){ public void init() { setDefaultCommand(new SetManipulatorSpeed(this, 0.05)); + //TODO probably am not supposed to use init as it does nothing lol } From 905547a3b562bfdc3889cc6b114446a50844599a Mon Sep 17 00:00:00 2001 From: Ryfi Date: Tue, 12 Sep 2023 14:41:28 -0400 Subject: [PATCH 6/9] Update src/main/java/frc/robot/subsystems/manipulator/Manipulator.java Co-authored-by: Everett Wilber <71281043+a1cd@users.noreply.github.com> --- .../java/frc/robot/subsystems/manipulator/Manipulator.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java index 50c29eb..bb4d81b 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java @@ -33,4 +33,11 @@ public void periodic() { io.updateInputs(inputs); } + public void setMotorVoltage(double voltage) { + io.setVoltage(voltage); + } + + public void setMotorPercent(double percentage) { + io.setPercentage(percentage); + } } From e7c3da3ae9b4c3b609d56f4c6c624b5503e6ab7c Mon Sep 17 00:00:00 2001 From: Ryfi Date: Tue, 12 Sep 2023 14:43:37 -0400 Subject: [PATCH 7/9] Apply suggestions from code review Co-authored-by: Everett Wilber <71281043+a1cd@users.noreply.github.com> --- .../manipulator/SetManipulatorSpeed.java | 2 +- .../subsystems/manipulator/Manipulator.java | 17 ++------- .../subsystems/manipulator/ManipulatorIO.java | 9 +++-- .../manipulator/ManipulatorIOSparkMax.java | 37 +++++-------------- 4 files changed, 20 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java b/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java index ef7ed0a..4ceb6f5 100644 --- a/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java +++ b/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java @@ -16,6 +16,6 @@ private void init() { } public void execute() { - manipulator.inputs.velocityRadPerSec = speed; + manipulator.setMotorPercentage(speed); } } diff --git a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java index bb4d81b..0108213 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java @@ -8,24 +8,13 @@ public class Manipulator extends SubsystemBase { public ManipulatorIO io; public ManipulatorIO.ManipulatorIOInputs inputs; - public int motorId; - public Manipulator(ManipulatorIO io, int motorId){ + public Manipulator(ManipulatorIO io){ this.io = io; - this.motorId = motorId; - - //TODO put this in constructor or init @everett - CANSparkMax motor = new CANSparkMax(motorId, CANSparkMaxLowLevel.MotorType.kBrushless); - motor.restoreFactoryDefaults(); - motor.setIdleMode(CANSparkMax.IdleMode.kBrake); - motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 10); - motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 20); + + setDefaultCommand(new SetManipulatorSpeed(this, 0.05)); } - public void init() { - setDefaultCommand(new SetManipulatorSpeed(this, 0.05)); - //TODO probably am not supposed to use init as it does nothing lol - } @Override diff --git a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java index 7ae58af..0b26772 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java +++ b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java @@ -23,8 +23,11 @@ public default void setVelocity(double velocityRadPerSec, double ffVolts) { /** Stop in open loop. */ public default void stop() { } - - /** Set velocity PID constants. */ - public default void configurePID(double kP, double kI, double kD) { + public default void setPercentage(double percentage) { } + + public default void setVoltage(double voltage) { + } + + /** Set velocity PID constants. */ } diff --git a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java index db2e73a..914ea87 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java @@ -9,26 +9,16 @@ import frc.robot.subsystems.flywheel.FlywheelIO; public class ManipulatorIOSparkMax implements ManipulatorIO { - public static final double GEAR_RATIO = 1; - public static final int deviceId = 6; //TODO fill this in + public static final int deviceId = 31; private final CANSparkMax motor; - private final RelativeEncoder encoder; - private final SparkMaxPIDController pid; public ManipulatorIOSparkMax(){ motor = new CANSparkMax(deviceId, CANSparkMaxLowLevel.MotorType.kBrushless); - encoder = motor.getEncoder(); - pid = motor.getPIDController(); - motor.restoreFactoryDefaults(); - - motor.setInverted(false); - - motor.enableVoltageCompensation(12.0); - motor.setSmartCurrentLimit(30); - - motor.burnFlash(); + motor.setIdleMode(CANSparkMax.IdleMode.kBrake); + motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 10); + motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 20); } @Override @@ -39,25 +29,18 @@ public void updateInputs(ManipulatorIOInputs inputs) { inputs.appliedVolts = motor.getAppliedOutput() * RobotController.getBatteryVoltage(); inputs.currentAmps = new double[] {motor.getOutputCurrent()}; } + public void setMotorVoltage(double voltage) { + motor.setVoltage(voltage); + } - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - pid.setReference( - Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) - * GEAR_RATIO, - CANSparkMax.ControlType.kVelocity, 0, ffVolts, SparkMaxPIDController.ArbFFUnits.kVoltage); + public void setPercentage(double percentage) { + motor.set(percentage); } + @Override public void stop() { motor.stopMotor(); } - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setP(kP, 0); - pid.setI(kI, 0); - pid.setD(kD, 0); - pid.setFF(0, 0); - } } From b96910c02f48dddb9d40cf6ba72de0e7da5b63d5 Mon Sep 17 00:00:00 2001 From: Everett Wilber <71281043+a1cd@users.noreply.github.com> Date: Tue, 12 Sep 2023 17:35:37 -0400 Subject: [PATCH 8/9] Apply suggestions from code review Quick simple compilation failures --- .../frc/robot/commands/manipulator/SetManipulatorSpeed.java | 2 +- .../robot/subsystems/manipulator/ManipulatorIOSparkMax.java | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java b/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java index 4ceb6f5..35cd95f 100644 --- a/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java +++ b/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java @@ -16,6 +16,6 @@ private void init() { } public void execute() { - manipulator.setMotorPercentage(speed); + manipulator.setMotorPercent(speed); } } diff --git a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java index 914ea87..4f13820 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java @@ -23,9 +23,6 @@ public ManipulatorIOSparkMax(){ @Override public void updateInputs(ManipulatorIOInputs inputs) { - inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / GEAR_RATIO); - inputs.velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( - encoder.getVelocity() / GEAR_RATIO); inputs.appliedVolts = motor.getAppliedOutput() * RobotController.getBatteryVoltage(); inputs.currentAmps = new double[] {motor.getOutputCurrent()}; } From c394e6ff9b268342bfce00f0147bb7edb36c7c7c Mon Sep 17 00:00:00 2001 From: Everett Wilber <71281043+a1cd@users.noreply.github.com> Date: Tue, 12 Sep 2023 17:39:51 -0400 Subject: [PATCH 9/9] remove stuff --- .../java/frc/robot/subsystems/manipulator/ManipulatorIO.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java index 0b26772..07d39ae 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java +++ b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java @@ -6,8 +6,6 @@ public interface ManipulatorIO { @AutoLog public static class ManipulatorIOInputs { - public double positionRad = 0.0; - public double velocityRadPerSec = 0.0; public double appliedVolts = 0.0; public double[] currentAmps = new double[] {0.0}; }