diff --git a/.gitignore b/.gitignore
index 00c879a..8668ff9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,6 +5,7 @@
.vscode/
.history
*.pyc
+*.bat
# Editor directories and files
.idea
diff --git a/mycobot_ai/scripts/ai_windows.py b/mycobot_ai/scripts/ai_windows.py
index 04afca4..4cd4c46 100644
--- a/mycobot_ai/scripts/ai_windows.py
+++ b/mycobot_ai/scripts/ai_windows.py
@@ -20,15 +20,21 @@ class Application(object):
self.run_py = ""
# 判断通信口并给权限
try:
- self.ports = os.popen("ls /dev/ttyUSB*").readline()
- if "dev" not in self.ports:
- self.ports = os.popen("ls /dev/ttyACM*").readline()[:-1]
- self.command = ''.format(
- self.ports)
- # 根据通信口修改ros启动文件
- os.system(
- "sed -i '2c {}' /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/launch/vision.launch"
- .format(self.command))
+ self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
+ self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
+ self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+ self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
+ if "dev" in self.robot_wio:
+ self.command = ''.format(
+ self.robot_wio)
+ # 根据通信口修改ros启动文件
+ os.system(
+ "sed -i '2c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch"
+ .format(self.command))
+ elif "dev" in self.robot_raspi:
+ self.change_file(self.robot_raspi)
+ elif "dev" in self.robot_jes:
+ self.change_file(self.robot_jes)
except Exception as e:
pass
@@ -36,9 +42,6 @@ class Application(object):
self.win.title("aikit启动工具")
self.win.geometry(
"550x350+100+100") # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置
- # 打开ros按钮
- self.btn = Button(self.win, text="open ROS", command=self.open_ros)
- self.btn.grid(row=0)
self.chanse_code = Label(self.win, text="选择程序:", width=10)
self.chanse_code.grid(row=1)
@@ -50,23 +53,7 @@ class Application(object):
self.add_btn = Button(self.win, text="添加新的物体图像", command=self.add_img)
self.add_btn.grid(row=1, column=2)
- # self.set_xy = Label(self.win, text="set_xy:", width=10)
- # self.set_xy.grid(row=1)
-
- # self.x = Label(self.win, text="x:")
- # self.x.grid(row=2)
- # self.v1 = StringVar()
- # self.e1 = Entry(self.win,textvariable=self.v1, width=10)
- # self.e1.insert(0,0)
- # self.e1.grid(row=2,column=1)
-
- # self.y = Label(self.win, text="y:")
- # self.y.grid(row=3)
- # self.v2 = StringVar()
- # self.e2 = Entry(self.win,textvariable=self.v2, width=10)
- # self.e2.insert(0,0)
- # self.e2.grid(row=3,column=1)
- self.tips = "1、首先打开ros,大概需要等待15s\n2、选择所要运行的程序点击运行即可,开启大概需要10秒,可以通过查看终端查看开启情况。\n\n添加新的图像：\n1、点击按钮，等待开启摄像头\n2、选中图像框，按z键拍照\n3、使用鼠标框出需要识别的图像区域\n4、按Enter键提取图像\n5、再次按Enter键保存即可"
+ self.tips = "1、等待打开ros,大概需要15s\n2、选择所要运行的程序点击运行即可,开启大概需要10秒,可以通过查看终端查看开启情况。\n\n添加新的图像：\n1、点击按钮，等待开启摄像头\n2、选中图像框，按z键拍照\n3、使用鼠标框出需要识别的图像区域\n4、按Enter键提取图像\n5、再次按Enter键保存即可"
self.btn = Button(self.win, text="运行", command=self.start_run)
self.btn.grid(row=5)
@@ -78,10 +65,23 @@ class Application(object):
self.log_data = Text(self.win, width=66, height=10)
self.log_data.grid(row=16, column=0, columnspan=10)
self.log_data.insert(END, self.tips)
- # self.code_list = ttk.Combobox(self.win, width=15)
- # self.code_list["value"] = ("颜色识别", "物体识别", "二维码识别")
- # self.code_list.current(0)
- # self.code_list.grid(row=1, column=1)
+
+ self.open_ros()
+ self.win.protocol('WM_DELETE_WINDOW', self.close_rviz)
+
+ def close_rviz(self):
+ os.system(
+ "ps -ef | grep -E mycobot.rviz | grep -v 'grep' | awk '{print $2}' | xargs kill -9")
+ sys.exit(0)
+
+ def change_file(self, port):
+ command1 = ''.format(port)
+ command2 = ''.format(1000000)
+ # 根据通信口修改ros启动文件
+ os.system(
+ "sed -i '2c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch".format(command1))
+ os.system(
+ "sed -i '3c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch".format(command2))
def start_run(self):
try:
@@ -108,28 +108,25 @@ class Application(object):
def open_py(self):
os.system(
- "python /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/scripts/detect_obj_img.py"
+ "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/detect_obj_img.py"
)
def open_py1(self):
os.system(
- "python /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/scripts/detect_obj_color.py"
+ "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/detect_obj_color.py"
)
def open_py2(self):
os.system(
- "python /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/scripts/detect_encode.py"
+ "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/detect_encode.py"
)
def add_img(self):
os.system(
- "python /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/scripts/add_img.py"
+ "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/add_img.py"
)
def open_ros(self):
- if self.ros:
- print("ros is opened")
- return
t1 = threading.Thread(target=self.ross)
t1.setDaemon(True)
t1.start()
@@ -137,7 +134,7 @@ class Application(object):
def ross(self):
os.system(
- "roslaunch ~/catkin_mycobot/src/mycobot_ros/mycobot_ai/launch/vision.launch"
+ "roslaunch ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch"
)
def close_py(self):
diff --git a/mycobot_ai/scripts/detect_encode.py b/mycobot_ai/scripts/detect_encode.py
index d469294..7bbe645 100644
--- a/mycobot_ai/scripts/detect_encode.py
+++ b/mycobot_ai/scripts/detect_encode.py
@@ -24,6 +24,7 @@ class Detect_marker(Movement):
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+ self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
self.raspi = False
if "dev" in self.robot_m5:
self.Pin = [2, 5]
@@ -31,17 +32,21 @@ class Detect_marker(Movement):
self.Pin = [20, 21]
for i in self.move_coords:
i[2] -= 20
- elif "dev" in self.robot_raspi:
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
import RPi.GPIO as GPIO
+ self.GPIO = GPIO
+ GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT)
self.raspi = True
- pump_y -= 5
-
+ if self.raspi:
+ self.gpio_status(False)
+ else:
+ self.pub_pump(False, self.Pin)
# Creating a Camera Object
cap_num = 0
- self.cap = cv.VideoCapture(cap_num)
+ self.cap = cv.VideoCapture(cap_num, cv.CAP_V4L)
# Get ArUco marker dict that can be detected.
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
# Get ArUco marker params.
@@ -93,7 +98,7 @@ class Detect_marker(Movement):
def move(self, x, y):
if self.raspi:
coords = [
- [145.6, -64.9, 285.2, 179.88, 7.67, -177.06],
+ [145.6, -64.9, 285.2, 179.88, 7.67, 179],
[130.1, -155.6, 243.9, 178.99, 5.38, -179.9]
]
else:
@@ -138,6 +143,7 @@ class Detect_marker(Movement):
else:
self.pub_pump(False, self.Pin)
# publish marker
+ time.sleep(1)
self.marker.header.stamp = rospy.Time.now()
self.marker.pose.position.x = coords[1][0]/1000.0
self.marker.pose.position.y = coords[1][1]/1000.0
@@ -148,11 +154,11 @@ class Detect_marker(Movement):
def gpio_status(self, flag):
if flag:
- GPIO.output(20, 0)
- GPIO.output(21, 0)
+ self.GPIO.output(20, 0)
+ self.GPIO.output(21, 0)
else:
- GPIO.output(20, 1)
- GPIO.output(21, 1)
+ self.GPIO.output(20, 1)
+ self.GPIO.output(21, 1)
# decide whether grab cube
def decide_move(self, x, y):
@@ -164,41 +170,44 @@ class Detect_marker(Movement):
return
else:
self.cache_x = self.cache_y = 0
+ if "dev" in self.robot_jes:
+ if x > -20:
+ y += 10
+ if y > -25:
+ x -= 5
+ x += 10
+
self.move(x, y)
# init mycobot
def init_mycobot(self):
- if self.raspi:
- self.gpio_status(False)
- else:
- self.pub_pump(False, self.Pin)
+
for _ in range(5):
print _
- self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, -177.06], 20, 1), 20, 1)
-
+ self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, 179], 20, 1)
time.sleep(0.5)
def run(self):
global pump_y, pump_x
self.init_mycobot()
- num=sum_x=sum_y=0
+ num = sum_x = sum_y = 0
while cv.waitKey(1) < 0:
- success, img=self.cap.read()
+ success, img = self.cap.read()
if not success:
print("It seems that the image cannot be acquired correctly.")
break
# transfrom the img to model of gray
- gray=cv.cvtColor(img, cv.COLOR_BGR2GRAY)
+ gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
# Detect ArUco marker.
- corners, ids, rejectImaPoint=cv.aruco.detectMarkers(
- gray, self.aruco_dict, parameters = self.aruco_params
+ corners, ids, rejectImaPoint = cv.aruco.detectMarkers(
+ gray, self.aruco_dict, parameters=self.aruco_params
)
if len(corners) > 0:
if ids is not None:
# get informations of aruco
- ret=cv.aruco.estimatePoseSingleMarkers(
+ ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.03, self.camera_matrix, self.dist_coeffs
)
# rvec:rotation offset,tvec:translation deviator
diff --git a/mycobot_ai/scripts/detect_obj_color.py b/mycobot_ai/scripts/detect_obj_color.py
index 8a27d06..e3d42df 100644
--- a/mycobot_ai/scripts/detect_obj_color.py
+++ b/mycobot_ai/scripts/detect_obj_color.py
@@ -26,15 +26,16 @@ class Object_detect(Movement):
dir_path = os.path.dirname(__file__)
# 移动角度
self.move_angles = [
- [-7.11, -6.94, -55.01, -24.16, 0, -38.84], # init the point
- [-1.14, -10.63, -87.8, 9.05, -3.07, -37.7], # point to grab
- [17.4, -10.1, -87.27, 5.8, -2.02, -37.7], # point to grab
+ [-7.11, -6.94, -55.01, -24.16, 0, 15], # init the point
+ [5, -10.63, -87.8, 9.05, -3.07, 15], # point to grab
+ [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab
]
# 移动坐标
self.move_coords = [
- [120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket
+ [120.1, -141.6, 240.9, -173.34, -8.15, -110.11], # above the red bucket
# above the yello bucket
- [215.2, -127.8, 260.9, -157.51, -17.5, -71.18],
+ #[215.2, -127.8, 260.9, -157.51, -17.5, -71.18],
+ [210.6, -130.5, 263.0, -150.99, -0.07, -107.35],
[209.7, -18.6, 230.4, -168.48, -9.86, -39.38],
[196.9, -64.7, 232.6, -166.66, -9.44, -52.47],
[126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
@@ -43,6 +44,7 @@ class Object_detect(Movement):
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+ self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
self.raspi = False
if "dev" in self.robot_m5:
self.Pin = [2, 5]
@@ -50,12 +52,20 @@ class Object_detect(Movement):
self.Pin = [20, 21]
for i in self.move_coords:
i[2] -= 20
- elif "dev" in self.robot_raspi:
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
import RPi.GPIO as GPIO
+ GPIO.setwarnings(False)
+ self.GPIO = GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT)
+ GPIO.output(20,1)
+ GPIO.output(21,1)
self.raspi = True
+ if self.raspi:
+ self.gpio_status(False)
+ else:
+ self.pub_pump(False, self.Pin)
# choose place to set cube
self.color = 0
@@ -120,11 +130,11 @@ class Object_detect(Movement):
def gpio_status(self, flag):
if flag:
- GPIO.output(20, 0)
- GPIO.output(21, 0)
+ self.GPIO.output(20, 0)
+ self.GPIO.output(21, 0)
else:
- GPIO.output(20, 1)
- GPIO.output(21, 1)
+ self.GPIO.output(20, 1)
+ self.GPIO.output(21, 1)
# Grasping motion
def move(self, x, y, color):
@@ -151,6 +161,7 @@ class Object_detect(Movement):
elif x < 135:
h = -20
self.pub_coords([x, y, 31.9+h, -178.9, -1, -25.95], 20, 1)
+
time.sleep(1.5)
# open pump
if self.raspi:
@@ -182,6 +193,7 @@ class Object_detect(Movement):
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
+ time.sleep(1)
if color == 1:
self.pub_marker(
self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
@@ -203,7 +215,7 @@ class Object_detect(Movement):
else:
self.cache_x = self.cache_y = 0
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
- if "dev" not in self.robot_wio:
+ if "dev" in self.robot_raspi:
if (y < -30 and x > 140) or (x > 150 and y < -10):
x -= 10
@@ -213,26 +225,26 @@ class Object_detect(Movement):
elif x > 170:
x -= 10
y += 10
- print x, y
- else:
+ elif "dev" in self.robot_wio:
if x > 160:
y += 10
elif y < -20:
x -= 10
y += 10
+ elif "dev" in self.robot_jes:
+ y+=13
+ x+=4
+ print x, y
self.move(x, y, color)
# init mycobot
def run(self):
-
+
for _ in range(5):
- self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -38.84], 20)
+ self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20)
print(_)
time.sleep(0.5)
- if self.raspi:
- self.gpio_status(False)
- else:
- self.pub_pump(False, self.Pin)
+
# draw aruco
def draw_marker(self, img, x, y):
@@ -374,7 +386,8 @@ class Object_detect(Movement):
if __name__ == "__main__":
# open the camera
cap_num = 0
- cap = cv2.VideoCapture(cap_num)
+ cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
+
if not cap.isOpened():
cap.open()
# init a class of Object_detect
diff --git a/mycobot_ai/scripts/detect_obj_img.py b/mycobot_ai/scripts/detect_obj_img.py
index eed80a2..1b7459a 100644
--- a/mycobot_ai/scripts/detect_obj_img.py
+++ b/mycobot_ai/scripts/detect_obj_img.py
@@ -27,15 +27,16 @@ class Object_detect(Movement):
dir_path = os.path.dirname(__file__)
# 移动角度
self.move_angles = [
- [-7.11, -6.94, -55.01, -24.16, 0, -38.84], # init the point
- [-1.14, -10.63, -87.8, 9.05, -3.07, -37.7], # point to grab
- [17.4, -10.1, -87.27, 5.8, -2.02, -37.7], # point to grab
+ [-7.11, -6.94, -55.01, -24.16, 0, 15], # init the point
+ [-1.14, -10.63, -87.8, 9.05, -3.07, 15], # point to grab
+ [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab
]
# 移动坐标
self.move_coords = [
- [120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket
+ [120.1, -141.6, 240.9, -173.34, -8.15, -110.11], # above the red bucket
# above the yello bucket
- [208.2, -127.8, 260.9, -157.51, -17.5, -71.18],
+ #[208.2, -127.8, 260.9, -157.51, -17.5, -71.18],
+ [205.6, -130.5, 263.0, -150.99, -0.07, -107.35],
[209.7, -18.6, 230.4, -168.48, -9.86, -39.38],
[196.9, -64.7, 232.6, -166.66, -9.44, -52.47],
[126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
@@ -44,6 +45,7 @@ class Object_detect(Movement):
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+ self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
self.raspi = False
if "dev" in self.robot_m5:
self.Pin = [2, 5]
@@ -51,13 +53,19 @@ class Object_detect(Movement):
self.Pin = [20, 21]
for i in self.move_coords:
i[2] -= 20
- elif "dev" in self.robot_raspi:
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
import RPi.GPIO as GPIO
+ self.GPIO = GPIO
+ GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT)
- self.raspi = True
+ self.raspi = True
+ if self.raspi:
+ self.gpio_status(False)
+ else:
+ self.pub_pump(False, self.Pin)
# choose place to set cube
self.color = 0
# parameters to calculate camera clipping parameters
@@ -126,11 +134,11 @@ class Object_detect(Movement):
def gpio_status(self, flag):
if flag:
- GPIO.output(20, 0)
- GPIO.output(21, 0)
+ self.GPIO.output(20, 0)
+ self.GPIO.output(21, 0)
else:
- GPIO.output(20, 1)
- GPIO.output(21, 1)
+ self.GPIO.output(20, 1)
+ self.GPIO.output(21, 1)
# Grasping motion
def move(self, x, y, color):
@@ -142,11 +150,12 @@ class Object_detect(Movement):
self.pub_angles(self.move_angles[2], 20)
time.sleep(1.5)
# send coordinates to move mycobot
- self.pub_coords([x, y, 165, -178.9, -1.57, -25.95], 20, 1)
+ self.pub_coords([x, y, 165, -178.9, -1.57, -66], 20, 1)
time.sleep(1.5)
# 根据不同底板机械臂，调整吸泵高度
if "dev" in self.robot_m5:
- self.pub_coords([x, y, 90, -178.9, -1.57, -25.95], 20, 1)
+ # m5 and jetson nano
+ self.pub_coords([x, y, 90, -178.9, -1.57, -66], 25, 1)
elif "dev" in self.robot_wio:
h = 0
if 165 < x < 180:
@@ -155,7 +164,12 @@ class Object_detect(Movement):
h = 20
elif x < 135:
h = -20
- self.pub_coords([x, y, 31.9+h, -178.9, -1, -25.95], 20, 1)
+ self.pub_coords([x, y, 31.9+h, -178.9, -1, -66], 20, 1)
+ elif "dev" in self.robot_jes:
+ h = 0
+ if x<130:
+ h=15
+ self.pub_coords([x, y, 90-h, -178.9, -1.57, -66], 25, 1)
time.sleep(1.5)
# open pump
if self.raspi:
@@ -177,7 +191,7 @@ class Object_detect(Movement):
time.sleep(1.5)
self.pub_marker(
self.move_coords[4][0]/1000.0, self.move_coords[4][1]/1000.0, self.move_coords[4][2]/1000.0)
-
+ print self.move_coords[color]
self.pub_coords(self.move_coords[color], 20, 1)
self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color]
[1]/1000.0, self.move_coords[color][2]/1000.0)
@@ -187,6 +201,7 @@ class Object_detect(Movement):
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
+ time.sleep(1)
if color == 1:
self.pub_marker(
self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
@@ -207,7 +222,7 @@ class Object_detect(Movement):
else:
self.cache_x = self.cache_y = 0
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
- if "dev" not in self.robot_wio:
+ if "dev" in self.robot_wio:
if (y < -30 and x > 140) or (x > 150 and y < -10):
x -= 10
y += 10
@@ -216,22 +231,25 @@ class Object_detect(Movement):
elif x > 170:
x -= 10
y += 10
- elif "dev" not in self.robot_m5:
+ elif "dev" in self.robot_m5:
y += 10
x -= 5
if y < -20:
y += 5
# print x,y
+ elif "dev" in self.robot_jes:
+ if y<0:
+ x+=5
+ y+=3
+ y+=10
+ print x,y
self.move(x, y, color)
# init mycobot
def run(self):
- if self.raspi:
- self.gpio_status(False)
- else:
- self.pub_pump(False, self.Pin)
+
for _ in range(5):
- self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -38.84], 20)
+ self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20)
print(_)
time.sleep(0.5)
@@ -459,7 +477,7 @@ def run():
for l in k:
goal.append(cv2.imread('local_photo/img/{}'.format(l)))
cap_num = 0
- cap = cv2.VideoCapture(cap_num)
+ cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
if not cap.isOpened():
cap.open()
# init a class of Object_detect
diff --git a/mycobot_ai/scripts/pump.py b/mycobot_ai/scripts/pump.py
index 3feb6d2..2f67e7b 100644
--- a/mycobot_ai/scripts/pump.py
+++ b/mycobot_ai/scripts/pump.py
@@ -5,19 +5,34 @@ import rospy
import time
from moving_utils import Movement
+
class Pump(Movement):
- def __init__(self):
- super(Pump, self).__init__()
- rospy.init_node("pump", anonymous=True)
+ def __init__(self):
+ super(Pump, self).__init__()
+ rospy.init_node("pump", anonymous=True)
+
+ def run(self):
+ self.pub_pump(False)
+ time.sleep(1)
+ self.pub_pump(True)
+ time.sleep(5)
+ self.pub_pump(False)
+
+ def gpiod(self):
+ import RPi.GPIO as GPIO
+ GPIO.setwarnings(False)
+ GPIO.setmode(GPIO.BCM)
+ GPIO.setup(20, GPIO.OUT)
+ GPIO.setup(21, GPIO.OUT)
+ GPIO.output(20, 0)
+ GPIO.output(21, 0)
+ time.sleep(3)
+ print "close"
+ GPIO.output(20, 1)
+ GPIO.output(21, 1)
- def run(self):
- self.pub_pump(False)
- time.sleep(1)
- self.pub_pump(True)
- time.sleep(5)
- self.pub_pump(False)
if __name__ == "__main__":
- pump = Pump()
- pump.run()
\ No newline at end of file
+ pump = Pump()
+ pump.gpiod()