def drive1(self):
x = self.xbox.analog_drive_x()
y = self.xbox.analog_drive_y()
r, th = joystick_as_polar(x, y)
th_l = throttle_angle_to_thrust_left(r, th)
th_r = throttle_angle_to_thrust_right(r, th)
self.lmotor.set(th_l)
self.rmotor.set(th_r)
def drive2(self):
x = self.xbox.analog_drive_x()
y = self.xbox.analog_drive_y()
r = -self.rstick.getRawAxis(1)
print (x, y)
r, th = joystick_as_polar(x, y)
desired_abs_heading = th - math.pi/2
desired_relative_heading = (desired_abs_heading -
(math.radians(self.gyro.getAngle())))
# if r2 < 0.2:
# desired_relative_heading = 0
desired_relative_heading = normrad(desired_relative_heading)
if abs(desired_relative_heading) > math.radians(10):
desired_relative_heading = math.copysign(
math.pi/2, desired_relative_heading)
#r = 0.8
th_l = throttle_angle_to_thrust_left(r, desired_relative_heading)
th_r = throttle_angle_to_thrust_right(r, desired_relative_heading)
self.lmotor.set(th_l)
self.rmotor.set(th_r)
def test_joystick_as_polar(self):
for (input_x, input_y, expected_magnitude, expected_theta) in joystick_magnitude_data:
(r, theta) = joystick_as_polar(input_x, input_y)
self.assertAlmostEqual(expected_magnitude, r)
self.assertAlmostEqual(expected_theta, theta)
