Skip to content

Anthony and avery #75

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
achung28 wants to merge 6 commits into
base: dev
Choose a base branch
from
Open

Anthony and avery #75

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
9f5877e
Anthony and Avery Intake
achung28 Dec 11, 2025
398997f
More intake changes
achung28 Dec 12, 2025
7c9dfff
Updated intake.py
achung28 Dec 16, 2025
cc5de26
Re-updated Intake.py
achung28 Dec 16, 2025
49e2de8
gear ratio in config
theA-Train Dec 17, 2025
48a9c98
no more statussignal
theA-Train Dec 17, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
157 changes: 82 additions & 75 deletions subsystem/intake.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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)

Comment thread
achung28 marked this conversation as resolved.
self.horizontal_motor_configs = (
configs.TalonFXConfiguration()
.with_motor_output(
configs.MotorOutputConfigs()
.with_inverted(signals.InvertedValue.CLOCKWISE_POSITIVE)
.with_neutral_mode(signals.NeutralModeValue.BRAKE)
)
)
Comment thread
achung28 marked this conversation as resolved.

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)
Copy link
Copy Markdown
Contributor

@e-bauman e-bauman Dec 16, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

include the gear ratio

).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)

)
Comment thread
achung28 marked this conversation as resolved.

)

self.intake_running: bool = False

self.pivot_angle = math.radians(0)
Expand All @@ -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()
Comment thread
achung28 marked this conversation as resolved.

self.pivot_motor.configurator.apply(self.pivot_motor_configs)
self.zero_pivot()
self._motion_magic_voltage = controls.MotionMagicVoltage(0)
Comment thread
achung28 marked this conversation as resolved.


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))
Comment thread
achung28 marked this conversation as resolved.
""" 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))
Comment thread
achung28 marked this conversation as resolved.
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)),
Comment thread
achung28 marked this conversation as resolved.
f"raw output: {0}",
Copy link
Copy Markdown
Contributor

@e-bauman e-bauman Dec 16, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

not sure what this line is for


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))
Comment thread
achung28 marked this conversation as resolved.
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))
Comment thread
achung28 marked this conversation as resolved.
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:
"""
Expand All @@ -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
Expand All @@ -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(
Comment thread
achung28 marked this conversation as resolved.
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

Loading