def turnLeft90Degrees(self):
# Ensure we have sufficient priviledges to access compass.
if not TiberiusConfigParser.isCompassEnabled():
raise SensorNotEnabledError("Compass is disabled, dependant \
function cannot be executed.")
old_bearing = self.compass.headingNormalized()
desired_bearing = (old_bearing - 90)
if(desired_bearing < -180):
desired_bearing += 360
print desired_bearing
self.turnTo(desired_bearing)
'''
data = self.lidar.get_lidar_data()
# put x in data for every x in data only if filtered_data() is true
data = [x for x in data if self.filtered_data(x)]
return data
# class Camera:
# '''
# Provides camera capture methods.
# '''
# camera = picamera.PiCamera()
#
# def capture_image(self):
# self.camera.resolution = (640,480)
# self.camera.capture('./pi_camera_image.jpg')
if TiberiusConfigParser.isCompassEnabled():
class Compass:
'''
Provides compass related methods, what more can I say?
'''
compass = cmps11.TiltCompensatedCompass(
TiberiusConfigParser.getCompassAddress())
def headingDegrees(self):
# Get the heading in degrees.
try:
raw = int(self.compass.heading())
except:
return 222.2
return raw / 10
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 turnTo(self, desired_bearing):
"""Turn the robot until it is facing desired_bearing.
Args:
desired_bearing (float): The bearing that the robot should be facing
upon completion of the function.
"""
# Ensure we have sufficient priviledges to access compass.
if not TiberiusConfigParser.isCompassEnabled():
raise SensorNotEnabledError("Compass is disabled, dependant \
function cannot be executed.")
count = 0
while(True):
count += 1
print 'Iteration: ' + str(count)
if count < 50:
time.sleep(0.1)
# actual_bearing = self.compass.headingNormalized()
actual_bearing = db_q.get_latest(CompassTable)
error = actual_bearing - desired_bearing
# self.logger.debug('Heading: ' + str(actual_bearing))
# self.logger.debug('Desired: ' + str(desired_bearing))
if(error < 5 and error > -5):
# self.logger.debug('At heading: ' + str(actual_bearing))
self.motors.stop()
break
if(error > 180):
#print 'error > 180'
error -= 360
if(error < -180):
#print 'error < -180'
error += 360
if(error > 0):
# print 'error < 0 turning left'
self.motors.setSpeedPercent(100)
self.motors.turnLeft()
# Reduce speed on approach to desired bearing
# Positive error is a left turn
if(error < 60):
self.motors.setSpeedPercent(70)
self.motors.turnLeft()
if(error < 30):
self.motors.setSpeedPercent(40)
self.motors.turnLeft()
if(error < 5):
self.motors.setSpeedPercent(20)
self.motors.turnLeft()
if(error < 0):
# print 'error > 0 turning right'
self.motors.setSpeedPercent(100)
self.motors.turnRight()
# Negative error is a right turn
if(error > -60):
self.motors.setSpeedPercent(70)
self.motors.turnRight()
if(error > -30):
self.motors.setSpeedPercent(40)
self.motors.turnRight()
if(error > -5):
self.motors.setSpeedPercent(20)
self.motors.turnRight()
# print str(error)
else:
print '50 Iterations Complete'
break
