def test_generate_routes(robot):
with open(sys.path[0] + ("/../" if os.getcwd()[-5:-1] == "test" else "/") +
"config/routes.json") as f:
routes = json.load(f)
for name, config in routes.items():
trajectories = []
for path_config in config["paths"]:
path = []
for point in path_config:
path.append(
pathfinder.Waypoint(point["x"], point["y"],
pathfinder.d2r(point["angle"])))
trajectories.append(
PathGenerator.generate(path, config["dt"],
config["max_velocity"],
config["max_acceleration"],
config["max_jerk"]))
PathGenerator.set(name, trajectories)
# ]
#
# GeneratePath(os.path.dirname(__file__), "right_cube_get_switch_prep", waypoints, settings, True)
#===================================================================================================
PathFinderSettings = namedtuple("PathFinderSettings", [
"order", "samples", "period", "maxVelocity", "maxAcceleration", "maxJerk"
])
settings = PathFinderSettings(
order=pf.FIT_HERMITE_CUBIC,
samples=1000000,
period=0.01,
#maxVelocity=3.0,#
maxVelocity=5.0,
#maxAcceleration=6,#
maxAcceleration=7,
maxJerk=30)
# The waypoints are entered as X, Y, and Theta. +X is forward, +Y is left, and +Theta is measured from +X to +Y
xOffset = 0.5 * X_ROBOT_LENGTH
yOffset = -(Y_WALL_TO_EXCHANGE_FAR + 0.5 * Y_ROBOT_WIDTH)
waypoints = [
pf.Waypoint(0, 0, pf.d2r(-45.0)),
pf.Waypoint(-40 / 12, 38 / 12, 0),
]
GenerateTalonMotionProfileArcPath(os.path.dirname(__file__),
"right_cube_get_switch_prep", waypoints,
settings, True)
# # used by the controller. For example, use either metric or imperial units. Also, use a
# # consistent frame of reference. This means that +X is forward, -X is backward, +Y is right, and
# # -Y is left, +headings are going from +X towards +Y, and -headings are going from +X to -Y.
# waypoints = [
# pf.Waypoint(0, 0, 0),
# pf.Waypoint(-60 / 12, 0, pf.d2r(-45.0)),
# ]
#
# GeneratePath(os.path.dirname(__file__), "left_switch_cube_retrieval", waypoints, settings, True)
#===================================================================================================
PathFinderSettings = namedtuple("PathFinderSettings", ["order", "samples", "period", "maxVelocity", "maxAcceleration", "maxJerk"])
settings = PathFinderSettings(order=pf.FIT_HERMITE_CUBIC,
samples=1000000,
period=0.01,
maxVelocity=3.0,
maxAcceleration=6,
maxJerk=30)
# The waypoints are entered as X, Y, and Theta. +X is forward, +Y is left, and +Theta is measured from +X to +Y
xOffset = 0.5 * X_ROBOT_LENGTH
yOffset = -(Y_WALL_TO_EXCHANGE_FAR + 0.5 * Y_ROBOT_WIDTH)
waypoints = [
pf.Waypoint(0, 0, 0),
pf.Waypoint(-60 / 12, 0, pf.d2r(-45.0)),
]
GenerateTalonMotionProfileArcPath(os.path.dirname(__file__), "left_switch_cube_retrieval", waypoints, settings)
# ]
#
# GeneratePath(os.path.dirname(__file__), "right_start_right_scale", waypoints, settings)
#===================================================================================================
PathFinderSettings = namedtuple("PathFinderSettings", [
"order", "samples", "period", "maxVelocity", "maxAcceleration", "maxJerk"
])
settings = PathFinderSettings(order=pf.FIT_HERMITE_QUINTIC,
samples=1000000,
period=0.02,
maxVelocity=7.0,
maxAcceleration=10,
maxJerk=30)
# The waypoints are entered as X, Y, and Theta. +X is forward, +Y is left, and +Theta is measured from +X to +Y
xOffset = 0.5 * X_ROBOT_LENGTH
yOffset = -(Y_WALL_TO_EXCHANGE_FAR + 0.5 * Y_ROBOT_WIDTH)
waypoints = [
pf.Waypoint(0, 0, 0),
pf.Waypoint(X_WALL_TO_SWITCH_FAR - 0.5 * X_ROBOT_LENGTH,
Y_WALL_TO_SCALE_FAR + 0.5 * Y_ROBOT_WIDTH, 0),
pf.Waypoint(
X_WALL_TO_SCALE_NEAR + math.sin(pf.d2r(20.0)) * 0.5 * Y_ROBOT_WIDTH -
0.5 * X_ROBOT_LENGTH, Y_WALL_TO_SCALE_FAR, pf.d2r(-20.0)),
]
GenerateTalonMotionProfileArcPath(os.path.dirname(__file__),
"left_start_left_scale", waypoints, settings)
Y_CENTER_TO_SWITCH_CENTER, Y_WALL_TO_EXCHANGE_FAR
from utilities.functions import GeneratePath
class settings():
order = pf.FIT_HERMITE_QUINTIC
samples = 1000000
period = 0.02
maxVelocity = 4.0
maxAcceleration = 10
maxJerk = 30
# The waypoints are entered as X, Y, and Theta. Theta is measured clockwise from the X-axis and
# is in units of radians. It is important to generate the paths in a consistent manner to those
# used by the controller. For example, use either metric or imperial units. Also, use a
# consistent frame of reference. This means that +X is forward, -X is backward, +Y is right, and
# -Y is left, +headings are going from +X towards +Y, and -headings are going from +X to -Y.
waypoints = [
pf.Waypoint(0.5 * X_ROBOT_LENGTH,
Y_WALL_TO_EXCHANGE_FAR + 0.5 * Y_ROBOT_WIDTH, 0),
pf.Waypoint(X_WALL_TO_SWITCH_NEAR / 2,
13.5 - Y_CENTER_TO_SWITCH_CENTER / 2, pf.d2r(-45.0)),
#pf.Waypoint(0.5 * X_ROBOT_LENGTH + 2, Y_WALL_TO_EXCHANGE_FAR + 0.5 * Y_ROBOT_WIDTH, 0),
#pf.Waypoint(X_WALL_TO_SWITCH_NEAR - 0.5 * X_ROBOT_LENGTH, 13.5 - Y_CENTER_TO_SWITCH_CENTER, 0),
]
GeneratePath(os.path.dirname(__file__), "middle_start_left_switch_path",
waypoints, settings)
# ]
#
# GeneratePath(os.path.dirname(__file__), "left_start_left_switch", waypoints, settings)
#===================================================================================================
PathFinderSettings = namedtuple("PathFinderSettings", ["order", "samples", "period", "maxVelocity", "maxAcceleration", "maxJerk"])
settings = PathFinderSettings(order=pf.FIT_HERMITE_QUINTIC,
samples=1000000,
period=0.01,
maxVelocity=5.0,
maxAcceleration=10,
maxJerk=30)
# The waypoints are entered as X, Y, and Theta. +X is forward, +Y is left, and +Theta is measured from +X to +Y
xOffset = 0.5 * X_ROBOT_LENGTH
yOffset = -(Y_WALL_TO_START + 0.5 * Y_ROBOT_WIDTH)
waypoints = [
pf.Waypoint(xOffset, yOffset, 0),
pf.Waypoint(X_WALL_TO_SWITCH_CENTER - 48 /12, 18 / 12 + yOffset, 0),
pf.Waypoint(X_WALL_TO_SWITCH_CENTER, -24 / 12 + yOffset, pf.d2r(90.0)),
]
GenerateTalonMotionProfileArcPath(os.path.dirname(__file__), "left_start_left_switch", waypoints, settings)