diff --git a/command/drivetrain.py b/command/drivetrain.py index 026076b..1b32bb8 100644 --- a/command/drivetrain.py +++ b/command/drivetrain.py @@ -61,6 +61,7 @@ def initialize(self) -> None: self.table.putBoolean("DriveSwerveCustom", True) def execute(self) -> None: + self.subsystem.aligned=False dx, dy, d_theta = ( self.subsystem.axis_dx.value * -1, self.subsystem.axis_dy.value * 1, @@ -232,6 +233,7 @@ def __init__(self, subsystem: Drivetrain, poses: list[Pose2d] = None): self.nt = ntcore.NetworkTableInstance.getDefault().getTable("drive to pose") def initialize(self): + self.subsystem.aligned=False self.current_pose = self.subsystem.get_estimated_pose() pose = self.current_pose.nearest(self.poses) @@ -274,11 +276,12 @@ def execute(self): ) def isFinished(self) -> bool: - return ( + self.subsystem.aligned= ( self.x_controller.atSetpoint() and self.y_controller.atSetpoint() and self.theta_controller.atSetpoint() ) + return self.subsystem.aligned def end(self, interrupted): self.subsystem.set_driver_centric((0, 0), 0) diff --git a/command/wrist.py b/command/wrist.py index d9faef6..9668384 100644 --- a/command/wrist.py +++ b/command/wrist.py @@ -81,10 +81,11 @@ def execute(self) -> None: pass def isFinished(self) -> bool: - return self.debouncer.calculate( + self.subsystem.coral_in_feed = self.debouncer.calculate( self.subsystem.feed_motor.get_motor_current() > config.back_current_threshold ) + return self.subsystem.coral_in_feed def end(self, interrupted) -> None: if interrupted: diff --git a/config.py b/config.py index 4383a0e..4dc311a 100644 --- a/config.py +++ b/config.py @@ -82,6 +82,9 @@ auto_translation_pid = PIDConstants(6, 0.0, 0.1) auto_rotation_pid = PIDConstants(5.0, 0.0, 0.0) +speed_tolerance = .1 # feet_per_second +rotation_speed_tolerance = 5 # degrees per second + # odometry odometry_tag_distance_threshold: meters = 2.5 @@ -102,7 +105,9 @@ wrist_feed_id = 15 WRIST_FEED_CONFIG = TalonConfig(1, 0, 0, 0, 0) wrist_id = 14 -WRIST_CONFIG = TalonConfig(48, 0, 0, 0.06, 0, motion_magic_cruise_velocity=97.75, motion_magic_acceleration=350) # 97.75 +WRIST_CONFIG = TalonConfig( + 48, 0, 0, 0.06, 0, motion_magic_cruise_velocity=97.75, motion_magic_acceleration=350 +) # 97.75 wrist_cancoder_id = 22 wrist_encoder_zero = 0.781 @@ -126,7 +131,9 @@ intake_pivot_id = 11 intake_encoder_zero = 0.075 INTAKE_CONFIG = TalonConfig(0, 0, 0, 0, 0, brake_mode=False) -INTAKE_PIVOT_CONFIG = TalonConfig(2, 0, 0, -0.195, 0, motion_magic_cruise_velocity=97, brake_mode=True) # 97 +INTAKE_PIVOT_CONFIG = TalonConfig( + 2, 0, 0, -0.195, 0, motion_magic_cruise_velocity=97, brake_mode=True +) # 97 intake_max_angle = math.radians(60) intake_min_angle = math.radians(0) @@ -146,7 +153,18 @@ elevator_height_threshold = 0.1 * inches_to_meters # placeholder -ELEVATOR_CONFIG = TalonConfig(4, 0, 0.1, 0.13, 0, 0, kG=0.28, brake_mode=True, motion_magic_cruise_velocity=94, motion_magic_acceleration=300) # 94 +ELEVATOR_CONFIG = TalonConfig( + 4, + 0, + 0.1, + 0.13, + 0, + 0, + kG=0.28, + brake_mode=True, + motion_magic_cruise_velocity=94, + motion_magic_acceleration=300, +) # 94 # TO CHANGE @@ -187,7 +205,7 @@ class TargetData: wrist_angle: radians wrist_feed_on: bool wrist_score_on: bool - + intake_angle: radians intake_in_run: bool intake_out_run: bool @@ -196,7 +214,6 @@ class TargetData: target_positions: dict[str, TargetData] = { - "IDLE": TargetData( elevator_idle=True, wrist_idle=True, @@ -207,9 +224,8 @@ class TargetData: wrist_score_on=False, intake_angle=0, intake_in_run=False, - intake_out_run=False + intake_out_run=False, ), - "STATION_INTAKING": TargetData( elevator_idle=True, wrist_idle=False, @@ -220,9 +236,8 @@ class TargetData: wrist_score_on=False, intake_angle=intake_coral_station_angle, intake_in_run=True, - intake_out_run=False + intake_out_run=False, ), - "L1": TargetData( elevator_idle=True, wrist_idle=False, @@ -233,9 +248,8 @@ class TargetData: wrist_score_on=True, intake_angle=0, intake_in_run=False, - intake_out_run=False + intake_out_run=False, ), - "L2": TargetData( elevator_idle=False, wrist_idle=False, @@ -246,9 +260,8 @@ class TargetData: wrist_score_on=True, intake_angle=0, intake_in_run=False, - intake_out_run=False + intake_out_run=False, ), - "L3": TargetData( elevator_idle=False, wrist_idle=False, @@ -259,9 +272,8 @@ class TargetData: wrist_score_on=True, intake_angle=0, intake_in_run=False, - intake_out_run=False + intake_out_run=False, ), - "L4": TargetData( elevator_idle=False, wrist_idle=False, @@ -272,9 +284,8 @@ class TargetData: wrist_score_on=True, intake_angle=0, intake_in_run=False, - intake_out_run=False + intake_out_run=False, ), - "DEALGAE_HIGH": TargetData( elevator_idle=False, wrist_idle=False, @@ -285,9 +296,8 @@ class TargetData: wrist_score_on=False, intake_angle=0, intake_in_run=False, - intake_out_run=False + intake_out_run=False, ), - "DEALGAE_LOW": TargetData( elevator_idle=False, wrist_idle=False, @@ -298,9 +308,8 @@ class TargetData: wrist_score_on=False, intake_angle=0, intake_in_run=False, - intake_out_run=False + intake_out_run=False, ), - "SCORE_BARGE": TargetData( elevator_idle=False, wrist_idle=False, @@ -311,9 +320,8 @@ class TargetData: wrist_score_on=True, intake_angle=0, intake_in_run=False, - intake_out_run=False + intake_out_run=False, ), - # "SCORE_PROCESSOR_INTAKE": TargetData( # elevator_idle=True, # wrist_idle=True, @@ -325,7 +333,6 @@ class TargetData: # intake_in_run=False, # intake_out_run=True # ), - "SCORE_PROCESSOR_WRIST": TargetData( elevator_idle=True, wrist_idle=False, @@ -336,9 +343,8 @@ class TargetData: wrist_score_on=True, intake_angle=0, intake_in_run=False, - intake_out_run=False + intake_out_run=False, ), - "INTAKE_ALGAE": TargetData( elevator_idle=True, wrist_idle=True, @@ -349,9 +355,8 @@ class TargetData: wrist_score_on=False, intake_angle=intake_algae_ground_angle, intake_in_run=True, - intake_out_run=False + intake_out_run=False, ), - "CLIMB": TargetData( elevator_idle=True, wrist_idle=True, @@ -364,5 +369,14 @@ class TargetData: intake_in_run=True, intake_out_run=False, intake_climb=True, - ) -} \ No newline at end of file + ), +} + +# Leds +leds_id = 8 # placeholder +leds_size = 27 # placeholder +leds_spacing = 1 / 120.0 # placeholder, density of __ per meter +leds_speed = 5 +leds_brightness = 128 +leds_saturation = 255 +leds_blink_frequency = 0.5 diff --git a/ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat new file mode 100644 index 0000000..ddcf921 Binary files /dev/null and b/ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat differ diff --git a/ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat new file mode 100644 index 0000000..78bcadb Binary files /dev/null and b/ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat differ diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat index 5f468a6..516f948 100644 Binary files a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat and b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat index 44553d8..582771f 100644 Binary files a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat index c55f10f..a7121c0 100644 Binary files a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat index b4344d8..144e92e 100644 Binary files a/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat index 435d205..771646c 100644 Binary files a/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat index 4334bc8..08deca9 100644 Binary files a/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat index 8a2e3ab..935c2f9 100644 Binary files a/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat index ddbbacc..bd88123 100644 Binary files a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat index cbf9df1..54dee86 100644 Binary files a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat index 55c7553..4b1dee6 100644 Binary files a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat index 3f40df3..2f7bbda 100644 Binary files a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat index cd2d914..9ea9d70 100644 Binary files a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat index 895b1f5..97a3fca 100644 Binary files a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 08 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 08 - 0 - ext.dat index 5d66d0e..908d8ac 100644 Binary files a/ctre_sim/Talon FX vers. C - 08 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 08 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat index 28c1d21..ba17770 100644 Binary files a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat differ diff --git a/pyproject.toml b/pyproject.toml index ce25c80..7c74bfb 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -6,7 +6,7 @@ [tool.robotpy] # Version of robotpy this project depends on -robotpy_version = "2025.3.1.0" +robotpy_version = "2025.3.1.1" # Which extra RobotPy components should be installed # -> equivalent to `pip install robotpy[extra1, ...] diff --git a/robot.py b/robot.py index 6129e1b..b58587d 100644 --- a/robot.py +++ b/robot.py @@ -4,11 +4,11 @@ import phoenix6 as ctre import ntcore -import phoenix6 as ctre + import wpilib import command -import math + import config # import constants @@ -16,7 +16,6 @@ Robot, Pneumatics, Sensors, - LEDs, PowerDistribution, Field, ) @@ -33,7 +32,6 @@ from wpilib import DriverStation - class _Robot(wpilib.TimedRobot): def __init__(self): super().__init__() @@ -92,7 +90,10 @@ def init_subsystems(): ctre.hardware.ParentDevice.optimize_bus_utilization_for_all() Field.update_field_table() - + + #alternates blue and gold on leds + Robot.led.set_Alternate(255,190,0,0,0,255) + self.log.complete("Robot initialized") def robotPeriodic(self): @@ -127,15 +128,24 @@ def robotPeriodic(self): self.log.error(e) self.nt.getTable("errors").putString("command scheduler", str(e)) raise e - + # Field.odometry.disable() pose = Field.odometry.update() - self.nt.getTable("Odometry").putNumberArray("Estimated pose", [ - pose.X(), - pose.Y(), - pose.rotation().radians() - ]) + self.nt.getTable("Odometry").putNumberArray( + "Estimated pose", [pose.X(), pose.Y(), pose.rotation().radians()] + ) + + if Robot.wrist.coral_in_feed: + Robot.led.set_Blink(0,255,0) #if coral is in the intake, blinking green + elif Robot.intake.intake_running: + Robot.led.set_Solid(250, 106, 20) #if intake is running but coral is not yet intook, solid orange + elif Robot.wrist.wrist_ejecting: + Robot.led.set_Solid(0,200,255) #if extaking, cyan + elif Robot.drivetrain.is_moving() and Robot.drivetrain.aligned: + Robot.led.set_Solid(255,0,255) #if aligned to score, purple + else: + Robot.led.set_Alternate(255,190,0,0,0,255) #idling is blue and gold Robot.drivetrain.update_tables() # Sensors.cam_controller.update_tables() @@ -148,37 +158,53 @@ def robotPeriodic(self): def teleopInit(self): OI.init() OI.map_controls() - self.scheduler.schedule(commands2.SequentialCommandGroup( - command.SetWrist(Robot.wrist, 0), - command.SetElevator(Robot.elevator, 0), - )) - self.scheduler.schedule(commands2.SequentialCommandGroup( + self.scheduler.schedule( + commands2.SequentialCommandGroup( + command.SetWrist(Robot.wrist, 0), + command.SetElevator(Robot.elevator, 0), + ) + ) + self.scheduler.schedule( + commands2.SequentialCommandGroup( command.DrivetrainZero(Robot.drivetrain), - command.DriveSwerveCustom(Robot.drivetrain) - )) + command.DriveSwerveCustom(Robot.drivetrain), + ) + ) self.log.info("Teleop initialized") - def teleopPeriodic(self): pass def autonomousInit(self): path = PathPlannerPath.fromChoreoTrajectory("Four L4 Left") - starting_pose = get_red_pose(path.getStartingHolonomicPose()) if wpilib.DriverStation.getAlliance() == wpilib.DriverStation.Alliance.kRed else path.getStartingHolonomicPose() + starting_pose = ( + get_red_pose(path.getStartingHolonomicPose()) + if wpilib.DriverStation.getAlliance() == wpilib.DriverStation.Alliance.kRed + else path.getStartingHolonomicPose() + ) Robot.drivetrain.reset_odometry_auto(starting_pose) - self.scheduler.schedule(commands2.SequentialCommandGroup( - command.DrivetrainZero(Robot.drivetrain, starting_pose.rotation().radians()), - AutoBuilder.followPath(path), - commands2.InstantCommand(lambda: Robot.drivetrain.set_robot_centric((0, 0, 0))) - )) + self.scheduler.schedule( + commands2.SequentialCommandGroup( + command.DrivetrainZero( + Robot.drivetrain, starting_pose.rotation().radians() + ), + AutoBuilder.followPath(path), + commands2.InstantCommand( + lambda: Robot.drivetrain.set_robot_centric((0, 0, 0)) + ), + ) + ) # self.scheduler.schedule(commands2.SequentialCommandGroup( # command.DrivetrainZero(Robot.drivetrain), # command.FindWheelRadius(Robot.drivetrain) # )) + #scrolling rainbow on leds + Robot.led.set_Rainbow_Ladder() + self.log.info("Autonomous initialized") - + def autonomousPeriodic(self): pass diff --git a/robot_systems.py b/robot_systems.py index 8875185..7d647ee 100644 --- a/robot_systems.py +++ b/robot_systems.py @@ -1,6 +1,8 @@ import subsystem import sensors -import wpilib #noqa +import wpilib +import config +import wpilib # noqa import config import constants from utils.field import ( @@ -27,6 +29,15 @@ class Robot: wrist = subsystem.Wrist() drivetrain = subsystem.Drivetrain() intake = subsystem.Intake() + led = subsystem.AddressableLEDStrip( + config.leds_id, + config.leds_size, + config.leds_speed, + config.leds_brightness, + config.leds_saturation, + config.leds_spacing, + config.leds_blink_frequency, + ) class Pneumatics: diff --git a/subsystem/__init__.py b/subsystem/__init__.py index b0f587e..67f5179 100644 --- a/subsystem/__init__.py +++ b/subsystem/__init__.py @@ -1,5 +1,5 @@ from subsystem.drivetrain import Drivetrain +from subsystem.leds import AddressableLEDStrip from subsystem.intake import Intake from subsystem.wrist import Wrist -from subsystem.elevator import Elevator - +from subsystem.elevator import Elevator diff --git a/subsystem/drivetrain.py b/subsystem/drivetrain.py index 1170635..05ae261 100644 --- a/subsystem/drivetrain.py +++ b/subsystem/drivetrain.py @@ -98,7 +98,7 @@ def __init__(self): self.sim_node_states: tuple[SwerveModuleState, SwerveModuleState, SwerveModuleState, SwerveModuleState] = None self.node_translations: tuple[Translation2d] | None = None self.nt = ntcore.NetworkTableInstance.getDefault().getTable("Drivetrain") - + self.aligned=False # auto setup self.pp_config = constants.auto_config @@ -255,6 +255,10 @@ def get_estimated_pose(self) -> Pose2d: pose = self.odometry_estimator.getEstimatedPosition() return pose + def is_moving(self)->bool: + speeds=self.get_speeds() + return abs(speeds.vx_fps)>config.speed_tolerance or abs(speeds.vy_fps)>config.speed_tolerance or abs(speeds.omega_dps)>config.rotation_speed_tolerance + def get_speeds(self) -> ChassisSpeeds: return self.chassis_speeds diff --git a/subsystem/leds.py b/subsystem/leds.py new file mode 100644 index 0000000..3b6194c --- /dev/null +++ b/subsystem/leds.py @@ -0,0 +1,110 @@ +from wpilib import AddressableLED, SmartDashboard, Color, LEDPattern +import math +from toolkit.subsystem import Subsystem +import ntcore + + +class AddressableLEDStrip(Subsystem): + def __init__( + self, + id: int, + size: int, + speed: int, + brightness: int, + saturation: int, + spacing: int, + blink_frequency: float, + ): + self.size = size + self.id = id + self.speed = speed + self.brightness = brightness + self.saturation = saturation + self.LEDSpacing = spacing + self.blink_frequency = blink_frequency # seconds + self.pattern = None + self.mode = "None" + + def init(self): + """ + initialize new LED strip connected to PWM port + """ + self.led = AddressableLED(self.id) + + self.ledBuffer = [self.led.LEDData() for i in range(self.size)] + self.enable() + # self.led.set_brightness(self.brightness) + # self.led.set_speed(self.speed) + SmartDashboard.putBoolean("LEDs Initialized", True) + + def enable(self): + """ + be sure to enable + """ + self.led.start() + + def disable(self): + self.led.stop() + self.mode = "None" + + def set_brightness(self, brightness: float): + self.brightness = brightness + + def set_speed(self, speed: float): + self.speed = speed + + def get_led_data(self): + return [self.led.LEDData() for i in range(self.size)].copy() + + def get_current_type(self): + # return self.pattern/mode + pass + + def set_Solid(self, r: int, g: int, b: int): + self.pattern = LEDPattern.solid(Color(g / 255, r / 255, b / 255)) + self.mode = f"Solid r:{r} g:{g} b:{b}" + + def set_Alternate(self, r1: int, g1: int, b1: int, r2: int, g2: int, b2: int): + self.alternate = [] + # red and green are switched for an unknown reason + for i in range(self.size): + if i % 2 == 0: + self.alternate.append( + (i / self.size, Color(g1 / 255, r1 / 255, b1 / 255)) + ) + elif i % 2 == 1: + self.alternate.append( + (i / self.size, Color(g2 / 255, r2 / 255, b2 / 255)) + ) + + self.pattern = LEDPattern.steps(self.alternate) + self.mode = f"Alternate" + + def set_Rainbow_Ladder(self): + """ + creates a scrolling rainbow on LEDs + """ + self.rainbow = LEDPattern.rainbow(self.saturation, self.brightness) + + self.pattern = self.rainbow.scrollAtAbsoluteSpeed(self.speed, self.LEDSpacing) + self.mode = "Rainbow Ladder" + + def set_Blink(self, r: int, g: int, b: int): + self.base = LEDPattern.solid((r / 255, g / 255, b / 255)) + self.pattern = self.base.blink(self.blink_frequency) + self.mode = f"Blink r:{r} g:{g} b:{b}" + + def periodic(self): + self.update_tables() + if not self.mode == "None": + + def set_pattern_writer(i: int, my_color: Color) -> None: + self.ledBuffer[i].setLED(my_color) + + ledreader = LEDPattern.LEDReader(self.ledBuffer.__getitem__, self.size) + self.pattern.applyTo(ledreader, set_pattern_writer) + self.led.setData(self.ledBuffer) + + def update_tables(self): + self.table = ntcore.NetworkTableInstance.getDefault().getTable("LEDS") + self.table.putString("mode", self.mode)