def path_init(self):
# Set up the trajectory
points = [
pf.Waypoint(0, 0, 0),
pf.Waypoint(6, -6, 0),
pf.Waypoint(11, -6, 0),
pf.Waypoint(8, 0, math.radians(-90)),
#pf.Waypoint(23.5, 8, 0),
]
def autonomousInit(self):
# Set up the trajectory
points = [pf.Waypoint(0, 0, 0), pf.Waypoint(9, 5, 0)]
info, trajectory = pf.generate(
points,
pf.FIT_HERMITE_CUBIC,
pf.SAMPLES_HIGH,
dt=self.getPeriod(),
max_velocity=self.MAX_VELOCITY,
max_acceleration=self.MAX_ACCELERATION,
max_jerk=120.0,
)
# Wheelbase Width = 2 ft
modifier = pf.modifiers.TankModifier(trajectory).modify(2.0)
# Do something with the new Trajectories...
left = modifier.getLeftTrajectory()
right = modifier.getRightTrajectory()
leftFollower = pf.followers.EncoderFollower(left)
leftFollower.configureEncoder(self.l_encoder.get(),
self.ENCODER_COUNTS_PER_REV,
self.WHEEL_DIAMETER)
leftFollower.configurePIDVA(1.0, 0.0, 0.0, 1 / self.MAX_VELOCITY, 0)
rightFollower = pf.followers.EncoderFollower(right)
rightFollower.configureEncoder(self.r_encoder.get(),
self.ENCODER_COUNTS_PER_REV,
self.WHEEL_DIAMETER)
rightFollower.configurePIDVA(1.0, 0.0, 0.0, 1 / self.MAX_VELOCITY, 0)
self.leftFollower = leftFollower
self.rightFollower = rightFollower
# This code renders the followed path on the field in simulation (requires pyfrc 2018.2.0+)
if wpilib.RobotBase.isSimulation():
from pyfrc.sim import get_user_renderer
renderer = get_user_renderer()
if renderer:
renderer.draw_pathfinder_trajectory(left,
color="#0000ff",
offset=(-1, 0))
renderer.draw_pathfinder_trajectory(modifier.source,
color="#00ff00",
show_dt=1.0,
dt_offset=0.0)
renderer.draw_pathfinder_trajectory(right,
color="#0000ff",
offset=(1, 0))
def mousePressEvent(self, QMouseEvent):
pos = QMouseEvent.pos()
if QApplication.keyboardModifiers() & Qt.ControlModifier:
pnt = self.get_closest_waypoint(pos, 20)
if pnt is not None:
index = self.points.index(pnt)
self.points.remove(pnt)
else:
pnt = self.get_closest_path_point(pos, 20)
new_point = pf.Waypoint(pnt.x, pnt.y, pnt.heading)
if pnt is not None:
index = None
for i in range(self.middle_profile.index(pnt),
len(self.middle_profile)):
profile_pnt = self.middle_profile[i]
for j in range(len(self.points)):
waypnt = self.points[j]
dist = math.sqrt((waypnt.y - profile_pnt.y)**2 +
(waypnt.x - profile_pnt.x)**2)
if dist < 0.2:
self.points.insert(j, new_point)
break
else:
continue
break
self.create_profiles()
self.repaint()
elif QApplication.keyboardModifiers() & Qt.AltModifier:
pnt = self.get_closest_arm_endpoint(pos, 20)
if pnt is not None:
if self.point_being_dragged is None:
self.drag_mode = "angle"
self.point_being_dragged = pnt
self.ghost_point = pf.Waypoint(pnt.x, pnt.y, pnt.angle)
arm_x, arm_y = self.get_arm_gui_point(
self.point_being_dragged)
self.drag_offset = [pos.x() - arm_x, pos.y() - arm_y]
else:
pnt = self.get_closest_waypoint(pos, 20)
if pnt is not None:
if self.point_being_dragged is None:
self.drag_mode = "pos"
self.point_being_dragged = pnt
self.ghost_point = pf.Waypoint(pnt.x, pnt.y, pnt.angle)
pnt_x, pnt_y = self.convert_pf_point_to_gui_point(pnt)
self.drag_offset = [pos.x() - pnt_x, pos.y() - pnt_y]
def generate_path():
#XXX need to pick good waypoints
# waypoints initialize at 0, 0
waypoints = [
pf.Waypoint(0, 0, 0),
pf.Waypoint(1, 1, 1),
pf.Waypoint(2, 2, 2)
]
# This returns a tuple of TrajectoryInfo and a trajectory constituting
# a list of points to follow over time)
info, trajectory = pf.generate(
waypoints,
pf.FIT_HERMITE_CUBIC,
#XXX not sure what that does
pf.SAMPLES_HIGH,
#XXX dt means change in time in seconds?
# 50ms
dt=0.05,
#XXX max velocity is probably in the units that your encoder and gyro
# calculate it in
max_velocity=1,
max_acceleration=2,
max_jerk=3)
# pass in wheelbase width in meters
#XXX wrong width
modifier = pf.modifiers.TankModifier(trajectory).modify(1)
mx, my = zip(*[(m.y, m.x) for m in points])
plt.scatter(mx, my, c="r")
# plot the trajectory
x, y = zip(*[(seg.y, seg.x) for seg in trajectory])
plt.plot(x, y)
# annotate with time
for i in range(0, len(trajectory), int(0.5 / dt)):
plt.annotate(
"t=%.2f" % (i * dt, ),
xy=(x[i], y[i]),
xytext=(-20, 20),
textcoords="offset points",
arrowprops=dict(arrowstyle="->", connectionstyle="arc3,rad=0"),
)
plt.show()
return modifier
def draw_field_box(self, qp, grid=False):
pen = QPen()
pen.setColor(QColor(100, 100, 100))
pen.setWidth(1)
qp.setPen(pen)
qp.drawRect(self.field_rect)
if grid:
pen.setColor(QColor(100, 100, 100))
pen.setWidth(1)
qp.setPen(pen)
for i in range(math.floor(52 * 0.3048)):
qp.drawLine(
*self.convert_pf_point_to_gui_point(
pf.Waypoint(i + 1, 0, 0)),
*self.convert_pf_point_to_gui_point(
pf.Waypoint(i + 1, 27 * 0.3048, 0)))
for i in range(math.floor(27 * 0.3048)):
qp.drawLine(
*self.convert_pf_point_to_gui_point(
pf.Waypoint(0, i + 1, 0)),
*self.convert_pf_point_to_gui_point(
pf.Waypoint(52 * 0.3048, i + 1, 0)))
def __init__(self, robot):
super().__init__()
points = [pf.Waypoint(0, 0, 0), pf.Waypoint(13, 0, 0)]
info, trajectory = pf.generate(
points,
pf.FIT_HERMITE_CUBIC,
pf.SAMPLES_HIGH,
max_velocity=PassAutoLine.MAXVELOCITY,
max_acceleration=PassAutoLine.MAXACCELERATION,
max_jerk=120.0,
)
# Wheelbase Width = 2 ft
modifier = pf.modifiers.TankModifier(trajectory).modify(
robotmap.robotDiameter / 12)
# Do something with the new Trajectories...
left = modifier.getLeftTrajectory()
right = modifier.getRightTrajectory()
leftFollower = pf.followers.EncoderFollower(left)
leftFollower.configureEncoder(robot.drivetrain.encoderLeft,
robotmap.encoderPerRev,
robotmap.wheelDiameter)
leftFollower.configurePIDVA(1.0, 0.0, 0.0,
1 / PassAutoLine.MAXVELOCITY, 0)
rightFollower = pf.followers.EncoderFollower(right)
rightFollower.configureEncoder(robot.drivetrain.encoderRight,
robotmap.encoderPerRev,
robotmap.wheelDiameter)
rightFollower.configurePIDVA(1.0, 0.0, 0.0,
1 / PassAutoLine.MAXVELOCITY, 0)
self.leftFollower = leftFollower
self.rightFollower = rightFollower
self.addSequential(driveForwards.DriveForward(robot, 160))
def __init__(self, parent, chart, legend_info):
super().__init__(parent)
self.parent = parent
self.chart = chart
self.legend_info = legend_info
self.setFixedSize(self.parent.size())
self.field_pixmap = QPixmap('img/powerup_field.png')
self.field_rect = QRect(33, 15, 732, 367)
self.last_points = None
self.points = [
pf.Waypoint(0.6, 1.2, 0),
pf.Waypoint(4.2, 1.2, 0),
pf.Waypoint(6, 3, math.radians(90)),
pf.Waypoint(5.9, 15 * 0.3, math.radians(95)),
pf.Waypoint(24.5 * 0.3, 20.5 * 0.3, math.radians(-10))
]
self.drag_mode = None
self.point_being_dragged = None
self.drag_offset = None
self.ghost_point = None
def move_to(self, x_position, y_position, angle=0, first=False):
"""
Generate path and set path variable
:param x_position: The x distance.
:param y_position: The y distance.
:param angle: The angle difference in degrees.
:param first: Whether or not this path should be completed next.
"""
waypoint = pf.Waypoint(float(x_position), float(y_position),
radians(angle))
info, trajectory = pf.generate([pf.Waypoint(0, 0, 0), waypoint],
pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH,
0.05, 1.7, 2.0, 60.0)
modifier = pf.modifiers.TankModifier(trajectory).modify(0.5)
right_trajectory = modifier.getRightTrajectory()
left_trajectory = modifier.getLeftTrajectory()
with self.cond:
if self.left_follower or self.right_follower is None:
self.right_follower = pf.followers.EncoderFollower(
right_trajectory)
self.left_follower = pf.followers.EncoderFollower(
left_trajectory)
if first:
self.trajectories.appendLeft({
'right': right_trajectory,
'left': left_trajectory
})
else:
self.trajectories.append({
'right': right_trajectory,
'left': left_trajectory
})
self.cond.notify()
def get_path_to_generate(self):
'''
unpickle the received path
'''
try:
result = []
waypoints = pickle.loads(self.SENT_PATH)
print("RECEIVED PATH TO GENERATE")
for waypoint in waypoints:
result.append(pf.Waypoint(*waypoint))
print("TRAJ:", result)
return result
except Exception as e:
print("ERROR:", e)
return None
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)
def __init__(self):
if wpilib.RobotBase.isSimulation():
# Generate trajectories during testing
paths = {
'forward': [
pf.Waypoint(-11, 0, 0),
pf.Waypoint(0, 0, 0)
],
'left': [
pf.Waypoint(0, 0, 0),
pf.Waypoint(6, -6, 0),
pf.Waypoint(11, -6, 0),
pf.Waypoint(8, 0, math.radians(-90)),
#pf.Waypoint(18, 0, 0)
#pf.Waypoint(14, 14, 0)
#pf.Waypoint(10, 15, 0),
#pf.Waypoint(14, 14, math.radians(-90))
#pf.Waypoint(-8, -5, 0),
#pf.Waypoint(-4, -1, math.radians(-45.0)), # Waypoint @ x=-4, y=-1, exit angle=-45 degrees
#pf.Waypoint(-2, -2, 0), # Waypoint @ x=-2, y=-2, exit angle=0 radians
]
}
trajectories = {}
for path in paths:
info, trajectory = pf.generate(paths[path],
pf.FIT_HERMITE_CUBIC,
pf.SAMPLES_HIGH,
dt = 0.05,
max_velocity = 1.7,
max_acceleration = 2,
max_jerk = 100
)
trajectories[path] = trajectory
self.trajectories = trajectories
# and then write it out
with open(pickle_file, 'wb') as fp:
pickle.dump(self.trajectories, fp)
else:
with open(pickle_file, 'rb') as fp:
self.trajectories = pickle.load(fp)
def convert_waypoints(path: List[Pose]):
for wp in path:
yield pf.Waypoint(wp.x, wp.y, wp.heading)
import pathfinder as pf
import pickle
import math
import csv
# Pathfinder constants
MAX_VELOCITY = 4 # ft/s
MAX_ACCELERATION = 4 #ft/s/s
PERIOD = 0.02
MAX_JERK = 120.0
# Set up the trajectory
points = [
pf.Waypoint(0, 0, 0),
pf.Waypoint(6, 3, math.pi / 2),
pf.Waypoint(0, 6, 0)
]
info, trajectory = pf.generate(
points,
pf.FIT_HERMITE_CUBIC,
pf.SAMPLES_HIGH,
dt=PERIOD,
max_velocity=MAX_VELOCITY,
max_acceleration=MAX_ACCELERATION,
max_jerk=MAX_JERK,
)
'''
with open('/home/ubuntu/out.csv', 'w+') as points:
points_writer = csv.writer(points, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
# waypoints = [
# pf.Waypoint(0, 0, 0),
# pf.Waypoint(100 / 12, 48 / 12, 0),
# ]
#
# GeneratePath(os.path.dirname(__file__), "middle_start_right_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_EXCHANGE_FAR + 0.5 * Y_ROBOT_WIDTH)
waypoints = [
pf.Waypoint(0, 0, 0),
pf.Waypoint(102 / 12, -52 / 12, 0),
]
GenerateTalonMotionProfileArcPath(os.path.dirname(__file__),
"middle_start_right_switch", waypoints,
settings)
import os
import pathfinder as pf
from constants import X_ROBOT_LENGTH, Y_ROBOT_WIDTH, X_WALL_TO_SWITCH_NEAR, Y_WALL_TO_START
from utilities.functions import GeneratePath
class settings():
order = pf.FIT_HERMITE_QUINTIC
samples = 1000000
period = 0.02
maxVelocity = 5.0
maxAcceleration = 10
maxJerk = 15
# 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, 0, 0),
pf.Waypoint(58 / 12, -5 / 12, 0),
]
GeneratePath(os.path.dirname(__file__), "first_cube_middle_start", waypoints, settings)
# import sys
# sys.path = ['/Library/Frameworks/Python.framework/Versions/3.9/lib/python39.zip', '/Library/Frameworks/Python.framework/Versions/3.9/lib/python3.9', '/Library/Frameworks/Python.framework/Versions/3.9/lib/python3.9/lib-dynload', '/Library/Frameworks/Python.framework/Versions/3.9/lib/python3.9/site-packages', '/Library/Frameworks/Python.framework/Versions/3.9/lib/python3.9/site-packages/pathfinder/_pathfinder.cpython-39-darwin.so']
import pathfinder as pf
import math
import numpy as np
from pathFunc import *
#change these points and run file
<<<<<<< HEAD
points = [
pf.Waypoint(-2.8, 0.1, 0), # Waypoint @ x=-4, y=-1, exit angle=-45 degrees
pf.Waypoint(-1.75, 0.1, 0),
pf.Waypoint(5, -1.75, math.pi/2), # Waypoint @ x=0, y=0, exit angle=0 radians
]
# # points = [pf.Waypoint(-4, -1, math.radians(-45.0)), pf.Waypoint(-2, -2, 0), pf.Waypoint(0, 0, 0),]
=======
# points = [
# pf.Waypoint(-4, -1, math.radians(-45.0)), # Waypoint @ x=-4, y=-1, exit angle=-45 degrees
# pf.Waypoint(-2, -2, 0), # Waypoint @ x=-2, y=-2, exit angle=0 radians
# pf.Waypoint(0, 0, 0), # Waypoint @ x=0, y=0, exit angle=0 radians
# # ]
# points = [pf.Waypoint(-4, 1, 0), pf.Waypoint(-2, -2, 0), pf.Waypoint(0, 0, 0)]
# points = [pf.Waypoint(-6, -5, 0), pf.Waypoint(-2, -3, 0), pf.Waypoint(0, 0, 0), pf.Waypoint(2, 3, 0)]
# points = [pf.Waypoint(-6, -5, 0),pf.Waypoint(-2, -3, 0), pf.Waypoint(1, 2, 0), pf.Waypoint(2, 3, 0), pf.Waypoint(3, 8, 0)]
points = [pf.Waypoint(-2, -1, 0),pf.Waypoint(0, -5, 0), pf.Waypoint(1, -7, 0)]
>>>>>>> 18c6b9a0acbc8d7219779cb1a10f8f226c6645f4
# info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=0.05,max_velocity=1.7,max_acceleration=2.0,max_jerk=60.0)
# modifier = pf.modifiers.TankModifier(trajectory).modify(0.5) #0.5 is the width of the wheelchair base need to change
# right = modifier.getRightTrajectory()
# ]
#
# 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,#
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, 0),
pf.Waypoint(-60 / 12, 0, pf.d2r(45.0)),
]
GenerateTalonMotionProfileArcPath(os.path.dirname(__file__),
"left_switch_cube_retrieval", waypoints,
settings, True)
# # 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, 0, 0),
# pf.Waypoint(48 / 12, 0, 0),
# ]
#
# GeneratePath(os.path.dirname(__file__), "switch_prep_left_switch", waypoints, settings, False)
#===================================================================================================
PathFinderSettings = namedtuple("PathFinderSettings", ["order", "samples", "period", "maxVelocity", "maxAcceleration", "maxJerk"])
settings = PathFinderSettings(order=pf.FIT_HERMITE_CUBIC,
samples=1000000,
period=0.01,
maxVelocity=5.0,
maxAcceleration=8,
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(70 / 12, 0, 0),
]
GenerateTalonMotionProfileArcPath(os.path.dirname(__file__), "switch_prep_left_switch", waypoints, settings)
import os
import pathfinder as pf
from utilities.functions import GenerateMotionProfile
SAMPLE_PERIOD = 10 # MS
DISTANCE = 8.05 # rotations (825 native units)
MAX_VELOCITY = 4.0 # rotations / second
MAX_ACCELERATION = 8.0 # rotations / second^2
MAX_JERK = 15.0 # rotations / second^3
POSITION_UNITS = 1023 * (1 / 10) # 10-bit ADC / 10-turn pot...rotations
VELOCITY_UNITS = 1023 / 100 # 10-bit ADC / 100ms...natural units of Talon velocity
# Generate the path
info, trajectory = pf.generate(
[pf.Waypoint(0.0, 0.0, 0.0),
pf.Waypoint(DISTANCE, 0.0, 0.0)], pf.FIT_HERMITE_CUBIC, 1000000,
SAMPLE_PERIOD / 1000, MAX_VELOCITY, MAX_ACCELERATION, MAX_JERK)
GenerateMotionProfile(os.path.dirname(__file__), "boom_intake_to_scale",
trajectory, POSITION_UNITS, VELOCITY_UNITS)
import math, cv2
import numpy as np
import pathfinder as pf
points = [
pf.Waypoint(-2, 5, math.radians(90 + 30)),
pf.Waypoint(0, 0, math.radians(90))
]
img = np.zeros((500, 500, 3))
info, trajectory = pf.generate(
points,
pf.FIT_HERMITE_CUBIC,
pf.SAMPLES_HIGH,
dt=0.05, # 50ms
max_velocity=5.0,
max_acceleration=15.0,
max_jerk=60.0)
modifier = pf.modifiers.TankModifier(trajectory).modify(
2.33) # TODO MEASURE WHEELBASE WIDTH
left = modifier.getLeftTrajectory()
right = modifier.getRightTrajectory()
for segment in left:
print(segment.x, segment.y)
cv2.circle(
img, ((img.shape[1] // 2) - int(segment.x * 50), int(segment.y * 50)),
3, (255, 255, 0), -1)
# ]
#
# 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)
from ftplib import FTP
import pathfinder as pf
import math
points = [
pf.Waypoint(0, 0, math.radians(0)),
pf.Waypoint(1, 1, math.radians(90))
]
info, trajectory = pf.generate(
points,
pf.FIT_HERMITE_CUBIC,
pf.SAMPLES_HIGH,
dt=0.05, # 50ms
max_velocity=24.0,
max_acceleration=10.0,
max_jerk=60.0)
pf.serialize("newTrajectory.bin", trajectory)
with FTP("roboRIO-2577-frc.local") as ftp:
ftp.login()
ftp.storbinary('STOR newTrajectory.bin',
open("newTrajectory.bin", mode='rb'))
def createPoints(points):
list = []
for p in points:
list.append(pf.Waypoint(p[0], p[1], p[2]))
return list
class AutoTrajectories():
RightRocketNear = [
pf.Waypoint(0, 0, 0),
pf.Waypoint(11, -7.5, -math.pi / 6)
]
RightRocketFar = [
pf.Waypoint(0, 0, 0),
pf.Waypoint(3, 0, 0),
pf.Waypoint(15.5, -2, -math.pi / 6),
pf.Waypoint(16.5, -7.5, math.pi / 5.5)
]
LeftRocketNear = [pf.Waypoint(0, 0, 0), pf.Waypoint(11, 7.5, math.pi / 6)]
LeftRocketFar = [
pf.Waypoint(0, 0, 0),
pf.Waypoint(3, 0, 0),
pf.Waypoint(15.5, 2, math.pi / 6),
pf.Waypoint(16.5, 7.5, -math.pi / 5.5)
]
#Right Start, Near hatch
#pf.Waypoint(0, 0, 0), pf.Waypoint(11, -7.5, -math.pi/6)
#Right Start, Far hatch
#pf.Waypoint(0, 0, 0),pf.Waypoint(3, 0, 0), pf.Waypoint(15.5, -2, -math.pi/6), pf.Waypoint(16.5, -7.5, math.pi/5.5)
#Left Start, Near hatch
#pf.Waypoint(0, 0, 0), pf.Waypoint(11, 7.5, math.pi/6)
#Left Start, Far hatch
#pf.Waypoint(0, 0, 0),pf.Waypoint(3, 0, 0), pf.Waypoint(15.5, 2, math.pi/6), pf.Waypoint(16.5, 7.5, -math.pi/5.5)
# drivetrain.drive_motion_profile(-100, 10, 1)
# drivetrain.drive_to_xy(0, -100, 0.5, 0.01)
# drivetrain.drive_pure_pursuit(bezier, 5, 1, 5, 1, True, 1)
# drivetrain.set_position(100, 0)
# drivetrain.follow_path(bezier, 1, 0.01, 0)
# drivetrain.drive_pure_pursuit_2(opposite_scale_auto, 10, 5, 0.1, True)
# drivetrain.drive_forward_PID(100, 0.1, 0)
# print(1)
# drivetrain.turn_to_angle_PID(180, 0.1, 0)
# drivetrain.drive_pure_pursuit_4(bezier, 10, 5, 0, True)
# drivetrain.graph()
# print(1)
# print(bezier_2.get_closest_point(0, 0))
right_switch_auto = pf.generate(
[pf.Waypoint(0, 0, math.radians(0)),
pf.Waypoint(5, 5, math.radians(0))],
pf.FIT_HERMITE_CUBIC,
pf.SAMPLES_HIGH,
dt=0.02, # 50WWms
max_velocity=5,
max_acceleration=2.0,
max_jerk=60.0)[1]
backwards_auto = pf.generate(
[pf.Waypoint(0, 0, math.radians(0)),
pf.Waypoint(-5, 5, math.radians(0))],
pf.FIT_HERMITE_CUBIC,
pf.SAMPLES_HIGH,
dt=0.02, # 50WWms
max_velocity=5,
# pf.Waypoint(50 / 12, 0, 0),
# ]
#
# GeneratePath(os.path.dirname(__file__), "right_cube_retrieval_cube_get", waypoints, settings, False, True, 45.0)
#===================================================================================================
PathFinderSettings = namedtuple("PathFinderSettings", [
"order", "samples", "period", "maxVelocity", "maxAcceleration", "maxJerk"
])
settings = PathFinderSettings(
order=pf.FIT_HERMITE_QUINTIC,
samples=1000000,
period=0.01,
maxVelocity=7.0,
maxAcceleration=10,
#maxJerk=30#
maxJerk=40)
# 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(50 / 12, 0, 0),
]
GenerateTalonMotionProfileArcPath(os.path.dirname(__file__),
"right_cube_retrieval_cube_get", waypoints,
settings, False, True, 45.0)
# ]
#
# 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)
import os
import pathfinder as pf
from constants import X_ROBOT_LENGTH, Y_ROBOT_WIDTH, X_WALL_TO_SWITCH_NEAR, Y_WALL_TO_START
from utilities.functions import GeneratePath
class settings():
order = pf.FIT_HERMITE_QUINTIC
samples = 1000000
period = 0.02
maxVelocity = 5.0
maxAcceleration = 10
maxJerk = 15
# 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_START + 0.5 * Y_ROBOT_WIDTH,
0),
pf.Waypoint(X_WALL_TO_SWITCH_NEAR - 0.5 * X_ROBOT_LENGTH,
Y_WALL_TO_START + 0.5 * Y_ROBOT_WIDTH, 0),
]
GeneratePath(os.path.dirname(__file__), "forward_path", waypoints)
#!/usr/bin/env python3
import pathfinder as pf
import math
if __name__ == "__main__":
points = [
pf.Waypoint(-4, -1, math.radians(-45.0)),
pf.Waypoint(-2, -2, 0),
pf.Waypoint(0, 0, 0),
]
info, trajectory = pf.generate(
points,
pf.FIT_HERMITE_CUBIC,
pf.SAMPLES_HIGH,
dt=0.05, # 50ms
max_velocity=1.7,
max_acceleration=2.0,
max_jerk=60.0,
)
# Do something with the new Trajectory...
import math
import os
import pathfinder as pf
from constants import X_ROBOT_LENGTH, Y_ROBOT_WIDTH, Y_WALL_TO_EXCHANGE_FAR, \
X_WALL_TO_SWITCH_NEAR
from utilities.functions import GeneratePath
class settings():
order = pf.FIT_HERMITE_QUINTIC
samples = 1000000
period = 0.02
maxVelocity = 6.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, 0, 0),
pf.Waypoint(60 / 12, 22 / 12, 0),
]
GeneratePath(os.path.dirname(__file__), "middle_start_left_switch_first_cube",
waypoints, settings)