def autonomousPeriodic(self): '''Called every 20ms in autonomous mode''' current = datetime.now() print (current-self.last) self.last = current if self.phase == 1: now = datetime.now() if (now - self.starttime).seconds > 2: self.phase = 2 self.starttime = now print ('phase 2') self.heading = self.heading + 80 theta = math.radians(self.heading - self.gyro.getAngle()) r = 0.8 elif self.phase == 2: now = datetime.now() if (self.heading - self.gyro.getAngle()) < 5: self.phase = 3 self.starttime = now print ('phase 3') # theta = math.radians(self.heading - self.gyro.getAngle()) theta = math.copysign(math.pi/2, self.heading - self.gyro.getAngle()) r = 0.5 else: theta = 0 r = 0 th_l = throttle_angle_to_thrust_left(r, theta) th_r = throttle_angle_to_thrust_right(r, theta) self.lmotor.set(th_l) self.rmotor.set(th_r)
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 da_test(r, theta_deg, expected_thrust): self.assertAlmostEqual(throttle_angle_to_thrust_right(r, math.radians(theta_deg)), expected_thrust)