def drive(self):
        global start
        global end
        global imu
        self.check_gps()
        self.check_imu()

        self.last_waypoint = start
        current_timestamp = time.time() # gpsp.get_timestamp()
        self.last_waypoint.set_timestamp(current_timestamp)
            
        data = imu.getIMUData()
        fusionPose = data["fusionPose"]
        yaw = math.degrees(fusionPose[2])
 
        heading = nav.yaw_to_heading(yaw, -90.0)
        print 'Heading: %f' % heading

        bearing = nav.get_bearing(start, end) 
        print 'Bearing: ', bearing

        P = 1.2
        I = 1
        D = 0
        L = 200

        pid = PID.PID(P, I, D)

        pid.SetPoint=bearing
        pid.setSampleTime(0.01)
        motor_val = 0

        thresh_up = 0.35*MAX_SPEED
        thresh_lo = 0.2*MAX_SPEED

        while not self.stopped():
            print nav.get_distance(self.last_waypoint, end)
            
	    if nav.get_distance(self.last_waypoint, end) < 5.0:
		print "Hit waypoint"
                break
            
            if abs(360.0 + heading - bearing) < abs(heading - bearing):
                feedback = heading + 360.0
            else:
                feedback = heading
        
            pid.update(feedback)
            output = pid.output

            if output >= 0.0:
                speed = thresh_up - output/4.0
            else:
                speed = thresh_up + output/4.0

            if speed < thresh_lo:
                drive = int(thresh_lo)
            else:
                drive = int(speed)

            if abs(heading - bearing) < 2.0:
                motors.motor1.setSpeed(int(1.2*thresh_up))
                motors.motor2.setSpeed(int(thresh_up))
                print 'Both'
            elif output > 0.0:
                motors.motor1.setSpeed(int(1.2*thresh_up))
                motors.motor2.setSpeed(drive)
  
                print 'Left'
            elif output < 0.0:
                motors.motor1.setSpeed(int(drive))
                motors.motor2.setSpeed(int(thresh_up))
                print 'Right'

            self.check_gps()
            self.check_imu()
            self.estimate_position()

            data = imu.getIMUData()
            fusionPose = data["fusionPose"]
            yaw = math.degrees(fusionPose[2])
            heading = nav.yaw_to_heading(yaw, -90.0)
            print 'Heading: %f' % heading
            bearing = nav.get_bearing(self.last_waypoint, end) 
            print 'Bearing: ', bearing

            sleep(0.1)
            motors.setSpeeds(0, 0)
Exemple #2
0
            yaw = math.degrees(fusionPose[2])
            print 'Read yaw: %f' % math.degrees(fusionPose[2])

            heading = nav.yaw_to_heading(yaw, -90.0)
            print 'Heading: %f' % heading

            current_timestamp = time.time() # gpsp.get_timestamp()
            estimator.set_state(last_waypoint.latitude, last_waypoint.longitude, 0, last_waypoint.timestamp) 
            estimator.k_filter(location[0], location[1], 2, current_timestamp)
            last_waypoint = gps_obj.GPS(estimator.lat, estimator.long)
            last_waypoint.set_timestamp(current_timestamp)
            print 'Filtered lat/lng: ', last_waypoint.latitude, ', ', last_waypoint.longitude
 #           print last_waypoint.latitude, ', ', last_waypoint.longitude

            bearing = nav.get_bearing(last_waypoint, next_waypoint)
            distance = nav.get_distance(last_waypoint, next_waypoint)

            print 'Bearing: ', bearing, ' Distance: ', distance
            print '------------------'
            sleep(1)
            location = gpsp.get_location()
            read = imu.IMURead() 	
            if read is None:
                break
            data = imu.getIMUData()
    
except (KeyboardInterrupt, SystemExit): #when you press ctrl+c
    print "\nKilling Thread..."
    gpsp.stop()
    gpsp.join() # wait for the thread to finish what it's doing