Skip to content

Commit

Permalink
inverted autos
Browse files Browse the repository at this point in the history
  • Loading branch information
@e-bauman
e-bauman committed Apr 18, 2024
1 parent 7e3e311 commit 01b89b8
Show file tree
Hide file tree
Showing 6 changed files with 609 additions and 26 deletions.
5 changes: 3 additions & 2 deletions autonomous/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,8 @@
)
from autonomous.routines.ROTATE.auto import routine as rotate # noqa
from autonomous.routines.TWO_NOTE.auto import routine as two_note # noqa
from autonomous.routines.MIDLINE_NOTES_2.auto import routine as mid_notes_2
from autonomous.routines.MIDLINE_NOTES.auto_2 import routine as mid_notes_2

from autonomous.routines.AMP_SKIP.auto import routine as amp_skip
from autonomous.routines.AMP_SKIP_2.auto import routine as amp_skip_2
from autonomous.routines.AMP_SKIP_2.auto import routine as amp_skip_2
from autonomous.routines.AMP_SKIP_2_2.auto import routine as amp_skip_2_2
225 changes: 225 additions & 0 deletions autonomous/routines/AMP_SKIP_2_2/auto.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,225 @@
from command.autonomous.custom_pathing import FollowPathCustom, AngleType
from command.autonomous.trajectory import CustomTrajectory, PoseType
from robot_systems import Robot, Field
from utils import POIPose
from command import *
import config
import math

from commands2 import (
InstantCommand,
SequentialCommandGroup,
ParallelCommandGroup,
ParallelDeadlineGroup,
WaitCommand,
ParallelRaceGroup
)

from autonomous.auto_routine import AutoRoutine
from autonomous.routines.AMP_SKIP_2_2.coords import (
shoot_first_note,
get_second_note,
shoot_second_note,
get_third_note,
shoot_third_note,
get_fourth_note,
far_to_mid,
mid_to_far,
initial
)

from wpimath.geometry import Pose2d, Translation2d

# path_1 = FollowPathCustom(
# subsystem=Robot.drivetrain,
# trajectory=CustomTrajectory(
# start_pose=shoot_first_note[0],
# waypoints=[coord for coord in shoot_first_note[1]],
# end_pose=shoot_first_note[2],
# max_velocity=config.drivetrain_max_vel_auto,
# max_accel=config.drivetrain_max_accel_auto - 1,
# start_velocity=0,
# end_velocity=0,
# rev=True
# ),
# theta_f=math.radians(-120)
# )

path_2 = FollowPathCustom(
subsystem=Robot.drivetrain,
trajectory=CustomTrajectory(
start_pose=POIPose(Pose2d(*get_second_note[0])),
# start_pose=PoseType.current,
waypoints=[coord for coord in get_second_note[1]],
end_pose=get_second_note[2],
max_velocity=config.drivetrain_max_vel_auto,
max_accel=config.drivetrain_max_accel_auto - 1,
start_velocity=0,
end_velocity=0,
rev=True,
start_rotation=get_second_note[0][2]
),
theta_f=math.radians(-155)
)

path_3 = FollowPathCustom(
subsystem=Robot.drivetrain,
trajectory=CustomTrajectory(
# start_pose=shoot_second_note[0],
start_pose=PoseType.current,
waypoints=[coord for coord in shoot_second_note[1]],
end_pose=shoot_second_note[2],
max_velocity=config.drivetrain_max_vel_auto,
max_accel=config.drivetrain_max_accel_auto - 1,
start_velocity=0,
end_velocity=0,
rev=False,
start_rotation=shoot_third_note[0].get().rotation().radians()
),
theta_f=AngleType.calculate
)

path_4 = FollowPathCustom(
subsystem=Robot.drivetrain,
trajectory=CustomTrajectory(
# start_pose=get_third_note[0],
start_pose=PoseType.current,
waypoints=[coord for coord in get_third_note[1]],
end_pose=get_third_note[2],
max_velocity=config.drivetrain_max_vel_auto,
max_accel=config.drivetrain_max_accel_auto - 1,
start_velocity=0,
end_velocity=1,
rev=True,
start_rotation=math.radians(-180)
),
theta_f=math.radians(-180)
)

path_5 = FollowPathCustom(
subsystem=Robot.drivetrain,
trajectory=CustomTrajectory(
# start_pose=shoot_third_note[0],
start_pose=PoseType.current,
waypoints=[coord for coord in shoot_third_note[1]],
end_pose=shoot_third_note[2],
max_velocity=config.drivetrain_max_vel_auto,
max_accel=config.drivetrain_max_accel_auto - 1,
start_velocity=0,
end_velocity=0,
rev=False,
start_rotation=math.radians(-180)
),
theta_f=AngleType.calculate
)

path_6 = FollowPathCustom(
subsystem=Robot.drivetrain,
trajectory=CustomTrajectory(
# start_pose=get_fourth_note[0],
start_pose=PoseType.current,
waypoints=[coord for coord in get_fourth_note[1]],
end_pose=get_fourth_note[2],
max_velocity=config.drivetrain_max_vel_auto,
max_accel=config.drivetrain_max_accel_auto - 1,
start_velocity=0,
end_velocity=0,
rev=True,
start_rotation=get_fourth_note[0].get().rotation().radians()
),
theta_f=(math.radians(-180))
)

# path_far_to_mid = FollowPathCustom(
# subsystem=Robot.drivetrain,
# trajectory=CustomTrajectory(
# start_pose=far_to_mid[0],
# waypoints=[Translation2d(*coord) for coord in far_to_mid[1]],
# end_pose=far_to_mid[2],
# max_velocity=12,
# max_accel=3,
# start_velocity=0,
# end_velocity=0,
# rev=False,
# ),
# period=0.03,
# )
#
# path_mid_to_far = FollowPathCustom(
# subsystem=Robot.drivetrain,
# trajectory=CustomTrajectory(
# start_pose=mid_to_far[0],
# waypoints=[coord for coord in mid_to_far[1]],
# end_pose=mid_to_far[2],
# max_velocity=config.drivetrain_max_vel_auto,
# max_accel=config.drivetrain_max_accel_auto - 1.5,
# start_velocity=0,
# end_velocity=0,
# rev=False
# ),
# )

auto = ParallelCommandGroup(
SetFlywheelShootSpeaker(Robot.flywheel, Field.calculations),
SequentialCommandGroup(
ZeroWrist(Robot.wrist),
ZeroElevator(Robot.elevator),

InstantCommand(lambda: Field.odometry.disable()),

# Shoot first note
DeployIntake(Robot.intake).withTimeout(0.3),
PassNote(Robot.wrist),

# Get second note
ParallelRaceGroup(
SequentialCommandGroup(
path_2,
path_3
),
SequentialCommandGroup(
IntakeStageNote(Robot.wrist, Robot.intake),
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
),
SequentialCommandGroup(
IntakeStageNote(Robot.wrist, Robot.intake),
AimWrist(Robot.wrist, Field.calculations)
)
),

# Shoot third note
InstantCommand(lambda: Field.odometry.enable()),
ShootAuto(Robot.drivetrain, Robot.wrist, Robot.flywheel, Field.calculations),
InstantCommand(lambda: Field.odometry.disable()),

PathUntilIntake(path_6, Robot.wrist, Robot.intake, 1)

),

# SequentialCommandGroup(
# InstantCommand(lambda: Field.odometry.disable()),
# path_2,
# path_3,
# path_4,
# path_5,
# path_6,
# )
# path_far_to_mid,
# path_mid_to_far

)

routine = AutoRoutine(Pose2d(initial[0], initial[1], math.radians(-120)), auto)
76 changes: 76 additions & 0 deletions autonomous/routines/AMP_SKIP_2_2/coords.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,76 @@
from units.SI import meters, radians
from robot_systems import Field
from utils import POIPose
from wpimath.geometry import Translation2d, Translation3d, Pose2d
# from constants.FieldPos import MidLine
from constants import field_width, FieldPos, drivetrain_length_with_bumpers, drivetrain_length
import math
import constants

coord = (meters, meters, radians)
waypoints = (meters, meters)
path = (coord, waypoints, coord)

start = Field.POI.Coordinates.Waypoints.Auto.kSubwooferLeft

initial = (0.68, field_width/2 + 2.6, math.radians(-120))

initial_shot_location = POIPose(Pose2d(initial[0] + 2.5, initial[1], math.radians(-180))).withRotation(-180)
shot_location = Field.POI.Coordinates.Notes.MidLine.kFarLeft.withOffset(Translation2d(-(constants.FieldPos.MidLine.mid_line - constants.FieldPos.wing_boundary) - 1.75, 0.4))

shoot_first_note = (
start,
[],
POIPose(Pose2d(initial[0] + 0.2, initial[1], math.radians(-120)))
)

get_second_note = (
initial,
[Field.POI.Coordinates.Structures.Scoring.kAmp.withOffset(Translation2d(0, 0.6)),
Field.POI.Coordinates.Structures.Scoring.kAmp.withOffset(Translation2d(2, 0.6))],
Field.POI.Coordinates.Notes.MidLine.kMidLeft.withOffset(Translation2d((-2 * constants.drivetrain_length / 3) + 0.5, 0)).withRotation(-135)
)

shoot_second_note = (
get_second_note[2],
[
# Field.POI.Coordinates.Notes.MidLine.kFarLeft.withOffset(
# Translation2d(-(constants.FieldPos.MidLine.mid_line - constants.FieldPos.wing_boundary) + 0.1, 0.3))
],
shot_location
)

get_third_note = (
shoot_second_note[2],
[
shoot_second_note[2].withOffset(Translation2d(1, 0))
],
Field.POI.Coordinates.Notes.MidLine.kFarLeft.withOffset(Translation2d(0.1, -0.2)).withRotation(-180)
)

shoot_third_note = (
get_third_note[2].withRotation(-135),
[
# Field.POI.Coordinates.Notes.MidLine.kFarLeft.withOffset(
# Translation2d(-(constants.FieldPos.MidLine.mid_line - constants.FieldPos.wing_boundary) + 0.1, 0.7))
],
shot_location
)

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))
)

far_to_mid = (
Field.POI.Coordinates.Notes.MidLine.kFarLeft.withRotation(-120),
[],
Field.POI.Coordinates.Notes.MidLine.kMidLeft.withOffset(Translation2d(0, -0.25)).withRotation(130)
)

mid_to_far = (
Field.POI.Coordinates.Notes.MidLine.kMidLeft.withRotation(0),
[],
Field.POI.Coordinates.Notes.MidLine.kFarLeft.withRotation(0)
)
Loading

0 comments on commit 01b89b8

Please sign in to comment.