def correct_heading(self):
curlocation = self.gps.getLocation()
heading = self.gps.getHeading(curlocation, self.station)
moveby = heading - self.curheading
bearing_math.normalize_bearing(moveby)
if moveby < 0:
self.moveServo(moveby, 1) # anti-clockwise
else:
self.moveServo(moveby, 0) # clockwise
self.curheading = heading
def getHeading(self, curlocation, destination):
theata1 = math.radians(curlocation[0])
theata2 = math.radians(destination[0])
deltaThetha = math.radians(destination[0] - curlocation[0])
deltaLambda = math.radians(destination[1] - curlocation[1])
y = math.sin(deltaLambda) * math.cos(theata2)
x = math.cos(theata1) * math.sin(theata2) - math.sin(theata1) * math.cos(theata2) * math.cos(deltaLambda)
bearing = math.degrees(math.atan2(y, x)) + 180
bearing_math.normalize_bearing(bearing)
return bearing
def driveStraight(self, speed_percent, duration, sensitivity=1):
# Ensure we have sufficient priviledges to access compass.
if not TiberiusConfigParser.isCompassEnabled():
raise SensorNotEnabledError("Compass is disabled, dependant \
function cannot be executed.")
desired_heading = self.compass.headingNormalized()
t = 0 # time
gain = 1440 # proportional Error multiplier
integral = 0 # Sum of all errors over time
i_factor = 1 # integral
d_factor = 1 # derivative
previous_error = 0
debug = False
left_speed = (speed_percent * 255) / 100 # 0-100 -> 0-255
right_speed = (speed_percent * 255) / 100
while t < duration:
actual_heading = self.compass.headingNormalized()
error_degrees = actual_heading - desired_heading
error_degrees = bearing_math.normalize_bearing(error_degrees)
'''if desired_heading > 0:
if actual_heading > 0:
error_degrees = actual_heading - desired_heading
elif actual_heading < 0:
error_degrees = actual_heading + desired_heading
elif desired_heading < 0:
if actual_heading > 0:
error_degrees = actual_heading + desired_heading
elif actual_heading < 0:
error_degrees = actual_heading - desired_heading
'''
# Make error between 1 and -1
error = error_degrees / float(180.0)
error *= sensitivity
integral += error
derivative = previous_error - error
previous_error = error
if error_degrees < 0: # Turn Right
r = right_speed - (abs(error) * gain) - \
(integral * i_factor) + (derivative * d_factor)
l = left_speed + (abs(error) * gain) - \
(integral * i_factor) + (derivative * d_factor)
if debug:
print 'Turning RIGHT'
elif error_degrees > 0: # Turn Left
r = right_speed + (abs(error) * gain) - \
(integral * i_factor) + (derivative * d_factor)
l = left_speed - (abs(error) * gain) - \
(integral * i_factor) + (derivative * d_factor)
if debug:
print 'Turning LEFT'
else:
l = left_speed
r = right_speed
integral = 0 # Reset integral error when on track
if debug:
print 'Going STRAIGHT'
if debug:
print 'Desired Heading (deg): ' + str(desired_heading)
print 'Actual Heading (deg): ' + str(actual_heading)
print 'Error (deg): ' + str(error_degrees)
print 'Error: ' + str(error)
print 'Max speed : ' + str(left_speed)
print 'Left speed : ' + str(l)
print 'Right speed : ' + str(r)
print 'Proportional: ' + str(error * gain)
print 'Integral : ' + str(integral * i_factor)
print 'Derivative : ' + str(derivative * d_factor)
self.motors.moveForwardDualSpeed(l, r)
time.sleep(0.1)
t += 0.1
self.motors.stop()
def getCurHeading(self):
compass_dict = db_q.get_latest(CompassTable)
heading = compass_dict.heading - 90
return bearing_math.normalize_bearing(heading)
