From 35d743e4b42f5cdc8d63ef04460c79c2995463d9 Mon Sep 17 00:00:00 2001 From: Eva-Unit-02 <93512609+Eva-Unit-02@users.noreply.github.com> Date: Mon, 26 Jun 2023 18:22:27 -0700 Subject: [PATCH 1/8] Basic subsystems --- src/main/java/frc/team3128/Constants.java | 27 ++++++ .../frc/team3128/subsystems/Manipulator.java | 94 +++++++++++++++++++ .../java/frc/team3128/subsystems/Wrist.java | 57 +++++++++++ 3 files changed, 178 insertions(+) create mode 100644 src/main/java/frc/team3128/subsystems/Manipulator.java create mode 100644 src/main/java/frc/team3128/subsystems/Wrist.java diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index a49bd29..7a2d916 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -504,6 +504,33 @@ public static class IntakeConstants { } + public static class WristConstants { + + public static final double kP = 0; + public static final double kI = 0; + public static final double kD = 0; + + public static final double kS = 0; + public static final double kV = 0; + public static final double kG = 0; + + public static final double GEAR_RATIO = 0.0; + + public static final double ROTATION_TO_DEGREES = 360; + public static final double ANGLE_OFFSET = 0; + + public static final int ENCODER_DIO_ID = 8; + + public static final double VELOCITY_SETPOINT = 0.5; + public static final double INTAKE_TOLERANCE = 7.5; + + //Motor ID + public static final int PIVOT_ID = 0; + + //Sensor IDs + + } + public static class ManipulatorConstants{ public static final int ROLLER_MOTOR_ID = 13; public static final double ROLLER_POWER = 0.6; diff --git a/src/main/java/frc/team3128/subsystems/Manipulator.java b/src/main/java/frc/team3128/subsystems/Manipulator.java new file mode 100644 index 0000000..56f25f7 --- /dev/null +++ b/src/main/java/frc/team3128/subsystems/Manipulator.java @@ -0,0 +1,94 @@ +package frc.team3128.subsystems; + +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team3128.common.hardware.motorcontroller.NAR_TalonSRX; +import frc.team3128.common.utility.NAR_Shuffleboard; +import static frc.team3128.Constants.ManipulatorConstants.*; +import com.ctre.phoenix.motorcontrol.NeutralMode; + +public class Manipulator extends SubsystemBase { + + public NAR_TalonSRX m_roller; + + private static Manipulator instance; + + public static boolean CONE = true; + + public boolean outtaking = false; + + public Manipulator(){ + configMotor(); + } + + public static Manipulator getInstance() { + if (instance == null){ + instance = new Manipulator(); + } + + return instance; + } + + @Override + public void periodic() { + if (Math.abs(getCurrent()) > ABSOLUTE_THRESHOLD + 40 && !outtaking) + stallPower(); + } + + public void configMotor(){ + m_roller = new NAR_TalonSRX(ROLLER_MOTOR_ID); + m_roller.setInverted(false); + m_roller.setNeutralMode(NeutralMode.Brake); + m_roller.enableVoltageCompensation(true); + } + + public void set(double power){ + m_roller.set(power); + } + + public void forward(){ + m_roller.set(ROLLER_POWER); + } + + public void reverse(){ + m_roller.set(-ROLLER_POWER); + } + + public void stopRoller(){ + m_roller.set(0); + } + + public double getCurrent(){ + return m_roller.getStatorCurrent(); + } + + public double getVoltage() { + return m_roller.getMotorOutputVoltage(); + } + + public boolean hasObjectPresent(){ + return Math.abs(getCurrent()) > ABSOLUTE_THRESHOLD; + } + + public void intake() { + outtaking = false; + if (CONE) reverse(); + else forward(); + } + + public void outtake(){ + outtaking = true; + if (!CONE) reverse(); + else forward(); + } + + public void stallPower() { + set(CONE ? -STALL_POWER : STALL_POWER); + } + + public void initShuffleboard() { + NAR_Shuffleboard.addData("Manipulator", "Manip current", () -> getCurrent(), 0, 1); + NAR_Shuffleboard.addData("Manipulator", "get", () -> m_roller.getMotorOutputPercent(), 0, 3); + NAR_Shuffleboard.addData("Manipulator", "ObjectPresent", ()-> hasObjectPresent(), 1, 1); + } +} diff --git a/src/main/java/frc/team3128/subsystems/Wrist.java b/src/main/java/frc/team3128/subsystems/Wrist.java new file mode 100644 index 0000000..6c242f8 --- /dev/null +++ b/src/main/java/frc/team3128/subsystems/Wrist.java @@ -0,0 +1,57 @@ +package frc.team3128.subsystems; + +import java.util.function.DoubleFunction; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.revrobotics.CANSparkMax.IdleMode; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.PIDSubsystem; +import static frc.team3128.Constants.WristConstants.*; +import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax; + +public class Wrist extends NAR_PIDSubsystem { + public NAR_CANSparkMax m_pivot; + + private static Wrist instance; + + public Wrist() { + super(new PIDController(kP, kI, kD), kS, kV, kG, kG -> (Double)(Math.sin(kG))); + configMotor(); + } + + public static Wrist getInstance() { + if (instance == null){ + instance = new Wrist(); + } + + return instance; + } + + public void configMotor() { + m_pivot = new NAR_CANSparkMax(PIVOT_ID); + m_pivot.setInverted(false); + m_pivot.setIdleMode(IdleMode.kBrake); + m_pivot.enableVoltageCompensation(12.0); + resetEncoder(); + } + + @Override + protected void useOutput(double output, double setpoint) { + m_pivot.set(MathUtil.clamp(output / 12.0, -1, 1)); + } + + @Override + protected double getMeasurement() { + return m_pivot.getSelectedSensorPosition() * ROTATION_TO_DEGREES / GEAR_RATIO; + } + + public void resetEncoder() { + m_pivot.setSelectedSensorPosition(0); + } + + + + +} From 654e355bab92875b2a5ca37a01a22d2e61b1b392 Mon Sep 17 00:00:00 2001 From: Eva-Unit-02 <93512609+Eva-Unit-02@users.noreply.github.com> Date: Tue, 27 Jun 2023 18:35:59 -0700 Subject: [PATCH 2/8] Temp --- .../java/frc/team3128/commands/CmdManager.java | 16 ++++++++++++++++ .../frc/team3128/subsystems/Manipulator.java | 5 +++-- src/main/java/frc/team3128/subsystems/Wrist.java | 12 +++++++++--- 3 files changed, 28 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 52195fc..f4b79a8 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -17,15 +17,31 @@ import frc.team3128.common.hardware.input.NAR_XboxController; import frc.team3128.common.narwhaldashboard.NarwhalDashboard; import frc.team3128.subsystems.Led; +import frc.team3128.subsystems.Wrist; +import frc.team3128.subsystems.Manipulator; import frc.team3128.subsystems.Vision; public class CmdManager { private static Led led = Led.getInstance(); + private static Wrist wrist = Wrist.getInstance(); + private static Manipulator manipulator = Manipulator.getInstance(); private static NAR_XboxController controller = RobotContainer.controller; private CmdManager() {} + public static CommandBase CMDWrist() { + return new InstantCommand(()-> wrist.); + } + + public static CommandBase ManipIntake(Boolean cone) { + return new InstantCommand(()-> manipulator.intake(cone)); + } + + public static CommandBase ManipOuttake() { + return new InstantCommand(()-> manipulator.outtake()); + } + public static CommandBase vibrateController() { return new ScheduleCommand(new WaitCommand(0.5).deadlineWith(new StartEndCommand(() -> RobotContainer.controller.startVibrate(), () -> RobotContainer.controller.stopVibrate()))); } diff --git a/src/main/java/frc/team3128/subsystems/Manipulator.java b/src/main/java/frc/team3128/subsystems/Manipulator.java index 56f25f7..db8570d 100644 --- a/src/main/java/frc/team3128/subsystems/Manipulator.java +++ b/src/main/java/frc/team3128/subsystems/Manipulator.java @@ -70,9 +70,10 @@ public boolean hasObjectPresent(){ return Math.abs(getCurrent()) > ABSOLUTE_THRESHOLD; } - public void intake() { + public void intake(boolean cone) { outtaking = false; - if (CONE) reverse(); + CONE = cone; + if (cone) reverse(); else forward(); } diff --git a/src/main/java/frc/team3128/subsystems/Wrist.java b/src/main/java/frc/team3128/subsystems/Wrist.java index 6c242f8..5fedec4 100644 --- a/src/main/java/frc/team3128/subsystems/Wrist.java +++ b/src/main/java/frc/team3128/subsystems/Wrist.java @@ -1,6 +1,7 @@ package frc.team3128.subsystems; import java.util.function.DoubleFunction; +import java.util.function.DoubleSupplier; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax.IdleMode; @@ -9,6 +10,8 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.PIDSubsystem; import static frc.team3128.Constants.WristConstants.*; + +import frc.team3128.RobotContainer; import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax; public class Wrist extends NAR_PIDSubsystem { @@ -37,6 +40,12 @@ public void configMotor() { resetEncoder(); } + @ + public void startPID(double anglePos) { + anglePos = MathUtil.clamp(anglePos,0,135); + super.startPID(anglePos); + } + @Override protected void useOutput(double output, double setpoint) { m_pivot.set(MathUtil.clamp(output / 12.0, -1, 1)); @@ -51,7 +60,4 @@ public void resetEncoder() { m_pivot.setSelectedSensorPosition(0); } - - - } From f42449be0c4ce0e9031d45ab239e55190d3463f6 Mon Sep 17 00:00:00 2001 From: Eva-Unit-02 <93512609+Eva-Unit-02@users.noreply.github.com> Date: Fri, 30 Jun 2023 17:03:53 -0700 Subject: [PATCH 3/8] Basic Wrist&Manip subsystem and commands --- src/main/java/frc/team3128/Constants.java | 5 +++- .../frc/team3128/commands/CmdManager.java | 22 +++++++++++---- .../java/frc/team3128/subsystems/Wrist.java | 28 +++++++++++-------- 3 files changed, 36 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 7a2d916..035abee 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -521,11 +521,14 @@ public static class WristConstants { public static final int ENCODER_DIO_ID = 8; + public static final int MIN_ANGLE = 0; + public static final int MAX_ANGLE = 0; + public static final double VELOCITY_SETPOINT = 0.5; public static final double INTAKE_TOLERANCE = 7.5; //Motor ID - public static final int PIVOT_ID = 0; + public static final int WRIST_ID = 0; //Sensor IDs diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index ffb87a0..469b16e 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -1,9 +1,12 @@ package frc.team3128.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.team3128.RobotContainer; import frc.team3128.common.hardware.input.NAR_XboxController; import frc.team3128.common.narwhaldashboard.NarwhalDashboard; @@ -21,16 +24,23 @@ public class CmdManager { private CmdManager() {} - public static CommandBase CMDWrist() { - return new InstantCommand(()-> wrist.); + public static CommandBase CmdWrist(double setpoint) { + return Commands.sequence( + new InstantCommand(()-> wrist.startPID(setpoint), wrist), + new WaitUntilCommand(()-> wrist.atSetpoint()) + ); } - public static CommandBase ManipIntake(Boolean cone) { - return new InstantCommand(()-> manipulator.intake(cone)); + public static CommandBase CmdManipIntake(Boolean cone) { + return new InstantCommand(()-> manipulator.intake(cone), manipulator); } - public static CommandBase ManipOuttake() { - return new InstantCommand(()-> manipulator.outtake()); + public static CommandBase CmdManipOuttake() { + return new InstantCommand(()-> manipulator.outtake(), manipulator); + } + + public static CommandBase CmdStopManip() { + return new InstantCommand(()-> manipulator.stopRoller(), manipulator); } public static CommandBase vibrateController() { diff --git a/src/main/java/frc/team3128/subsystems/Wrist.java b/src/main/java/frc/team3128/subsystems/Wrist.java index 5fedec4..309474a 100644 --- a/src/main/java/frc/team3128/subsystems/Wrist.java +++ b/src/main/java/frc/team3128/subsystems/Wrist.java @@ -13,14 +13,16 @@ import frc.team3128.RobotContainer; import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax; +import frc.team3128.common.utility.NAR_Shuffleboard; public class Wrist extends NAR_PIDSubsystem { - public NAR_CANSparkMax m_pivot; + public NAR_CANSparkMax m_wrist; private static Wrist instance; public Wrist() { super(new PIDController(kP, kI, kD), kS, kV, kG, kG -> (Double)(Math.sin(kG))); + setConstraints(MIN_ANGLE, MAX_ANGLE); configMotor(); } @@ -33,31 +35,33 @@ public static Wrist getInstance() { } public void configMotor() { - m_pivot = new NAR_CANSparkMax(PIVOT_ID); - m_pivot.setInverted(false); - m_pivot.setIdleMode(IdleMode.kBrake); - m_pivot.enableVoltageCompensation(12.0); + m_wrist = new NAR_CANSparkMax(WRIST_ID); + m_wrist.setInverted(false); + m_wrist.setIdleMode(IdleMode.kBrake); + m_wrist.enableVoltageCompensation(12.0); resetEncoder(); } - @ - public void startPID(double anglePos) { - anglePos = MathUtil.clamp(anglePos,0,135); - super.startPID(anglePos); + public double getAngle() { + return m_wrist.getSelectedSensorPosition() * ROTATION_TO_DEGREES / GEAR_RATIO; } @Override protected void useOutput(double output, double setpoint) { - m_pivot.set(MathUtil.clamp(output / 12.0, -1, 1)); + m_wrist.set(MathUtil.clamp(output / 12.0, -1, 1)); } @Override protected double getMeasurement() { - return m_pivot.getSelectedSensorPosition() * ROTATION_TO_DEGREES / GEAR_RATIO; + return getAngle(); } public void resetEncoder() { - m_pivot.setSelectedSensorPosition(0); + m_wrist.setSelectedSensorPosition(0); + } + + public void initShuffleboard() { + NAR_Shuffleboard.addData("Wrist", "Wrist Angle", () -> getAngle(), 0, 1); } } From 82d74f28584fc73166401d44e04c5edd8dc44ea4 Mon Sep 17 00:00:00 2001 From: Mason-Lam <97353903+Mason-Lam@users.noreply.github.com> Date: Mon, 24 Jul 2023 16:14:17 -0700 Subject: [PATCH 4/8] Code Review 7/24 --- src/main/java/frc/team3128/Constants.java | 10 +-- .../frc/team3128/subsystems/Manipulator.java | 62 +++++++++---------- .../java/frc/team3128/subsystems/Wrist.java | 38 ++++-------- 3 files changed, 48 insertions(+), 62 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 035abee..d5b13c2 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -506,7 +506,7 @@ public static class IntakeConstants { public static class WristConstants { - public static final double kP = 0; + public static final double kP = 0.4; public static final double kI = 0; public static final double kD = 0; @@ -514,21 +514,21 @@ public static class WristConstants { public static final double kV = 0; public static final double kG = 0; - public static final double GEAR_RATIO = 0.0; + public static final double GEAR_RATIO = 1.0; public static final double ROTATION_TO_DEGREES = 360; public static final double ANGLE_OFFSET = 0; public static final int ENCODER_DIO_ID = 8; - public static final int MIN_ANGLE = 0; - public static final int MAX_ANGLE = 0; + public static final int MIN_ANGLE = -45; + public static final int MAX_ANGLE = 90; public static final double VELOCITY_SETPOINT = 0.5; public static final double INTAKE_TOLERANCE = 7.5; //Motor ID - public static final int WRIST_ID = 0; + public static final int WRIST_ID = 10; //Sensor IDs diff --git a/src/main/java/frc/team3128/subsystems/Manipulator.java b/src/main/java/frc/team3128/subsystems/Manipulator.java index db8570d..7c0af33 100644 --- a/src/main/java/frc/team3128/subsystems/Manipulator.java +++ b/src/main/java/frc/team3128/subsystems/Manipulator.java @@ -1,6 +1,5 @@ package frc.team3128.subsystems; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.team3128.common.hardware.motorcontroller.NAR_TalonSRX; import frc.team3128.common.utility.NAR_Shuffleboard; @@ -13,13 +12,9 @@ public class Manipulator extends SubsystemBase { private static Manipulator instance; - public static boolean CONE = true; + public static boolean isCone = true; - public boolean outtaking = false; - - public Manipulator(){ - configMotor(); - } + public boolean isOuttaking = false; public static Manipulator getInstance() { if (instance == null){ @@ -29,17 +24,39 @@ public static Manipulator getInstance() { return instance; } + public Manipulator(){ + configMotor(); + initShuffleboard(); + } + + //Move this to a trigger in robot container @Override public void periodic() { - if (Math.abs(getCurrent()) > ABSOLUTE_THRESHOLD + 40 && !outtaking) + if (Math.abs(getCurrent()) > ABSOLUTE_THRESHOLD + 40 && !isOuttaking) stallPower(); } - public void configMotor(){ + private void configMotor(){ m_roller = new NAR_TalonSRX(ROLLER_MOTOR_ID); m_roller.setInverted(false); m_roller.setNeutralMode(NeutralMode.Brake); - m_roller.enableVoltageCompensation(true); + } + + public void intake(boolean cone) { + isOuttaking = false; + isCone = cone; + if (cone) reverse(); + else forward(); + } + + public void outtake(){ + isOuttaking = true; + if (!isCone) reverse(); + else forward(); + } + + public void stallPower() { + set(isCone ? -STALL_POWER : STALL_POWER); } public void set(double power){ @@ -58,33 +75,16 @@ public void stopRoller(){ m_roller.set(0); } - public double getCurrent(){ - return m_roller.getStatorCurrent(); - } - - public double getVoltage() { - return m_roller.getMotorOutputVoltage(); - } - public boolean hasObjectPresent(){ return Math.abs(getCurrent()) > ABSOLUTE_THRESHOLD; } - public void intake(boolean cone) { - outtaking = false; - CONE = cone; - if (cone) reverse(); - else forward(); - } - - public void outtake(){ - outtaking = true; - if (!CONE) reverse(); - else forward(); + public double getCurrent(){ + return m_roller.getStatorCurrent(); } - public void stallPower() { - set(CONE ? -STALL_POWER : STALL_POWER); + public double getVoltage() { + return m_roller.getMotorOutputVoltage(); } public void initShuffleboard() { diff --git a/src/main/java/frc/team3128/subsystems/Wrist.java b/src/main/java/frc/team3128/subsystems/Wrist.java index 309474a..899f22d 100644 --- a/src/main/java/frc/team3128/subsystems/Wrist.java +++ b/src/main/java/frc/team3128/subsystems/Wrist.java @@ -1,67 +1,53 @@ package frc.team3128.subsystems; -import java.util.function.DoubleFunction; -import java.util.function.DoubleSupplier; - -import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.PIDSubsystem; +import edu.wpi.first.math.util.Units; import static frc.team3128.Constants.WristConstants.*; -import frc.team3128.RobotContainer; import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax; -import frc.team3128.common.utility.NAR_Shuffleboard; +//Set up absolute encoder code to easily switch public class Wrist extends NAR_PIDSubsystem { public NAR_CANSparkMax m_wrist; private static Wrist instance; - public Wrist() { - super(new PIDController(kP, kI, kD), kS, kV, kG, kG -> (Double)(Math.sin(kG))); - setConstraints(MIN_ANGLE, MAX_ANGLE); - configMotor(); - } - public static Wrist getInstance() { if (instance == null){ instance = new Wrist(); } - return instance; } - public void configMotor() { + public Wrist() { + super(new PIDController(kP, kI, kD), kS, kV, kG); + setkG_Function(()-> Math.cos(Units.degreesToRadians(getMeasurement()))); + setConstraints(MIN_ANGLE, MAX_ANGLE); + configMotor(); + } + + private void configMotor() { m_wrist = new NAR_CANSparkMax(WRIST_ID); m_wrist.setInverted(false); m_wrist.setIdleMode(IdleMode.kBrake); - m_wrist.enableVoltageCompensation(12.0); resetEncoder(); } - public double getAngle() { - return m_wrist.getSelectedSensorPosition() * ROTATION_TO_DEGREES / GEAR_RATIO; - } - @Override protected void useOutput(double output, double setpoint) { m_wrist.set(MathUtil.clamp(output / 12.0, -1, 1)); } @Override - protected double getMeasurement() { - return getAngle(); + public double getMeasurement() { + return m_wrist.getSelectedSensorPosition() * ROTATION_TO_DEGREES / GEAR_RATIO; } public void resetEncoder() { m_wrist.setSelectedSensorPosition(0); } - public void initShuffleboard() { - NAR_Shuffleboard.addData("Wrist", "Wrist Angle", () -> getAngle(), 0, 1); - } - } From 86adad63fabfccd84d53c65bc31573bc9f62711f Mon Sep 17 00:00:00 2001 From: Eva-Unit-02 <93512609+Eva-Unit-02@users.noreply.github.com> Date: Mon, 24 Jul 2023 17:45:04 -0700 Subject: [PATCH 5/8] Add absolute encoder --- src/main/java/frc/team3128/Constants.java | 4 +++- .../java/frc/team3128/subsystems/Manipulator.java | 4 ++-- src/main/java/frc/team3128/subsystems/Wrist.java | 14 ++++++++++++-- 3 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index d5b13c2..e2b7de9 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -517,8 +517,10 @@ public static class WristConstants { public static final double GEAR_RATIO = 1.0; public static final double ROTATION_TO_DEGREES = 360; - public static final double ANGLE_OFFSET = 0; + public static final double ENCODER_CONVERSION_FACTOR_TO_DEGREES = 360; + + public static final double ANGLE_OFFSET = 0; public static final int ENCODER_DIO_ID = 8; public static final int MIN_ANGLE = -45; diff --git a/src/main/java/frc/team3128/subsystems/Manipulator.java b/src/main/java/frc/team3128/subsystems/Manipulator.java index 7c0af33..a89b900 100644 --- a/src/main/java/frc/team3128/subsystems/Manipulator.java +++ b/src/main/java/frc/team3128/subsystems/Manipulator.java @@ -29,7 +29,7 @@ public Manipulator(){ initShuffleboard(); } - //Move this to a trigger in robot container + //TODO Move this to a trigger in robot container @Override public void periodic() { if (Math.abs(getCurrent()) > ABSOLUTE_THRESHOLD + 40 && !isOuttaking) @@ -45,7 +45,7 @@ private void configMotor(){ public void intake(boolean cone) { isOuttaking = false; isCone = cone; - if (cone) reverse(); + if (isCone) reverse(); else forward(); } diff --git a/src/main/java/frc/team3128/subsystems/Wrist.java b/src/main/java/frc/team3128/subsystems/Wrist.java index 899f22d..0d0fd01 100644 --- a/src/main/java/frc/team3128/subsystems/Wrist.java +++ b/src/main/java/frc/team3128/subsystems/Wrist.java @@ -5,14 +5,17 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DutyCycleEncoder; + import static frc.team3128.Constants.WristConstants.*; import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax; -//Set up absolute encoder code to easily switch public class Wrist extends NAR_PIDSubsystem { public NAR_CANSparkMax m_wrist; + private DutyCycleEncoder m_encoder; + private static Wrist instance; public static Wrist getInstance() { @@ -27,15 +30,21 @@ public Wrist() { setkG_Function(()-> Math.cos(Units.degreesToRadians(getMeasurement()))); setConstraints(MIN_ANGLE, MAX_ANGLE); configMotor(); + configEncoders(); } private void configMotor() { m_wrist = new NAR_CANSparkMax(WRIST_ID); m_wrist.setInverted(false); m_wrist.setIdleMode(IdleMode.kBrake); + m_wrist.setSmartCurrentLimit(40); resetEncoder(); } + private void configEncoders() { + m_encoder = new DutyCycleEncoder(ENCODER_DIO_ID); + } + @Override protected void useOutput(double output, double setpoint) { m_wrist.set(MathUtil.clamp(output / 12.0, -1, 1)); @@ -43,7 +52,8 @@ protected void useOutput(double output, double setpoint) { @Override public double getMeasurement() { - return m_wrist.getSelectedSensorPosition() * ROTATION_TO_DEGREES / GEAR_RATIO; + // return m_wrist.getSelectedSensorPosition() * ROTATION_TO_DEGREES / GEAR_RATIO; + return MathUtil.inputModulus(-m_encoder.get() * ENCODER_CONVERSION_FACTOR_TO_DEGREES - ANGLE_OFFSET,-180, 180); } public void resetEncoder() { From 45e8e0cc9397fb1f89312ceb6a964d727072f9b3 Mon Sep 17 00:00:00 2001 From: Eva-Unit-02 <93512609+Eva-Unit-02@users.noreply.github.com> Date: Sun, 10 Sep 2023 13:04:56 -0700 Subject: [PATCH 6/8] wrist enum --- .../frc/team3128/commands/CmdManager.java | 17 ++++++++++++---- .../java/frc/team3128/subsystems/Wrist.java | 20 +++++++++++++++++++ 2 files changed, 33 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 469b16e..9cf4849 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -1,17 +1,19 @@ package frc.team3128.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.Commands; +import static edu.wpi.first.wpilibj2.command.Commands.*; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.team3128.RobotContainer; +import frc.team3128.Constants.WristConstants; import frc.team3128.common.hardware.input.NAR_XboxController; import frc.team3128.common.narwhaldashboard.NarwhalDashboard; import frc.team3128.subsystems.Led; import frc.team3128.subsystems.Wrist; +import frc.team3128.subsystems.Wrist.WristPosition; import frc.team3128.subsystems.Manipulator; import frc.team3128.subsystems.Vision; @@ -25,9 +27,16 @@ public class CmdManager { private CmdManager() {} public static CommandBase CmdWrist(double setpoint) { - return Commands.sequence( - new InstantCommand(()-> wrist.startPID(setpoint), wrist), - new WaitUntilCommand(()-> wrist.atSetpoint()) + return sequence( + runOnce(()-> wrist.startPID(setpoint), wrist), + waitUntil(()-> wrist.atSetpoint()) + ); + } + + public static CommandBase CmdWrist(WristPosition position) { + return sequence( + runOnce(()-> wrist.startPID(position.wristAngle), wrist), + waitUntil(()-> wrist.atSetpoint()) ); } diff --git a/src/main/java/frc/team3128/subsystems/Wrist.java b/src/main/java/frc/team3128/subsystems/Wrist.java index 0d0fd01..c79dcbe 100644 --- a/src/main/java/frc/team3128/subsystems/Wrist.java +++ b/src/main/java/frc/team3128/subsystems/Wrist.java @@ -17,6 +17,26 @@ public class Wrist extends NAR_PIDSubsystem { private DutyCycleEncoder m_encoder; private static Wrist instance; + + public enum WristPosition { + SCORE_CONE(0), + SCORE_CUBE(0), + + NEUTRAL(0), + + SINGLE_SHELF_CONE(0), + SINGLE_SHELF_CUBE(0), + DOUBLE_SHELF_CONE(0), + DOUBLE_SHELF_CUBE(0), + GROUND_PICKUP_CONE(0), + GROUND_PICKUP_CUBE(0); + + public final double wristAngle; + + private WristPosition(double wristAngle) { + this.wristAngle = wristAngle; + } + } public static Wrist getInstance() { if (instance == null){ From 5df38cd80d40abe2acf8e2e9ea384561745c6900 Mon Sep 17 00:00:00 2001 From: Eva-Unit-02 <93512609+Eva-Unit-02@users.noreply.github.com> Date: Mon, 11 Sep 2023 18:06:37 -0700 Subject: [PATCH 7/8] 9/11 testing changes --- src/main/java/frc/team3128/Constants.java | 14 ++++---- .../frc/team3128/commands/CmdManager.java | 34 +++++++++++-------- .../frc/team3128/subsystems/Manipulator.java | 6 ++-- 3 files changed, 31 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index e2b7de9..e70decd 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -537,14 +537,16 @@ public static class WristConstants { } public static class ManipulatorConstants{ - public static final int ROLLER_MOTOR_ID = 13; - public static final double ROLLER_POWER = 0.6; - public static final double STALL_POWER = 0.25; + public static final int ROLLER_MOTOR_ID = 4; + public static final double ROLLER_POWER = 0.9; + public static final double STALL_POWER_CONE = 0.2; + public static final double STALL_POWER_CUBE = 0.0; - public static final double CURRENT_THRESHOLD = 5; - public static final double ABSOLUTE_THRESHOLD = 20; - public static final double ROLLER_VOLTAGE = 8; + public static final double CURRENT_THRESHOLD_CONE = 20; + public static final double CURRENT_THRESHOLD_CUBE = 15; + + public static final double ABSOLUTE_THRESHOLD = 20; } public static class BalanceConstants{ diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 9cf4849..3982e84 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -19,29 +19,35 @@ public class CmdManager { private static Led led = Led.getInstance(); - private static Wrist wrist = Wrist.getInstance(); + // private static Wrist wrist = Wrist.getInstance(); private static Manipulator manipulator = Manipulator.getInstance(); private static NAR_XboxController controller = RobotContainer.controller; private CmdManager() {} - public static CommandBase CmdWrist(double setpoint) { - return sequence( - runOnce(()-> wrist.startPID(setpoint), wrist), - waitUntil(()-> wrist.atSetpoint()) - ); - } + // public static CommandBase CmdWrist(double setpoint) { + // return sequence( + // runOnce(()-> wrist.startPID(setpoint), wrist), + // waitUntil(()-> wrist.atSetpoint()) + // ); + // } - public static CommandBase CmdWrist(WristPosition position) { - return sequence( - runOnce(()-> wrist.startPID(position.wristAngle), wrist), - waitUntil(()-> wrist.atSetpoint()) - ); - } + // public static CommandBase CmdWrist(WristPosition position) { + // return sequence( + // runOnce(()-> wrist.startPID(position.wristAngle), wrist), + // waitUntil(()-> wrist.atSetpoint()) + // ); + // } public static CommandBase CmdManipIntake(Boolean cone) { - return new InstantCommand(()-> manipulator.intake(cone), manipulator); + return sequence( + runOnce(()-> manipulator.intake(cone), manipulator), + waitSeconds(0.4), + waitUntil(()-> manipulator.hasObjectPresent()), + waitSeconds(cone ? 0.15 : 0), + runOnce(()-> manipulator.stallPower()) + ); } public static CommandBase CmdManipOuttake() { diff --git a/src/main/java/frc/team3128/subsystems/Manipulator.java b/src/main/java/frc/team3128/subsystems/Manipulator.java index a89b900..ec23a61 100644 --- a/src/main/java/frc/team3128/subsystems/Manipulator.java +++ b/src/main/java/frc/team3128/subsystems/Manipulator.java @@ -38,7 +38,7 @@ public void periodic() { private void configMotor(){ m_roller = new NAR_TalonSRX(ROLLER_MOTOR_ID); - m_roller.setInverted(false); + m_roller.setInverted(true); m_roller.setNeutralMode(NeutralMode.Brake); } @@ -56,7 +56,7 @@ public void outtake(){ } public void stallPower() { - set(isCone ? -STALL_POWER : STALL_POWER); + set(isCone ? -STALL_POWER_CONE : STALL_POWER_CUBE); } public void set(double power){ @@ -76,7 +76,7 @@ public void stopRoller(){ } public boolean hasObjectPresent(){ - return Math.abs(getCurrent()) > ABSOLUTE_THRESHOLD; + return isCone ? Math.abs(getCurrent()) > CURRENT_THRESHOLD_CONE : Math.abs(getCurrent()) > CURRENT_THRESHOLD_CUBE; } public double getCurrent(){ From bf2c7a0632fc1b1f2626409019c46464a81bdf87 Mon Sep 17 00:00:00 2001 From: Eva-Unit-02 <93512609+Eva-Unit-02@users.noreply.github.com> Date: Sat, 16 Sep 2023 15:52:38 -0700 Subject: [PATCH 8/8] Wrist changes --- src/main/java/frc/team3128/Constants.java | 6 ++-- .../frc/team3128/commands/CmdManager.java | 30 +++++++++++-------- .../java/frc/team3128/subsystems/Wrist.java | 28 ++++++++++------- 3 files changed, 37 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index e70decd..8f2368f 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -514,7 +514,7 @@ public static class WristConstants { public static final double kV = 0; public static final double kG = 0; - public static final double GEAR_RATIO = 1.0; + public static final double GEAR_RATIO = 108.0; public static final double ROTATION_TO_DEGREES = 360; @@ -530,14 +530,14 @@ public static class WristConstants { public static final double INTAKE_TOLERANCE = 7.5; //Motor ID - public static final int WRIST_ID = 10; + public static final int WRIST_ID = 25; //Sensor IDs } public static class ManipulatorConstants{ - public static final int ROLLER_MOTOR_ID = 4; + public static final int ROLLER_MOTOR_ID = 0; public static final double ROLLER_POWER = 0.9; public static final double STALL_POWER_CONE = 0.2; public static final double STALL_POWER_CUBE = 0.0; diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 3982e84..5e5f866 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -19,26 +19,30 @@ public class CmdManager { private static Led led = Led.getInstance(); - // private static Wrist wrist = Wrist.getInstance(); + private static Wrist wrist = Wrist.getInstance(); private static Manipulator manipulator = Manipulator.getInstance(); private static NAR_XboxController controller = RobotContainer.controller; private CmdManager() {} - // public static CommandBase CmdWrist(double setpoint) { - // return sequence( - // runOnce(()-> wrist.startPID(setpoint), wrist), - // waitUntil(()-> wrist.atSetpoint()) - // ); - // } + public static CommandBase CmdWrist(double setpoint) { + return sequence( + runOnce(()-> wrist.startPID(setpoint), wrist), + waitUntil(()-> wrist.atSetpoint()) + ); + } + + public static CommandBase CmdWrist(WristPosition position) { + return sequence( + runOnce(()-> wrist.startPID(position.wristAngle), wrist), + waitUntil(()-> wrist.atSetpoint()) + ); + } - // public static CommandBase CmdWrist(WristPosition position) { - // return sequence( - // runOnce(()-> wrist.startPID(position.wristAngle), wrist), - // waitUntil(()-> wrist.atSetpoint()) - // ); - // } + public static CommandBase CmdMoveWrist(double power) { + return new InstantCommand(() -> wrist.set(power), wrist); + } public static CommandBase CmdManipIntake(Boolean cone) { return sequence( diff --git a/src/main/java/frc/team3128/subsystems/Wrist.java b/src/main/java/frc/team3128/subsystems/Wrist.java index c79dcbe..7690ebd 100644 --- a/src/main/java/frc/team3128/subsystems/Wrist.java +++ b/src/main/java/frc/team3128/subsystems/Wrist.java @@ -1,21 +1,21 @@ package frc.team3128.subsystems; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DutyCycleEncoder; import static frc.team3128.Constants.WristConstants.*; import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax; +import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax.EncoderType; +import frc.team3128.common.utility.NAR_Shuffleboard; public class Wrist extends NAR_PIDSubsystem { public NAR_CANSparkMax m_wrist; - private DutyCycleEncoder m_encoder; - private static Wrist instance; public enum WristPosition { @@ -50,21 +50,16 @@ public Wrist() { setkG_Function(()-> Math.cos(Units.degreesToRadians(getMeasurement()))); setConstraints(MIN_ANGLE, MAX_ANGLE); configMotor(); - configEncoders(); } private void configMotor() { - m_wrist = new NAR_CANSparkMax(WRIST_ID); + m_wrist = new NAR_CANSparkMax(WRIST_ID, EncoderType.Absolute, MotorType.kBrushless); m_wrist.setInverted(false); m_wrist.setIdleMode(IdleMode.kBrake); m_wrist.setSmartCurrentLimit(40); resetEncoder(); } - private void configEncoders() { - m_encoder = new DutyCycleEncoder(ENCODER_DIO_ID); - } - @Override protected void useOutput(double output, double setpoint) { m_wrist.set(MathUtil.clamp(output / 12.0, -1, 1)); @@ -72,12 +67,23 @@ protected void useOutput(double output, double setpoint) { @Override public double getMeasurement() { - // return m_wrist.getSelectedSensorPosition() * ROTATION_TO_DEGREES / GEAR_RATIO; - return MathUtil.inputModulus(-m_encoder.get() * ENCODER_CONVERSION_FACTOR_TO_DEGREES - ANGLE_OFFSET,-180, 180); + // return m_wrist.getSelectedSensorPosition() * ROTATION_TO_DEGREES / GEAR_RATIO + ANGLE_OFFSET; + return 12.3; + // return MathUtil.inputModulus(-m_encoder.get() * ENCODER_CONVERSION_FACTOR_TO_DEGREES - ANGLE_OFFSET,-180, 180); } public void resetEncoder() { m_wrist.setSelectedSensorPosition(0); } + public void set(double power) { + m_wrist.set(power); + } + + public void initShuffleboard() { + // NAR_Shuffleboard.addData("Manipulator", "Manip current", () -> getCurrent(), 0, 1); + // NAR_Shuffleboard.addData("Manipulator", "get", () -> m_roller.getMotorOutputPercent(), 0, 3); + NAR_Shuffleboard.addData("Wrist", "angle", ()-> m_wrist.getSelectedSensorPosition(), 1, 1); + } + }