def poll_HW(self):
"""Called repeatedly by thread. Check for interlock, if OK read HW
Stores response in self.response, returns a status code, "OK" if so"""
if self.interlock:
if self.verbose:
#print("in poll_HW: poll locked out")
ard_log.debug("in poll_HW: poll locked out")
self.response = None
sys.stdout.flush()
return "interlock" # someone else is using serial port, wait till done!
self.interlock = True # set interlock so we won't be interrupted
# read message until \n is encountered
response = self.ser.read_until()
self.interlock = False
if response:
if len(response) > 0: # did something write to us?
response = response.strip() #get rid of newline, whitespace
if len(response) > 0: # if an actual character
if self.verbose:
self.response = response.decode(
"utf-8") #added decode for python 3
ard_log.debug("poll response: " + self.response)
self.evaluate_response()
sys.stdout.flush()
if self.callback:
#a valid response so send it
self.callback(self.response)
return "OK"
return "null" # got no response
def calibrate_gripper(self):
while (self.pressure_received < 5):
self.close_gripper(100, 5)
while (self.pressure_received > 10):
self.open_gripper(50, 5)
ard_log.debug(
"Claw calibrated to be closed with a pressure less than 10, opening Gripper with 6000 pulses"
)
self.open_gripper(6000, 0)
self.pulses = 0
def activate_solenoid(self):
self.command = "Deactivate Solenoid"
ard_log.debug("in function: " + self.command)
self.ID_sent = 2
msg = str(self.ID_sent) + "," + str(1) + "," + str(1) + ";"
self.Arduino_replied = False #set the flag to false
self.send(msg)
#waiting for Arduino to reply
while (self.Arduino_replied == False
): #flag will be set to TRUE when checking return message
Event().wait(
1.0
) #creates a new thread that acts as a timer to reduce CPU useage
ard_log.debug("Command successful")
return
def open_gripper(self, pulses, pressure):
'''
This command will close the gripper completely and make sure that the force between pinchers reaches 24N, given the slack allowed in command_close.
It creates a message as follows: [ID, length, pulses; pressure;]
'''
#Saves the relevant information into variables to be compared with when a response from the arduino comes
self.command = "Open Gripper"
self.pulses_sent = pulses
self.pressure_sent = pressure
self.ID_sent = 1
#constructing and sending message to the arduino
msg = str(self.ID_sent) + "," + str(2) + "," + str(
self.pulses_sent) + ";" + str(self.pressure_sent) + ";"
self.Arduino_replied = False #set the flag to false
self.send(msg)
#waiting for Arduino to reply
while (self.Arduino_replied == False
): #flag will be set to TRUE when checking return message
Event().wait(
3.0
) #creates a new thread that acts as a timer to reduce CPU useage
#ard_log.debug("inside the wait timer thingy")
#Make sure that we don't get stuck in too many recursions
if (self.adjust_counter >= self.max_recursion):
ard_log.error("in function: " + self.command +
" Maximum number of recursion reached")
self.adjust_counter = 0
return
#reply from arduino has been compared, and different flags have been set accordingly.
#depending on combination of flags, a certain case is true, and possible adjustments made
if (not self.pressureHigh and not self.pressureLow
and not self.pulseLow):
ard_log.debug("Command successful")
self.adjust_counter = 0
return
elif (self.pressureHigh):
ard_log.debug("in function: " + self.command +
" Pressure_received " + str(self.pressure_received) +
" higher than Pressure_sent " +
str(self.pressure_sent) + " , adjusting by opening")
self.adjust_counter = self.adjust_counter + 1
self.open_gripper(self.adjustment_pulses, self.pressure_sent)
return
elif (self.pressureLow):
ard_log.debug("in function: " + self.command +
" Pressure_received " + str(self.pressure_received) +
" lower than Pressure_sent " +
str(self.pressure_sent) + " , adjusting by closing")
self.adjust_counter = self.adjust_counter + 1
self.close_gripper(self.adjustment_pulses, self.pressure_sent)
return
elif (self.pulseLow):
self.obstacleFlag = True
ard_log.error(
"in function: " + self.command +
" Obstacle error, cease operation\n Flags: pulseLow:%r pressureLow:%r pressureHigh:%r"
% (self.pulseLow, self.pressureLow, self.pressureHigh))
return
else:
ard_log.debug("Jumping into else, some undefined case")
print("self.pulseLow, self.pressureLow, self.pressureHigh \n")
print(self.pulseLow, self.pressureLow, self.pressureHigh)
print("data_sent \n")
print(self.data_sent)
print("data_received \n")
print(self.data_received)
return
def send(self, val):
val = str(val)
#self.message_sanity_check(val,"send")
ard_log.debug("sending message " + val)
sys.stdout.flush()
self.write_HW(val)
