def __init__(self):
self.tcp_flag = False
self.led = Led()
self.adc = ADS7830()
self.servo = Servo()
self.buzzer = Buzzer()
self.control = Control()
self.sonic = Ultrasonic()
self.control.Thread_conditiona.start()
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)
"""
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 Execute(self):
self.count = 0 # counts the number of reading taken
self.ResetHeightArray() # set all values of the array to 0
while (
self.loop
): #Loop continues to iterate until mean value of reading is < 25cm
time.sleep(0.5)
self.reading = Ultrasonic.getReading2() # reads height from ground
if self.ValidateReading(): # if 13cm < reading <= 200cm
self.count += 1 #increament the count
for i in range(self.windowLength - 1):
self.heightArray[i] = self.heightArray[
i + 1] #arrange value in the FIFO array
self.heightArray[
self.windowLength -
1] = self.reading #attach value at the tail of the array
if self.count >= 10: # after ten readings
self.count = 0 # reset the count value
self.mean = np.mean(
self.heightArray) # take the mean value of the arrray
print("Mean height: ", self.mean)
if self.mean >= 14 and self.mean <= 25: # if 14cm <= mean value <= 25 cm
self.nextState = "Landing" # set the next state
self.loop = False # break the loop
self.Exit()
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')
Data_Copy = open('Data_PI.csv', 'w')
Data_Copy.write("Date;Movement_Type;\n")
while True:
def Write_into_CSV(body):
x = format(body).split("%")
Data_Copy = open('Data_PI.csv', 'a')
Movement_Type = int(x[1])
Data_Copy.write("%s;"%time.ctime())
Data_Copy.write("%d;"%Movement_Type)
Data_Copy.close()
if(Movement_Type == 5):
PWM.setMotorModel(600,600,600,600)
elif(Movement_Type == 1):
PWM.setMotorModel(-2000,-2000,4000,4000)
elif(Movement_Type == 3):
PWM.setMotorModel(-1200,-1200,2000,2000)
elif(Movement_Type == 7):
PWM.setMotorModel(2000,2000,-1200,-1200)
elif(Movement_Type == 9):
PWM.setMotorModel(4000,4000,-2000,-2000)
elif(Movement_Type == 0):
PWM.setMotorModel(0,0,0,0)
def callback(ch, method, properties, body):
print('Received from PC: {}'.format(body))
Write_into_CSV(body)
channel.basic_consume(queue='PC2Pi.Processed_Data', on_message_callback=callback, auto_ack=True)
print('Waiting for Messages from PC')
channel.start_consuming()
def __init__(self):
self._interrupted = False
self._lock = threading.RLock()
self._cmdThread = None
self.motor = Motor.Motor()
self.sonic = Ultrasonic.Ultrasonic()
self.adc = ADC.Adc()
self.servo = servo.Servo()
self.stop()
self.pointSonic()
def __init__(self, sonicTrig, sonicEcho, servoChannel):
self.trigger = sonicTrig
self.echo = sonicEcho
self.servoChannel = servoChannel
if self._DEBUG:
print self._DEBUG_INFO, "Debug on"
self.servo = Servo.Servo(servoChannel, self.servoStart,
self.servoStart - 1, self.servoStop)
self.ultra = Ultrasonic.Ultrasonic(sonicTrig, sonicEcho)
def getAverageDistance( count, sleepMs ):
total = 0
for i in range(count):
dist = Ultrasonic.getDistance()
total += dist
print(">",dist)
if ( i < count-1 ):
time.sleep(sleepMs)
return total / count
def setup():
global SerialPort1
global SerialPort2
global SerialPort3
global LeftSensor
global CenterSensor
global RightSensor
SerialPort1 = '/dev/ttyUSB0'
SerialPort2 = '/dev/ttyUSB1' # run ls/dev/tty* to see which usb ports it is connected to.
SerialPort3 = '/dev/ttyUSB2'
LeftSensor = Ultrasonic.MySensor(SerialPort1)
CenterSensor = Ultrasonic.MySensor(SerialPort2)
RightSensor = Ultrasonic.MySensor(SerialPort3)
LeftSensor.start()
CenterSensor.start()
RightSensor.start()
Motors.still()
os.system("flite -t 'Delta Task started'")
def rc_alt(self, target_alt):
"""
Will send copter based off of throttle to 'target_alt'
"""
#Calculate delta t and set previous time ot current time
current_time = ((time.time() - self.timer) * 10)
delta_t = current_time - self.previous_time_alt
self.previous_time_alt = current_time
#Get error
#Changed to not use vicon positioning
self.distance = ultra.get_dist()
if self.distance > 3000:
self.distance = 200
else:
self.distance = self.distance
self.error_alt= target_alt - self.distance
logging.debug('ultrasonic distance is %f' %self.distance)
#print('ultrasonic distance is %f' %self.distance)
#Get error I
if abs(self.error_alt) < 1000:
self.I_error_alt += self.error_alt * delta_t
else:
pass
#Get error D
if self.previous_error_alt == None:
self.previous_error_alt = self.error_alt
#Check to make sure error and previous error are not the same so D is not 0
if self.error_alt != self.previous_error_alt:
self.D_error_alt = (self.error_alt-self.previous_error_alt)/delta_t
self.previous_error_alt = self.error_alt
#filter for D values
if self.D_error_alt != self.filter_value(5000,-5000,self.D_error_alt):
self.vidro.set_rc_throttle(self.vidro.base_rc_throttle)
self.P_error_alt = 0
self.I_error_alt = 0
return
#Send RC value
self.vidro.set_rc_throttle(round(self.vidro.base_rc_throttle + self.error_alt*self.alt_K_P + self.I_error_alt*self.alt_K_I + self.D_error_alt*self.alt_K_D))
return self.error_alt
def VerifyStatus(self):
while (True):
time.sleep(self.duration / self.windowLength)
self.reading = Ultrasonic.getReading2()
if self.ValidateReading():
self.count += 1
for i in range(self.windowLength - 1):
self.heightArray[i] = self.heightArray[i + 1]
self.heightArray[self.windowLength - 1] = self.reading
if self.count >= self.windowLength:
self.count = 0
self.mean = np.mean(self.heightArray)
print("mean height: ", self.mean)
if self.mean >= 14 and self.mean <= 18:
return True
else:
print("Go back to state Air")
return False
def run():
print("The car start running...")
ultra = ul.UL()
shuttle = shu.SHU()
f, r = shuttle.find()
print(len(f), len(f[0]))
shuttle.show(f, [])
try:
while True:
frame, rect = shuttle.find()
dist = ultra.dist()
shuttle.show(frame, rect)
print('Distance: {}cm'.format(dist),
". Found: {} units".format(len(rect)))
print(rect)
if dist <= 20:
print('Run into blank wall')
mo.backward()
mo.belt()
time.sleep(0.06)
else:
if len(rect):
mo.setmode()
mo.forward()
mo.brush()
mo.belt()
time.sleep(2)
mo.stop_wheels()
mo.stop_brush()
print('Finish collect!')
time.sleep(2)
mo.stop_belt()
print('Finish transmit!')
else:
mo.setmode()
mo.stop_all()
#print('I could not find shuttlecocks!')
except KeyboardInterrupt:
GPIO.cleanup()
shuttle.close()
def Execute(self):
self.count = 0
self.ResetHeightArray()
while (self.loop):
time.sleep(0.5)
self.reading = Ultrasonic.getReading2()
if self.ValidateReading():
self.count += 1
for i in range(self.windowLength - 1):
self.heightArray[i] = self.heightArray[i + 1]
self.heightArray[self.windowLength - 1] = self.reading
if self.count >= 10:
self.count = 0
self.mean = np.mean(self.heightArray)
print("Mean height: ", self.mean)
if self.mean >= 14 and self.mean <= 25:
self.nextState = "Landing"
self.loop = False
self.Exit()
def HowClose(self):
page = 0
while(True):
time.sleep(1)
self.readDistance = Ultrasonic.getReading1()
if self.ValidateReading():
page += 1
for iteration in range(self.window):
self.distanceArray[iteration] = self.distanceArray[iteration+1]
self.distanceArray[self.window] = self.readDistance
print("distanceArray: ", self.distanceArray)
if (page >= 10):
page = 0
self.averageDistance = np.mean(self.distanceArray)
if self.averageDistance <= 18 and self.averageDistance >= 11:
self.isCloseEnough = True
else:
self.isCloseEnough = False
print("self.averageDistance: ", self.averageDistance)
print("self.isCloseEnough: ", self.isCloseEnough)
break
def Execute(self):
page = 0
self.ResetHeightArray()
while (True):
time.sleep(0.5)
self.height = Ultrasonic.getReading2()
if self.ValidateReading():
page += 1
for i in range(self.windowLength - 1):
self.heightArray[i] = self.heightArray[i + 1]
self.heightArray[self.windowLength - 1] = self.height
print("Height: ", self.heightArray)
if (page >= 10):
page = 0
self.meanHeight = np.mean(self.heightArray)
self.meanHeight = round(self.meanHeight, 0)
print("self.meanHeight: ", self.meanHeight)
if self.meanHeight >= 25:
print("self.meanHeight: ", self.meanHeight)
break
self.Exit()
def HowClose(self):
page = 0
while (True):
time.sleep(1) # reading is taken every second
self.readDistance = Ultrasonic.getReading1()
if self.ValidateReading(): #true if 14 <= reading <= 200cm
page += 1 # increaments after each reading
for iteration in range(self.window):
self.distanceArray[iteration] = self.distanceArray[
iteration + 1]
self.distanceArray[self.window] = self.readDistance
print("distanceArray: ", self.distanceArray)
if (page >= 10): # after 10 readings
page = 0 # reset the number of counted reading
self.averageDistance = np.mean(self.distanceArray)
if self.averageDistance <= 15 and self.averageDistance >= 1: # 1 cm to !5cm then object is close enough
self.isCloseEnough = True
else:
self.isCloseEnough = False
print("self.averageDistance: ", self.averageDistance)
print("self.isCloseEnough: ", self.isCloseEnough)
break
def Execute(self):
page = 0
self.ResetHeightArray(
) #when this state initiates reset the height array
while (True):
time.sleep(0.5) # take a reading after half a second break
self.height = Ultrasonic.getReading2() # take reading
if self.ValidateReading():
page += 1
for i in range(self.windowLength - 1):
self.heightArray[i] = self.heightArray[i + 1]
self.heightArray[self.windowLength - 1] = self.height
print("Height: ", self.heightArray)
if (page >= 10): # after ten validated readings
page = 0
self.meanHeight = np.mean(self.heightArray) # average height
self.meanHeight = round(self.meanHeight, 0)
print("self.meanHeight: ", self.meanHeight)
if self.meanHeight >= 25: #if height > 25cm then drone is on air
print("self.meanHeight: ", self.meanHeight)
break
self.Exit()
def VerifyStatus(self): # reassures the drone is on ground
while (True): # iterate until 10 readings are taken
time.sleep(self.duration /
self.windowLength) # sleep time is 0.3 se
self.reading = Ultrasonic.getReading2()
if self.ValidateReading(): # makes sure the reading is credible
self.count += 1
for i in range(self.windowLength - 1):
self.heightArray[i] = self.heightArray[
i + 1] # rearrange the FIFO array
self.heightArray[
self.windowLength -
1] = self.reading #new value is tailed at the array
if self.count >= self.windowLength:
self.count = 0
self.mean = np.mean(self.heightArray)
print("mean height: ", self.mean)
if self.mean >= 14 and self.mean <= 18: #if 14 cm <= meanvalue <= 18cm then drone is firmly rested on ground
return True
else:
print("Go back to state Air"
) # returns back to the previous state
return False
import Ultrasonic
import time
SerialPort1 = '/dev/ttyUSB0' # Type ls/dev/tty in the terminal then tab to see which usb ports are connected be default, 0-2 will be for US.
#SerialPort2 = '/dev/ttyUSB1'
#SerialPort3 = '/dev/ttyUSB2'
LeftSensor = Ultrasonic.MySensor(SerialPort1)
#CenterSensor = Ultrasonic.MySensor(SerialPort2)
#RightSensor = Ultrasonic.MySensor(SerialPort3)
LeftSensor.start()
#CenterSensor.start()
#RightSensor.start()
while 1:
print(
'Left Sensor ', LeftSensor.getLastEvent()
) #'\t Center Sensor ', CenterSensor.getLastEvent(), '\t Right Sensor ', RightSensor.getLastEvent())
# LeftSensor.stop stops the reading for the Left Sensor
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")
parser.add_argument("--closeLimit", type=float, default=3, help="close limit in feet ")
parser.add_argument("--farLimit", type=float, default=6, help="far limit in feet")
args = parser.parse_args()
io.setwarnings(False)
io.setmode(io.BCM)
trigger = 5
echo = 6
red = 26
green = 19
blue = 13
beeper = 21
Ultrasonic.initialize(trigger, echo )
io.setup( beeper, io.OUT )
io.setup( red, io.OUT )
io.setup( green, io.OUT )
io.setup( blue, io.OUT )
_averageCount = 3 # how many distance reading to average
_sleepSec = .100
################ MAIN
io.output( trigger, io.LOW )
time.sleep(.5) # let it settle
try:
ADC.setup()
#Setup Logging
logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)
handler = logging.FileHandler(filename = 'logs/ultra1.log', mode = 'w')
handler.setLevel(logging.INFO)
logger.addHandler(handler)
logger.info('Starting Log')
#Run Forever
while(1):
#Take a reading
distance = ultra.get_dist()
error = 1000 - distance
#Print and Log Distance values
#print "Distance is %f" %distance
logging.info('Distance is %f' %distance)
#Print and Log Error values
#print "Error is %f" %error
logging.info('Error is %f' %error)
#Sleep so that we take readings at 20Hz to prevent terminal from stalling
time.sleep(0.05)
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)
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
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
from Ultrasonic import * #import Led
ultrasonic=Ultrasonic() #create an object
data=ultrasonic.get_distance() #Get the value
print ("Obstacle distance is "+str(data)+"CM")
from servo import * #import Led
pwm = Servo() #create an object
#Servo rotates from 30 degrees to 150 degrees
for i in range(30, 150, 1) :
pwm.setServoPwm('0', i)
time.sleep(0.01)
#Servo rotates from 150 degrees to 0 degrees
for i in range(150, 30, -1) :
pwm.setServoPwm('0', i)
time.sleep(0.01)
from Motor import * #import Motor
PWM=Motor() #create an object
PWM.setMotorModel(2000,2000,2000,2000) #Forward
time.sleep(3) #waiting 3 second
PWM.setMotorModel(0,0,0,0) #Stop
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 Server:
def __init__(self):
self.tcp_flag = False
self.led = Led()
self.adc = ADS7830()
self.servo = Servo()
self.buzzer = Buzzer()
self.control = Control()
self.sonic = Ultrasonic()
self.control.Thread_conditiona.start()
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 turn_on_server(self):
#ip adress
HOST = self.get_interface_ip()
#Port 8002 for video transmission
self.server_socket = socket.socket()
self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT,
1)
self.server_socket.bind((HOST, 8002))
self.server_socket.listen(1)
#Port 5002 is used for instruction sending and receiving
self.server_socket1 = socket.socket()
self.server_socket1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT,
1)
self.server_socket1.bind((HOST, 5002))
self.server_socket1.listen(1)
print('Server address: ' + HOST)
def turn_off_server(self):
try:
self.connection.close()
self.connection1.close()
except:
print('\n' + "No client connection")
def reset_server(self):
self.turn_off_server()
self.turn_on_server()
self.video = threading.Thread(target=self.transmission_video)
self.instruction = threading.Thread(target=self.receive_instruction)
self.video.start()
self.instruction.start()
def send_data(self, connect, data):
try:
connect.send(data.encode('utf-8'))
#print("send",data)
except Exception as e:
print(e)
def transmission_video(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
camera.saturation = 80 # Set image video saturation
camera.brightness = 50 # Set the brightness of the image (50 indicates the state of white balance)
#camera.iso = 400
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()
lengthBin = struct.pack('L', len(b))
self.connection.write(lengthBin)
self.connection.write(b)
stream.seek(0)
stream.truncate()
except BaseException as e:
#print (e)
print("End transmit ... ")
break
except BaseException as e:
#print(e)
print("Camera unintall")
def receive_instruction(self):
try:
self.connection1, self.client_address1 = self.server_socket1.accept(
)
print("Client connection successful !")
except:
print("Client connect failed")
self.server_socket1.close()
while True:
try:
allData = self.connection1.recv(1024).decode('utf-8')
except:
if self.tcp_flag:
self.reset_server()
break
else:
break
if allData == "" and self.tcp_flag:
self.reset_server()
break
else:
cmdArray = allData.split('\n')
print(cmdArray)
if cmdArray[-1] != "":
cmdArray == cmdArray[:-1]
for oneCmd in cmdArray:
data = oneCmd.split("#")
if data == None or data[0] == '':
continue
elif cmd.CMD_BUZZER in data:
self.buzzer.run(data[1])
elif cmd.CMD_POWER in data:
batteryVoltage = self.adc.batteryPower()
command = cmd.CMD_POWER + "#" + str(
batteryVoltage[0]) + "#" + str(
batteryVoltage[1]) + "\n"
#print(command)
self.send_data(self.connection1, command)
if batteryVoltage[0] < 5.5 or batteryVoltage[1] < 6:
for i in range(3):
self.buzzer.run("1")
time.sleep(0.15)
self.buzzer.run("0")
time.sleep(0.1)
elif cmd.CMD_LED in data:
try:
stop_thread(thread_led)
except:
pass
thread_led = threading.Thread(target=self.led.light,
args=(data, ))
thread_led.start()
elif cmd.CMD_LED_MOD in data:
try:
stop_thread(thread_led)
#print("stop,yes")
except:
#print("stop,no")
pass
thread_led = threading.Thread(target=self.led.light,
args=(data, ))
thread_led.start()
elif cmd.CMD_SONIC in data:
command = cmd.CMD_SONIC + "#" + str(
self.sonic.getDistance()) + "\n"
self.send_data(self.connection1, command)
elif cmd.CMD_HEAD in data:
if len(data) == 3:
self.servo.setServoAngle(int(data[1]), int(data[2]))
elif cmd.CMD_CAMERA in data:
if len(data) == 3:
x = self.control.restriction(int(data[1]), 50, 180)
y = self.control.restriction(int(data[2]), 0, 180)
self.servo.setServoAngle(0, x)
self.servo.setServoAngle(1, y)
elif cmd.CMD_RELAX in data:
#print(data)
if self.control.relax_flag == False:
self.control.relax(True)
self.control.relax_flag = True
else:
self.control.relax(False)
self.control.relax_flag = False
elif cmd.CMD_SERVOPOWER in data:
if data[1] == "0":
GPIO.output(self.control.GPIO_4, True)
else:
GPIO.output(self.control.GPIO_4, False)
else:
self.control.order = data
self.control.timeout = time.time()
try:
stop_thread(thread_led)
except:
pass
try:
stop_thread(thread_sonic)
except:
pass
print("close_recv")
stop = True
while True:
event = gamepad.read_one()
if (event):
print(event)
stop = False
if event.type == ecodes.EV_ABS:
skip2 += 1
if (skip2 == 1):
analog = categorize(event)
axis = ecodes.bytype[analog.event.type][analog.event.code]
value = analog.event.value
if (axis == "ABS_Y"):
if (value == 0):
print("right")
Ultrasonic.rt()
if (value == 2):
print("left")
Ultrasonic.lt()
if (axis == "ABS_X"):
if (value == 0):
print("up")
Ultrasonic.fd()
if (value == 2):
print("down")
Ultrasonic.bd()
if (skip2 == 2):
skip2 = 0
elif event.type == ecodes.EV_KEY:
print(event.code)
time.sleep(1)
PWM.setMotorModel(-1500, -1500, 2000, 2000) #Left
print "The car is turning left"
time.sleep(1)
PWM.setMotorModel(2000, 2000, -1500, -1500) #Right
print "The car is turning right"
time.sleep(1)
PWM.setMotorModel(0, 0, 0, 0) #Stop
print "\nEnd of program"
except KeyboardInterrupt:
PWM.setMotorModel(0, 0, 0, 0)
print "\nEnd of program"
from Ultrasonic import *
ultrasonic = Ultrasonic()
def test_Ultrasonic():
try:
pwm = Servo()
pwm.setServoPwm('0', 90)
while True:
data = ultrasonic.get_distance() #Get the value
print("Obstacle distance is " + str(data) + "CM")
time.sleep(1)
except KeyboardInterrupt:
print "\nEnd of program"
from Line_Tracking import *
import Ultrasonic
import time
SerialPort1 = '/dev/ttyUSB0' # Type ls/dev/tty in the terminal then tab to see which usb ports are connected be default, 0-2 will be for US.
SerialPort2 = '/dev/ttyUSB1'
SerialPort3 = '/dev/ttyUSB2'
LeftSensor = Ultrasonic.MySensor(SerialPort1)
CenterSensor = Ultrasonic.MySensor(SerialPort2)
RightSensor = Ultrasonic.MySensor(SerialPort3)
LeftSensor.start()
CenterSensor.start()
RightSensor.start()
while 1 :
print('Left Sensor ', LeftSensor.getLastEvent(), '\t Center Sensor ', CenterSensor.getLastEvent(), '\t Right Sensor ', RightSensor.getLastEvent())
time.sleep(1)
# LeftSensor.stop stops the reading for the Left Sensor