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 4 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
155 changes: 87 additions & 68 deletions subsystem/intake.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,31 +1,61 @@
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
Comment thread
achung28 marked this conversation as resolved.
Outdated

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)

@e-bauman e-bauman Dec 16, 2025

Copy link
Copy Markdown
Contributor

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 +69,61 @@ 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}",

@e-bauman e-bauman Dec 16, 2025

Copy link
Copy Markdown
Contributor

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()
self.horizontal_motor_current.refresh()
Comment thread
achung28 marked this conversation as resolved.
Outdated
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
Comment thread
achung28 marked this conversation as resolved.
Outdated


def limit_angle(self, angle: radians) -> radians:
"""
Expand All @@ -112,20 +147,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)

@e-bauman e-bauman Dec 16, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

include the gear ratio in the feedback config so that you don't need to here

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

@e-bauman e-bauman Dec 16, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

if you include the gear ratio in the config you don't need to do it here

)
return self.pivot_angle

Expand All @@ -138,33 +169,21 @@ def set_pivot_angle(self, angle: radians) -> None:
"""

ff = config.intake_max_ff * math.cos(config.intake_ff_offset - angle)

@e-bauman e-bauman Dec 16, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

if the gear ratio is done above you don't need to do the gravity feedforward here

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