Skip to content

Commit

Permalink
Use serial_call_with_retry on pump control
Browse files Browse the repository at this point in the history
  • Loading branch information
@iory
iory committed Oct 31, 2024
1 parent a40e2f6 commit ae9155d
Show file tree
Hide file tree
Showing 2 changed files with 56 additions and 37 deletions.
16 changes: 8 additions & 8 deletions rcb4/armh7interface.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1288,19 +1288,19 @@ def read_pressure_sensor(self, board_idx):

def start_pump(self):
"""Drive pump. There is supposed to be one pump for the entire system."""
self.cfunc_call("pump_switch", True)
return self.cfunc_call("pump_switch", True)

def stop_pump(self):
"""Stop driving pump. There should be one pump for the entire system."""
self.cfunc_call("pump_switch", False)
return self.cfunc_call("pump_switch", False)

def open_air_connect_valve(self):
"""Open valve to release air to atmosphere"""
self.cfunc_call("valve_switch", True)
return self.cfunc_call("valve_switch", True)

def close_air_connect_valve(self):
"""Close valve to shut off air from atmosphere"""
self.cfunc_call("valve_switch", False)
return self.cfunc_call("valve_switch", False)

def gpio_mode(self, board_idx):
"""Return current GPIO mode of air relay board
Expand All @@ -1318,7 +1318,7 @@ def open_work_valve(self, board_idx):
"""
gpio_mode = self.gpio_mode(board_idx)
command = gpio_mode | 0b00000001
self.cfunc_call("gpio_cmd", board_idx, command)
return self.cfunc_call("gpio_cmd", board_idx, command)

def close_work_valve(self, board_idx):
"""Close work valve
Expand All @@ -1327,7 +1327,7 @@ def close_work_valve(self, board_idx):
"""
gpio_mode = self.gpio_mode(board_idx)
command = gpio_mode & 0b11111110
self.cfunc_call("gpio_cmd", board_idx, command)
return self.cfunc_call("gpio_cmd", board_idx, command)

def open_relay_valve(self, board_idx):
"""Open valve to relay air to next work
Expand All @@ -1336,7 +1336,7 @@ def open_relay_valve(self, board_idx):
"""
gpio_mode = self.gpio_mode(board_idx)
command = gpio_mode | 0b00000010
self.cfunc_call("gpio_cmd", board_idx, command)
return self.cfunc_call("gpio_cmd", board_idx, command)

def close_relay_valve(self, board_idx):
"""Close valve to relay air to next work
Expand All @@ -1345,7 +1345,7 @@ def close_relay_valve(self, board_idx):
"""
gpio_mode = self.gpio_mode(board_idx)
command = gpio_mode & 0b11111101
self.cfunc_call("gpio_cmd", board_idx, command)
return self.cfunc_call("gpio_cmd", board_idx, command)

@property
def servo_id_to_worm_id(self):
Expand Down
77 changes: 48 additions & 29 deletions ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -766,11 +766,9 @@ def pressure_control_loop(self, idx, start_pressure, stop_pressure, release):
if pressure is None:
rospy.sleep()
if vacuum_on is False and pressure > start_pressure:
self.start_vacuum(idx)
vacuum_on = True
vacuum_on = self.start_vacuum(idx)
if vacuum_on and pressure <= stop_pressure:
self.stop_vacuum(idx)
vacuum_on = False
vacuum_on = not self.stop_vacuum(idx)
rospy.sleep(0.1)

@property
Expand All @@ -786,39 +784,60 @@ def release_vacuum(self, idx):
After 1s, all valves are closed and pump is stopped.
"""
if not self.interface.is_opened():
return
try:
self.interface.stop_pump()
self.interface.open_work_valve(idx)
self.interface.open_air_connect_valve()
rospy.sleep(1) # Wait until air is completely released
self.interface.close_air_connect_valve()
self.interface.close_work_valve(idx)
except serial.serialutil.SerialException as e:
rospy.logerr(f"[release_vacuum] {e!s}")
return False
ret = serial_call_with_retry(self.interface.stop_pump, max_retries=3)
if ret is None:
return False
ret = serial_call_with_retry(self.interface.open_work_valve, idx, max_retries=3)
if ret is None:
return False
ret = serial_call_with_retry(self.interface.open_air_connect_valve,
max_retries=3)
if ret is None:
return False
rospy.sleep(1) # Wait until air is completely released
ret = serial_call_with_retry(self.interface.close_air_connect_valve,
max_retries=3)
if ret is None:
return False
ret = serial_call_with_retry(self.interface.close_work_valve,
idx, max_retries=3)
if ret is None:
return False
return True

def start_vacuum(self, idx):
"""Vacuum air in work"""
if not self.interface.is_opened():
return
try:
self.interface.start_pump()
self.interface.open_work_valve(idx)
self.interface.close_air_connect_valve()
except serial.serialutil.SerialException as e:
rospy.logerr(f"[start_vacuum] {e!s}")
return False

ret = serial_call_with_retry(self.interface.start_pump, max_retries=3)
if ret is None:
return False
ret = serial_call_with_retry(self.interface.open_work_valve, idx, max_retries=3)
if ret is None:
return False
ret = serial_call_with_retry(self.interface.close_air_connect_valve, max_retries=3)
if ret is None:
return False
return True

def stop_vacuum(self, idx):
"""Seal air in work"""
if not self.interface.is_opened():
return
try:
self.interface.close_work_valve(idx)
self.interface.close_air_connect_valve()
rospy.sleep(0.3) # Wait for valve to close completely
self.interface.stop_pump()
except serial.serialutil.SerialException as e:
rospy.logerr(f"[stop_vacuum] {e!s}")
return False

ret = serial_call_with_retry(self.interface.close_work_valve, idx, max_retries=3)
if ret is None:
return False
ret = serial_call_with_retry(self.interface.close_air_connect_valve, max_retries=3)
if ret is None:
return False
rospy.sleep(0.3) # Wait for valve to close completely
ret = serial_call_with_retry(self.interface.stop_pump, max_retries=3)
if ret is None:
return False
return True

def pressure_control_callback(self, goal):
if not self.interface.is_opened():
Expand Down

0 comments on commit ae9155d

Please sign in to comment.