Skip to content

Commit c792e6a

Browse files
e-baumane-bauman
authored
Merge pull request #110 from Choate-Robotics/dev
Dev
2 parents cc39140 + 44dfeb3 commit c792e6a

File tree

48 files changed

+3350
-541
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

48 files changed

+3350
-541
lines changed

‎autonomous/__init__.py

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,4 +21,10 @@
2121
)
2222
from autonomous.routines.ROTATE.auto import routine as rotate # noqa
2323
from autonomous.routines.TWO_NOTE.auto import routine as two_note # noqa
24-
from autonomous.routines.MIDLINE_NOTES_2.auto import routine as mid_notes_2
24+
from autonomous.routines.MIDLINE_NOTES.auto_2 import routine as mid_notes_2
25+
26+
from autonomous.routines.AMP_SKIP.auto import routine as amp_skip
27+
from autonomous.routines.AMP_SKIP_2.auto import routine as amp_skip_2
28+
from autonomous.routines.AMP_SKIP_2_2.auto import routine as amp_skip_2_2
29+
from autonomous.routines.AMP_SKIP_2_2.auto_2 import routine as amp_skip_2_2_2
30+
from autonomous.routines.AMP_SKIP_NEW.auto import routine as amp_skip_new

‎autonomous/auto_routine.py

Lines changed: 17 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -4,9 +4,10 @@
44
from commands2 import CommandBase
55
from wpimath.geometry import Pose2d
66

7+
import config
78
from robot_systems import Robot
89
from utils import POIPose
9-
10+
from math import radians
1011

1112
@dataclass
1213
class AutoRoutine:
@@ -26,9 +27,21 @@ def run(self):
2627
"""
2728
Runs the autonomous routine
2829
"""
29-
30-
# Robot.drivetrain.gyro.reset_angle(self.initial_robot_pose.rotation().radians())
31-
Robot.drivetrain.reset_odometry(POIPose(self.initial_robot_pose).get())
30+
31+
32+
33+
init_robot_pose = self.initial_robot_pose
34+
35+
# if config.active_team == config.Team.RED:
36+
# init_robot_pose = Pose2d(self.initial_robot_pose.X(), self.initial_robot_pose.Y(), self.initial_robot_pose.rotation().radians() * -1)
37+
# print('SHIFTING ROTATION', init_robot_pose)
38+
39+
# Robot.drivetrain.gyro.reset_angle(init_robot_pose.rotation().radians())
40+
41+
Robot.drivetrain.gyro.reset_angle(POIPose(self.initial_robot_pose).get().rotation().radians())
42+
Robot.drivetrain.reset_odometry_auto(POIPose(self.initial_robot_pose).get())
43+
44+
3245

3346
commands2.CommandScheduler.getInstance().schedule(self.command)
3447

‎autonomous/routines/AMP_SKIP/auto.py

Lines changed: 198 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,198 @@
1+
from command.autonomous.custom_pathing import FollowPathCustom, AngleType
2+
from command.autonomous.trajectory import CustomTrajectory, PoseType
3+
from robot_systems import Robot, Field
4+
from utils import POIPose
5+
from command import *
6+
import config
7+
import math
8+
9+
from commands2 import (
10+
InstantCommand,
11+
SequentialCommandGroup,
12+
ParallelCommandGroup,
13+
ParallelDeadlineGroup,
14+
WaitCommand
15+
)
16+
17+
from autonomous.auto_routine import AutoRoutine
18+
from autonomous.routines.AMP_SKIP.coords import (
19+
shoot_first_note,
20+
get_second_note,
21+
shoot_second_note,
22+
get_third_note,
23+
shoot_third_note,
24+
far_to_mid,
25+
mid_to_far,
26+
initial
27+
)
28+
29+
from wpimath.geometry import Pose2d, Translation2d
30+
31+
path_1 = FollowPathCustom(
32+
subsystem=Robot.drivetrain,
33+
trajectory=CustomTrajectory(
34+
start_pose=POIPose(Pose2d(*shoot_first_note[0])),
35+
waypoints=[coord for coord in shoot_first_note[1]],
36+
end_pose=shoot_first_note[2],
37+
max_velocity=config.drivetrain_max_vel_auto,
38+
max_accel=config.drivetrain_max_accel_auto - 1,
39+
start_velocity=0,
40+
end_velocity=0,
41+
rev=True
42+
),
43+
theta_f=math.radians(-180)
44+
)
45+
46+
path_2 = FollowPathCustom(
47+
subsystem=Robot.drivetrain,
48+
trajectory=CustomTrajectory(
49+
# start_pose=get_second_note[0],
50+
start_pose=PoseType.current,
51+
waypoints=[coord for coord in get_second_note[1]],
52+
end_pose=get_second_note[2],
53+
max_velocity=config.drivetrain_max_vel_auto,
54+
max_accel=config.drivetrain_max_accel_auto - 1,
55+
start_velocity=0,
56+
end_velocity=0,
57+
rev=True,
58+
start_rotation=get_second_note[0].get().rotation().radians()
59+
),
60+
theta_f=math.radians(-180)
61+
)
62+
63+
path_3 = FollowPathCustom(
64+
subsystem=Robot.drivetrain,
65+
trajectory=CustomTrajectory(
66+
# start_pose=shoot_second_note[0],
67+
start_pose=PoseType.current,
68+
waypoints=[coord for coord in shoot_second_note[1]],
69+
end_pose=shoot_second_note[2],
70+
max_velocity=config.drivetrain_max_vel_auto,
71+
max_accel=config.drivetrain_max_accel_auto - 1,
72+
start_velocity=0,
73+
end_velocity=0,
74+
rev=False,
75+
start_rotation=shoot_second_note[0].get().rotation().radians()
76+
),
77+
theta_f=math.radians(163.5)
78+
)
79+
80+
path_4 = FollowPathCustom(
81+
subsystem=Robot.drivetrain,
82+
trajectory=CustomTrajectory(
83+
# start_pose=get_third_note[0],
84+
start_pose=PoseType.current,
85+
waypoints=[coord for coord in get_third_note[1]],
86+
end_pose=get_third_note[2],
87+
max_velocity=config.drivetrain_max_vel_auto,
88+
max_accel=config.drivetrain_max_accel_auto - 1,
89+
start_velocity=0,
90+
end_velocity=0,
91+
rev=True,
92+
start_rotation=get_third_note[0].get().rotation().radians()
93+
),
94+
theta_f=math.radians(-135)
95+
)
96+
97+
path_5 = FollowPathCustom(
98+
subsystem=Robot.drivetrain,
99+
trajectory=CustomTrajectory(
100+
# start_pose=shoot_third_note[0],
101+
start_pose=PoseType.current,
102+
waypoints=[coord for coord in shoot_third_note[1]],
103+
end_pose=shoot_third_note[2],
104+
max_velocity=config.drivetrain_max_vel_auto,
105+
max_accel=config.drivetrain_max_accel_auto - 1,
106+
start_velocity=0,
107+
end_velocity=0,
108+
rev=False,
109+
start_rotation=shoot_third_note[0].get().rotation().radians()
110+
),
111+
theta_f=math.radians(163.5)
112+
)
113+
114+
path_far_to_mid = FollowPathCustom(
115+
subsystem=Robot.drivetrain,
116+
trajectory=CustomTrajectory(
117+
start_pose=far_to_mid[0],
118+
waypoints=[Translation2d(*coord) for coord in far_to_mid[1]],
119+
end_pose=far_to_mid[2],
120+
max_velocity=12,
121+
max_accel=3,
122+
start_velocity=0,
123+
end_velocity=0,
124+
rev=False,
125+
),
126+
period=0.03,
127+
)
128+
129+
path_mid_to_far = FollowPathCustom(
130+
subsystem=Robot.drivetrain,
131+
trajectory=CustomTrajectory(
132+
start_pose=mid_to_far[0],
133+
waypoints=[coord for coord in mid_to_far[1]],
134+
end_pose=mid_to_far[2],
135+
max_velocity=config.drivetrain_max_vel_auto,
136+
max_accel=config.drivetrain_max_accel_auto - 1.5,
137+
start_velocity=0,
138+
end_velocity=0,
139+
rev=False
140+
),
141+
)
142+
143+
auto = ParallelCommandGroup(
144+
SetFlywheelShootSpeaker(Robot.flywheel, Field.calculations),
145+
SequentialCommandGroup(
146+
ZeroWrist(Robot.wrist),
147+
ZeroElevator(Robot.elevator),
148+
149+
InstantCommand(lambda: Field.odometry.disable()),
150+
151+
ParallelCommandGroup(
152+
path_1.raceWith(AimWrist(Robot.wrist, Field.calculations)),
153+
DeployIntake(Robot.intake),
154+
),
155+
156+
# Shoot first note
157+
InstantCommand(lambda: Field.odometry.enable()),
158+
ShootAuto(Robot.drivetrain, Robot.wrist, Robot.flywheel, Field.calculations),
159+
InstantCommand(lambda: Field.odometry.disable()),
160+
161+
# Get second note
162+
PathUntilIntake(path_2, Robot.wrist, Robot.intake, 1),
163+
164+
# Go to shot location
165+
path_3.raceWith(AimWrist(Robot.wrist, Field.calculations)),
166+
167+
# Shoot second note
168+
InstantCommand(lambda: Field.odometry.enable()),
169+
ShootAuto(Robot.drivetrain, Robot.wrist, Robot.flywheel, Field.calculations),
170+
InstantCommand(lambda: Field.odometry.disable()),
171+
172+
# Get third note
173+
PathUntilIntake(path_4, Robot.wrist, Robot.intake),
174+
175+
# Go to shot location
176+
path_5.raceWith(AimWrist(Robot.wrist, Field.calculations)),
177+
178+
# Shoot third note
179+
InstantCommand(lambda: Field.odometry.enable()),
180+
ShootAuto(Robot.drivetrain, Robot.wrist, Robot.flywheel, Field.calculations),
181+
InstantCommand(lambda: Field.odometry.disable()),
182+
183+
),
184+
185+
# SequentialCommandGroup(
186+
# InstantCommand(lambda: Field.odometry.disable()),
187+
# path_1,
188+
# path_2,
189+
# path_3,
190+
# path_4,
191+
# path_5,
192+
# )
193+
# path_far_to_mid,
194+
# path_mid_to_far
195+
196+
)
197+
198+
routine = AutoRoutine(Pose2d(*initial), auto)

‎autonomous/routines/AMP_SKIP/coords.py

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
from units.SI import meters, radians
2+
from robot_systems import Field
3+
from utils import POIPose
4+
from wpimath.geometry import Translation2d, Translation3d, Pose2d
5+
# from constants.FieldPos import MidLine
6+
from constants import field_width, FieldPos, drivetrain_length_with_bumpers, drivetrain_length
7+
import math
8+
import constants
9+
10+
coord = (meters, meters, radians)
11+
waypoints = (meters, meters)
12+
path = (coord, waypoints, coord)
13+
14+
initial = (1.9 - constants.drivetrain_length_with_bumpers/2, (constants.FieldPos.Wing.note_init + constants.FieldPos.Wing.note_gap * 1.5) + 0.15, math.radians(-180))
15+
16+
initial_shot_location = POIPose(Pose2d(initial[0] + 2.5, initial[1], math.radians(-180))).withRotation(-180)
17+
shot_location = POIPose(Pose2d(initial[0] + 2.5, initial[1], math.radians(163.5))).withRotation(163.5)
18+
19+
shoot_first_note = (
20+
initial,
21+
[],
22+
initial_shot_location
23+
)
24+
25+
get_second_note = (
26+
shoot_first_note[2],
27+
[Field.POI.Coordinates.Notes.MidLine.kFarLeft.withOffset(
28+
Translation2d(-(constants.FieldPos.MidLine.mid_line - constants.FieldPos.wing_boundary) + 0.1, 0.25))],
29+
Field.POI.Coordinates.Notes.MidLine.kFarLeft.withOffset(
30+
Translation2d((-2 * constants.drivetrain_length / 3) + 0.5, -0.125))
31+
)
32+
33+
shoot_second_note = (
34+
get_second_note[2],
35+
[Field.POI.Coordinates.Notes.MidLine.kFarLeft.withOffset(
36+
Translation2d(-(constants.FieldPos.MidLine.mid_line - constants.FieldPos.wing_boundary) + 0.1, 0.3))],
37+
shot_location
38+
)
39+
40+
get_third_note = (
41+
shoot_second_note[2],
42+
[Field.POI.Coordinates.Structures.Obstacles.kStageLeftPost.withOffset(Translation3d(-0.125, -1.65, 0))],
43+
Field.POI.Coordinates.Notes.MidLine.kMidLeft.withOffset(Translation2d((-2 * constants.drivetrain_length / 3) + 0.5, -0.05)).withRotation(-135)
44+
)
45+
46+
shoot_third_note = (
47+
get_third_note[2].withRotation(-135),
48+
[Field.POI.Coordinates.Notes.MidLine.kFarLeft.withOffset(
49+
Translation2d(-(constants.FieldPos.MidLine.mid_line - constants.FieldPos.wing_boundary) + 0.1, 0.7))],
50+
shot_location
51+
)
52+
53+
far_to_mid = (
54+
Field.POI.Coordinates.Notes.MidLine.kFarLeft.withRotation(-120),
55+
[],
56+
Field.POI.Coordinates.Notes.MidLine.kMidLeft.withOffset(Translation2d(0, -0.25)).withRotation(130)
57+
)
58+
59+
mid_to_far = (
60+
Field.POI.Coordinates.Notes.MidLine.kMidLeft.withRotation(0),
61+
[],
62+
Field.POI.Coordinates.Notes.MidLine.kFarLeft.withRotation(0)
63+
)

0 commit comments

Comments
 (0)