Ejemplo n.º 1
0
    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)
Ejemplo n.º 2
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)
Ejemplo n.º 3
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)
Ejemplo n.º 4
0
 def da_test(r, theta_deg, expected_thrust):
     self.assertAlmostEqual(throttle_angle_to_thrust_right(r, math.radians(theta_deg)), expected_thrust)