def update_segments(self, new_segment_size, new_segment_number=-1):
# update servo's segment-size (in servo-values) & -number
# CAUTION: ALL VALUES OF SENSOR 2 WILL BE DELETED HEREBY!
# CAUTION: size must be multiple of 10 and number must be integer.
# also (number * size == MAX-MIN) must be true!
# ether via segment-size
if (new_segment_number == -1):
self.servo_segment_size = round(new_segment_size, -1)
self.servo_segments = int( (self.servo_MAX - self.servo_MIN) / self.servo_segment_size )
# or via segment_number
else:
self.servo_segments = int(new_segment_number)
self.servo_segment_size = round((self.servo_MAX - self.servo_MIN)/self.servo_segments, -1)
if (self.servo_segments*self.servo_segment_size!=self.servo_MAX-self.servo_MIN):
lg.prt("ERROR: Servo-Segment configuration not correct! Stop.", lv=100000, inst=__name__)
quit()
# update segment-servo-values & initialize measurement-list
# NOTE: There is one more measurement than segments!
self.servo_segment_values = []
(self.measurements)[1] = []
for i in range(0,self.servo_segments+1,1):
self.servo_segment_values.append(self.servo_MIN + (i*self.servo_segment_size) )
self.measurements[1].append([0,0])
def gap_finding(Scan, obstacle, narrow, medium, wide): #take sensor data, obstacle distance & lists for gaps
N_free = 0 # and give back the indices for free gaps ordered as wide, medium, narrow
# (written into the given lists)
for i in range(len(Scan)):
if Scan[i] <= obstacle or i == len(Scan)-1: #obstacle:
if N_free > 3:
for j in range(N_free-1, -1, -1):
wide.extend([(i-1-j)])
elif N_free == 3:
for j in range(N_free-1, -1, -1):
medium.extend([(i-1-j)])
elif N_free < 3 and N_free != 0:
for j in range(N_free-1, -1, -1):
narrow.extend([(i-1-j)])
elif N_free == 0:
pass
else:
lg.prt('error in gap finding!', inst=__name__, lv=100000)
N_free = 0
else:
N_free += 1
lg.prt("\t\t\t wide=",wide, ", medium=", medium, ", narrow=", narrow, inst=__name__, lv=150)
def steer(current_status, desired_status):
steering_error=0
if (current_status[0]=='forward') or (current_status[0]=='break'):
if desired_status[2]=='left':
# Set steering servo on GPIO_Servo to left
servo.set_servo(GPIO_Servo, Left)
elif desired_status[2]=='half-left':
# Set steering servo on GPIO_Servo to half left
servo.set_servo(GPIO_Servo, Half_Left)
elif desired_status[2]=='right':
# Set steering servo on GPIO_Servo to right
servo.set_servo(GPIO_Servo, Right)
elif desired_status[2]=='half-right':
# Set steering servo on GPIO_Servo to half right
servo.set_servo(GPIO_Servo, Half_Right)
elif desired_status[2]=='straight':
# Set steering servo on GPIO_Servo to straight
servo.set_servo(GPIO_Servo, Straight)
else:
steering_error=1
lg.prt('ERROR: Non directional information passed to steering function: current_status[0]=forward or break, desired_status[2]=' + desired_status[2] + ' is unknown!', lv = 100000, inst=__name__)
elif current_status[0]=='backward':
if desired_status[2]=='left':
# Set steering servo on GPIO_Servo to left (backw.)
servo.set_servo(GPIO_Servo, Right)
elif desired_status[2]=='half-left':
# Set steering servo on GPIO_Servo to half left (backw.)
servo.set_servo(GPIO_Servo, Half_Right)
elif desired_status[2]=='right':
# Set steering servo on GPIO_Servo to right (backw.)
servo.set_servo(GPIO_Servo, Left)
elif desired_status[2]=='half-right':
# Set steering servo on GPIO_Servo to half right (backw.)
servo.set_servo(GPIO_Servo, Half_Left)
elif desired_status[2]=='straight':
# Set steering servo on GPIO_Servo to straight (backw.)
servo.set_servo(GPIO_Servo, Straight)
else:
steering_error=1
lg.prt('ERROR: Non directional information passed to steering function: current_status[0]=backward, desired_status[2]=' + desired_status[2] + ' is unknown!', lv = 100000, inst=__name__)
else:
steering_error=1
lg.prt('ERROR: Unknown driving direction passed to steering function: currrent_status[0]=' + current_status[0] + ' is unknown!', lv=100000, inst=__name__)
# update current_status if operation succeeded
if(steering_error==0):
current_status[2]=desired_status[2]
def run(self):
if (self.running==False):
self.running = True
if (self.pins_set == False):
self.set_PINS()
# define cycle-variable:
i = 0 # position in scanning cycle: which sensor will be evaluated next?
j = int(self.servo_segments/2) # position in segment cycle: which segment will be evaluated next?
servo_direction = +1 # for scanning: which direction? +1 -> moves to right-hand-side -1 -> moves to left-hand-side
# once more, check for correct segment-configuration; just for safety
self.update_segments(0, new_segment_number=self.servo_segments)
while (self.running == True):
time.sleep(self.scan_relaxation_time)
# mode 0 = update all sensors in cycle 0-1-2-1-0-1-2-1-0-..., servo not moved
if (self.mode == 0):
cycle=[0,1,2,1] # define cycle for current mode
result = self.get_sensor(cycle[i]) # update next sensor
# on no-echo-in- or unknown-error: try again (5 times)
tries = 1
while (result[0]==self.no_echo_in_value or result[0]==self.unknown_error_value):
if tries < self.measure_tries:
result = self.get_sensor(cycle[i])
tries += 1
else:
break
#save the result
if cycle[i] != 1:
if result[0] > self.sensors_min: self.measurements[cycle[i]] = result
else:
if result[0] > self.sensors_min: self.measurements[cycle[i]] = [result]
# servo is not moved
i=i+1 # count to next cycle-entry
if (i>=len(cycle)): # jump to cycle-beginning
i=0
# mode 1 = scanning all sensors in cycle 0-1-2-1-0-1-2-1-0-...,
# servo scans: from left to right to left to right... starting at center
elif (self.mode == 1):
cycle=[0,1,2,1] # define cycle for current mode
result = self.get_sensor(cycle[i]) # update next sensor
# on no-echo-in- or unknown-error: try again (5 times)
tries = 1
while (result[0]==self.no_echo_in_value or result[0]==self.unknown_error_value):
if tries < self.measure_tries:
result = self.get_sensor(cycle[i])
tries += 1
else:
break
#save the result
if cycle[i] != 1:
if result[0] > self.sensors_min: self.measurements[cycle[i]] = result
else:
current_segment=int((self.servo_position-self.servo_MIN)/self.servo_segment_size)
if result[0] > self.sensors_min: self.measurements[cycle[i]][current_segment] = result
if (cycle[i] == 1): # move servo to next position
if ( (j == 0) and (servo_direction == -1) ): # servo is at left end and wants to move further left
servo_direction = +1 # from now: move right!
elif ( (j == self.servo_segments) and (servo_direction == +1) ) : # servo is at right end and wants to move further right
servo_direction = -1 # from now: move left!
# move servo to next position
j = j + servo_direction
self.move_servo(self.servo_segment_values[j])
time.sleep(.1) # the servo needs time to move
i=i+1 # count to next cycle-entry
if (i>=len(cycle)): # jump to cycle-beginning
i=0
# mode 2 = scanning all sensors in cycle 0-1-2-1-0-1-2-1-0-...,
# servo scans: from left to right jump, form left to right, ... starting at center
elif (self.mode == 2):
cycle=[0,1,2,1] # define cycle for current mode
result = self.get_sensor(cycle[i]) # update next sensor
# on no-echo-in- or unknown-error: try again (5 times)
tries = 1
while (result[0]==self.no_echo_in_value or result[0]==self.unknown_error_value):
if tries < self.measure_tries:
result = self.get_sensor(cycle[i])
tries += 1
else:
break
#save the result
if cycle[i] != 1:
if result[0] > self.sensors_min: self.measurements[cycle[i]] = result
else:
current_segment=int((self.servo_position-self.servo_MIN)/self.servo_segment_size)
if result[0] > self.sensors_min: self.measurements[cycle[i]][current_segment] = result
if (cycle[i] == 1): # move servo to next position
if (j == self.servo_segments) : # servo is at right end and wants to move further right
j = -1 # jump to left end
# move servo to next position
j = j + servo_direction
self.move_servo(self.servo_segment_values[j])
if (j == 0): # the servo needs time to come from right end to left end
time.sleep(.6)
else:
time.sleep(.1)
i=i+1 # count to next cycle-entry
if (i>=len(cycle)): # jump to cycle-beginning
i=0
# mode 3 = scanning all sensors in cycle 0-1-2-1-0-1-2-1-0-...,
# servo scans: from left to right jump, form left to right, ... starting at center
# like mode 2, but averaging over 2 consecutive measurements
elif (self.mode == 3):
cycle=[0,1,2,1] # define cycle for current mode
# do many measurements to average
av_sum = 0
time_stamp = time.time()
nmb_of_measurements = 0
for meas_nmb in range(self.averaging_number):
has_error = False
result = self.get_sensor(cycle[i]) # update next sensor
# on no-echo-in- or unknown-error: try again (5 times)
tries = 1
while (result[0]==self.no_echo_in_value or result[0]==self.unknown_error_value):
if tries < self.measure_tries:
result = self.get_sensor(cycle[i])
tries += 1
else:
has_error = True
break
if not has_error:
av_sum += result[0]
nmb_of_measurements += 1
if nmb_of_measurements == 0:
result = [self.unknown_error_value, time_stamp]
else:
result = [av_sum/nmb_of_measurements, time_stamp]
#save the result
if cycle[i] != 1:
if result[0] > self.sensors_min: self.measurements[cycle[i]] = result
else:
current_segment=int((self.servo_position-self.servo_MIN)/self.servo_segment_size)
if result[0] > self.sensors_min: self.measurements[cycle[i]][current_segment] = result
if (cycle[i] == 1): # move servo to next position
if (j == self.servo_segments) : # servo is at right end and wants to move further right
j = -1 # jump to left end
# move servo to next position
j = j + servo_direction
self.move_servo(self.servo_segment_values[j])
if (j == 0): # the servo needs time to come from right end to left end
time.sleep(.6)
else:
time.sleep(.1)
i=i+1 # count to next cycle-entry
if (i>=len(cycle)): # jump to cycle-beginning
i=0
else:
lg.prt("Unknown mode set to sensor-scanning-thread! Waiting for corrent mode...", lv=100000, inst=__name__)
def get_sensor(self, sensor_number):
# wait, if there is a measure at the moment
while(self.is_at_measure==True):
time.sleep(0.01)
# tell, that there is measure now
self.is_at_measure = True
# MEASURE
self.lock.acquire()
# TRIGGER
GPIO.output(self.TRIG[sensor_number], True)
time.sleep(0.00001)
GPIO.output(self.TRIG[sensor_number], False)
# WAIT FOR ECHO
try: # added because of error 'referrenced before assignement'
echo_start = time.time() # serves also for time-stamp of measurement
while ( (GPIO.input(self.ECHO[sensor_number])==0) ):
start_time = time.time()
if (time.time()-echo_start>self.ECHO_in_timeout): ## CHECK THIS!!!
self.is_at_measure = False
result = [self.no_echo_in_value, echo_start]
# return distance (in cm) & measure-time in time.time()-format
self.lock.release()
return result
# MEASURE ECHO-DURATION
while (GPIO.input(self.ECHO[sensor_number])==1):
dt = time.time() - start_time
if(dt>self.out_of_sight_time): ## CHECK THIS!!!!
self.is_at_measure = False
result = [self.out_of_sight_value, echo_start]
# return distance (in cm) & measure-time in time.time()-format
self.lock.release()
return result
# tell, that there is no measure now
self.is_at_measure = False
# RETURN DISTANCE
dist = dt*self.dt_to_distance
# save the result
result = [dist, echo_start]
# return distance (in cm) & measure-time in time.time()-format
self.lock.release()
return result
except: # any unknown error
self.is_at_measure = False
lg.prt("exception from sensor " + str(sensor_number + 1) + " of 3", inst=__name__, lv=10000) # + "): unknown error from sensor (probably \'referenced before assignment\')"
GPIO.output(self.TRIG[sensor_number], False)
time.sleep(0.2)
self.lock.release()
return [self.unknown_error_value, echo_start]
def accelerate(current_status, desired_status):
acceleration_error=0
if current_status[0]=='forward':
if desired_status[0]=='break':
# Set motor on GPIO_Motor to fast backward in order to break
servo.set_servo(GPIO_Motor, (Null-Fast)) #break
sleep(Sleeping_Time)
servo.set_servo(GPIO_Motor, Null) #stop
elif desired_status[0]=='forward':
if desired_status[1]=='fast':
# Set motor on GPIO_Motor to fast forward
servo.set_servo(GPIO_Motor, (Null+Fast))
elif desired_status[1]=='middle':
# Set motor on GPIO_Motor to middle forward
servo.set_servo(GPIO_Motor, (Null+Middle))
elif desired_status[1]=='slow':
# Set motor on GPIO_Motor to slow forward
servo.set_servo(GPIO_Motor, (Null+Slow-Slow_Offset))
elif desired_status[1]=='null':
# Set motor on GPIO_Motor to null position #Leerlauf
servo.set_servo(GPIO_Motor, (Null))
else:
acceleration_error=1
lg.prt('ERROR: Non speed information passed to accelerate function: current_status[0]=forward, desired_status[0]=forward, desired_status[1]=' + desired_status[1] + ' is unknown!', lv=100000, inst=__name__)
elif desired_status[0]=='backward':
if desired_status[1]=='fast':
# Break and then set motor on GPIO_Motor to fast backwards
servo.set_servo(GPIO_Motor, (Null-Fast)) #break
sleep(Sleeping_Time)
servo.set_servo(GPIO_Motor, (Null)) #stop
sleep(Sleeping_Time)
servo.set_servo(GPIO_Motor, (Null-Fast)) #fast backward
elif desired_status[1]=='middle':
# Break and then set motor on GPIO_Motor to middle backwards
servo.set_servo(GPIO_Motor, (Null-Fast)) #break
sleep(Sleeping_Time)
servo.set_servo(GPIO_Motor, (Null)) #stop
sleep(Sleeping_Time)
servo.set_servo(GPIO_Motor, (Null-Middle)) #middle backward
elif desired_status[1]=='slow':
# Break and then set motor on GPIO_Motor to slow backward
servo.set_servo(GPIO_Motor, (Null-Fast)) #break
sleep(Sleeping_Time)
servo.set_servo(GPIO_Motor, (Null)) #stop
sleep(Sleeping_Time)
servo.set_servo(GPIO_Motor, (Null-Slow-Slow_Offset)) #slow backward
elif desired_status[1]=='null':
# Set motor on GPIO_Motor to null position #Leerlauf
servo.set_servo(GPIO_Motor, (Null))
else:
acceleration_error=1
lg.prt('ERROR: Non speed information passed to accelerate function: current_status[0]=forward, desired_status[0]=backward, desired_status[1]=' + desired_status[1] + ' is unknown!', lv=100000, inst=__name__)
else:
acceleration_error=1
lg.prt('ERROR: Unknown driving directive passed to accelerate function: current_status[0]=forward, desired_status[0]=' + desired_status[0] + ' is unknown!', lv=100000, inst=__name__)
elif current_status[0]=='backward':
if desired_status[0]=='break':
# Set motor on GPIO_Motor to null
servo.set_servo(GPIO_Motor, Null)
elif desired_status[0]=='backward':
if desired_status[1]=='fast':
# Set motor on GPIO_Motor to fast backward
servo.set_servo(GPIO_Motor, (Null-Fast))
elif desired_status[1]=='middle':
# Set motor on GPIO_Motor to middle backward
servo.set_servo(GPIO_Motor, (Null-Middle))
elif desired_status[1]=='slow':
# Set motor on GPIO_Motor to slow backward
servo.set_servo(GPIO_Motor, (Null-Slow-Slow_Offset))
elif desired_status[1]=='null':
# Set motor on GPIO_Motor to null position #Leerlauf
servo.set_servo(GPIO_Motor, (Null))
else:
acceleration_error=1
lg.prt('ERROR: Non speed information passed to accelerate function: current_status[0]=backward, desired_status[0]=backward, desired_status[1]=' + desired_status[1] + ' is unknown!', lv=100000, inst=__name__)
elif desired_status[0]=='forward':
if desired_status[1]=='fast':
# Set motor on GPIO_Motor to fast forward
servo.set_servo(GPIO_Motor, (Null+Fast))
elif desired_status[1]=='middle':
# Set motor on GPIO_Motor to middle forward
servo.set_servo(GPIO_Motor, (Null+Middle))
elif desired_status[1]=='slow':
# Set motor on GPIO_Motor to slow forward
servo.set_servo(GPIO_Motor, (Null+Slow-Slow_Offset))
elif desired_status[1]=='null':
# Set motor on GPIO_Motor to null position #Leerlauf
servo.set_servo(GPIO_Motor, (Null))
else:
acceleration_error=1
lg.prt('ERROR: Non speed information passed to accelerate function: current_status[0]=backward, desired_status[0]=forward, desired_status[1]=' + desired_status[1] + ' is unknown!', lv=100000, inst=__name__)
else:
acceleration_error=1
lg.prt( 'ERROR: Unknown driving directive passed to accelerate function: current_status[0]=backward, desired_status[0]=' + desired_status[0] + ' is unknown!', lv=100000, inst=__name__)
elif current_status[0]=='break':
if desired_status[0]=='break':
servo.set_servo(GPIO_Motor, Null) #stop
elif desired_status[0]=='forward':
if desired_status[1]=='fast':
# Set motor on GPIO_Motor to fast forward
servo.set_servo(GPIO_Motor, (Null+Fast))
elif desired_status[1]=='middle':
# Set motor on GPIO_Motor to middle forward
servo.set_servo(GPIO_Motor, (Null+Middle))
elif desired_status[1]=='slow':
# Set motor on GPIO_Motor to slow forward
servo.set_servo(GPIO_Motor, (Null+Slow-Slow_Offset))
elif desired_status[1]=='null':
# Set motor on GPIO_Motor to null position #Leerlauf
servo.set_servo(GPIO_Motor, (Null))
else:
acceleration_error=1
lg.prt('ERROR: Non speed information passed to accelerate function: current_status[0]=break, desired_status[0]=forward, desired_status[1]=' + desired_status[1] + ' is unknown!', lv=100000, inst=__name__)
elif desired_status[0]=='backward':
if desired_status[1]=='fast':
# Set motor on GPIO_Motor to fast backward
servo.set_servo(GPIO_Motor, (Null-Fast))
elif desired_status[1]=='middle':
# Set motor on GPIO_Motor to middle backward
servo.set_servo(GPIO_Motor, (Null-Middle))
elif desired_status[1]=='slow':
# Set motor on GPIO_Motor to slow backward
servo.set_servo(GPIO_Motor, (Null-Slow-Slow_Offset))
elif desired_status[1]=='null':
# Set motor on GPIO_Motor to null position #Leerlauf
servo.set_servo(GPIO_Motor, (Null))
else:
acceleration_error=1
lg.prt('ERROR: Non speed information passed to accelerate function: current_status[0]=break, desired_status[0]=backward, desired_status[1]=' + desired_status[1] + ' is unknown!', lv=100000, inst=__name__)
else:
acceleration_error=1
lg.prt( 'ERROR: Unknown driving directive passed to accelerate function: current_status[0]=break, desired_status[0]=' + desired_status[0] + ' is unknown!', lv=100000, inst=__name__)
else:
acceleration_error=1
lg.prt( 'ERROR: Unknown driving directive passed to accelerate function: current_status[0]=' + current_status[0] + ' is unknown!', lv=100000, inst=__name__)
# update current_status if operation succeeded
if(acceleration_error==0):
for i in (0,1):
current_status[i]=desired_status[i]
