From 2051054cf7385cdf943eda20d40fc0328d02282c Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Mon, 27 Jan 2025 20:47:54 +1100 Subject: [PATCH 01/16] create wrist encoder variable, encoder offset varaible and wrist encoder id --- components/wrist.py | 9 +++++++-- ids.py | 7 +++++++ 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/components/wrist.py b/components/wrist.py index 6f308a05..985866d4 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -3,15 +3,16 @@ from magicbot import feedback, tunable from rev import ClosedLoopSlot, SparkMax, SparkMaxConfig -from wpilib import DigitalInput +from wpilib import AnalogEncoder, DigitalInput from wpimath.controller import ArmFeedforward from wpimath.trajectory import TrapezoidProfile -from ids import DioChannel, SparkId +from ids import AnalogChannel, DioChannel, SparkId from utilities.functions import clamp class WristComponent: + ENCODER_ZERO_OFFSET = math.tau / 10 MAXIMUM_DEPRESSION = math.radians(-113.0) MAXIMUM_ELEVATION = math.radians(-10.0) NEUTRAL_ANGLE = math.radians(-90.0) @@ -30,6 +31,10 @@ class WristComponent: def __init__(self): self.switch = DigitalInput(DioChannel.WRIST_LIMIT_SWITCH) + self.wrist_encoder = AnalogEncoder( + AnalogChannel.WRIST_ENCODER, math.tau, self.ENCODER_ZERO_OFFSET + ) + self.wrist = SparkMax(SparkId.WRIST, SparkMax.MotorType.kBrushless) self.wrist_controller = self.wrist.getClosedLoopController() diff --git a/ids.py b/ids.py index 1f1be3e9..f939ca0e 100644 --- a/ids.py +++ b/ids.py @@ -77,3 +77,10 @@ class RioSerialNumber(enum.StrEnum): TEST_BOT = "0305cc42" COMP_BOT = "03062898" + + +@enum.unique +class AnalogChannel(enum.IntEnum): + """roboRIO Analog input channel number""" + + WRIST_ENCODER = 0 From edb479788e062d8654f0805c95994c2cd9c245f2 Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Mon, 27 Jan 2025 20:54:28 +1100 Subject: [PATCH 02/16] change inclination get function to report shaft encoder value --- components/wrist.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/wrist.py b/components/wrist.py index 985866d4..1f836899 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -98,7 +98,7 @@ def wrist_at_bottom_limit(self) -> bool: @feedback def inclination(self) -> float: - return self.encoder.getPosition() + return self.wrist_encoder.get() @feedback def inclination_deg(self) -> float: From 240f8426fae7b7c44373002d83b1b9c2512514b5 Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Mon, 27 Jan 2025 23:40:24 +1100 Subject: [PATCH 03/16] invert wrisdt encoder, rework variables to seperate wrist encoder and motor encoder --- components/wrist.py | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/components/wrist.py b/components/wrist.py index 1f836899..8abfcd40 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -34,10 +34,11 @@ def __init__(self): self.wrist_encoder = AnalogEncoder( AnalogChannel.WRIST_ENCODER, math.tau, self.ENCODER_ZERO_OFFSET ) + self.wrist_encoder.setInverted(True) - self.wrist = SparkMax(SparkId.WRIST, SparkMax.MotorType.kBrushless) + self.wrist_motor = SparkMax(SparkId.WRIST, SparkMax.MotorType.kBrushless) - self.wrist_controller = self.wrist.getClosedLoopController() + self.wrist_motor_controller = self.wrist_motor.getClosedLoopController() wrist_config = SparkMaxConfig() wrist_config.inverted(False) @@ -60,13 +61,13 @@ def __init__(self): (1 / 60) * math.tau * (1 / self.wrist_gear_ratio) ) - self.wrist.configure( + self.wrist_motor.configure( wrist_config, SparkMax.ResetMode.kResetSafeParameters, SparkMax.PersistMode.kPersistParameters, ) - self.encoder = self.wrist.getEncoder() + self.motor_encoder = self.wrist_motor.getEncoder() self.desired_angle = WristComponent.NEUTRAL_ANGLE @@ -74,7 +75,7 @@ def on_enable(self): self.tilt_to(WristComponent.NEUTRAL_ANGLE) wrist_config = SparkMaxConfig() wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kBrake) - self.wrist.configure( + self.wrist_motor.configure( wrist_config, SparkMax.ResetMode.kNoResetSafeParameters, SparkMax.PersistMode.kNoPersistParameters, @@ -83,7 +84,7 @@ def on_enable(self): def on_disable(self): wrist_config = SparkMaxConfig() wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kCoast) - self.wrist.configure( + self.wrist_motor.configure( wrist_config, SparkMax.ResetMode.kNoResetSafeParameters, SparkMax.PersistMode.kNoPersistParameters, @@ -102,7 +103,7 @@ def inclination(self) -> float: @feedback def inclination_deg(self) -> float: - return math.degrees(self.encoder.getPosition()) + return math.degrees(self.wrist_encoder.get()) @feedback def shoot_angle_deg(self) -> float: @@ -110,7 +111,7 @@ def shoot_angle_deg(self) -> float: @feedback def current_velocity(self) -> float: - return self.encoder.getVelocity() + return self.motor_encoder.getVelocity() @feedback def at_setpoint(self) -> bool: @@ -128,11 +129,11 @@ def reset_windup(self) -> None: def execute(self) -> None: if self.wrist_at_bottom_limit(): - self.encoder.setPosition(self.MAXIMUM_DEPRESSION) + self.motor_encoder.setPosition(self.MAXIMUM_DEPRESSION) self.has_indexed = True if not self.has_indexed: - self.wrist.setVoltage(self.zeroing_voltage) + self.wrist_motor.setVoltage(self.zeroing_voltage) return desired_state = self.wrist_profile.calculate( @@ -141,7 +142,8 @@ def execute(self) -> None: TrapezoidProfile.State(self.desired_angle, 0.0), ) ff = self.wrist_ff.calculate(desired_state.position, desired_state.velocity) - self.wrist_controller.setReference( + + self.wrist_motor_controller.setReference( desired_state.position, SparkMax.ControlType.kPosition, slot=ClosedLoopSlot.kSlot0, From 6946f303b5bd132c9c5aaa448e1ae2561920c215 Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Tue, 28 Jan 2025 19:37:12 +1100 Subject: [PATCH 04/16] rename vars --- components/wrist.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/components/wrist.py b/components/wrist.py index 8abfcd40..0c79eb15 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -36,9 +36,9 @@ def __init__(self): ) self.wrist_encoder.setInverted(True) - self.wrist_motor = SparkMax(SparkId.WRIST, SparkMax.MotorType.kBrushless) + self.motor = SparkMax(SparkId.WRIST, SparkMax.MotorType.kBrushless) - self.wrist_motor_controller = self.wrist_motor.getClosedLoopController() + self.motor_controller = self.motor.getClosedLoopController() wrist_config = SparkMaxConfig() wrist_config.inverted(False) @@ -61,13 +61,13 @@ def __init__(self): (1 / 60) * math.tau * (1 / self.wrist_gear_ratio) ) - self.wrist_motor.configure( + self.motor.configure( wrist_config, SparkMax.ResetMode.kResetSafeParameters, SparkMax.PersistMode.kPersistParameters, ) - self.motor_encoder = self.wrist_motor.getEncoder() + self.motor_encoder = self.motor.getEncoder() self.desired_angle = WristComponent.NEUTRAL_ANGLE @@ -75,7 +75,7 @@ def on_enable(self): self.tilt_to(WristComponent.NEUTRAL_ANGLE) wrist_config = SparkMaxConfig() wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kBrake) - self.wrist_motor.configure( + self.motor.configure( wrist_config, SparkMax.ResetMode.kNoResetSafeParameters, SparkMax.PersistMode.kNoPersistParameters, @@ -84,7 +84,7 @@ def on_enable(self): def on_disable(self): wrist_config = SparkMaxConfig() wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kCoast) - self.wrist_motor.configure( + self.motor.configure( wrist_config, SparkMax.ResetMode.kNoResetSafeParameters, SparkMax.PersistMode.kNoPersistParameters, @@ -133,7 +133,7 @@ def execute(self) -> None: self.has_indexed = True if not self.has_indexed: - self.wrist_motor.setVoltage(self.zeroing_voltage) + self.motor.setVoltage(self.zeroing_voltage) return desired_state = self.wrist_profile.calculate( @@ -143,7 +143,7 @@ def execute(self) -> None: ) ff = self.wrist_ff.calculate(desired_state.position, desired_state.velocity) - self.wrist_motor_controller.setReference( + self.motor_controller.setReference( desired_state.position, SparkMax.ControlType.kPosition, slot=ClosedLoopSlot.kSlot0, From 748c185aedfdeb7c4338f140d3119de130bece59 Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Tue, 28 Jan 2025 19:40:25 +1100 Subject: [PATCH 05/16] remove zeroing functionality --- components/wrist.py | 17 +---------------- 1 file changed, 1 insertion(+), 16 deletions(-) diff --git a/components/wrist.py b/components/wrist.py index 0c79eb15..1053d558 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -1,7 +1,7 @@ import math import time -from magicbot import feedback, tunable +from magicbot import feedback from rev import ClosedLoopSlot, SparkMax, SparkMaxConfig from wpilib import AnalogEncoder, DigitalInput from wpimath.controller import ArmFeedforward @@ -24,10 +24,6 @@ class WristComponent: wrist_gear_ratio = 432.0 TOLERANCE = math.radians(3.0) - zeroing_voltage = tunable(-1.0) - - has_indexed = tunable(False) - def __init__(self): self.switch = DigitalInput(DioChannel.WRIST_LIMIT_SWITCH) @@ -90,9 +86,6 @@ def on_disable(self): SparkMax.PersistMode.kNoPersistParameters, ) - def zero_wrist(self) -> None: - self.has_indexed = False - @feedback def wrist_at_bottom_limit(self) -> bool: return not self.switch.get() @@ -128,14 +121,6 @@ def reset_windup(self) -> None: self.tilt_to(self.inclination()) def execute(self) -> None: - if self.wrist_at_bottom_limit(): - self.motor_encoder.setPosition(self.MAXIMUM_DEPRESSION) - self.has_indexed = True - - if not self.has_indexed: - self.motor.setVoltage(self.zeroing_voltage) - return - desired_state = self.wrist_profile.calculate( time.monotonic() - self.last_setpoint_update_time, TrapezoidProfile.State(self.inclination(), self.current_velocity()), From fd75894d6a0c52ba5c34972f1ce7110b91db77e5 Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Tue, 28 Jan 2025 21:26:27 +1100 Subject: [PATCH 06/16] remove zero wrist button in test --- robot.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/robot.py b/robot.py index 39ffdff8..4187fc64 100644 --- a/robot.py +++ b/robot.py @@ -261,8 +261,6 @@ def testPeriodic(self) -> None: self.algae_manipulator_component.execute() - if self.gamepad.getLeftBumper(): - self.wrist.zero_wrist() if self.gamepad.getLeftTriggerAxis() > 0.3: self.wrist.tilt_to(self.inclination_angle) self.wrist.execute() From 896cb5d4a56a7b00af1afbaace2cce1a047be537 Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Sun, 2 Feb 2025 13:51:06 +1100 Subject: [PATCH 07/16] Remove motor controller PID --- components/wrist.py | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/components/wrist.py b/components/wrist.py index 1053d558..31f2fdfc 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -2,9 +2,13 @@ import time from magicbot import feedback -from rev import ClosedLoopSlot, SparkMax, SparkMaxConfig +from rev import ( + ClosedLoopSlot, + SparkMax, + SparkMaxConfig, +) from wpilib import AnalogEncoder, DigitalInput -from wpimath.controller import ArmFeedforward +from wpimath.controller import ArmFeedforward, PIDController from wpimath.trajectory import TrapezoidProfile from ids import AnalogChannel, DioChannel, SparkId @@ -39,14 +43,8 @@ def __init__(self): wrist_config = SparkMaxConfig() wrist_config.inverted(False) wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kBrake) - wrist_config.closedLoop.P( - 7.6813, - ClosedLoopSlot.kSlot0, - ) - wrist_config.closedLoop.D(69.887, ClosedLoopSlot.kSlot0) - self.wrist_profile = TrapezoidProfile( - TrapezoidProfile.Constraints(self.WRIST_MAX_VEL, self.WRIST_MAX_ACC) - ) + + self.pid = PIDController(Kp=7.6813, Ki=0, Kd=69.887) self.wrist_ff = ArmFeedforward(kS=0.42619, kG=0.09, kV=8.42, kA=0.0) From ba856660507457abdde356625e02d30fc00af45f Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Sun, 2 Feb 2025 13:52:27 +1100 Subject: [PATCH 08/16] fix removal of PID --- components/wrist.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/components/wrist.py b/components/wrist.py index 31f2fdfc..10fc131c 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -44,6 +44,10 @@ def __init__(self): wrist_config.inverted(False) wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kBrake) + self.wrist_profile = TrapezoidProfile( + TrapezoidProfile.Constraints(self.WRIST_MAX_VEL, self.WRIST_MAX_ACC) + ) + self.pid = PIDController(Kp=7.6813, Ki=0, Kd=69.887) self.wrist_ff = ArmFeedforward(kS=0.42619, kG=0.09, kV=8.42, kA=0.0) From 4527c650698afd5ab3533801f60ed50d0b8ad606 Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Sun, 2 Feb 2025 14:08:30 +1100 Subject: [PATCH 09/16] implement rio-based PID control on wrist motor --- components/wrist.py | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/components/wrist.py b/components/wrist.py index 10fc131c..8337976a 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -3,7 +3,6 @@ from magicbot import feedback from rev import ( - ClosedLoopSlot, SparkMax, SparkMaxConfig, ) @@ -23,7 +22,7 @@ class WristComponent: WRIST_MAX_VEL = math.radians(30.0) WRIST_MAX_ACC = math.radians(15.0) - + # New gear ratio to be calculated with wrist encoder angle_change_rate_while_zeroing = tunable(math.radians(0.1)) wrist_gear_ratio = 432.0 TOLERANCE = math.radians(3.0) @@ -38,8 +37,6 @@ def __init__(self): self.motor = SparkMax(SparkId.WRIST, SparkMax.MotorType.kBrushless) - self.motor_controller = self.motor.getClosedLoopController() - wrist_config = SparkMaxConfig() wrist_config.inverted(False) wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kBrake) @@ -47,7 +44,7 @@ def __init__(self): self.wrist_profile = TrapezoidProfile( TrapezoidProfile.Constraints(self.WRIST_MAX_VEL, self.WRIST_MAX_ACC) ) - + # Values need to be modified with new gear ratio self.pid = PIDController(Kp=7.6813, Ki=0, Kd=69.887) self.wrist_ff = ArmFeedforward(kS=0.42619, kG=0.09, kV=8.42, kA=0.0) @@ -130,9 +127,6 @@ def execute(self) -> None: ) ff = self.wrist_ff.calculate(desired_state.position, desired_state.velocity) - self.motor_controller.setReference( - desired_state.position, - SparkMax.ControlType.kPosition, - slot=ClosedLoopSlot.kSlot0, - arbFeedforward=ff, + self.motor.setVoltage( + self.pid.calculate(self.inclination(), self.desired_angle) + ff ) From 6fccdce4ec0fcf5f0c8d7ee1b3dcc7dfa76ea16d Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Sun, 2 Feb 2025 14:09:07 +1100 Subject: [PATCH 10/16] remove bottom limit check --- components/wrist.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/components/wrist.py b/components/wrist.py index 8337976a..559eb404 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -85,10 +85,6 @@ def on_disable(self): SparkMax.PersistMode.kNoPersistParameters, ) - @feedback - def wrist_at_bottom_limit(self) -> bool: - return not self.switch.get() - @feedback def inclination(self) -> float: return self.wrist_encoder.get() From 675d13833920192d78614e721fdcfca49f610f74 Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Sun, 2 Feb 2025 14:50:19 +1100 Subject: [PATCH 11/16] kill the switch --- components/wrist.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/components/wrist.py b/components/wrist.py index 559eb404..e65bb809 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -6,11 +6,11 @@ SparkMax, SparkMaxConfig, ) -from wpilib import AnalogEncoder, DigitalInput +from wpilib import AnalogEncoder from wpimath.controller import ArmFeedforward, PIDController from wpimath.trajectory import TrapezoidProfile -from ids import AnalogChannel, DioChannel, SparkId +from ids import AnalogChannel, SparkId from utilities.functions import clamp @@ -28,8 +28,6 @@ class WristComponent: TOLERANCE = math.radians(3.0) def __init__(self): - self.switch = DigitalInput(DioChannel.WRIST_LIMIT_SWITCH) - self.wrist_encoder = AnalogEncoder( AnalogChannel.WRIST_ENCODER, math.tau, self.ENCODER_ZERO_OFFSET ) From 3d34328c78bdd94431ea74844f4b729b38d8e90e Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Sun, 2 Feb 2025 15:08:35 +1100 Subject: [PATCH 12/16] make motor track pid state --- components/wrist.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/wrist.py b/components/wrist.py index e65bb809..0abdf942 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -122,5 +122,5 @@ def execute(self) -> None: ff = self.wrist_ff.calculate(desired_state.position, desired_state.velocity) self.motor.setVoltage( - self.pid.calculate(self.inclination(), self.desired_angle) + ff + self.pid.calculate(self.inclination(), desired_state.position) + ff ) From 60a2c9e5f974f305d5bb93fbf74e854436742eed Mon Sep 17 00:00:00 2001 From: LH Date: Mon, 3 Feb 2025 19:01:49 +1100 Subject: [PATCH 13/16] Added missing import --- components/wrist.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/wrist.py b/components/wrist.py index 0abdf942..d817d037 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -1,7 +1,7 @@ import math import time -from magicbot import feedback +from magicbot import feedback, tunable from rev import ( SparkMax, SparkMaxConfig, From 85e1b1e75434362e3edd436aafb85d57c1d1b50e Mon Sep 17 00:00:00 2001 From: LH Date: Mon, 3 Feb 2025 19:40:51 +1100 Subject: [PATCH 14/16] renamed wrist.wrist to wrist.motor --- physics.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/physics.py b/physics.py index 225bc399..77b48d26 100644 --- a/physics.py +++ b/physics.py @@ -120,7 +120,7 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot): ] self.wrist_motor = rev.SparkMaxSim( - sparkMax=robot.wrist.wrist, motor=DCMotor.NEO(1) + sparkMax=robot.wrist.motor, motor=DCMotor.NEO(1) ) self.imu = robot.chassis.imu.sim_state From 1139185e0c714b41429c9841a778ba252087578b Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Mon, 3 Feb 2025 20:57:45 +1100 Subject: [PATCH 15/16] implement new encoder and calculate new gear ration --- components/wrist.py | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/components/wrist.py b/components/wrist.py index d817d037..2c184ccf 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -6,7 +6,7 @@ SparkMax, SparkMaxConfig, ) -from wpilib import AnalogEncoder +from wpilib import AnalogEncoder, AnalogInput from wpimath.controller import ArmFeedforward, PIDController from wpimath.trajectory import TrapezoidProfile @@ -15,23 +15,23 @@ class WristComponent: - ENCODER_ZERO_OFFSET = math.tau / 10 + ENCODER_ZERO_OFFSET = 4.427696 MAXIMUM_DEPRESSION = math.radians(-113.0) MAXIMUM_ELEVATION = math.radians(-10.0) NEUTRAL_ANGLE = math.radians(-90.0) WRIST_MAX_VEL = math.radians(30.0) WRIST_MAX_ACC = math.radians(15.0) - # New gear ratio to be calculated with wrist encoder angle_change_rate_while_zeroing = tunable(math.radians(0.1)) - wrist_gear_ratio = 432.0 + wrist_gear_ratio = 350.628 TOLERANCE = math.radians(3.0) def __init__(self): - self.wrist_encoder = AnalogEncoder( - AnalogChannel.WRIST_ENCODER, math.tau, self.ENCODER_ZERO_OFFSET - ) - self.wrist_encoder.setInverted(True) + self.wrist_encoder_raw = AnalogInput(AnalogChannel.WRIST_ENCODER) + + self.wrist_encoder = AnalogEncoder(self.wrist_encoder_raw, math.tau, 0) + self.wrist_encoder.setInverted(False) + self.wrist_encoder.setVoltagePercentageRange(0.2 / 5, 4.8 / 5) self.motor = SparkMax(SparkId.WRIST, SparkMax.MotorType.kBrushless) @@ -83,13 +83,17 @@ def on_disable(self): SparkMax.PersistMode.kNoPersistParameters, ) + @feedback + def encoder_raw_volts(self) -> float: + return self.wrist_encoder_raw.getVoltage() + @feedback def inclination(self) -> float: - return self.wrist_encoder.get() + return self.wrist_encoder.get() - self.ENCODER_ZERO_OFFSET @feedback def inclination_deg(self) -> float: - return math.degrees(self.wrist_encoder.get()) + return math.degrees(self.inclination()) @feedback def shoot_angle_deg(self) -> float: From ad8acccad42b321e35f179477ffec9fe2d97221e Mon Sep 17 00:00:00 2001 From: LeanMeanBeanMachine4774 Date: Mon, 3 Feb 2025 21:41:07 +1100 Subject: [PATCH 16/16] modified maximum angle, pid, ff --- components/wrist.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/components/wrist.py b/components/wrist.py index 2c184ccf..31af71a4 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -1,7 +1,7 @@ import math import time -from magicbot import feedback, tunable +from magicbot import feedback from rev import ( SparkMax, SparkMaxConfig, @@ -17,12 +17,11 @@ class WristComponent: ENCODER_ZERO_OFFSET = 4.427696 MAXIMUM_DEPRESSION = math.radians(-113.0) - MAXIMUM_ELEVATION = math.radians(-10.0) + MAXIMUM_ELEVATION = math.radians(0) NEUTRAL_ANGLE = math.radians(-90.0) WRIST_MAX_VEL = math.radians(30.0) WRIST_MAX_ACC = math.radians(15.0) - angle_change_rate_while_zeroing = tunable(math.radians(0.1)) wrist_gear_ratio = 350.628 TOLERANCE = math.radians(3.0) @@ -43,9 +42,9 @@ def __init__(self): TrapezoidProfile.Constraints(self.WRIST_MAX_VEL, self.WRIST_MAX_ACC) ) # Values need to be modified with new gear ratio - self.pid = PIDController(Kp=7.6813, Ki=0, Kd=69.887) + self.pid = PIDController(Kp=13.387, Ki=0, Kd=0.0078403) - self.wrist_ff = ArmFeedforward(kS=0.42619, kG=0.09, kV=8.42, kA=0.0) + self.wrist_ff = ArmFeedforward(kS=0.42619, kG=0.13, kV=6.83, kA=0.0) wrist_config.encoder.positionConversionFactor( math.tau * (1 / self.wrist_gear_ratio)