def sweep_routine(self, approach_angle):
return sw3.LoopRoutine(
#sw3.Forward(BACKWARD_SPEED, BACKUP_TIME),
sw3.Forward(0, 1),
#sw3.SetYaw(self.reference_angle),
sw3.SetRotate(0.3, 1),
sw3.SetRotate(-0.3, 2),
sw3.SetYaw(approach_angle),
)
def update_axis(event):
global yaw_heading
angle = event.angle_radians
mag = max(min(event.magnitude, 1.0), -1.0)
forward = -(mag * sin(angle))
rate = (mag * cos(angle))
total = abs(forward) + abs(rate)
if total == 0:
yaw_heading = sw3.data.imu.yaw()
sw3.nav.do(
sw3.CompoundRoutine(
(sw3.SetYaw(yaw_heading), sw3.Forward(forward))))
else:
for_p = forward / total
rate_p = rate / total
total = min(total, 1.0)
forward = for_p * total
rate = rate_p * total
sw3.nav.do(
sw3.CompoundRoutine((sw3.SetRotate(rate), sw3.Forward(forward))))
def update_axis(event):
global yaw_heading
global x_activated
global y_activated
# x and y start out at zero, however, 0 is NOT center! When an x event
# comes in, but y has not been touched, y should not react (and vice
# versa). This prevents an axis form moving unless it has gotten a nonzero
# value before.
if event.x != 0:
x_activated = True
if event.y != 0:
y_activated = True
if not x_activated:
turn_rate = 0
else:
# Steering wheel goes from left 0 to right 32767.
# Normalize so 0 is center and scale from -1 to 1.
turn_rate = (event.x - 32767 / 2) / (32767 / 2)
if not y_activated:
forward_rate = 0
elif event.y < 15000: # Forward
forward_rate = 1 - event.y / 15000
elif event.y > 20000: # Backward
forward_rate = -1 * (event.y - 20000) / (32767 - 20000)
else:
forward_rate = 0
if abs(turn_rate) > 0.25:
sw3.nav.do(
sw3.CompoundRoutine(
[sw3.SetRotate(turn_rate),
sw3.Forward(forward_rate)]))
else:
yaw_heading = sw3.data.imu.yaw()
sw3.nav.do(
sw3.CompoundRoutine(
[sw3.SetYaw(yaw_heading),
sw3.Forward(forward_rate)]))