diff --git a/autonomous/auto_base.py b/autonomous/auto_base.py index c58f31dc..a2374152 100644 --- a/autonomous/auto_base.py +++ b/autonomous/auto_base.py @@ -10,11 +10,6 @@ from wpimath.kinematics import ChassisSpeeds from components.chassis import ChassisComponent -from components.coral_depositor import CoralDepositorComponent -from components.injector import InjectorComponent -from components.shooter import ShooterComponent -from controllers.algae_shooter import AlgaeShooter -from controllers.reef_intake import ReefIntake from utilities import game x_controller = PIDController(2.0, 0.0, 0.0) @@ -25,12 +20,12 @@ class AutoBase(AutonomousStateMachine): - algae_shooter: AlgaeShooter - reef_intake: ReefIntake + # algae_shooter: AlgaeShooter + # reef_intake: ReefIntake - coral_depositor_component: CoralDepositorComponent - shooter_component: ShooterComponent - injector_component: InjectorComponent + # coral_depositor_component: CoralDepositorComponent + # shooter_component: ShooterComponent + # injector_component: InjectorComponent chassis: ChassisComponent field: wpilib.Field2d @@ -95,6 +90,7 @@ def on_disable(self) -> None: @state(first=True) def initialising(self) -> None: + return # Add any tasks that need doing first self.reef_intake.holding_coral = True self.chassis.do_smooth = False @@ -103,6 +99,7 @@ def initialising(self) -> None: @state def tracking_trajectory(self, initial_call, state_tm) -> None: + return if initial_call: self.current_leg += 1 @@ -170,6 +167,7 @@ def follow_trajectory(self, sample: SwerveSample): @timed_state(duration=1.0, next_state="tracking_trajectory") def intaking_algae(self) -> None: + return if self.current_leg == 0: self.coral_depositor_component.deposit() if self.injector_component.has_algae(): @@ -178,6 +176,7 @@ def intaking_algae(self) -> None: @timed_state(duration=1.0, next_state="tracking_trajectory") def shooting_algae(self) -> None: + return self.algae_shooter.shoot() if not self.algae_shooter.is_executing: diff --git a/physics.py b/physics.py index 3f860794..a75b5a73 100644 --- a/physics.py +++ b/physics.py @@ -13,7 +13,6 @@ from pyfrc.physics.core import PhysicsInterface from wpilib.simulation import ( DCMotorSim, - DIOSim, DutyCycleEncoderSim, PWMSim, SingleJointedArmSim, @@ -23,8 +22,6 @@ from wpimath.units import kilogram_square_meters from components.chassis import SwerveModule -from components.intake import IntakeComponent -from components.wrist import WristComponent from utilities import game from utilities.functions import constrain_angle @@ -156,58 +153,58 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot): ) for module in robot.chassis.modules ] - self.flywheels = [ - TalonFXMotorSim( - DCMotor.falcon500, - robot.shooter_component.top_flywheel, - gearing=1 / 1, - moi=0.00105679992, - ), - TalonFXMotorSim( - DCMotor.falcon500, - robot.shooter_component.bottom_flywheel, - gearing=1 / 1, - moi=0.00156014845, - ), - ] + # self.flywheels = [ + # TalonFXMotorSim( + # DCMotor.falcon500, + # robot.shooter_component.top_flywheel, + # gearing=1 / 1, + # moi=0.00105679992, + # ), + # TalonFXMotorSim( + # DCMotor.falcon500, + # robot.shooter_component.bottom_flywheel, + # gearing=1 / 1, + # moi=0.00156014845, + # ), + # ] # Intake arm simulation - intake_arm_gearbox = DCMotor.NEO(1) - self.intake_arm_motor = rev.SparkMaxSim( - robot.intake_component.arm_motor, intake_arm_gearbox - ) - self.intake_arm_encoder_sim = DutyCycleEncoderSim( - robot.intake_component.encoder - ) - self.intake_arm = SparkArmSim( - SingleJointedArmSim( - intake_arm_gearbox, - IntakeComponent.gear_ratio, - moi=IntakeComponent.ARM_MOI, - armLength=0.22, - minAngle=IntakeComponent.DEPLOYED_ANGLE_LOWER, - maxAngle=IntakeComponent.RETRACTED_ANGLE, - simulateGravity=True, - startingAngle=IntakeComponent.RETRACTED_ANGLE, - ), - self.intake_arm_motor, - ) - - wrist_gearbox = DCMotor.NEO(1) - wrist_motor = rev.SparkMaxSim(robot.wrist.motor, wrist_gearbox) - self.wrist_encoder_sim = DutyCycleEncoderSim(robot.wrist.wrist_encoder) - - wrist_sim = SingleJointedArmSim( - wrist_gearbox, - WristComponent.wrist_gear_ratio, - moi=0.295209215, - armLength=0.5, - minAngle=WristComponent.MAXIMUM_DEPRESSION, - maxAngle=WristComponent.MAXIMUM_ELEVATION, - simulateGravity=True, - startingAngle=WristComponent.MAXIMUM_DEPRESSION, - ) - self.wrist = SparkArmSim(wrist_sim, wrist_motor) + # intake_arm_gearbox = DCMotor.NEO(1) + # self.intake_arm_motor = rev.SparkMaxSim( + # robot.intake_component.arm_motor, intake_arm_gearbox + # ) + # self.intake_arm_encoder_sim = DutyCycleEncoderSim( + # robot.intake_component.encoder + # ) + # self.intake_arm = SparkArmSim( + # SingleJointedArmSim( + # intake_arm_gearbox, + # IntakeComponent.gear_ratio, + # moi=IntakeComponent.ARM_MOI, + # armLength=0.22, + # minAngle=IntakeComponent.DEPLOYED_ANGLE_LOWER, + # maxAngle=IntakeComponent.RETRACTED_ANGLE, + # simulateGravity=True, + # startingAngle=IntakeComponent.RETRACTED_ANGLE, + # ), + # self.intake_arm_motor, + # ) + + # wrist_gearbox = DCMotor.NEO(1) + # wrist_motor = rev.SparkMaxSim(robot.wrist.motor, wrist_gearbox) + # self.wrist_encoder_sim = DutyCycleEncoderSim(robot.wrist.wrist_encoder) + + # wrist_sim = SingleJointedArmSim( + # wrist_gearbox, + # WristComponent.wrist_gear_ratio, + # moi=0.295209215, + # armLength=0.5, + # minAngle=WristComponent.MAXIMUM_DEPRESSION, + # maxAngle=WristComponent.MAXIMUM_ELEVATION, + # simulateGravity=True, + # startingAngle=WristComponent.MAXIMUM_DEPRESSION, + # ) + # self.wrist = SparkArmSim(wrist_sim, wrist_motor) self.imu = robot.chassis.imu.sim_state @@ -217,7 +214,7 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot): self.starboard_camera = PhotonCameraSim( robot.starboard_vision.camera, properties ) - self.port_camera = PhotonCameraSim(robot.port_vision.camera, properties) + # self.port_camera = PhotonCameraSim(robot.port_vision.camera, properties) self.starboard_camera.setMaxSightRange(5.0) self.starboard_visual_localiser = robot.starboard_vision self.vision_sim.addCamera( @@ -226,30 +223,30 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot): wpilib.Timer.getFPGATimestamp() ), ) - self.port_camera.setMaxSightRange(5.0) - self.port_visual_localiser = robot.port_vision - self.vision_sim.addCamera( - self.port_camera, - self.port_visual_localiser.robot_to_camera(wpilib.Timer.getFPGATimestamp()), - ) + # self.port_camera.setMaxSightRange(5.0) + # self.port_visual_localiser = robot.port_vision + # self.vision_sim.addCamera( + # self.port_camera, + # self.port_visual_localiser.robot_to_camera(wpilib.Timer.getFPGATimestamp()), + # ) self.vision_sim_counter = 0 self.starboard_vision_servo_sim = PWMSim(self.starboard_visual_localiser.servo) self.starboard_vision_encoder_sim = DutyCycleEncoderSim( self.starboard_visual_localiser.encoder ) - self.port_vision_servo_sim = PWMSim(self.port_visual_localiser.servo) - self.port_vision_encoder_sim = DutyCycleEncoderSim( - self.port_visual_localiser.encoder - ) - - self.algae_limit_switch_sim = DIOSim( - robot.injector_component.algae_limit_switch - ) - self.algae_pickup_counter = 0 - self.floor_intake = robot.floor_intake - self.reef_intake = robot.reef_intake - self.algae_shooter = robot.algae_shooter + # self.port_vision_servo_sim = PWMSim(self.port_visual_localiser.servo) + # self.port_vision_encoder_sim = DutyCycleEncoderSim( + # self.port_visual_localiser.encoder + # ) + + # self.algae_limit_switch_sim = DIOSim( + # robot.injector_component.algae_limit_switch + # ) + # self.algae_pickup_counter = 0 + # self.floor_intake = robot.floor_intake + # self.reef_intake = robot.reef_intake + # self.algae_shooter = robot.algae_shooter def update_sim(self, now: float, tm_diff: float) -> None: # Enable the Phoenix6 simulated devices @@ -261,8 +258,8 @@ def update_sim(self, now: float, tm_diff: float) -> None: wheel.update(tm_diff) for steer in self.steer: steer.update(tm_diff) - for flywheel in self.flywheels: - flywheel.update(tm_diff) + # for flywheel in self.flywheels: + # flywheel.update(tm_diff) speeds = self.kinematics.toChassisSpeeds( ( @@ -290,18 +287,18 @@ def update_sim(self, now: float, tm_diff: float) -> None: ) ) - self.port_vision_encoder_sim.set( - constrain_angle( - ( - ( - self.port_visual_localiser.servo_offsets.full_range - - self.port_visual_localiser.servo_offsets.neutral - ) - * (2.0 * self.port_visual_localiser.servo.getPosition() - 1.0) - + self.port_visual_localiser.servo_offsets.neutral - ).radians() - ) - ) + # self.port_vision_encoder_sim.set( + # constrain_angle( + # ( + # ( + # self.port_visual_localiser.servo_offsets.full_range + # - self.port_visual_localiser.servo_offsets.neutral + # ) + # * (2.0 * self.port_visual_localiser.servo.getPosition() - 1.0) + # + self.port_visual_localiser.servo_offsets.neutral + # ).radians() + # ) + # ) # Simulate slow vision updates. self.vision_sim_counter += 1 @@ -312,44 +309,44 @@ def update_sim(self, now: float, tm_diff: float) -> None: wpilib.Timer.getFPGATimestamp() ), ) - self.vision_sim.adjustCamera( - self.port_camera, - self.port_visual_localiser.robot_to_camera( - wpilib.Timer.getFPGATimestamp() - ), - ) + # self.vision_sim.adjustCamera( + # self.port_camera, + # self.port_visual_localiser.robot_to_camera( + # wpilib.Timer.getFPGATimestamp() + # ), + # ) self.vision_sim.update(self.physics_controller.get_pose()) self.vision_sim_counter = 0 # Update intake arm simulation - self.intake_arm.update(tm_diff) - self.intake_arm_encoder_sim.set( - self.intake_arm.mech_sim.getAngle() + IntakeComponent.ARM_ENCODER_OFFSET - ) - - # Update wrist simulation - self.wrist.update(tm_diff) - self.wrist_encoder_sim.set( - self.wrist.mech_sim.getAngle() + WristComponent.ENCODER_ZERO_OFFSET - ) - - # Simulate algae pick up - if self.floor_intake.current_state == "intaking": - # Simulate driving around for a couple of seconds - self.algae_pickup_counter += 1 - if self.algae_pickup_counter == 100: - self.algae_limit_switch_sim.setValue(False) - else: - self.algae_pickup_counter = 0 - if self.reef_intake.current_state == "intaking": - # Check near reef - pose = self.physics_controller.get_pose() - pos = pose.translation() - if ( - pos.distance(game.BLUE_REEF_POS) < 2.0 - or pos.distance(game.RED_REEF_POS) < 2.0 - ): - self.algae_limit_switch_sim.setValue(False) - # Algae shooting - if self.algae_shooter.current_state == "shooting": - self.algae_limit_switch_sim.setValue(True) + # self.intake_arm.update(tm_diff) + # self.intake_arm_encoder_sim.set( + # self.intake_arm.mech_sim.getAngle() + IntakeComponent.ARM_ENCODER_OFFSET + # ) + + # # Update wrist simulation + # self.wrist.update(tm_diff) + # self.wrist_encoder_sim.set( + # self.wrist.mech_sim.getAngle() + WristComponent.ENCODER_ZERO_OFFSET + # ) + + # # Simulate algae pick up + # if self.floor_intake.current_state == "intaking": + # # Simulate driving around for a couple of seconds + # self.algae_pickup_counter += 1 + # if self.algae_pickup_counter == 100: + # self.algae_limit_switch_sim.setValue(False) + # else: + # self.algae_pickup_counter = 0 + # if self.reef_intake.current_state == "intaking": + # # Check near reef + # pose = self.physics_controller.get_pose() + # pos = pose.translation() + # if ( + # pos.distance(game.BLUE_REEF_POS) < 2.0 + # or pos.distance(game.RED_REEF_POS) < 2.0 + # ): + # self.algae_limit_switch_sim.setValue(False) + # # Algae shooting + # if self.algae_shooter.current_state == "shooting": + # self.algae_limit_switch_sim.setValue(True) diff --git a/robot.py b/robot.py index f8b7f42d..ba4c7671 100644 --- a/robot.py +++ b/robot.py @@ -9,20 +9,8 @@ from wpimath.geometry import Rotation2d, Translation3d from autonomous.auto_base import AutoBase -from components.ballistics import BallisticsComponent from components.chassis import ChassisComponent, SwerveConfig -from components.climber import ClimberComponent -from components.coral_depositor import CoralDepositorComponent -from components.injector import InjectorComponent -from components.intake import IntakeComponent -from components.led_component import LightStrip -from components.shooter import ShooterComponent from components.vision import ServoOffsets, VisualLocalizer -from components.wrist import WristComponent -from controllers.algae_shooter import AlgaeShooter -from controllers.climber import ClimberStateMachine -from controllers.floor_intake import FloorIntake -from controllers.reef_intake import ReefIntake from ids import DioChannel, PwmChannel, RioSerialNumber from utilities.game import is_red from utilities.scalers import rescale_js @@ -30,23 +18,23 @@ class MyRobot(magicbot.MagicRobot): # Controllers - reef_intake: ReefIntake - algae_shooter: AlgaeShooter - floor_intake: FloorIntake - climber_state_machine: ClimberStateMachine + # reef_intake: ReefIntake + # algae_shooter: AlgaeShooter + # floor_intake: FloorIntake + # climber_state_machine: ClimberStateMachine # Components chassis: ChassisComponent - climber: ClimberComponent - coral_depositor_component: CoralDepositorComponent - shooter_component: ShooterComponent - injector_component: InjectorComponent + # climber: ClimberComponent + # coral_depositor_component: CoralDepositorComponent + # shooter_component: ShooterComponent + # injector_component: InjectorComponent starboard_vision: VisualLocalizer - port_vision: VisualLocalizer - wrist: WristComponent - intake_component: IntakeComponent - status_lights: LightStrip - ballistics_component: BallisticsComponent + # port_vision: VisualLocalizer + # wrist: WristComponent + # intake_component: IntakeComponent + # status_lights: LightStrip + # ballistics_component: BallisticsComponent max_speed = tunable(0.8 * 5.0) # m/s lower_max_speed = tunable(1.0) # m/s @@ -130,19 +118,19 @@ def createObjects(self) -> None: Rotation2d(1.377), ) - self.port_vision_name = "port_turret" - self.port_vision_turret_pos = Translation3d(-0.010, 0.300, 0.660) - self.port_vision_turret_rot = Rotation2d.fromDegrees(90.0) - self.port_vision_camera_offset = Translation3d(0.021, 0, 0) - self.port_vision_camera_pitch = math.radians(10.0) - self.port_vision_encoder_offset = Rotation2d(6.103) - self.port_vision_servo_offsets = ServoOffsets( - neutral=Rotation2d(1.052), full_range=Rotation2d(3.121) - ) - self.port_vision_rotation_range = ( - Rotation2d(1.733), - Rotation2d(5.034), - ) + # self.port_vision_name = "port_turret" + # self.port_vision_turret_pos = Translation3d(-0.010, 0.300, 0.660) + # self.port_vision_turret_rot = Rotation2d.fromDegrees(90.0) + # self.port_vision_camera_offset = Translation3d(0.021, 0, 0) + # self.port_vision_camera_pitch = math.radians(10.0) + # self.port_vision_encoder_offset = Rotation2d(6.103) + # self.port_vision_servo_offsets = ServoOffsets( + # neutral=Rotation2d(1.052), full_range=Rotation2d(3.121) + # ) + # self.port_vision_rotation_range = ( + # Rotation2d(1.733), + # Rotation2d(5.034), + # ) else: self.chassis_swerve_config = SwerveConfig( @@ -221,16 +209,16 @@ def teleopPeriodic(self) -> None: local_driving = self.gamepad.getRightBumperButton() if local_driving: - if ( - self.injector_component.has_algae() - or self.floor_intake.is_executing - or self.reef_intake.is_executing - ): - # Make the shooter behave like the front of the robot - self.chassis.drive_local(-drive_x, -drive_y, drive_z) - else: - # Climber is front as defined in the chassis - self.chassis.drive_local(drive_x, drive_y, drive_z) + # if ( + # self.injector_component.has_algae() + # or self.floor_intake.is_executing + # or self.reef_intake.is_executing + # ): + # # Make the shooter behave like the front of the robot + # self.chassis.drive_local(-drive_x, -drive_y, drive_z) + # else: + # Climber is front as defined in the chassis + self.chassis.drive_local(drive_x, drive_y, drive_z) else: if is_red(): drive_x = -drive_x @@ -241,51 +229,51 @@ def teleopPeriodic(self) -> None: if drive_z != 0: self.chassis.stop_snapping() - if ( - self.is_left_trigger_pressed(self.gamepad.getLeftTriggerAxis()) - and not self.reef_intake.is_executing - and not self.algae_shooter.is_executing - ): - if self.floor_intake.current_state == "intaking": - self.floor_intake.intake_upper = not self.floor_intake.intake_upper - self.floor_intake.intake() - - if ( - self.gamepad.getLeftBumperButton() - and not self.floor_intake.is_executing - and not self.algae_shooter.is_executing - ): - self.reef_intake.intake() - - if self.gamepad.getYButton(): - self.climber_state_machine.deploy() - if self.gamepad.getAButton(): - self.climber_state_machine.retract() - - if self.gamepad.getBButton(): - self.reef_intake.done() - self.floor_intake.done() - self.climber_state_machine.done() - - if self.gamepad.getRightTriggerAxis() > 0.5: - self.algae_shooter.shoot() - - # Override Climber Retract - if self.gamepad.getStartButton(): - self.climber_state_machine.engage("retracting", force=True) - - # Force servo neutral - if self.gamepad.getLeftStickButton(): - self.port_vision.zero_servo_() - self.starboard_vision.zero_servo_() - - def is_left_trigger_pressed(self, trigger_value: float) -> bool: - if trigger_value < 0.3: - self.left_trigger_reset = True - if self.left_trigger_reset and trigger_value > 0.5: - self.left_trigger_reset = False - return True - return False + # if ( + # self.is_left_trigger_pressed(self.gamepad.getLeftTriggerAxis()) + # and not self.reef_intake.is_executing + # and not self.algae_shooter.is_executing + # ): + # if self.floor_intake.current_state == "intaking": + # self.floor_intake.intake_upper = not self.floor_intake.intake_upper + # self.floor_intake.intake() + + # if ( + # self.gamepad.getLeftBumperButton() + # and not self.floor_intake.is_executing + # and not self.algae_shooter.is_executing + # ): + # self.reef_intake.intake() + + # if self.gamepad.getYButton(): + # self.climber_state_machine.deploy() + # if self.gamepad.getAButton(): + # self.climber_state_machine.retract() + + # if self.gamepad.getBButton(): + # self.reef_intake.done() + # self.floor_intake.done() + # self.climber_state_machine.done() + + # if self.gamepad.getRightTriggerAxis() > 0.5: + # self.algae_shooter.shoot() + + # # Override Climber Retract + # if self.gamepad.getStartButton(): + # self.climber_state_machine.engage("retracting", force=True) + + # # Force servo neutral + # if self.gamepad.getLeftStickButton(): + # self.port_vision.zero_servo_() + # self.starboard_vision.zero_servo_() + + # def is_left_trigger_pressed(self, trigger_value: float) -> bool: + # if trigger_value < 0.3: + # self.left_trigger_reset = True + # if self.left_trigger_reset and trigger_value > 0.5: + # self.left_trigger_reset = False + # return True + # return False def testInit(self) -> None: self.chassis.set_coast_in_neutral(True) @@ -310,56 +298,56 @@ def testPeriodic(self) -> None: else: self.chassis.stop() - if self.gamepad.getBButton(): - self.reef_intake.done() - self.floor_intake.done() + # if self.gamepad.getBButton(): + # self.reef_intake.done() + # self.floor_intake.done() self.chassis.update_odometry() - if self.gamepad.getRightTriggerAxis() > 0.5: - self.algae_shooter.shoot() + # if self.gamepad.getRightTriggerAxis() > 0.5: + # self.algae_shooter.shoot() - if self.gamepad.getYButton(): - self.climber_state_machine.deploy_without_localization() + # if self.gamepad.getYButton(): + # self.climber_state_machine.deploy_without_localization() - if self.gamepad.getAButton(): - self.climber_state_machine.retract() + # if self.gamepad.getAButton(): + # self.climber_state_machine.retract() - if self.gamepad.getXButton(): - self.coral_depositor_component.deposit() - elif self.gamepad.getAButton(): - self.coral_depositor_component.retract() - elif self.gamepad.getBackButton(): - self.coral_depositor_component.tuck() + # if self.gamepad.getXButton(): + # self.coral_depositor_component.deposit() + # elif self.gamepad.getAButton(): + # self.coral_depositor_component.retract() + # elif self.gamepad.getBackButton(): + # self.coral_depositor_component.tuck() - if self.gamepad.getLeftBumperButton(): - self.reef_intake.intake() + # if self.gamepad.getLeftBumperButton(): + # self.reef_intake.intake() - if self.gamepad.getLeftTriggerAxis() > 0.5: - self.floor_intake.intake() + # if self.gamepad.getLeftTriggerAxis() > 0.5: + # self.floor_intake.intake() # Controllers - self.reef_intake.execute() - self.algae_shooter.execute() - self.floor_intake.execute() - self.climber_state_machine.execute() + # self.reef_intake.execute() + # self.algae_shooter.execute() + # self.floor_intake.execute() + # self.climber_state_machine.execute() # Components - self.climber.execute() - self.coral_depositor_component.execute() - self.shooter_component.execute() - self.injector_component.execute() + # self.climber.execute() + # self.coral_depositor_component.execute() + # self.shooter_component.execute() + # self.injector_component.execute() if self.gamepad.getLeftStickButton(): self.starboard_vision.zero_servo_() - self.port_vision.zero_servo_() + # self.port_vision.zero_servo_() elif self.gamepad.getRightStickButton(): self.starboard_vision.full_range_servo_() - self.port_vision.full_range_servo_() + # self.port_vision.full_range_servo_() self.starboard_vision.execute() - self.port_vision.execute() - self.wrist.execute() - self.intake_component.execute() - self.status_lights.execute() + # self.port_vision.execute() + # self.wrist.execute() + # self.intake_component.execute() + # self.status_lights.execute() # self.ballistics_component.execute() def disabledPeriodic(self) -> None: @@ -368,45 +356,45 @@ def disabledPeriodic(self) -> None: self.chassis.update_alliance() self.chassis.update_odometry() - self.intake_component.correct_and_predict() + # self.intake_component.correct_and_predict() self.starboard_vision.execute() - self.port_vision.execute() + # self.port_vision.execute() if self.gamepad.getBackButtonPressed(): self._display_auto_trajectory() - if ( - self.starboard_vision.sees_multi_tag_target() - or self.port_vision.sees_multi_tag_target() - ): - selected_auto = self._automodes.chooser.getSelected() - if isinstance(selected_auto, AutoBase): - intended_start_pose = selected_auto.get_starting_pose() - current_pose = self.chassis.get_pose() - if intended_start_pose is not None: - self.field.getObject("Intended start pos").setPose( - intended_start_pose - ) - relative_translation = intended_start_pose.relativeTo( - current_pose - ).translation() - if relative_translation.norm() > self.START_POS_TOLERANCE: - self.status_lights.invalid_start( - relative_translation, self.START_POS_TOLERANCE - ) - else: - self.status_lights.rainbow() - else: - self.status_lights.no_auto() - else: - self.status_lights.vision_timeout() - - self.status_lights.execute() - - if self.coast_button_pressed_event.getAsBoolean(): - self.chassis.toggle_coast_in_neutral() - self.wrist.toggle_neutral_mode() + # if ( + # self.starboard_vision.sees_multi_tag_target() + # # or self.port_vision.sees_multi_tag_target() + # ): + # selected_auto = self._automodes.chooser.getSelected() + # if isinstance(selected_auto, AutoBase): + # intended_start_pose = selected_auto.get_starting_pose() + # current_pose = self.chassis.get_pose() + # if intended_start_pose is not None: + # self.field.getObject("Intended start pos").setPose( + # intended_start_pose + # ) + # relative_translation = intended_start_pose.relativeTo( + # current_pose + # ).translation() + # if relative_translation.norm() > self.START_POS_TOLERANCE: + # self.status_lights.invalid_start( + # relative_translation, self.START_POS_TOLERANCE + # ) + # else: + # self.status_lights.rainbow() + # else: + # self.status_lights.no_auto() + # else: + # self.status_lights.vision_timeout() + + # self.status_lights.execute() + + # if self.coast_button_pressed_event.getAsBoolean(): + # self.chassis.toggle_coast_in_neutral() + # self.wrist.toggle_neutral_mode() def _display_auto_trajectory(self) -> None: selected_auto = self._automodes.chooser.getSelected() @@ -415,7 +403,7 @@ def _display_auto_trajectory(self) -> None: def robotPeriodic(self) -> None: super().robotPeriodic() - self.intake_component.periodic() + # self.intake_component.periodic() # Clear component per-loop caches. self.starboard_vision._per_loop_cache.clear() - self.port_vision._per_loop_cache.clear() + # self.port_vision._per_loop_cache.clear()