def run():
pwm_S = Servo()
pwm_S.setServoPwm('0', SERVO_ORIZON)
pwm_S.setServoPwm('1', SERVO_VERTIC)
ultrasonic = Ultrasonic()
cli = Redis('localhost')
while True:
M = ultrasonic.get_distance()
cli.set('ultrasonic_M', M)
time.sleep(0.2)
"""
import time
from Motor import *
PWM=Motor()
from Ultrasonic import *
ultrasonic=Ultrasonic()
try:
while True:
data=ultrasonic.get_distance() #Get the value
print ("Obstacle distance is "+str(data)+"CM")
if(data<10):
PWM.setMotorModel(-1000,-1000,-1000,-1000) #Back
print ("The car is going backwards")
time.sleep(1)
else:
PWM.setMotorModel(1000,1000,1000,1000) #Forward
print ("The car is moving forward")
time.sleep(1)
except KeyboardInterrupt:
print ("\nEnd of program")
class Server:
def __init__(self):
self.PWM = Motor()
self.servo = Servo()
self.led = Led()
self.ultrasonic = Ultrasonic()
self.buzzer = Buzzer()
self.adc = Adc()
self.light = Light()
self.infrared = Line_Tracking()
self.tcp_Flag = True
self.sonic = False
self.Light = False
self.Mode = 'one'
self.endChar = '\n'
self.intervalChar = '#'
def get_interface_ip(self):
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
return socket.inet_ntoa(
fcntl.ioctl(s.fileno(), 0x8915, struct.pack('256s',
b'wlan0'[:15]))[20:24])
def StartTcpServer(self):
HOST = str(self.get_interface_ip())
self.server_socket1 = socket.socket()
self.server_socket1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT,
1)
self.server_socket1.bind((HOST, 5000))
self.server_socket1.listen(1)
self.server_socket = socket.socket()
self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT,
1)
self.server_socket.bind((HOST, 8000))
self.server_socket.listen(1)
print('Server address: ' + HOST)
def StopTcpServer(self):
try:
self.connection.close()
self.connection1.close()
except Exception as e:
print('\n' + "No client connection")
def Reset(self):
self.StopTcpServer()
self.StartTcpServer()
self.SendVideo = Thread(target=self.sendvideo)
self.ReadData = Thread(target=self.readdata)
self.SendVideo.start()
self.ReadData.start()
def send(self, data):
self.connection1.send(data.encode('utf-8'))
def sendvideo(self):
try:
self.connection, self.client_address = self.server_socket.accept()
self.connection = self.connection.makefile('wb')
except:
pass
self.server_socket.close()
try:
with picamera.PiCamera() as camera:
camera.resolution = (400, 300) # pi camera resolution
camera.framerate = 15 # 15 frames/sec
time.sleep(2) # give 2 secs for camera to initilize
start = time.time()
stream = io.BytesIO()
# send jpeg format video stream
print("Start transmit ... ")
for foo in camera.capture_continuous(stream,
'jpeg',
use_video_port=True):
try:
self.connection.flush()
stream.seek(0)
b = stream.read()
length = len(b)
if length > 5120000:
continue
lengthBin = struct.pack('L', length)
self.connection.write(lengthBin)
self.connection.write(b)
stream.seek(0)
stream.truncate()
except Exception as e:
print(e)
print("End transmit ... ")
break
except:
#print "Camera unintall"
pass
def stopMode(self):
try:
stop_thread(self.infraredRun)
self.PWM.setMotorModel(0, 0, 0, 0)
except:
pass
try:
stop_thread(self.lightRun)
self.PWM.setMotorModel(0, 0, 0, 0)
except:
pass
try:
stop_thread(self.ultrasonicRun)
self.PWM.setMotorModel(0, 0, 0, 0)
self.servo.setServoPwm('0', 90)
self.servo.setServoPwm('1', 90)
except:
pass
def readdata(self):
try:
try:
self.connection1, self.client_address1 = self.server_socket1.accept(
)
print("Client connection successful ! ---- SURE?")
except:
print("Client connect failed")
restCmd = ""
self.server_socket1.close()
while True:
try:
AllData = restCmd + self.connection1.recv(1024).decode(
'utf-8')
except:
if self.tcp_Flag:
self.Reset()
break
print('AllData is', AllData)
if len(AllData) < 5:
restCmd = AllData
if restCmd == '' and self.tcp_Flag:
self.Reset()
break
restCmd = ""
if AllData == '':
break
else:
cmdArray = AllData.split("\n")
if (cmdArray[-1] != ""):
restCmd = cmdArray[-1]
cmdArray = cmdArray[:-1]
for oneCmd in cmdArray:
data = oneCmd.split("#")
if data == None:
continue
elif cmd.CMD_MODE in data:
if data[1] == 'one':
self.stopMode()
self.Mode = 'one'
elif data[1] == 'two':
self.stopMode()
self.Mode = 'two'
self.lightRun = Thread(target=self.light.run)
self.lightRun.start()
elif data[1] == 'three':
self.stopMode()
self.Mode = 'three'
self.ultrasonicRun = threading.Thread(
target=self.ultrasonic.run)
self.ultrasonicRun.start()
elif data[1] == 'four':
self.stopMode()
self.Mode = 'four'
self.infraredRun = threading.Thread(
target=self.infrared.run)
self.infraredRun.start()
elif (cmd.CMD_MOTOR in data) and self.Mode == 'one':
print('Data is', data)
try:
data1 = int(data[1])
data2 = int(data[2])
data3 = int(data[3])
data4 = int(data[4])
if data1 == None or data2 == None or data2 == None or data3 == None:
continue
self.PWM.setMotorModel(data1, data2, data3, data4)
except:
pass
elif cmd.CMD_SERVO in data:
try:
data1 = data[1]
data2 = int(data[2])
if data1 == None or data2 == None:
continue
self.servo.setServoPwm(data1, data2)
except:
pass
elif cmd.CMD_LED in data:
try:
data1 = int(data[1])
data2 = int(data[2])
data3 = int(data[3])
data4 = int(data[4])
if data1 == None or data2 == None or data2 == None or data3 == None:
continue
self.led.ledIndex(data1, data2, data3, data4)
except:
pass
elif cmd.CMD_LED_MOD in data:
self.LedMoD = data[1]
if self.LedMoD == '0':
try:
stop_thread(Led_Mode)
except:
pass
self.led.ledMode(self.LedMoD)
time.sleep(0.1)
self.led.ledMode(self.LedMoD)
else:
try:
stop_thread(Led_Mode)
except:
pass
time.sleep(0.1)
Led_Mode = Thread(target=self.led.ledMode,
args=(data[1], ))
Led_Mode.start()
elif cmd.CMD_SONIC in data:
if data[1] == '1':
self.sonic = True
self.ultrasonicTimer = threading.Timer(
0.5, self.sendUltrasonic)
self.ultrasonicTimer.start()
else:
self.sonic = False
elif cmd.CMD_BUZZER in data:
try:
self.buzzer.run(data[1])
except:
pass
elif cmd.CMD_LIGHT in data:
if data[1] == '1':
self.Light = True
self.lightTimer = threading.Timer(
0.3, self.sendLight)
self.lightTimer.start()
else:
self.Light = False
elif cmd.CMD_POWER in data:
ADC_Power = self.adc.recvADC(2) * 3
try:
self.send(cmd.CMD_POWER + '#' + str(ADC_Power) +
'\n')
except:
pass
except Exception as e:
print(e)
self.StopTcpServer()
def sendUltrasonic(self):
if self.sonic == True:
ADC_Ultrasonic = self.ultrasonic.get_distance()
if ADC_Ultrasonic == self.ultrasonic.get_distance():
try:
self.send(cmd.CMD_SONIC + "#" + str(ADC_Ultrasonic) + '\n')
except:
self.sonic = False
self.ultrasonicTimer = threading.Timer(0.13, self.sendUltrasonic)
self.ultrasonicTimer.start()
def sendLight(self):
if self.Light == True:
ADC_Light1 = self.adc.recvADC(0)
ADC_Light2 = self.adc.recvADC(1)
try:
self.send(cmd.CMD_LIGHT + '#' + str(ADC_Light1) + '#' +
str(ADC_Light2) + '\n')
except:
self.Light = False
self.lightTimer = threading.Timer(0.17, self.sendLight)
self.lightTimer.start()
def Power(self):
while True:
ADC_Power = self.adc.recvADC(2) * 3
time.sleep(3)
if ADC_Power < 6.8:
for i in range(4):
self.buzzer.run('1')
time.sleep(0.1)
self.buzzer.run('0')
time.sleep(0.1)
elif ADC_Power < 7:
for i in range(2):
self.buzzer.run('1')
time.sleep(0.1)
self.buzzer.run('0')
time.sleep(0.1)
else:
self.buzzer.run('0')
class SPUG:
def run(self):
self.Ultrasonic = Ultrasonic()
self.Pwm = Servo()
credentials = pika.PlainCredentials('newuser1', 'password')
connection = pika.BlockingConnection(
pika.ConnectionParameters('192.168.1.12', 5672, '/', credentials))
channel = connection.channel()
channel.exchange_declare(exchange='PC2Pi.topic',
exchange_type='topic',
durable=True,
auto_delete=False)
channel.queue_declare(queue='PC2Pi.Processed_Data')
channel.queue_bind(queue='PC2Pi.Processed_Data',
exchange='PC2Pi.topic',
routing_key='Processed_Data')
while True:
self.LMR = 0x00
if GPIO.input(IR01) == True:
self.LMR = (self.LMR | 4)
if GPIO.input(IR02) == True:
self.LMR = (self.LMR | 2)
if GPIO.input(IR03) == True:
self.LMR = (self.LMR | 1)
self.Pwm.setServoPwm('0', 90)
self.Pwm.setServoPwm('1', 90)
distance = self.Ultrasonic.get_distance()
IRSensor1 = (GPIO.input(IR01) == True)
IRSensor2 = (GPIO.input(IR02) == True)
IRSensor3 = (GPIO.input(IR03) == True)
if distance > 10:
if self.LMR == 2:
PWM.setMotorModel(575, 575, 575, 575)
elif self.LMR == 4:
PWM.setMotorModel(-1500, -1500, 2500, 2500)
elif self.LMR == 6:
PWM.setMotorModel(-2000, -2000, 4000, 4000)
elif self.LMR == 1:
PWM.setMotorModel(2500, 2500, -1500, -1500)
elif self.LMR == 3:
PWM.setMotorModel(4000, 4000, -2000, -2000)
elif self.LMR == 7:
pass
else:
PWM.setMotorModel(0, 0, 0, 0)
message1 = time.ctime() + " Distance to Obstacle: "+ str(distance)+ \
"%" + "IR Sensor1: %"+ str(IRSensor1)+"%"+ "IR Sensor2: %"+ str(IRSensor2)+"%" \
+ "IR Sensor3: %"+ str(IRSensor3)+"%"
channel.basic_publish(
exchange='Pi2PC.topic',
routing_key='Cart101.ShoppingMall.DistanceIRSensorVal',
body=message1)
print('Sent_1' + message1)
time.sleep(timeout)
class SPUG:
def Initialize_Values(self):
#Itial position
self.x_init = 0
self.y_init = 0
#Product Location Positions
self.x_pro_des = 3 #Test Initialization
self.y_pro_des = 3 #Test Initialization
#Intermediate Destination Positions
self.x_inter_des = 3 #Test Initialization
self.y_inter_des = 3 #Test Initialization
#Local Position of the SPUG
self.l_x = 0
self.l_y = 0
#SPUG Starts at Coordinate [0 ,0] facing North
self.Orientation = "North"
#SPUG Number
self.Cart_Number = 4
#Variable to calculate the Position of SPUG after movement
self.Moved_Straight_New = 0
self.Moved_Straight_Old = 0
self.Moved_Reverse_New = 0
self.Moved_Reverse_Old = 0
self.Moved_Left_New = 0
self.Moved_Left_Old = 0
self.Moved_Right_New = 0
self.Moved_Right_Old = 0
#Total moves made by SPUG
self.Total_Moves = 0
#Intermediate Position Reached by SPUG
self.Inter_Target_Reached = 0
#Product Location Reached by SPUG
self.Product_Des_Reached = 0
#SPUG presesnt at junction
self.Junction = 0
#SPUG at [0 ,0] coordinate:
# 0 - Present at [0,0] coordinate
# 1 - Not pressent at [0,0] coordinate
self.IntialJunction = 0
#Function to set the intermediate coordinates to navigate SPUG
def Set_intermediate_destnation_position(self, x_destination,
y_destnation):
self.x_inter_des = x_destination
self.y_inter_des = y_destnation
self.Inter_Target_Reached = 0
#Function to set the Product locations to message the server if the product location is reached
def Set_product_destnation_position(self, x_destination, y_destnation):
self.x_pro_des = x_destination
self.y_pro_des = y_destnation
self.Product_Des_Reached = 0
#Function to get the current coordinates of SPUG
def Get_curent_position(self):
return self.l_x, self.l_y
#Function to get if Intermediate position is reached
def Is_Intermediate_DestinTion_Reached(self):
return self.Inter_Target_Reached
#Function to get if Product location is reached
def Is_Product_DestinTion_Reached(self):
return self.Product_Des_Reached
#Function to update the coordinates and get the direction to move
def Mov_According_To_Specified_position(self):
#Intial Position Before Movement
self.x_init = self.l_x
self.y_init = self.l_y
#Do not update the coordinates for the first junction
if (self.IntialJunction != 0):
Coordinates_Updated = 0
#Code for updating the co-ordinates
if (self.Moved_Straight_New != self.Moved_Straight_Old):
if (self.Orientation == "North"):
self.l_x = self.l_x
self.l_y = self.l_y + 1
elif (self.Orientation == "South"):
self.l_x = self.l_x
self.l_y = self.l_y - 1
elif (self.Orientation == "East"):
self.l_x = self.l_x + 1
self.l_y = self.l_y
elif (self.Orientation == "West"):
self.l_x = self.l_x - 1
self.l_y = self.l_y
Coordinates_Updated = 1
elif (self.Moved_Reverse_New != self.Moved_Reverse_Old):
if (self.Orientation == "North"):
self.l_x = self.l_x
self.l_y = self.l_y - 1
elif (self.Orientation == "South"):
self.l_x = self.l_x
self.l_y = self.l_y + 1
elif (self.Orientation == "East"):
self.l_x = self.l_x - 1
self.l_y = self.l_y
elif (self.Orientation == "West"):
self.l_x = self.l_x + 1
self.l_y = self.l_y
Coordinates_Updated = 1
elif (self.Moved_Left_New != self.Moved_Left_Old):
if (self.Orientation == "North"):
self.l_x = self.l_x - 1
self.l_y = self.l_y
self.Orientation = 'West'
elif (self.Orientation == "South"):
self.l_x = self.l_x + 1
self.l_y = self.l_y
self.Orientation = 'East'
elif (self.Orientation == "East"):
self.l_x = self.l_x
self.l_y = self.l_y + 1
self.Orientation = 'North'
elif (self.Orientation == "West"):
self.l_x = self.l_x
self.l_y = self.l_y - 1
self.Orientation = 'South'
Coordinates_Updated = 1
elif (self.Moved_Right_New != self.Moved_Right_Old):
if (self.Orientation == "North"):
self.l_x = self.l_x + 1
self.l_y = self.l_y
self.Orientation = 'East'
elif (self.Orientation == "South"):
self.l_x = self.l_x - 1
self.l_y = self.l_y
self.Orientation = 'West'
elif (self.Orientation == "East"):
self.l_x = self.l_x
self.l_y = self.l_y - 1
self.Orientation = 'South'
elif (self.Orientation == "West"):
self.l_x = self.l_x
self.l_y = self.l_y + 1
self.Orientation = 'North'
Coordinates_Updated = 1
if (Coordinates_Updated != 0):
self.Total_Moves = self.Total_Moves + 1
self.Moved_Straight_New = self.Moved_Straight_Old
self.Moved_Reverse_New = self.Moved_Reverse_Old
self.Moved_Left_New = self.Moved_Left_Old
self.Moved_Right_New = self.Moved_Right_Old
#Calculate the next move
if (self.Orientation == "North"):
if ((self.y_inter_des - self.l_y) > 0):
self.Moved_Straight_New = self.Moved_Straight_Old + 1
ret = 5 #Straight
elif ((self.x_inter_des - self.l_x) > 0):
self.Moved_Right_New = self.Moved_Right_Old + 1
ret = 9 #Right
elif ((self.x_inter_des - self.l_x) < 0):
self.Moved_Left_New = self.Moved_Left_Old + 1
ret = 1 #Left
elif ((self.y_inter_des - self.l_y) < 0):
self.Moved_Reverse_New = self.Moved_Reverse_Old + 1
ret = 11 #Reverse
elif ((self.y_inter_des == self.l_y)
and (self.x_inter_des == self.l_x)):
ret = 0 #Stop
elif (self.Orientation == "South"):
if ((self.y_inter_des - self.l_y) < 0):
self.Moved_Straight_New = self.Moved_Straight_Old + 1
ret = 5 #Straight
elif ((self.x_inter_des - self.l_x) < 0):
self.Moved_Right_New = self.Moved_Right_Old + 1
ret = 9 #Right
elif ((self.x_inter_des - self.l_x) > 0):
self.Moved_Left_New = self.Moved_Left_Old + 1
ret = 1 #Left
elif ((self.y_inter_des - self.l_y) > 0):
self.Moved_Reverse_New = self.Moved_Reverse_Old + 1
ret = 11 #Reverse
elif ((self.y_inter_des == self.l_y)
and (self.x_inter_des == self.l_x)):
ret = 0 #Stop
elif (self.Orientation == "East"):
if ((self.x_inter_des - self.l_x) > 0):
self.Moved_Straight_New = self.Moved_Straight_Old + 1
ret = 5 #Straight
elif ((self.y_inter_des - self.l_y) < 0):
self.Moved_Right_New = self.Moved_Right_Old + 1
ret = 9 #Right
elif ((self.y_inter_des - self.l_y) > 0):
self.Moved_Left_New = self.Moved_Left_Old + 1
ret = 1 #Left
elif ((self.x_inter_des - self.l_x) < 0):
self.Moved_Reverse_New = self.Moved_Reverse_Old + 1
ret = 11 #Reverse
elif ((self.y_inter_des == self.l_y)
and (self.x_inter_des == self.l_x)):
ret = 0 #Stop
elif (self.Orientation == "West"):
if ((self.x_inter_des - self.l_x) < 0):
self.Moved_Straight_New = self.Moved_Straight_Old + 1
ret = 5 #Straight
elif ((self.y_inter_des - self.l_y) > 0):
self.Moved_Right_New = self.Moved_Right_Old + 1
ret = 9 #Right
elif ((self.y_inter_des - self.l_y) < 0):
self.Moved_Left_New = self.Moved_Left_Old + 1
ret = 1 #Left
elif ((self.x_inter_des - self.l_x) > 0):
self.Moved_Reverse_New = self.Moved_Reverse_Old + 1
ret = 11 #Reverse
elif ((self.y_inter_des == self.l_y)
and (self.x_inter_des == self.l_x)):
ret = 0 #Stop
#Set the inital junction value to 1
self.IntialJunction = 1
#Update if the intermediate position is reached
if ((self.y_inter_des == self.l_y) and (self.x_inter_des == self.l_x)):
self.Inter_Target_Reached = 1
else:
self.Inter_Target_Reached = 0
#Update if the product location coordinate is reached
if ((self.y_pro_des == self.l_y) and (self.x_pro_des == self.l_x)):
self.Product_Des_Reached = 1
else:
self.Product_Des_Reached = 0
#------------------------------------------------------------------------------------------------------------------------
#--------------------Print data for debugging-------------------------------------------------------------------------
print("Orientation - %s" % self.Orientation)
print("X Coordinate - %d " % self.l_x)
print("Y Coordinate - %d " % self.l_y)
print("Intermediate Destination Reached - %d " %
self.Inter_Target_Reached)
print("Product Destination Reached - %d " % self.Product_Des_Reached)
print("Total Moves - %d " % self.Total_Moves)
#------------------------------------------------------------------------------------------------------------------------
#----------------------Write data into Google Firebase Cloud Database-------------------------------------------------
# Create a dictionary to store the data before sending to the database
data_to_upload = {
'Time': time.ctime(),
'Current X Coordinate': self.l_x,
'Current Y Coordinate': self.l_y,
'Intermed Destination X': self.x_inter_des,
'Intermed Destination Y': self.y_inter_des,
'Product Destination X': self.x_pro_des,
'Product Destination Y': self.y_pro_des,
'Movement': ret,
'Orientation': self.Orientation,
'Intermed Destination Reached': self.Inter_Target_Reached,
'Product Destination Reached': self.Product_Des_Reached,
'Total Moves': self.Total_Moves
}
#Post the data to the appropriate folder/branch within your database
result = FBConn.post('/Cart4/SPUG_PI2PC_Coordinates_Data/',
data_to_upload)
#------------------------------------------------------------------------------------------------------------------------
#----------------------------------------Send MQTT Path Occupy Message-------------------------------------------------
client = mqtt.Client("RaspBerry_PI_1") #create new instance
client.connect('192.168.1.9', 1883, 70) #connect to broker
client.loop_start()
message1 = {
"X": str(self.x_inter_des),
"Y": str(str(self.y_inter_des))
}
msg1 = json.dumps(message1)
client.publish("pointOccupy/", msg1, 2)
time.sleep(0.1)
print("Point Occupy message Sent from Pi :" + time.ctime() + " " +
msg1)
#------------------------------------------------------------------------------------------------------------------------
#----------------------------------------Send MQTT Path Un-Occupy Message-------------------------------------------------
if (self.Is_Intermediate_DestinTion_Reached()):
client = mqtt.Client("RaspBerry_PI_2") #create new instance
client.connect('192.168.1.9', 1883, 70) #connect to broker
client.loop_start()
message2 = {
"X": str(self.x_inter_des),
"Y": str(str(self.y_inter_des))
}
msg2 = json.dumps(message2)
client.publish("pointUnoccupy/", msg2, 2)
time.sleep(0.1)
print("Point Un-Occupy message Sent from Pi :" + time.ctime() +
" " + msg2)
#-------------------------------------------------------------------------------------------------------------------------
#----------------------------------------Send MQTT Item Purchased Message-------------------------------------------------
if (self.Is_Product_DestinTion_Reached()):
client = mqtt.Client("RaspBerry_PI_3") #create new instance
client.connect('192.168.1.9', 1883, 70) #connect to broker
client.loop_start()
message3 = { "itemPurchasedX" : str(self.l_x), "itemPurchasedY" : str(self.l_y), \
"cartName" : "cart"+str(self.Cart_Number)}
msg3 = json.dumps(message3)
client.publish("item/", msg3, 2)
time.sleep(0.1)
print("Item Purchased message Sent from Pi :" + time.ctime() +
" " + msg3)
self.LED.ledIndex(0xFF, 0, 0, 0) # Switch off all LED's
self.Buzzer.run('1')
time.sleep(0.5)
self.Buzzer.run('0')
#---------------------------------------------------------------------------------------------------------------------
#Return Value from the Calculations
return int(ret)
def Move_Cart(self, l_Movement_Type):
if (l_Movement_Type == 5):
self.LED.ledIndex(0xFF, 0, 0, 0) #Turn Off
self.LED.ledIndex(0xF0, 0, 255, 0) #Green
#PWM.setMotorModel(800,800,800,800)
#time.sleep(0.1)
#PWM.setMotorModel(0,0,0,0)
elif (l_Movement_Type == 1):
self.LED.ledIndex(0xFF, 0, 0, 0) #Turn Off
self.LED.ledIndex(0x3C, 0, 255, 0) #Green
#PWM.setMotorModel(-2000,-2000,4000,4000)
#time.sleep(0.2)
elif (l_Movement_Type == 3):
self.LED.ledIndex(0xFF, 0, 0, 0) #Turn Off
self.LED.ledIndex(0x3C, 0, 0, 0) #Green
#PWM.setMotorModel(-1200,-1200,2000,2000)
#time.sleep(0.2)
elif (l_Movement_Type == 7):
self.LED.ledIndex(0xFF, 0, 0, 0) #Turn Off
self.LED.ledIndex(0xC3, 0, 0, 0) #Green
#PWM.setMotorModel(2000,2000,-1200,-1200)
#time.sleep(0.2)
elif (l_Movement_Type == 9):
self.LED.ledIndex(0xFF, 0, 0, 0) #Turn Off
self.LED.ledIndex(0xC3, 0, 255, 0) #Green
#PWM.setMotorModel(4000,4000,-2000,-2000)
#time.sleep(0.5)
elif (l_Movement_Type == 11):
self.LED.ledIndex(0xFF, 0, 0, 0) #Turn Off
self.LED.ledIndex(0x0F, 0, 255, 0) #Green
#PWM.setMotorModel(-600,-600,-600,-600)
elif (l_Movement_Type == 0):
time.sleep(0.001)
#PWM.setMotorModel(0,0,0,0)
def Run_Cart4(self):
#------------------------------------------Ultrasonic sensor and Servo Motor Class
self.Ultrasonic = Ultrasonic()
self.LED = Led()
self.Buzzer = Buzzer()
self.Junction = 0
#--------------------------------------------------------------------------------------
while True:
self.Movement_Type = 0
Distance = self.Ultrasonic.get_distance()
IR_Left = GPIO.input(IR01)
IR_Mid = GPIO.input(IR02)
IR_Right = GPIO.input(IR03)
if (Distance > 15.0):
if ((IR_Left == 0) and (IR_Mid == 1) and (IR_Right == 0)):
if (self.Moved_Reverse_New != self.Moved_Reverse_Old):
self.Movement_Type = 11 #Move Reverse
else:
self.Movement_Type = 5 #Move Forward
self.Junction = 0 #Clear the Junction Variable
elif ((IR_Left == 1) and (IR_Mid == 0) and (IR_Right == 0)):
self.Movement_Type = 3 #Take Slight Left
elif ((IR_Left == 1) and (IR_Mid == 1) and (IR_Right == 0)):
self.Movement_Type = 3 #Take Slight Left
elif ((IR_Left == 0) and (IR_Mid == 0) and (IR_Right == 1)):
self.Movement_Type = 7 #Take Slight Right
elif ((IR_Left == 0) and (IR_Mid == 1) and (IR_Right == 1)):
self.Movement_Type = 7 #Take Slight Right
elif ((IR_Mid == 1) and (IR_Left == 1) and (IR_Right == 1)):
if (self.Junction == 0):
self.Movement_Type = self.Mov_According_To_Specified_position(
)
self.Junction = 1
else:
pass
else:
self.Movement_Type = 0 #Stop Movement
self.LED.ledIndex(0x20, 255, 125, 0) #Orange
self.LED.ledIndex(0x40, 255, 125, 0) #Orange
time.sleep(1)
self.LED.ledIndex(0x60, 0, 0, 0)
#Actuate the motors to move cart to the specific Coordinate
self.Move_Cart(self.Movement_Type)
if (self.Is_Intermediate_DestinTion_Reached()):
break
def dist():
ultrasonic = Ultrasonic() # create an object
data = ultrasonic.get_distance() # Get the value
print("Obstacle distance is " + str(data) + "CM")
return data
def run_ultrasonic(self):
IR01 = 14
IR02 = 15
IR03 = 23
GPIO.setmode(GPIO.BCM)
GPIO.setup(IR01, GPIO.IN)
GPIO.setup(IR02, GPIO.IN)
GPIO.setup(IR03, GPIO.IN)
self.LMR0 = 0x00
if GPIO.input(IR01) == False:
self.LMR0 = (self.LMR0 | 4)
if GPIO.input(IR02) == False:
self.LMR0 = (self.LMR0 | 2)
if GPIO.input(IR03) == False:
self.LMR0 = (self.LMR0 | 1)
if self.LMR0 > 0:
self.LMR0 = 7
ttable = [[1, 0], [2, 4], [3, 5], [1, 1], [0, 0], [0, 0]]
x = 40
ultra = Ultrasonic()
print "Auto drive Start!"
cur_state = 0
while self.automode:
if cur_state == 0:
self.display.show(1, "FORWARD")
self.PWM.slowforward()
ultra.look_forward()
elif cur_state == 1:
self.display.show(1, "LOOKLEFT")
self.PWM.stopMotor()
ultra.look_left()
elif cur_state == 2:
self.display.show(1, "LOOKRITE")
self.PWM.stopMotor()
ultra.look_right()
elif cur_state == 3:
self.display.show(1, "GOBACK")
self.PWM.backup()
elif cur_state == 4:
self.display.show(1, "TURNLEFT")
self.PWM.turnLeft()
time.sleep(0.2)
elif cur_state == 5:
self.display.show(1, "TURNRITE")
self.PWM.turnRight()
time.sleep(0.2)
else:
print "Wrong state?"
cur_state = 0
time.sleep(0.1)
d = ultra.get_distance()
self.LMR = 0x00
if GPIO.input(IR01) == False:
self.LMR = (self.LMR | 4)
if GPIO.input(IR02) == False:
self.LMR = (self.LMR | 2)
if GPIO.input(IR03) == False:
self.LMR = (self.LMR | 1)
if (self.LMR <> self.LMR0):
cur_state = 3
else:
e = 0 if d < x else 1
cur_state = ttable[cur_state][e]
print "Auto drive End!"