From e39d1631ac1c226214fa31ffb1e35381fde882f5 Mon Sep 17 00:00:00 2001 From: oliverw10 Date: Sat, 26 Mar 2022 23:41:15 +1100 Subject: [PATCH 1/4] added move-shoot and 6 ball path --- autonomous/autonomous.py | 125 +++++++++++++++++++++++++++++++++------ 1 file changed, 106 insertions(+), 19 deletions(-) diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index c99e5ef..a40000d 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -26,6 +26,7 @@ class WaypointType(Enum): PICKUP_TO_TWO = auto() SHOOT = auto() SIMPLE = auto() + MOVE_SHOOT = auto() @dataclass @@ -82,7 +83,10 @@ def __init__(self): max_speed = Chassis.max_attainable_wheel_speed * 0.7 self.trajectory_config = TrajectoryConfig( maxVelocity=max_speed, maxAcceleration=2.9 - ) # Acceleration expressed as max_speed / t where t is time taken to reach max speed + ) + self.shoot_trajectory_config = TrajectoryConfig( + maxVelocity=1, maxAcceleration=2.5 + ) self.movements: List[Movement] = [] self.start_pose = Pose2d(0, 0, 0) @@ -119,8 +123,7 @@ def startup(self, tm: float) -> None: self.trajectory_start_time = tm self.next_state("move") - @state - def move(self, tm: float) -> None: + def do_move(self, tm: float) -> None: # indexer controller will hanle it self raising and lowering if self.indexer.ready_to_intake(): self.intake.deployed = True @@ -132,18 +135,6 @@ def move(self, tm: float) -> None: current_pose = self.chassis.get_pose() - if traj_time > self.current_trajectory.totalTime() and ( - self.drive_controller.atReference() or wpilib.RobotBase.isSimulation() - ): - self.logger.info( - f"Got to end of movement {self.current_movement_idx} at {tm}" - ) - if self.current_movement.type is WaypointType.SHOOT: - self.next_state("firing") - elif self.current_movement.type is WaypointType.PICKUP_TO_TWO: - self.next_state("pickup_to_two") - else: - self.move_next_waypoint(tm) self.chassis_speeds = self.drive_controller.calculate( currentPose=current_pose, desiredState=target_state, @@ -161,6 +152,40 @@ def move(self, tm: float) -> None: self._maybe_hail_mary() + @state + def move(self, tm: float) -> None: + self.do_move(tm) + self.shooter_control.lead_shots = False + traj_time = tm - self.trajectory_start_time + if traj_time > self.current_trajectory.totalTime() and ( + self.drive_controller.atReference() or wpilib.RobotBase.isSimulation() + ): + self.logger.info( + f"Got to end of movement {self.current_movement_idx} at {tm}" + ) + if self.current_movement.type is WaypointType.SHOOT: + self.next_state("firing") + elif self.current_movement.type is WaypointType.PICKUP_TO_TWO: + self.next_state("pickup") + else: + self.move_next_waypoint(tm) + if self.current_state == "finished": + return + if self.current_movement.type == WaypointType.MOVE_SHOOT: + self.next_state("move_shoot") + + @state + def move_shoot(self, tm: float) -> None: + self.do_move(tm) + self.shooter_control.lead_shots = True + self.shooter_control.fire() + + traj_time = tm - self.trajectory_start_time + if traj_time > self.current_trajectory.totalTime() and ( + self.drive_controller.atReference() or wpilib.RobotBase.isSimulation() + ): + self.next_state("firing") + @state def pickup_to_two(self, state_tm: float, tm: float) -> None: """Waits until full""" @@ -171,8 +196,13 @@ def pickup_to_two(self, state_tm: float, tm: float) -> None: and self.indexer.has_cargo_in_tunnel() or state_tm > 1.5 ) or (wpilib.RobotBase.isSimulation() and state_tm > 1): - self.next_state("move") self.move_next_waypoint(tm) + if self.current_state == "finished": + return + if self.current_movement.type == WaypointType.MOVE_SHOOT: + self.next_state("move_shoot") + else: + self.next_state("move") self._maybe_hail_mary() @@ -192,10 +222,15 @@ def firing(self, state_tm: float, tm: float) -> None: or self.indexer.has_cargo_in_tunnel() or self.indexer_control.current_state == "transferring_to_chimney" or self.indexer_control.current_state == "firing" - or wpilib.RobotBase.isSimulation() + # or wpilib.RobotBase.isSimulation() ): - self.next_state("move") self.move_next_waypoint(tm) + if self.current_state == "finished": + return + if self.current_movement.type == WaypointType.MOVE_SHOOT: + self.next_state("move_shoot") + else: + self.next_state("move") @state def finished(self) -> None: @@ -328,7 +363,7 @@ def setup(self) -> None: end=Pose2d(-5.0, -2.0, Rotation2d.fromDegrees(0)), interior=[], config=self.trajectory_config, - chassis_heading=Rotation2d.fromDegrees(-135), + chassis_heading=Rotation2d.fromDegrees(-110), ), Movement( WaypointType.SIMPLE, @@ -342,6 +377,58 @@ def setup(self) -> None: super().setup() +class SixBall(AutoBase): + MODE_NAME = "Flex O'clock" + + def setup(self) -> None: + super().setup() + self.movements = [ + Movement( + WaypointType.SHOOT, + start_direction=Rotation2d.fromDegrees(-90), + end=Pose2d(-0.65, -3.55, Rotation2d.fromDegrees(-90)), + interior=[], + config=self.trajectory_config, + chassis_heading=Rotation2d.fromDegrees(-88.5), + ), + Movement( + WaypointType.SHOOT, + start_direction=Rotation2d.fromDegrees(100), + end=Pose2d(-3.4, -2.1, Rotation2d.fromDegrees(180)), + interior=[ + Translation2d(-1.8, -2.3), + ], + config=self.trajectory_config, + chassis_heading=Rotation2d.fromDegrees(180), + ), + Movement( + WaypointType.PICKUP_TO_TWO, + start_direction=Rotation2d.fromDegrees(170), + end=Pose2d(-6.95, -2.8, Rotation2d.fromDegrees(-135)), + interior=[], + config=self.trajectory_config, + chassis_heading=Rotation2d.fromDegrees(-135), + ), + Movement( + WaypointType.MOVE_SHOOT, + start_direction=Rotation2d.fromDegrees(45), + end=Pose2d(-4.23, 0.8, Rotation2d.fromDegrees(45)), + interior=[], + config=self.shoot_trajectory_config, + chassis_heading=Rotation2d.fromDegrees(45), + ), + Movement( + WaypointType.SHOOT, + start_direction=Rotation2d.fromDegrees(45), + end=Pose2d(-3.45, 1.85, Rotation2d.fromDegrees(45)), + interior=[], + config=self.trajectory_config, + chassis_heading=Rotation2d.fromDegrees(45), + ), + ] + super().setup() + + class TwoBall(AutoBase): """Auto starting middle of left tarmac, picking up ball 1 In case we have a partner who can do a five ball From e40b5e4089387a3536dbcbefbb2322f34ed451e1 Mon Sep 17 00:00:00 2001 From: oliverw10 Date: Sun, 27 Mar 2022 00:17:56 +1100 Subject: [PATCH 2/4] increased speeds --- autonomous/autonomous.py | 2 +- crazy-frog.chrp | Bin 0 -> 1116 bytes 2 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 crazy-frog.chrp diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index a40000d..43521ad 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -85,7 +85,7 @@ def __init__(self): maxVelocity=max_speed, maxAcceleration=2.9 ) self.shoot_trajectory_config = TrajectoryConfig( - maxVelocity=1, maxAcceleration=2.5 + maxVelocity=1.3, maxAcceleration=2.5 ) self.movements: List[Movement] = [] diff --git a/crazy-frog.chrp b/crazy-frog.chrp new file mode 100644 index 0000000000000000000000000000000000000000..4b03e38e64cde0884b4ed733bd4037ec3def0d59 GIT binary patch literal 1116 zcmXBTUr1A77zXfXn`XJxX{A|enPz{cMg&VuU72YsFUmh@&h4C?lp@Lug0dit@FLNh z#QuE3ZH?Dwc2Hjea4RYV zKh1W@OaLBwlX3Fhp3t+2vi9Q!{llGCj+!)qwI%pje0V*lcRLqCOw-=6SMUFXX%%0*|*@{3@kG8 zd{+)o&qya%X~`Dtq=BYv)lPQNIYv5(-v&e&>3NDi-maaD(d%xR380VuW~7t2T;M$; zo&2OldD=-Gjm_6ia_A@{XKg!xj-9f$@X7<+*{z*iC;-X}Wj}~vdZkF#272fWBb`L= z0p2ju$#>dUtep(e%O!fYo1QP#_pkB-1$$-RLKkh^r!(j1b4EIur2S>uNtj;TubuSL z>T;Pk(Mt0U=rc=cOoeumN`El&zvk(YO6}wfbsyADD(PKDzLODp+NYg#(zS>5tVyS; zWNjh68u-h|-T7DpJgk-dAd>3%y)n|sEcG4JPMT>}z3hkJrSpv39ajUt2S(2Rq92ay z`wun(xBaqjVTO)2>C6O84rnLEbdiy}lM>{8WaRG5(IG?TO+;w7sn5JmPlmLUc3Ni1 sOaQg?86)4xIPE&2oea Date: Tue, 29 Mar 2022 09:55:28 +1100 Subject: [PATCH 3/4] fix six ball waypoints and stopped instant shooting in shoot-move --- autonomous/autonomous.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index 43521ad..adef57b 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -175,10 +175,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 ( @@ -411,16 +412,16 @@ def setup(self) -> None: ), Movement( WaypointType.MOVE_SHOOT, - start_direction=Rotation2d.fromDegrees(45), - end=Pose2d(-4.23, 0.8, Rotation2d.fromDegrees(45)), + start_direction=Rotation2d.fromDegrees(53), + end=Pose2d(-4.23, 0.8, Rotation2d.fromDegrees(53)), interior=[], config=self.shoot_trajectory_config, - chassis_heading=Rotation2d.fromDegrees(45), + chassis_heading=Rotation2d.fromDegrees(-120), ), Movement( WaypointType.SHOOT, - start_direction=Rotation2d.fromDegrees(45), - end=Pose2d(-3.45, 1.85, Rotation2d.fromDegrees(45)), + start_direction=Rotation2d.fromDegrees(53), + end=Pose2d(-3.45, 1.85, Rotation2d.fromDegrees(53)), interior=[], config=self.trajectory_config, chassis_heading=Rotation2d.fromDegrees(45), From 862ba585053ac29991ce745ddc313311794cf7f3 Mon Sep 17 00:00:00 2001 From: oliverw10 Date: Wed, 30 Mar 2022 09:39:42 +1100 Subject: [PATCH 4/4] added six ball start pose --- autonomous/autonomous.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index adef57b..261a381 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -166,7 +166,7 @@ def move(self, tm: float) -> None: if self.current_movement.type is WaypointType.SHOOT: self.next_state("firing") elif self.current_movement.type is WaypointType.PICKUP_TO_TWO: - self.next_state("pickup") + self.next_state("pickup_to_two") else: self.move_next_waypoint(tm) if self.current_state == "finished": @@ -382,7 +382,7 @@ class SixBall(AutoBase): MODE_NAME = "Flex O'clock" def setup(self) -> None: - super().setup() + self.start_pose = right_mid_start self.movements = [ Movement( WaypointType.SHOOT,