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)
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