def _calc(point1, knot1, knot2, point2):
# Chris made this in desmos. I don't know how it works. Don't ask me.
# https://www.desmos.com/calculator/yri3mwz7kn
sgn_theta1 = -mathutils.sgn((knot2.y - knot1.y) / (knot2.x - knot1.x) *
(point1.x - knot1.x) + knot1.y - point1.y)
num = knot2.dx2(point1) + knot2.dy2(point1) - knot1.dx2(point1) - knot1.dy2(point1) - knot1.dx2(knot2) - \
knot1.dy2(knot2)
angle_theta1 = math.acos(num /
(-2 * knot1.dist(knot2) * knot1.dist(point1)))
theta1 = sgn_theta1 * angle_theta1
sgn_theta2 = -mathutils.sgn((knot2.y - knot1.y) / (knot2.x - knot1.x) *
(point2.x - knot2.x) + knot2.y - point2.y)
num = knot1.dx2(point2) + knot1.dy2(point2) - knot2.dx2(point2) - knot2.dy2(point2) - knot1.dx2(knot2) - \
knot1.dy2(knot2)
print(num / (-2 * knot1.dist(knot2) * knot1.dist(point1)))
angle_theta2 = math.acos(
num / (-2 * knot1.dist(knot2) * knot2.dist(point2))) - math.pi
theta2 = sgn_theta2 * angle_theta2
d = knot1.dist(knot2)
print(mathutils.normalize_angle(theta1))
print(mathutils.normalize_angle(theta2))
return mathutils.normalize_angle(theta1), d, mathutils.normalize_angle(
theta2)
def radius_turn(self, speed, radius):
# (Vo + Vi) / 2 = pow
# Vo*r = Vi
# Vo(1+r)/2 = pow
# 2*pow/(1+r) = Vo
# 2*r*pow/(1+r) = Vi
# With this calculation, it's possible to saturate the motors. If that happens, rescale to maintain the curve.
turn_dir = mathutils.sgn(radius)
radius = abs(radius)
r = self.radius_ratio(radius)
Vo = 2 * speed / (1 + r)
Vi = r * Vo
if Vo > self.max_speed:
Vi /= Vo
Vo /= Vo
Vo *= self.max_speed
Vi *= self.max_speed
Vi /= self.max_speed
Vo /= self.max_speed
if turn_dir > 0:
self._set_motor_outputs(Vo, Vi)
# self.drive_profile_open_loop(Vo, Vi, 0, 0)
else:
self._set_motor_outputs(Vi, Vo)
def _radius_turn(self, pow, radius):
D = self.robot_width / 2
turn_dir = mathutils.sgn(radius)
radius = abs(radius)
Vo = pow
Vi = Vo * (radius - D) / (radius + D)
if turn_dir > 0:
self._set_motor_outputs(Vo, Vi)
else:
self._set_motor_outputs(Vi, Vo)
def motion_magic(self, distance: float, speed: float, acc: float, curvature: float = 0):
"""
Set the talons to drive in an arc or straight at speed, accelerating at acc.
Only needs to be called once.
If curvature != 0, the outside wheel goes distance at speed and acc, and the inner wheel speed is decreased.
:param distance: Distance in feet to travel
:param speed: Speed (in feet/sec) to cruise at
:param acc: Acceleration (in feet/sec^2) to accelerate/decelerate at
:param curvature: 1/radius of turn. If 0, drive straight.
:return:
"""
if curvature == 0:
ratio = 1
turn_dir = 1
else:
radius = 1 / curvature
D = self.robot_width / 2
turn_dir = mathutils.sgn(radius)
radius = abs(radius)
ratio = (radius - D) / (radius + D)
# Change units to what the talons are expecting
vel_rpm = self.fps_to_rpm(speed)
acc_rpm = self.fps_to_rpm(acc) # Works because required unit is rpm/sec for no real good reason.
dist_revs = self.feet_to_revs(distance)
print(dist_revs)
# Don't set encoder position to 0, because that would mess up pose estimation
# Instead, set to current position, plus however far we want to go
left_current_pos = self._left_motor.getPosition()
right_current_pos = self._right_motor.getPosition()
# Set the talon parameters
# If turn > 0, left is outside
if turn_dir > 0:
self._left_motor.setMotionMagicCruiseVelocity(vel_rpm)
self._right_motor.setMotionMagicCruiseVelocity(vel_rpm * ratio)
self._left_motor.setMotionMagicAcceleration(acc_rpm)
self._right_motor.setMotionMagicAcceleration(acc_rpm * ratio)
self._left_motor.set(left_current_pos + dist_revs)
self._right_motor.set(right_current_pos + dist_revs * ratio)
else:
self._left_motor.setMotionMagicCruiseVelocity(vel_rpm * ratio)
self._right_motor.setMotionMagicCruiseVelocity(vel_rpm)
self._left_motor.setMotionMagicAcceleration(acc_rpm * ratio)
self._right_motor.setMotionMagicAcceleration(acc_rpm)
self._left_motor.set(left_current_pos + dist_revs * ratio)
self._right_motor.set(right_current_pos + dist_revs)
def radius_drive(self, forward_power, turn_power, power_factor, deadband=0.05):
if self.is_manual_control_mode():
if abs(turn_power) < deadband:
self._set_motor_outputs(forward_power * power_factor, forward_power * power_factor)
return
if abs(forward_power) < deadband:
self._set_motor_outputs(0, 0)
return
turn_power = mathutils.signed_power(turn_power, 1/3)
radius = self.robot_width / 2 + self.max_turn_radius * (1 - abs(turn_power))
self._radius_turn(forward_power * power_factor,
radius * mathutils.sgn(turn_power))
else:
warnings.warn("Not in a control mode for Radius Drive", RuntimeWarning)
self._set_motor_outputs(0, 0)
def __init__(self, start, end, cruise_speed, acc, frequency=100):
"""
Generate a trapezoidal motion profile, starting at `start` and ending at `end`
:param start:
:param end:
:param cruise_speed: Magnitude of cruise velocity
:param acc: Magnitude of acceleration
:param frequency: Number of points to generate per second
"""
# https://www.desmos.com/calculator/ponjr7cwze
# If we're going in reverse we need to flip the sign of speed and acc
# Although in theory these are vectors, it makes the API easier if they're given as magnitude
dist = end - start
cruise_speed = copysign(cruise_speed, dist)
acc = copysign(acc, dist)
self.start = start
self.end = end
self.displacement = dist
ramp_time = cruise_speed / acc
ramp_dist = acc * ramp_time ** 2 / 2
cruise_dist = dist - 2 * ramp_dist
cruise_time = cruise_dist / cruise_speed
if sgn(cruise_dist) != sgn(dist):
# All ramp, no cruise. Fix parameters to match
cruise_time = 0
cruise_dist = 0
ramp_time = (dist / acc) ** 0.5
ramp_dist = dist / 2
time = cruise_time + 2 * ramp_time
def get_pos(t):
if t <= ramp_time:
return start + acc * t ** 2 / 2
elif t <= ramp_time + cruise_time:
return start + ramp_dist + (t - ramp_time) * get_vel(ramp_time)
else:
tp = (t - ramp_time - cruise_time)
return start + ramp_dist + cruise_dist + get_vel(ramp_time + cruise_time) * tp - acc * tp ** 2 / 2
def get_vel(t):
if t <= ramp_time:
return get_acc(t) * t
elif t <= ramp_time + cruise_time:
return cruise_speed
else:
tp = (t - ramp_time - cruise_time)
return get_vel(ramp_time + cruise_time) - acc * tp
def get_acc(t):
if t <= ramp_time:
return acc
elif t <= ramp_time + cruise_time:
return 0
else:
return -acc
self._points = []
for i in range(int(time * frequency) + 1):
t = i / frequency
self._points.append(TrajectoryPoint(position=get_pos(t),
velocity=get_vel(t),
acc=get_acc(t),
time=t))
