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)