## ----------------------------------------------------------------------------------------------------------
## TEMPLATE
## Please DO NOT change the naming convention within this template. Some changes may
## lead to your program not functioning as intended.
import sys
sys.path.append('../')
from Common_Libraries.p2_sim_lib import *
import os
from Common_Libraries.repeating_timer_lib import repeating_timer
def update_sim():
try:
arm.ping()
except Exception as error_update_sim:
print (error_update_sim)
arm = qarm()
update_thread = repeating_timer(2, update_sim)
#Name: Michael Bradshaw - bradsm1
#Student #: 400362936
#Last date I worked: December 2nd, 2021
#Description: A program that allows user to control the arm, gripper, and opening-closing a Autoclave uses ECG sensors.
#Version: 7.0
#Name: Muhammad Elahi - elahim5
#Student #: 400368151
#Last date I worked: December 2nd, 2021
#Description: A program that allows user to control the arm, gripper, and opening-closing a Autoclave uses ECG sensors.
#Version: 7.0
#---------------------------------------------------------------------------------
# STUDENT CODE BEGINS
#---------------------------------------------------------------------------------
#libraries needed:
import random
#Variables:
#holds container/autoclave info: 1-3 for small red, green blue. i: 4-6 for large red, green, blue.
autoClaveInfos = [[(-0.622, 0.24, 0.379), 'red', 'small'], [(0, -0.656, 0.379), 'green', 'small'], [(0, 0.656, 0.379), 'blue', 'small'],
[(-0.422, 0.24, 0.379), 'red', 'large'], [(0, -0.385, 0.379), 'green', 'large'], [(0, 0.385, 0.379), 'blue', 'large']] #list of container information, manually generated info. #[location = (x,y,z), colour, size)[]
autoclaveindex = [1] #1 updates by one, once each is placed. index of which container or which autoclave bin to go to.
#currentArmLocation = (1,0,0.5)#all #disregard.
currentPos = 0 #the current pos the arm is moving to.
pickupLocation = (0.5064, 0.0, 0.026) # always the same, home pos, but the z is shifted down.
griped = [False]#see if the arm is gripped.
moveon = [False]#a variable to tell the function if it can move on to the next set of code in the control arm function.
place = [False]#sees if the container is placed in the autoclave bin.
putin = [1]#how many containers are placed.
indexofContainersPlaced = []#use the sum function of list to see if it equals 15
#Functions:
#This function takes in a location(turple) and moves the arm to that location (x,y,z) if the emg sensors match a threshold.
def move_end_effector(location): #emg sensor variable to grant access to move object. <- use the chart thing.
if arm.emg_left() >= 0.5 and arm.emg_right() <= 0.07:
print('Moves')
arm.move_arm(location[0], location[1],location[2]) # location turple.
return True #to move on.
return False #to not move on.
def indentifyAutoClavePosition(autoclaveindex):# the function takes in the autoclave location
index = 0 #starts with the index 0 since python starts from 0 with lists.
for container in autoClaveInfos:#loops through all the containers that need to be placed.
if index == autoclaveindex[0]:#if the index is autoclaveindex selected than the function will get the location at the autoclave index in the list.
currentautoclaveLocation = autoClaveInfos[autoclaveindex[0]][0] #gets location.
return currentautoclaveLocation #returns the autoclave location
index += 1 #updates the index being checked by one.
#function controls the gripper on the arm.
def gripUngrip(grip): #grip is boolean, if true, grip by 45, else -45
#we use emg sensor
if arm.emg_left() <= 0.07 and arm.emg_right() >= 0.5:
if grip:
print('Grips')
arm.control_gripper(45)
return True
else:
print('Drops')
arm.control_gripper(-45)
time.sleep(3)
return False
return None
#function opens or closes the autoclave bin drawer based on size and colour.
def openAutoClaveBinDrawer(open):#booleans open, opens if true
if arm.emg_left() >= 0.5 and arm.emg_right() >= 0.5:
colour = autoClaveInfos[autoclaveindex[0]][1]
print(colour)
if open:
print('Opens Autoclave')
if colour == 'red':
arm.open_red_autoclave(True)
elif colour == 'blue':
arm.open_blue_autoclave(True)
else:
arm.open_green_autoclave(True)
return True#opens
else:
print('Closes Autoclave')
if colour == 'red':
arm.open_red_autoclave(False)
elif colour == 'blue':
arm.open_blue_autoclave(False)
else:
arm.open_green_autoclave(False)
return False #closes
def control_Arm():#function controls arm based on user input.
#do not want it to repeat the same thing over and over agian, we want it to do it once than move onto the next container.
if moveon[0] == False: #is at home and not gripping.
if not griped[0]:
moveon[0] = move_end_effector((0.5064, 0.0, 0.026))#move arm to contaienr location
if moveon[0] == True:
time.sleep(3)
griped[0] = gripUngrip(True)#grips
time.sleep(3)
if griped[0] and griped[0] != None:
arm.move_arm(0.406, 0.0, 0.483)#move arm to home location, change!!!!!!
time.sleep(3)
moveon[0] = move_end_effector(currentPos)#move to the autoclave location.
time.sleep(4)
#moveon[0] = True
elif griped[0] or griped[0] == None:
size = autoClaveInfos[autoclaveindex[0]][2]
print(size)
if size == 'large':
if not place[0] or place[0] == None:#calls to open.
if openAutoClaveBinDrawer(True):
time.sleep(1)#waits for it to open.
place[0] = not gripUngrip(False) #drops
time.sleep(3)
print(place[0], 'Drops')
#place[0] = True #container is placed/droped.
elif place[0] == True and place[0] != None:#closes autoclave.
if not openAutoClaveBinDrawer(False):
time.sleep(1)
arm.home() #returns home
time.sleep(3)#waits for the arm to return.
randomlySelectContainer() #picks a new container.
if putin[0] <= 6:
spawnCage(autoclaveindex[0])#spawns a new container.
time.sleep(1)#waits for it to spawn.
place[0] = False #resets the placed container.
moveon[0] = False #go to the next container or proccess.
griped[0] = False
putin[0] += 1 #counter to see if done
else:# if small
#griped[0] = False #is not gripped
griped[0] = gripUngrip(False) #drops container.
time.sleep(3)#waits for it to drop
if griped[0] == False and griped[0] != None:
time.sleep(4)
arm.home() #returns home
time.sleep(4)#waits for the arm to return.
randomlySelectContainer() #picks a new container.
if putin[0] <= 6:
spawnCage(autoclaveindex[0])#spawns a new container.
time.sleep(1)#waits for it to spawn.
#griped[0] = False #is not gripped
moveon[0] = False #go to the next container or proccess.
putin[0] += 1 #counter to see if done.
def spawnCage(autoclaveindex):#spawns container.
print(indexofContainersPlaced)
arm.spawn_cage(autoclaveindex + 1)#spawns the container, adds one since python starts at zero.
def terminateContinue():
if sum(indexofContainersPlaced) == 15 and putin[0] > 6:#terminate the program if the sum of all the index are equal to 15, so 0 + 1 + 2 + 3 + 4 + 5, 0 to 5 because python starts at 0 and if you place all containers you have a sum of 15.
#and if all are put in, if 6.
print('Program has finished placing containers!')
return True #end program.
else:
return False #continue program.
def randomlySelectContainer(counter = 0):#takes in list of autoClaveInfos. #number of times the function picks a autoclaveindex, if the index is already choosen the counter will equal by 1 or return the index choosen.
indexChoosen = autoClaveInfos.index(random.choice(autoClaveInfos))#chooses the index of the autoclave. uses random and list methods.
if counter == 5:#if the counter is 5, starting from 0, than all containers are placed.
return False
if indexChoosen not in indexofContainersPlaced:#checks if the index choosen is not in the list containing the indexs of all containers placed.
autoclaveindex[0] = indexChoosen#sets the index of the current container.
indexofContainersPlaced.append(autoclaveindex[0])#appends the index to the list containing the indexs of all containers placed.
else:
counter += 1 #increase counter by 1.
return randomlySelectContainer(counter)#calls the function in a recurive loop till the indexchoosen is not the one already placed.
#Main :
randomlySelectContainer() #picks a new container.
spawnCage(autoclaveindex[0])#spawns a new container.
time.sleep(1)#waits for it to spawn.
while terminateContinue() == False: #calls to see if to continue!
currentPos = indentifyAutoClavePosition(autoclaveindex)
control_Arm() #calls the function to control the arm.
#---------------------------------------------------------------------------------
# STUDENT CODE ENDS
#---------------------------------------------------------------------------------