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)