wall_follower_head2.py

#!/usr/bin/env python3
import RPi.GPIO as GPIO
import time
from pololu_drv8835_rpi import motors, MAX_SPEED
import subprocess
import serial
import sys
import os
import pyzbar.pyzbar as pyzbar
import cv2
import numpy as np
from picamera.array import PiRGBArray
from picamera import PiCamera

Kp = 10
Kd = 2#for both walls together

Kp1 = 10
#for one wall
Kd1 = 1.2

error_limit = 40

turn_speed = 90

distance_new_cell = 9.5#distance to stop from wall

left_base =72#base speed of motors
right_base = 67

jhatka_speed = 90

left_turn_angle = 63.00

right_turn_angle = 60.00




left_thresh = 17#reference distance from two sides
right_thresh = 17

centre_thresh =28.5#reference distance from front

wall_pid = 9.5# threshold for pid

fo = open("path.txt","r")

my_path = fo.read()

print(my_path)

fo.close()

#my_path =  "SRRSLSRLLS"#"SRLSLRSSRSSRSRLSLRRLLRRL"#path



oldErrorP = 0

oldErrorP1 = 0

forward_delay = 0.21

block_lift_delay = 2

threshArea = [20000.0,200000.0]

camera= PiCamera()
camera.resolution = (640,480) 						#Setting up picam
rawCapture= PiRGBArray(camera, size=(640,480))

color = {"Hue":[40,182],"Sat":[114,255],"Val":[34,255],"name":"something"}


try:

	motors.setSpeeds(0, 0)

	GPIO.setmode(GPIO.BCM)

	PIN_TRIGGER_1 = 16#PIN DEFINITION OF ULTRASOUND PINS
	PIN_ECHO_1 = 27

	PIN_TRIGGER_2 = 4
	PIN_ECHO_2 = 17

	PIN_TRIGGER_3 = 22
	PIN_ECHO_3 = 23

	GPIO.setup(PIN_TRIGGER_1, GPIO.OUT)
	GPIO.setup(PIN_ECHO_1, GPIO.IN)
	GPIO.setup(PIN_TRIGGER_2, GPIO.OUT)
	GPIO.setup(PIN_ECHO_2, GPIO.IN)
	GPIO.setup(PIN_TRIGGER_3, GPIO.OUT)
	GPIO.setup(PIN_ECHO_3,GPIO.IN)
	GPIO.setup(18, GPIO.OUT)
	GPIO.setup(8, GPIO.IN, pull_up_down=GPIO.PUD_UP)	#Reset_Pin
	GPIO.setup(7, GPIO.IN, pull_up_down=GPIO.PUD_UP)	#Start_Pin
	GPIO.setup(25, GPIO.IN, pull_up_down=GPIO.PUD_UP)	#Start_Map_again


	reset=8
	start=7
	start_map=25

	def reset_callback(channel):
    	print("Reseting")				#Restart the Pi code
    	os.execv(__file__, sys.argv)

    def map_again_callback(channel):	
    	print("Mapping again")
    	subprocess.call(["python","wall_follower_head2.py"])	


    GPIO.add_event_detect(25,GPIO.FALLING,callback=map_again_callback)	
    GPIO.add_event_detect(8,GPIO.FALLING,callback=reset_callback)

	servo = GPIO.PWM(18, 50)

	servo.start(7.5)

	servo.ChangeDutyCycle(12.5) # turn towards 180 degree(Left)

	Led_array = [19,20,21,26]

	for i in Led_array:
		GPIO.setup(i,GPIO.OUT)
		GPIO.output(i,GPIO.LOW)

	GPIO.output(PIN_TRIGGER_1, GPIO.LOW)
	GPIO.output(PIN_TRIGGER_2, GPIO.LOW)
	GPIO.output(PIN_TRIGGER_3, GPIO.LOW)




	def check_block(): #Image Processing
		global color
		camera.capture(rawCapture, format= "bgr")
		frame = rawCapture.array

		decodedObjects = pyzbar.decode(frame)

		if len(decodedObjects)>0:
		    for obj in decodedObjects:
		        print('Type : ', obj.type)
		        print('Data : ', obj.data,'\n')
		        rawCapture.truncate(0)
		        return True
		 
		else:
		    frameBGR = cv2.GaussianBlur(frame, (7, 7), 0)
		   
		    hsv = cv2.cvtColor(frameBGR, cv2.COLOR_BGR2HSV)





		    colorLow = np.array([color["Hue"][0],color["Sat"][0],color["Val"][0]])
		    colorHigh = np.array([color["Hue"][1],color["Sat"][1],color["Val"][1]])
		    mask = cv2.inRange(hsv, colorLow, colorHigh)
		    # Show the first mask
		    #cv2.imshow('mask-plain', mask)
		    #cv2.waitKey(0)
		    kernal = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (7, 7))
		    mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernal)
		    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernal)
		     
		    im2, contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
		    cv2.drawContours(frame, contours, -1, (0,255,0), 3)


		    contour_sizes = [(cv2.contourArea(contour), contour) for contour in contours]

		    if len(contour_sizes) >0:

		        biggest_contour = max(contour_sizes, key=lambda x: x[0])[1]
		        cv2.drawContours(frame, biggest_contour, -1, (0,255,0), 3)


		        biggest_contour_area = cv2.contourArea(biggest_contour)
		        
		        if (biggest_contour_area > threshArea[0]) and (biggest_contour_area < threshArea[1]):
		            #blockDetected = True
		            #print (color["name"] + "found!!" + str(cv2.contourArea(biggest_contour)))
		            rawCapture.truncate(0)
		            return True

		            cv2.imwrite('RESULT.jpg',mask)
		        
		        

		        #print(cv2.contourArea(biggest_contour))
		    else :
		        #print("Patrao Kai Na")
		        rawCapture.truncate(0)
		        return False
		         
		    
		rawCapture.truncate(0)



	def calc_dist(PIN_TRIGGER,PIN_ECHO):
		#print "Calculating distance"

		#GPIO.output (PIN_TRIGGER,GPIO.LOW)                                 
		#time.sleep (0.00005)

		GPIO.output(PIN_TRIGGER, GPIO.HIGH)

		time.sleep(0.00001)

		GPIO.output(PIN_TRIGGER, GPIO.LOW)

		intial_time = time.time()
		pulse_start_time = intial_time

		while GPIO.input(PIN_ECHO)==0:
		  pulse_start_time = time.time()
		  if pulse_start_time - intial_time > 0.02:
		  	return 100.0

		pulse_end_time = time.time()
		while GPIO.input(PIN_ECHO)==1:
		  pulse_end_time = time.time()
		  if pulse_end_time - pulse_start_time > 0.02:
		    GPIO.setup(PIN_ECHO, GPIO.OUT)
		    GPIO.output(PIN_ECHO, GPIO.LOW)
		    time.sleep(0.00001)
		    GPIO.setup(PIN_ECHO, GPIO.IN)

		    return 100.0

		pulse_duration = pulse_end_time - pulse_start_time
		distance = round(pulse_duration * 17150, 2)
		return distance

	def mes_sense():
		left_sensor = calc_dist(PIN_TRIGGER_1,PIN_ECHO_1)
		right_sensor = calc_dist(PIN_TRIGGER_2,PIN_ECHO_2)
		front_sensor = calc_dist(PIN_TRIGGER_3,PIN_ECHO_3)

		for i in range(5):

			left_sensor = (calc_dist(PIN_TRIGGER_1,PIN_ECHO_1)+left_sensor)
			front_sensor = (calc_dist(PIN_TRIGGER_3,PIN_ECHO_3)+front_sensor)

			right_sensor = (calc_dist(PIN_TRIGGER_2,PIN_ECHO_2)+right_sensor)

		front_sensor = front_sensor/6

		left_sensor = left_sensor/6
		
		right_sensor = right_sensor/6	


		sensor_array = [left_sensor,front_sensor,right_sensor]
		print (sensor_array)
		return sensor_array	

			

	def PID_calc(sensor_array):

		global oldErrorP,Kp,Kd

		errorP = sensor_array[0] - sensor_array[2] 
		errorD = errorP - oldErrorP
		total_error = Kp*errorP + Kd*errorD
		
		if total_error > error_limit:
			total_error= error_limit
		if total_error < -error_limit:
			total_error = -error_limit		
		#print(total_error)
		
		oldErrorP = errorP

		return total_error

	def side_PID_calc(sensor):
		global oldErrorP1,Kp1,Kd1


		errorP = wall_pid - sensor
		errorD = errorP - oldErrorP1

		oldErrorP1 = errorP

		total_error = Kp1*errorP + Kd1*errorD
		
		if total_error > error_limit:
			total_error= error_limit
		if total_error < -error_limit:
			total_error = -error_limit
		#print(total_error)
	
		return total_error 	

	def motor_control(error):

		global right_base,left_base

		right_motor_speed = right_base + error

		left_motor_speed = left_base - error

		motors.motor1.setSpeed(int(left_motor_speed))

		motors.motor2.setSpeed(int(right_motor_speed))

	def turn_motors(left_motor,right_motor):
		motors.motor1.setSpeed(left_motor)

		motors.motor2.setSpeed(right_motor)

	def block_action():
		global normal_blocks,qr_block_flag,qr_blocks
		servo.ChangeDutyCycle(7.8)  # turn towards 90 degree(Front)

		if check_block()=="Normal":
			if qr_block_flag=1 and normal_blocks=1:
				while check_block()=="Normal":
					print("Normal block found(Qr already!)")
				spot_right()
				spot_right()
				turn_motors(0,0)

			else:
				while check_block()=="Normal":
					print("Normal block found")
					
			normal_blocks = normal_blocks + 1
			return True

		elif check_block()=="QR":
			if normal_blocks == 2:
				if qr_blocks==2:
					sys.exit("Dart feko")
				else:	
					qr_blocks=qr_blocks+1
					normal_blocks=0
					qr_block_flag=0	

			else:
				qr_block_flag=1
			return True	

			
		else:
			return False			
				
		servo.ChangeDutyCycle(12.5)  # turn towards 90 degree(Left)		

	
	def spot_right():
		ser.flushInput()
		start=float(ser.readline())
		output = start
		turn_motors(turn_speed,-turn_speed)

		if start < (-180 + right_turn_angle):
			while output  < 0 :
				print("Acheiving +")
				ser.flushInput()
				output=float(ser.readline())
				print(output)	#right

			while (360 - output +start) < right_turn_angle:
				print ("Acheiving -")
				ser.flushInput()
				output=float(ser.readline())
				print(output)

		else:

			if start >0 :
				while(output<0):
					print ("correcting")
					ser.flushInput()
					output=float(ser.readline())
					print(output)


			while (start-output) < right_turn_angle:
				print ("Normal")
				ser.flushInput()
				output=float(ser.readline())
				print(output)
		#start_pos = start_pos - right_turn_angle
		#if start_pos < -180:
		#	start_pos = 360+start_pos
		#elif start_pos >180:
		#	start_pos = start_pos - 360 	


		turn_motors(0,0)

		block_action()


	def spot_left():
		ser.flushInput()
		start = float(ser.readline())
		output=start
		turn_motors(-turn_speed,turn_speed)
				

		if start > (180 -left_turn_angle):
			while output > 0:
				print ("Acheiving +")
				ser.flushInput()
				output=float(ser.readline())
				print(output)

			while (360 + output -start) < left_turn_angle:
				print ("Acheiving -")
				ser.flushInput()
				output=float(ser.readline())
				print(output)

		else:

			if start <0 :
				while(output>0):
					print ("correcting")
					ser.flushInput()
					output=float(ser.readline())
					print(output)

			while (output - start) < left_turn_angle:
				print ("Normal")
				ser.flushInput()
				output=float(ser.readline())
				print(output)
		#start_pos = start_pos + left_turn_angle	
		#if start_pos < -180:
		#	start_pos = 360+start_pos
		#elif start_pos >180:
		#	start_pos = start_pos - 360	
		turn_motors(0,0)

		servo.ChangeDutyCycle(7.8)  # turn towards 90 degree(Front)

		block_action()	
						

	def new_cell_adjust():
		sensor_array = mes_sense()

		if sensor_array[1] < centre_thresh:
			print ("Going for the wall")
			'''
			available_walls = check_available_walls()
			final_pid(available_walls,distance_new_cell)
			'''
			while sensor_array[1] > distance_new_cell:
				turn_motors(left_base,right_base)
				sensor_array = mes_sense()
		else:
			turn_motors(left_base+jhatka_speed,right_base+jhatka_speed)
			time.sleep(forward_delay)
			print("JHATKA!")
					




	def check_available_walls():
			sensor_array = mes_sense()
			if (sensor_array[0]< left_thresh and sensor_array[2]<right_thresh):
				return "both"		
			elif (sensor_array[0]< left_thresh and sensor_array[2]>right_thresh):
				return "left"			
			elif (sensor_array[0]> left_thresh and sensor_array[2]<right_thresh):
				return "right"
			elif (sensor_array[0]> left_thresh and sensor_array[2]>right_thresh):
				return "no_wall"

	def final_pid(wall,front_select,check):
		global wall_pid,distance_new_cell
		sensor_array = mes_sense()

		if front_select == centre_thresh:


			if wall == "both":

				while(sensor_array[0]< left_thresh and sensor_array[2]<right_thresh):

					sensor_array = mes_sense()

					if sensor_array[1] < distance_new_cell :

						if check:
							block_action()
						else:
							break				




					error = PID_calc(sensor_array)

					motor_control(error)




			elif wall == "left":

				wall_pid = sensor_array[0] 
				
				while (sensor_array[0]< left_thresh and sensor_array[2]>right_thresh):

					sensor_array = mes_sense()

					if sensor_array[1] < front_select :

						if check:
							block_action()	
						else:
							break


					error = side_PID_calc(sensor_array[0])

					#print(sensor_array)
					motor_control(-error)

					
					
					if (sensor_array[2]<right_thresh):
						time.sleep(0.2)
						sensor_array = mes_sense()
					
				print("Done with left_wall pid")		
					



			elif wall == "right":

				wall_pid = sensor_array[2]

				while (sensor_array[0]> left_thresh and sensor_array[2]<right_thresh):

					sensor_array = mes_sense()

					if sensor_array[1] < front_select :

						if check:
							block_action()		
						else:
							break



					error = side_PID_calc(sensor_array[2])
					motor_control(error)
					
					if (sensor_array[0] < left_thresh ):
						time.sleep(0.2)
						sensor_array = mes_sense()
					
					
								

			elif wall=="no_wall":
				while (sensor_array[1]> front_select):
					#print(sensor_array[1])
					
					turn_motors(left_base,right_base)
					sensor_array = mes_sense()
					if sensor_array[1] < front_select :

						if check:
							block_action()		
						else:
							break


		else:

			if wall == "both":

				while(sensor_array[1] > front_select):

					sensor_array = mes_sense()
					error = PID_calc(sensor_array)

					motor_control(error)		


			elif wall == "left":

				wall_pid = sensor_array[0] 
				
				while (sensor_array[1] > front_select):

					sensor_array = mes_sense()


					error = side_PID_calc(sensor_array[0])

					#print(sensor_array)
					motor_control(-error)
					
				print("Done with left_wall pid")		
					



			elif wall == "right":

				wall_pid = sensor_array[2]

				while (sensor_array[1] > front_select):

					sensor_array = mes_sense()
					error = side_PID_calc(sensor_array[2])
					motor_control(error)
								

			elif wall=="no_wall":
				while (sensor_array[1]> front_select):
					#print(sensor_array[1])
					
					turn_motors(left_base,right_base)
					sensor_array = mes_sense()
		


		turn_motors(0,0)

	def Led_control(control_array):
		global Led_array
		for i in range(4):
			GPIO.output(Led_array[i],control_array[i])


	def startup():

		while GPIO.input(start) == GPIO.HIGH:
			"Waiting for the start"
		wall = check_available_walls()

		wall = check_available_walls()

		if wall == "both":

			while True:

				sensor_array = mes_sense()
				error = PID_calc(sensor_array)

				print (error)
				

				if error > 25:
					Led_control([0,0,1,1])

				elif error > 5:	 
					Led_control([0,1,1,1])

				elif error < -25:
					Led_control([1,1,0,0])

				elif error < -5:
					Led_control([1,1,1,0])

				else:
					Led_control([1,1,1,1])
					time.sleep(2)
					Led_control([0,0,0,0])
					#sys.exit("Jal Gaya")
					return
				
			





		elif wall == "left":

				
			print("Done with left_wall pid")		
				



		elif wall == "right":
			pass
							

		elif wall=="no_wall":
			pass	


				
	i =0
	
	startup()

	ser=serial.Serial("/dev/ttyUSB0",9600)
	ser.baudrate=9600

	start_t = time.time()

	while (time.time()-start_t < 9):

		Led_control([1,1,1,1])
		time.sleep(0.2)
		Led_control([0,0,0,0])
		time.sleep(0.2)
	Led_control([0,0,0,0])	

	camera.start_preview()
	#ser.flushInput()
	#start_pos = float(ser.readline())

	GPIO.output(PIN_TRIGGER_1, GPIO.LOW)
	GPIO.output(PIN_TRIGGER_2, GPIO.LOW)
	GPIO.output(PIN_TRIGGER_3, GPIO.LOW)

	

	while True:




		print("	I am going " + my_path[i])

		if my_path[i] == 'S':


			sensor_array = mes_sense()


			available_walls = check_available_walls()


			print ("Available walls are:" + available_walls)

			final_pid(available_walls,centre_thresh,1)
			'''

			available_walls = check_available_walls()

			print ("Available walls for delay are:" + available_walls)

			final_pid_delay(available_walls)

			#print "New cell identified"
			'''

			


			while my_path[i+1] == 'S':	# Going straight if junction not required
				#print "Just keep going straight"
				print("Going straight!!!!")
				available_walls = check_available_walls()
				final_pid(available_walls,centre_thresh,1)
				i=i+1

			
			'''
			sensor_array = mes_sense()
											#now just adjusting into new cell
			initial_pos = sensor_array[1]

			#print "Inital Position is" + str(initial_pos)
			final_pos = sensor_array[1]

			

			while ((initial_pos - final_pos < distance_new_cell)):


				turn_motors(left_base,right_base)
				sensor_array = mes_sense()

				final_pos = sensor_array[1]

			#print final_pos

		


				

			print "Final Position is" + str(final_pos)

			'''
			

			new_cell_adjust()


			motors.setSpeeds(0,0) # Stopping motors in new cell
			#input ("Keep me")
			
	

			print("New cell has been acheived")

			i=i+1		

		elif my_path[i] == 'R':
			

			spot_right()


			print("Turn completed!")

			#time.sleep(4)




			available_walls = check_available_walls()

			print ("Available walls are:" + available_walls)

			#sys.exit("fgfhg")

			final_pid(available_walls,centre_thresh,0)

			#sys.exit("OHO")

			'''

			available_walls = check_available_walls()

			print ("Available walls for delay are:" + available_walls)

			final_pid_delay(available_walls)

			'''


			while my_path[i+1] == 'S':	# Going straight if junction not required
				available_walls = check_available_walls()
				print("Going straight!!!!")
				print ("Available walls are:" + available_walls)
				final_pid(available_walls,centre_thresh,1)
				#sys.exit("Oho!")
				print ("Available walls are:" + available_walls)
				#sys.exit("Oho!")
				i=i+1
			

			'''
			sensor_array = mes_sense()
											#now just adjusting into new cell
			initial_pos = sensor_array[1]

			print "Inital Position is" + str(initial_pos)
			final_pos = sensor_array[1]

			

			while ((initial_pos - final_pos < distance_new_cell)):


				turn_motors(left_base,right_base)
				sensor_array = mes_sense()

				final_pos = sensor_array[1]

				

			print "Final Position is" + str(final_pos)
			'''	
			new_cell_adjust()

			motors.setSpeeds(0,0) # Stopping motors in new cell

			#sys.exit("Hello")
			#sys.exit("OHO")



			#input ("Keep me")
			


			print("New cell has been acheived")

			
			i=i+1


		elif my_path[i] == 'L':
			
			spot_left()
			print("Turn completed!")

			#time.sleep(4)

			available_walls = check_available_walls()	

			print ("Available walls are:" + available_walls)

			final_pid(available_walls,centre_thresh,0)

			'''
			available_walls = check_available_walls()

			print ("Available walls for delay are:" + available_walls)

			final_pid_delay(available_walls)
			'''

			while my_path[i+1] == 'S':	# Going straight if junction not required
				print("Going straight!!!!")
				available_walls = check_available_walls()
				print ("Available walls are:" + available_walls)
				final_pid(available_walls,centre_thresh,1)
				i=i+1

			'''	

			sensor_array = mes_sense()
											#now just adjusting into new cell
			initial_pos = sensor_array[1]

			#print "Inital Position is" + str(initial_pos)
			final_pos = sensor_array[1]

			

			while ((initial_pos - final_pos < distance_new_cell)):


				turn_motors(left_base,right_base)
				sensor_array = mes_sense()

				final_pos = sensor_array[1]
			'''
			
			new_cell_adjust()

			motors.setSpeeds(0,0) # Stopping motors in new cell

			#input ("Keep me")
			


			print ("New cell has been acheived")

			i=i+1 

		elif  my_path[i] == 'B':
		
			spot_right()
			spot_right()
			print("Going Back")
			i=i+1	



			


finally:
	motors.setSpeeds(0,0)
	servo.stop()