Skip to content

Test Bot Code #425

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

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

Test Bot Code #425

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
19 changes: 9 additions & 10 deletions autonomous/auto_base.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand Down Expand Up @@ -95,15 +90,17 @@

@state(first=True)
def initialising(self) -> None:
return
# Add any tasks that need doing first
self.reef_intake.holding_coral = True

Check failure on line 95 in autonomous/auto_base.py

View workflow job for this annotation

GitHub Actions / mypy 

Statement is unreachable
self.chassis.do_smooth = False
self.chassis.heading_controller.setPID(Kp=1.0, Ki=0.0, Kd=0.0)
self.next_state("tracking_trajectory")

@state
def tracking_trajectory(self, initial_call, state_tm) -> None:
return
Copy link
Member

@auscompgeek auscompgeek Jun 21, 2025

Choose a reason for hiding this comment

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

wat

Copy link
Contributor Author

@LeanMeanBeanMachine4774 LeanMeanBeanMachine4774 Jun 21, 2025

Choose a reason for hiding this comment

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

It was done, so the code wouldn't crash due to the auto requiring stuff that was removed. I guess the tracking trajectory one wouldn't though

Copy link
Member

@auscompgeek auscompgeek Jun 22, 2025

Choose a reason for hiding this comment

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

I think we definitely want to be able to run paths on the test bot

Copy link
Contributor Author

@LeanMeanBeanMachine4774 LeanMeanBeanMachine4774 Jun 22, 2025

Choose a reason for hiding this comment

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

Yes, definitely, just did this to get it working

if initial_call:

Check failure on line 103 in autonomous/auto_base.py

View workflow job for this annotation

GitHub Actions / mypy 

Statement is unreachable
self.current_leg += 1

if self.current_leg == len(self.trajectories):
Expand Down Expand Up @@ -170,7 +167,8 @@

@timed_state(duration=1.0, next_state="tracking_trajectory")
def intaking_algae(self) -> None:
return
if self.current_leg == 0:

Check failure on line 171 in autonomous/auto_base.py

View workflow job for this annotation

GitHub Actions / mypy 

Statement is unreachable
self.coral_depositor_component.deposit()
if self.injector_component.has_algae():
self.coral_depositor_component.tuck()
Expand All @@ -178,7 +176,8 @@

@timed_state(duration=1.0, next_state="tracking_trajectory")
def shooting_algae(self) -> None:
return
self.algae_shooter.shoot()

Check failure on line 180 in autonomous/auto_base.py

View workflow job for this annotation

GitHub Actions / mypy 

Statement is unreachable

if not self.algae_shooter.is_executing:
self.next_state("tracking_trajectory")
243 changes: 120 additions & 123 deletions physics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@
from pyfrc.physics.core import PhysicsInterface
from wpilib.simulation import (
DCMotorSim,
DIOSim,
DutyCycleEncoderSim,
PWMSim,
SingleJointedArmSim,
Expand All @@ -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

Expand Down Expand Up @@ -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

Expand All @@ -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(
Expand All @@ -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
Expand All @@ -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(
(
Expand Down Expand Up @@ -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
Expand All @@ -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)
Loading
Loading