diff --git a/subsystem/intake.py b/subsystem/intake.py index b95ecb7..aa9b2d1 100644 --- a/subsystem/intake.py +++ b/subsystem/intake.py @@ -1,31 +1,55 @@ 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): 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) + ) + ) + + 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( + 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) + + ) + + ) + self.intake_running: bool = False self.pivot_angle = math.radians(0) @@ -39,56 +63,59 @@ 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(controls.DutyCycleOut.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(controls.DutyCycleOut.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(controls.DutyCycleOut.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(controls.DutyCycleOut.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(controls.DutyCycleOut.with_output(config.extake_algae_speed)) self.intake_running = True def get_horizontal_motor_current(self) -> float: - return self.horizontal_motor.get_motor_current() + return self.horizontal_motor.get_motor_current().value def get_pivot_motor_current(self) -> float: - return self.pivot_motor.get_motor_current() + return self.pivot_motor.get_motor_current().value + def limit_angle(self, angle: radians) -> radians: """ @@ -110,24 +137,17 @@ 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) - ) + (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 def get_pivot_angle(self): "returns current angle of pivot" - self.pivot_angle = ( - self.pivot_motor.get_sensor_position() - / constants.intake_pivot_gear_ratio - * math.pi - * 2 - ) - 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 @@ -137,34 +157,21 @@ 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 - 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(controls.DutyCycleOut.with_output(0)) def periodic(self) -> None: - if config.NT_INTAKE: - self.update_table() - + #if config.NT_INTAKE: + #self.update_table() + pass +