fix six ball waypoints and stopped instant shooting in shoot-move

Author: OliverW10

autonomous/autonomous.py

@@ -172,10 +172,11 @@ def move(self, tm: float) -> None:
                     self.next_state("move_shoot")
 
     @state
-    def move_shoot(self, tm: float) -> None:
+    def move_shoot(self, tm: float, state_tm: float) -> None:
         self.do_move(tm)
         self.shooter_control.lead_shots = True
-        self.shooter_control.fire()
+        if state_tm > 0.2:
+            self.shooter_control.fire()
 
         traj_time = tm - self.trajectory_start_time
         if traj_time > self.current_trajectory.totalTime() and (
@@ -461,22 +462,22 @@ def setup(self) -> None:
             Movement(
                 WaypointType.MOVE_SHOOT,
                 TrajectoryGenerator.generateTrajectory(
-                    start=Pose2d(-6.95, -2.8, Rotation2d.fromDegrees(45)),
-                    end=Pose2d(-4.23, 0.8, Rotation2d.fromDegrees(45)),
+                    start=Pose2d(-6.95, -2.8, Rotation2d.fromDegrees(53)),
+                    end=Pose2d(-4.23, 0.8, Rotation2d.fromDegrees(53)),
                     interiorWaypoints=[],
                     config=self.shoot_trajectory_config,
                 ),
-                Rotation2d.fromDegrees(45),
+                Rotation2d.fromDegrees(-120),
             ),
             Movement(
                 WaypointType.SHOOT,
                 TrajectoryGenerator.generateTrajectory(
-                    start=Pose2d(-4.23, 0.8, Rotation2d.fromDegrees(45)),
-                    end=Pose2d(-3.45, 1.85, Rotation2d.fromDegrees(45)),
+                    start=Pose2d(-4.23, 0.8, Rotation2d.fromDegrees(53)),
+                    end=Pose2d(-3.45, 1.85, Rotation2d.fromDegrees(53)),
                     interiorWaypoints=[],
                     config=self.trajectory_config,
                 ),
-                Rotation2d.fromDegrees(45),
+                Rotation2d.fromDegrees(60),
             ),
         ]