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