battlecry changes

Choate-Robotics · Jun 9, 2024 · 3454d48 · 3454d48
1 parent c792e6a
commit 3454d48
Show file tree

Hide file tree

Showing 5 changed files with 30 additions and 21 deletions.
diff --git 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,27 +174,34 @@
         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()),
         ShootAuto(Robot.drivetrain, Robot.wrist, Robot.flywheel, Field.calculations),
         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
@@ -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
@@ -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
@@ -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
@@ -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