Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- from gopigo import *
- import time
- import pigpio
- #This program scans a 180 degree map of its surroundings then turns towards its most distant measurement
- #Methods to control the robot better
- #!/usr/bin/python
- '''
- This module contains convenience functions to simplify
- the coding of simple tasks.
- This really needs to be moved to a GoPiGo package
- e.g. from gopigo.control import *
- '''
- en_debug=1
- ## 360 rotation is ~64 encoder pulses or 5 deg/pulse
- ## DPR is the Deg:Pulse Ratio or the # of degrees per
- ## encoder pulse.
- DPR = 360.0/64
- WHEEL_RAD = 3.25 # Wheels are ~6.5 cm diameter.
- CHASS_WID = 11 # Chassis is ~13.5 cm wide.
- def left_deg(deg=None):
- '''
- Turn chassis left by a specified number of degrees.
- DPR is the #deg/pulse (Deg:Pulse ratio)
- This function sets the encoder to the correct number
- of pulses and then invokes left().
- '''
- if deg is not None:
- pulse= int(deg/DPR)
- enc_tgt(0,1,pulse)
- left()
- def right_deg(deg=None):
- '''
- Turn chassis right by a specified number of degrees.
- DPR is the #deg/pulse (Deg:Pulse ratio)
- This function sets the encoder to the correct number
- of pulses and then invokes right().
- '''
- if deg is not None:
- pulse= int(deg/DPR)
- enc_tgt(0,1,pulse)
- right()
- def fwd_cm(dist=None):
- '''
- Move chassis fwd by a specified number of cm.
- This function sets the encoder to the correct number
- of pulses and then invokes fwd().
- '''
- if dist is not None:
- pulse = int(cm2pulse(dist))
- enc_tgt(1,1,pulse)
- fwd()
- def bwd_cm(dist=None):
- '''
- Move chassis bwd by a specified number of cm.
- This function sets the encoder to the correct number
- of pulses and then invokes bwd().
- '''
- if dist is not None:
- pulse = int(cm2pulse(dist))
- enc_tgt(1,1,pulse)
- bwd()
- def cm2pulse(dist):
- '''
- Calculate the number of pulses to move the chassis dist cm.
- pulses = dist * [pulses/revolution]/[dist/revolution]
- '''
- wheel_circ = 2*math.pi*WHEEL_RAD # [cm/rev] cm traveled per revolution of wheel
- revs = dist/wheel_circ
- PPR = 18 # [p/rev] encoder Pulses Per wheel Revolution
- pulses = PPR*revs # [p] encoder pulses required to move dist cm.
- if en_debug:
- print 'WHEEL_RAD',WHEEL_RAD
- print 'revs',revs
- print 'pulses',pulses
- return pulses
- def int sleep = .5
- def int servo_angle = 36
- def center_angle(servo_angle):
- a = int(180/servo_angle)
- return a
- def scanArea():
- distance = [0] * int(180/servo_angle)
- for i in range(0, 5):
- servo(i*servo_angle) #Moves the servo at the angles 0, 10 up to 180
- time.sleep(sleep) #Gives time for the sensor to be in position before start measuring
- distance[i] = us_dist(15)
- time.sleep(sleep) #So that the servo doesn't spas out
- return distance
- def determineAngle(distance):
- m = max(distance) #finds max measured distance in the array
- angles = [i for i, j in enumerate(distance) if j == m]
- #might return two angles if the values are the same
- #returns indexes of largest values
- #indexes are used to determine the angles
- #enumerate creates a list [(index, value) etc]
- #the for loop loops through the list
- #the first i is the index of what it puts in the new list
- #the second i and the j get increased until the if statement is met
- #once it is met the index second i gets put in the first i
- #I don't fully understand it but this is a really smart way to do it
- return max(angles) #always picks the lowest value because it corresponds to the rightmost value
- while True:
- distance = scanArea()
- print(distance)
- angle = determineAngle(distance) #Angles: 0 to 8 is right, 9 is front, 10 to 18 is left
- print(angle)
- center_angle = center_angle(servo_angle)
- #Example of angles:
- #Right 0 1 2 3 4 5 6 7 8 9* 10 11 12 13 14 15 16 17 18
- #angle value times servo_angle represents the angle to go to
- rotation = 90-(angle*servo_angle) #Right will be a positive value, left will be negative
- if center_angle % 2 == 0: #even amount of mesurements will not have a forward mesurement
- if angle < center_angle:
- right_deg(rotation) #Rotate right by that angle
- time.sleep(1)
- fwd_cm(distance[angle]-1) #Go forward one less
- elif angle > center_angle:
- left_deg(-rotation) #Rotate left by that
- time.sleep(1)
- fwd_cm(distance[angle]-1)
- else: #odd amount of measurements will have a forward measurement
- if angle < center_angle:
- right_deg(rotation) #Rotate right by that angle
- time.sleep(1)
- fwd_cm(distance[angle]-1) #Go forward one less
- elif angle > center_angle:
- left_deg(-rotation) #Rotate left by that
- time.sleep(1)
- fwd_cm(distance[angle]-1)
- elif angle == center_angle:
- fwd_cm(distance[angle]-1)
- time.sleep(.2)
- time.sleep(3) #Waits for the robot to finish going forward before taking the next measurement
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement