From 9f5877e14767049967c1e3bd506fdd62ab87e0aa Mon Sep 17 00:00:00 2001 From: achung28 Date: Thu, 11 Dec 2025 17:20:36 -0500 Subject: [PATCH 1/6] Anthony and Avery Intake --- subsystem/intake.py | 141 +++++++++++++++++++++++--------------------- 1 file changed, 74 insertions(+), 67 deletions(-) diff --git a/subsystem/intake.py b/subsystem/intake.py index b95ecb7..0384aaf 100644 --- a/subsystem/intake.py +++ b/subsystem/intake.py @@ -1,31 +1,49 @@ import config import constants -from toolkit.motors.ctre_motors import TalonFX -from toolkit.subsystem import Subsystem from phoenix6.hardware import CANcoder import ntcore - +from phoenix6 import StatusSignal, controls, configs, hardware, signals import math from units.SI import radians +import commands2 - - -class Intake(Subsystem): +class Intake(commands2.subsystem): + + pivot_motor_current: StatusSignal + pivot_motor_pos: StatusSignal + horizontal_motor_current: StatusSignal + horizontal_motor_pos: StatusSignal def __init__(self): super().__init__() - self.horizontal_motor: TalonFX = TalonFX( - config.horizontal_id, - config.foc_active, - inverted=False, - config=config.INTAKE_CONFIG, - ) - self.pivot_motor: TalonFX = TalonFX( - config.intake_pivot_id, - config.foc_active, - inverted=False, - config=config.INTAKE_PIVOT_CONFIG, - ) + + self.horizontal_motor = hardware.TalonFX(config.horizontal_id) + self.pivot_motor = hardware.TalonFX(config.intake_pivot_id) + self.horizontal_motor_out = controls.DutyCycleOut(0) + + self.horizontal_motor_configs = ( + configs.TalonFXConfiguration() + .with_motor_output( + configs.MotorOutputConfigs() + .with_inverted(signals.InvertedValue.CLOCKWISE_POSITIVE) + .with_neutral_mode(signals.NeutralModeValue.BRAKE) + ).with_feedback( + + + )) + + self.pivot_motor_configs = ( + configs.TalonFXConfiguration() + .with_motor_output( + configs.MotorOutputConfigs() + .with_inverted(signals.InvertedValue.CLOCKWISE_POSITIVE) + .with_neutral_mode(signals.NeutralModeValue.BRAKE) + ).with_feedback( + + ) + + ) + self.intake_running: bool = False self.pivot_angle = math.radians(0) @@ -39,56 +57,61 @@ def __init__(self): def init(self): - self.horizontal_motor.init() + self.horizontal_motor.configurator.apply(self.horizontal_motor_configs) self.pivot_motor.init() - + self.pivot_motor.configurator.apply(self.pivot_motor_configs) self.zero_pivot() + self._motion_magic_voltage = controls.MotionMagicVoltage(0) + def roll_in(self) -> None: """ spin the motors inwards to collect the coral """ - self.horizontal_motor.set_raw_output( + + self.horizontal_motor.set_control(self.control.with_output(config.horizontal_intake_speed)) + """ self.horizontal_motor.set_raw_output( config.horizontal_intake_speed ) - self.intake_running = True + """ + + self.intake_running = True def intake_algae(self) -> None: - self.horizontal_motor.set_raw_output( - -config.intake_algae_speed - ) + self.horizontal_motor.set_control(self.control.with_output(-config.horizontal_intake_speed)) self.intake_running = True def stop(self) -> None: """ stop the motors """ - self.horizontal_motor.set_raw_output(0) + self.horizontal_motor.set_control(self._duty_cycle_out.with_output(0)), + f"raw output: {0}", + self.intake_running = False def roll_out(self, speed: float = config.horizontal_intake_speed) -> None: """ eject coral in the intake """ - self.horizontal_motor.set_raw_output( - -speed - ) + + "" + self.horizontal_motor.set_control(self.control.with_output(config.speed)) self.intake_running = True def extake_algae(self) -> None: - - self.horizontal_motor.set_raw_output( - config.extake_algae_speed - ) - + self.horizontal_motor.set_control(self.control.with_output(config.extake_algae_speed)) self.intake_running = True def get_horizontal_motor_current(self) -> float: - return self.horizontal_motor.get_motor_current() + self.horizontal_motor_current.refresh() + return self.horizontal_motor_current.value def get_pivot_motor_current(self) -> float: - return self.pivot_motor.get_motor_current() + self.pivot_motor_current.refresh() + return self.pivot_motor_current.value + def limit_angle(self, angle: radians) -> radians: """ @@ -112,20 +135,16 @@ def zero_pivot(self) -> None: self.pivot_angle = ( (self.encoder.get_absolute_position().value - config.intake_encoder_zero) / constants.intake_encoder_gear_ratio * 2 * math.pi ) - - self.pivot_motor.set_sensor_position( - self.pivot_angle * constants.intake_pivot_gear_ratio / (2 * math.pi) - ) + pos = self.pivot_angle * constants.intake_pivot_gear_ratio / (2 * math.pi) + self.pivot_motor.set_position(pos), f"sensor position: {pos}" self.pivot_zeroed = True def get_pivot_angle(self): "returns current angle of pivot" + self.pivot_motor_pos.refresh() self.pivot_angle = ( - self.pivot_motor.get_sensor_position() - / constants.intake_pivot_gear_ratio - * math.pi - * 2 + self.pivot_motor_pos.value / constants.intake_pivot_gear_ratio * math.pi * 2 ) return self.pivot_angle @@ -138,33 +157,21 @@ def set_pivot_angle(self, angle: radians) -> None: """ ff = config.intake_max_ff * math.cos(config.intake_ff_offset - angle) + rotations = angle / (2 * math.pi) * constants.intake_pivot_gear_ratio self.target_angle = angle - self.pivot_motor.set_target_position( - (angle / (2 * math.pi)) * constants.intake_pivot_gear_ratio, - ff + self.moto + self.error_check( + self.pivot_motor.set_control(self._motion_magic_voltage.with_position(rotations), + f"target position: {rotations}, arbFF: {ff}",) ) + def stop_pivot(self) -> None: - self.pivot_motor.set_raw_output(0) - - def update_table(self) -> None: - table = ntcore.NetworkTableInstance.getDefault().getTable("intake") - - table.putBoolean("intake running", self.intake_running) - table.putNumber("horizontal current", self.get_horizontal_motor_current()) - table.putNumber("pivot current", self.pivot_motor.get_motor_current()) - table.putNumber("pivot applied output", self.pivot_motor.get_applied_output()) - table.putNumber("pivot angle", math.degrees(self.get_pivot_angle())) - table.putNumber("pivot absolute angle", math.degrees((self.encoder.get_absolute_position().value - config.intake_encoder_zero) / constants.intake_encoder_gear_ratio * 2 * math.pi)) - table.putNumber("absolute position", self.encoder.get_absolute_position().value) - table.putBoolean("pivot moving", self.intake_pivoting) - table.putBoolean("pivot zeroed", self.pivot_zeroed) - table.putNumber("pivot target angle", math.degrees(self.target_angle)) - table.putNumber("pivot velocity", self.pivot_motor.get_sensor_velocity()) - table.putNumber("pivot acceleration", self.pivot_motor.get_sensor_acceleration()) + self.pivot_motor.set_control(self.control.with_output(0)) def periodic(self) -> None: - if config.NT_INTAKE: - self.update_table() - + #if config.NT_INTAKE: + #self.update_table() + pass + \ No newline at end of file From 398997f4a79d9ef8e39f5f3ff90bd1b5eb7e02b6 Mon Sep 17 00:00:00 2001 From: achung28 Date: Fri, 12 Dec 2025 22:07:42 +0000 Subject: [PATCH 2/6] More intake changes --- subsystem/intake.py | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/subsystem/intake.py b/subsystem/intake.py index 0384aaf..aa2263a 100644 --- a/subsystem/intake.py +++ b/subsystem/intake.py @@ -1,7 +1,6 @@ import config import constants from phoenix6.hardware import CANcoder -import ntcore from phoenix6 import StatusSignal, controls, configs, hardware, signals import math from units.SI import radians @@ -30,7 +29,9 @@ def __init__(self): ).with_feedback( - )) + ) + + ) self.pivot_motor_configs = ( configs.TalonFXConfiguration() @@ -39,6 +40,21 @@ def __init__(self): .with_inverted(signals.InvertedValue.CLOCKWISE_POSITIVE) .with_neutral_mode(signals.NeutralModeValue.BRAKE) ).with_feedback( + configs.FeedbackConfigs() + .with_feedback_remote_sensor_id(signals.FeedbackSensorSourceValue.FUSED_CANCODER) + ).with_motion_magic( + configs.MotionMagicConfigs() + .with_motion_magic_cruise_velocity(97) + + ).with_slot1( + configs.Slot1Configs() + .with_k_p(2) + .with_k_i(0) + .with_k_d(0) + .with_k_s(-0.195) + .with_k_v(0) + .with_k_a(0) + .with_gravity_type(signals.GravityTypeValue.ARM_COSINE) ) From 7c9dfff4db81f46ebcb0057cc933e7bd5f977e36 Mon Sep 17 00:00:00 2001 From: achung28 Date: Tue, 16 Dec 2025 16:22:09 -0500 Subject: [PATCH 3/6] Updated intake.py --- subsystem/intake.py | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/subsystem/intake.py b/subsystem/intake.py index aa2263a..bf4a506 100644 --- a/subsystem/intake.py +++ b/subsystem/intake.py @@ -26,11 +26,7 @@ def __init__(self): configs.MotorOutputConfigs() .with_inverted(signals.InvertedValue.CLOCKWISE_POSITIVE) .with_neutral_mode(signals.NeutralModeValue.BRAKE) - ).with_feedback( - - ) - ) self.pivot_motor_configs = ( @@ -85,7 +81,7 @@ def roll_in(self) -> None: spin the motors inwards to collect the coral """ - self.horizontal_motor.set_control(self.control.with_output(config.horizontal_intake_speed)) + self.horizontal_motor.set_control(controls.with_output(config.horizontal_intake_speed)) """ self.horizontal_motor.set_raw_output( config.horizontal_intake_speed ) @@ -95,14 +91,14 @@ def roll_in(self) -> None: def intake_algae(self) -> None: - self.horizontal_motor.set_control(self.control.with_output(-config.horizontal_intake_speed)) + self.horizontal_motor.set_control(controls.with_output(-config.horizontal_intake_speed)) self.intake_running = True def stop(self) -> None: """ stop the motors """ - self.horizontal_motor.set_control(self._duty_cycle_out.with_output(0)), + self.horizontal_motor.set_control(controls.DutyCycleOut.with_output(0)), f"raw output: {0}", self.intake_running = False @@ -113,11 +109,11 @@ def roll_out(self, speed: float = config.horizontal_intake_speed) -> None: """ "" - self.horizontal_motor.set_control(self.control.with_output(config.speed)) + self.horizontal_motor.set_control(controls.DutyCycleOut.with_output(config.speed)) self.intake_running = True def extake_algae(self) -> None: - self.horizontal_motor.set_control(self.control.with_output(config.extake_algae_speed)) + self.horizontal_motor.set_control(controls.duty_cycle_out.with_output(config.extake_algae_speed)) self.intake_running = True def get_horizontal_motor_current(self) -> float: From cc5de2633ed689a0976bf6a826188e15cb48241c Mon Sep 17 00:00:00 2001 From: achung28 Date: Tue, 16 Dec 2025 16:24:55 -0500 Subject: [PATCH 4/6] Re-updated Intake.py --- subsystem/intake.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/subsystem/intake.py b/subsystem/intake.py index bf4a506..0c1d7a7 100644 --- a/subsystem/intake.py +++ b/subsystem/intake.py @@ -81,7 +81,7 @@ def roll_in(self) -> None: spin the motors inwards to collect the coral """ - self.horizontal_motor.set_control(controls.with_output(config.horizontal_intake_speed)) + self.horizontal_motor.set_control(controls.DutyCycleOut.with_output(config.horizontal_intake_speed)) """ self.horizontal_motor.set_raw_output( config.horizontal_intake_speed ) @@ -91,7 +91,7 @@ def roll_in(self) -> None: def intake_algae(self) -> None: - self.horizontal_motor.set_control(controls.with_output(-config.horizontal_intake_speed)) + self.horizontal_motor.set_control(controls.DutyCycleOut.with_output(-config.horizontal_intake_speed)) self.intake_running = True def stop(self) -> None: @@ -113,7 +113,7 @@ def roll_out(self, speed: float = config.horizontal_intake_speed) -> None: self.intake_running = True def extake_algae(self) -> None: - self.horizontal_motor.set_control(controls.duty_cycle_out.with_output(config.extake_algae_speed)) + self.horizontal_motor.set_control(controls.DutyCycleOut.with_output(config.extake_algae_speed)) self.intake_running = True def get_horizontal_motor_current(self) -> float: @@ -180,7 +180,7 @@ def set_pivot_angle(self, angle: radians) -> None: def stop_pivot(self) -> None: - self.pivot_motor.set_control(self.control.with_output(0)) + self.pivot_motor.set_control(controls.DutyCycleOut.with_output(0)) def periodic(self) -> None: #if config.NT_INTAKE: From 49e2de870d7aca883749d37c560f65e28f7ec699 Mon Sep 17 00:00:00 2001 From: theA-Train <146664346+theA-Train@users.noreply.github.com> Date: Wed, 17 Dec 2025 14:24:17 -0500 Subject: [PATCH 5/6] gear ratio in config --- subsystem/intake.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/subsystem/intake.py b/subsystem/intake.py index 0c1d7a7..b5fa31b 100644 --- a/subsystem/intake.py +++ b/subsystem/intake.py @@ -145,9 +145,8 @@ def zero_pivot(self) -> None: """ self.pivot_angle = ( - (self.encoder.get_absolute_position().value - config.intake_encoder_zero) / constants.intake_encoder_gear_ratio * 2 * math.pi - ) - pos = self.pivot_angle * constants.intake_pivot_gear_ratio / (2 * math.pi) + (self.encoder.get_absolute_position().value - config.intake_encoder_zero) * 2 * math.pi) + pos = self.pivot_angle self.pivot_motor.set_position(pos), f"sensor position: {pos}" self.pivot_zeroed = True @@ -156,7 +155,7 @@ def get_pivot_angle(self): "returns current angle of pivot" self.pivot_motor_pos.refresh() self.pivot_angle = ( - self.pivot_motor_pos.value / constants.intake_pivot_gear_ratio * math.pi * 2 + self.pivot_motor_pos / 2 * math.pi ) return self.pivot_angle @@ -168,7 +167,6 @@ def set_pivot_angle(self, angle: radians) -> None: setting the angle of the pivot """ - ff = config.intake_max_ff * math.cos(config.intake_ff_offset - angle) rotations = angle / (2 * math.pi) * constants.intake_pivot_gear_ratio self.target_angle = angle @@ -186,4 +184,4 @@ def periodic(self) -> None: #if config.NT_INTAKE: #self.update_table() pass - \ No newline at end of file + From 48a9c98a3a2cbbfab22d7b416f8aeb4854a2bad6 Mon Sep 17 00:00:00 2001 From: theA-Train <146664346+theA-Train@users.noreply.github.com> Date: Wed, 17 Dec 2025 14:56:28 -0500 Subject: [PATCH 6/6] no more statussignal --- subsystem/intake.py | 22 ++++++---------------- 1 file changed, 6 insertions(+), 16 deletions(-) diff --git a/subsystem/intake.py b/subsystem/intake.py index b5fa31b..aa9b2d1 100644 --- a/subsystem/intake.py +++ b/subsystem/intake.py @@ -7,12 +7,6 @@ import commands2 class Intake(commands2.subsystem): - - pivot_motor_current: StatusSignal - pivot_motor_pos: StatusSignal - horizontal_motor_current: StatusSignal - horizontal_motor_pos: StatusSignal - def __init__(self): super().__init__() @@ -117,12 +111,10 @@ def extake_algae(self) -> None: self.intake_running = True def get_horizontal_motor_current(self) -> float: - self.horizontal_motor_current.refresh() - return self.horizontal_motor_current.value + return self.horizontal_motor.get_motor_current().value def get_pivot_motor_current(self) -> float: - self.pivot_motor_current.refresh() - return self.pivot_motor_current.value + return self.pivot_motor.get_motor_current().value def limit_angle(self, angle: radians) -> radians: @@ -145,7 +137,7 @@ def zero_pivot(self) -> None: """ self.pivot_angle = ( - (self.encoder.get_absolute_position().value - config.intake_encoder_zero) * 2 * math.pi) + (self.encoder.get_absolute_position().value - config.intake_encoder_zero) / (2 * math.pi)) pos = self.pivot_angle self.pivot_motor.set_position(pos), f"sensor position: {pos}" @@ -153,11 +145,9 @@ def zero_pivot(self) -> None: def get_pivot_angle(self): "returns current angle of pivot" - self.pivot_motor_pos.refresh() - self.pivot_angle = ( - self.pivot_motor_pos / 2 * math.pi - ) - return self.pivot_angle + return (self.pivot_angle.get_position().value/ + (2 + * math.pi)) def is_at_angle(self, angle: radians) -> bool: return abs(self.get_pivot_angle() - angle) < config.intake_angle_threshold