diff --git a/autonomous/routines/AMP_SKIP_2/auto.py b/autonomous/routines/AMP_SKIP_2/auto.py index eeb22ae9..afbe40aa 100644 --- a/autonomous/routines/AMP_SKIP_2/auto.py +++ b/autonomous/routines/AMP_SKIP_2/auto.py @@ -1,6 +1,6 @@ from command.autonomous.custom_pathing import FollowPathCustom, AngleType from command.autonomous.trajectory import CustomTrajectory, PoseType -from robot_systems import Robot, Field +from robot_systems import Robot, Field, Sensors from utils import POIPose from command import * import config @@ -12,7 +12,9 @@ ParallelCommandGroup, ParallelDeadlineGroup, WaitCommand, - ParallelRaceGroup + ParallelRaceGroup, + ConditionalCommand, + WaitUntilCommand ) from autonomous.auto_routine import AutoRoutine @@ -55,7 +57,7 @@ max_velocity=config.drivetrain_max_vel_auto, max_accel=config.drivetrain_max_accel_auto - 1, start_velocity=0, - end_velocity=1, + end_velocity=0, rev=True, start_rotation=get_second_note[0][2] ), @@ -172,13 +174,19 @@ PassNote(Robot.wrist), # Get second note - ParallelRaceGroup( + PathUntilIntake(path_2, Robot.wrist, Robot.intake).raceWith( SequentialCommandGroup( - path_2, - path_3 - ), - IntakeThenAim(Robot.intake, Robot.wrist, Field.calculations) + WaitCommand(1.75), + WaitUntilCommand(lambda: Sensors.limelight_intake.ta > 0.4 and Sensors.limelight_intake.ta < 1.75) + ) + ), + ConditionalCommand( + AutoPickupNote(Robot.drivetrain, Robot.wrist, Robot.intake, Sensors.limelight_intake), + WaitCommand(0), + lambda: Sensors.limelight_intake.ta > 0.4 and not Robot.wrist.note_in_feeder() ), + + path_3.raceWith(AimWrist(Robot.wrist, Field.calculations)), # Shoot second note InstantCommand(lambda: Field.odometry.enable()), @@ -186,13 +194,14 @@ InstantCommand(lambda: Field.odometry.disable()), # Get third note - ParallelRaceGroup( - SequentialCommandGroup( - path_4, - path_5 - ), - IntakeThenAim(Robot.intake, Robot.wrist, Field.calculations) + PathUntilIntake(path_4, Robot.wrist, Robot.intake).raceWith(WaitUntilCommand(lambda: Sensors.limelight_intake.ta > 0.4 and Sensors.limelight_intake.ta < 1.75)), + ConditionalCommand( + AutoPickupNote(Robot.drivetrain, Robot.wrist, Robot.intake, Sensors.limelight_intake), + WaitCommand(0), + lambda: Sensors.limelight_intake.ta > 0.4 and not Robot.wrist.note_in_feeder() ), + + path_5.raceWith(AimWrist(Robot.wrist, Field.calculations)), # Shoot third note InstantCommand(lambda: Field.odometry.enable()), diff --git a/autonomous/routines/AMP_SKIP_2/coords.py b/autonomous/routines/AMP_SKIP_2/coords.py index e4f1a27a..4110cb4e 100644 --- a/autonomous/routines/AMP_SKIP_2/coords.py +++ b/autonomous/routines/AMP_SKIP_2/coords.py @@ -59,7 +59,7 @@ get_fourth_note = ( shoot_third_note[2].withRotation(-100), [Field.POI.Coordinates.Structures.Obstacles.kStage.withOffset(Translation2d(0.15, -0.45))], - Field.POI.Coordinates.Notes.MidLine.kCenter.withOffset(Translation2d(0.1, -0.5)) + Field.POI.Coordinates.Notes.MidLine.kCenter.withOffset(Translation2d(0.2, -0.5)) ) far_to_mid = ( diff --git a/autonomous/routines/MIDLINE_NOTES/auto.py b/autonomous/routines/MIDLINE_NOTES/auto.py index 13f036e5..4d76c3a2 100644 --- a/autonomous/routines/MIDLINE_NOTES/auto.py +++ b/autonomous/routines/MIDLINE_NOTES/auto.py @@ -66,7 +66,7 @@ max_velocity=config.drivetrain_max_vel_auto, max_accel=config.drivetrain_max_accel_auto - 1.25, start_velocity=0, - end_velocity=1.5, + end_velocity=0.5, rev=False, start_rotation=shoot_second_note[0].get().rotation().radians() ), @@ -151,7 +151,7 @@ max_velocity=config.drivetrain_max_vel_auto, max_accel=config.drivetrain_max_accel_auto - 1.25, start_velocity=0, - end_velocity=1.5, + end_velocity=0.5, rev=False, start_rotation=come_back_with_third[0].get().rotation().radians() ), diff --git a/autonomous/routines/MIDLINE_NOTES/coords.py b/autonomous/routines/MIDLINE_NOTES/coords.py index 4c08f3d6..3576f0f4 100644 --- a/autonomous/routines/MIDLINE_NOTES/coords.py +++ b/autonomous/routines/MIDLINE_NOTES/coords.py @@ -25,7 +25,7 @@ [ # Field.POI.Coordinates.Structures.Obstacles.kStageRightPost.withOffset(Translation3d(0, 1.3, 0)), ], - Field.POI.Coordinates.Notes.MidLine.kFarRight.withOffset(Translation2d(0.1, 0.25)) # 0.25 + Field.POI.Coordinates.Notes.MidLine.kFarRight.withOffset(Translation2d(0.1, 0.1)) # 0.25 ) shoot_second_note = ( @@ -45,7 +45,7 @@ come_back_with_third = ( get_third_note[2].withRotation(135), [Field.POI.Coordinates.Structures.Obstacles.kStageRightPost.withOffset(Translation3d(0, 1.5, 0))], - Field.POI.Coordinates.Structures.Obstacles.kStageCenterPost.withOffset(Translation3d(-0.2, 1.1, 0)).withRotation(-135) + Field.POI.Coordinates.Structures.Obstacles.kStageCenterPost.withOffset(Translation3d(-0.1, 1.0, 0)).withRotation(-135) ) shoot_third_note = ( diff --git a/config.py b/config.py index 5160c44e..3bc0990b 100644 --- a/config.py +++ b/config.py @@ -197,7 +197,7 @@ class LimelightPosition: elevator_ramp_rate: float = 0.0 elevator_feed_forward: float = 0.0 elevator_climb_ff: float = -1 -elevator_climb_current_limit: float = 55 +elevator_climb_current_limit: float = 65 elevator_zeroed_pos = 0.036 if comp_bot.get() else 0.023 # Wrist @@ -259,7 +259,7 @@ class LimelightPosition: drivetrain_aiming_move_speed_threshold: meters_per_second = 0.4 drivetrain_aiming_tilt_threshold: radians = 3 * degrees_to_radians shot_height_offset: inches = 0 # inches -shot_angle_offset: degrees = 0.4 if active_team == Team.RED else 0.4 +shot_angle_offset: degrees = 0.55 if active_team == Team.RED else 0.55 wrist_shot_tolerance: degrees = 2#1.75 if comp_bot.get() else 2 wrist_velocity_shot_tolerance: degrees = 1 shot_height_offset_scalar: float = 0.014