Beispiel #1
0
 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)
Beispiel #2
0
 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)