Exemple #1
0
	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)
Exemple #3
0
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]
Exemple #4
0
	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__)
Exemple #5
0
	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]
Exemple #6
0
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]