Sáng nay , mình vừa thuyết trình xong về dự án nghiên cứu khoa học của bọn mình ( cấp trường thôi)
mặc dù công nghệ sử dụng không có gì nổi bật lắm , Nhưng mình muốn chia sẻ lại cho mọi người biết
Về phần cứng , bọn mình sử dụng 1 cánh tay robot 6 bậc . (cái này cảm ơn thầy Vũ Huấn và anh Nguyễn Giang đã tài trợ cho bọn em )
1 arduino uno r3
1 mạch điều khiển servo 16 kênh
và 1 cục nguồn 6v3a để cấp đủ điện cho cái tay robot hoạt động
tiếp đến là đoạn code trên arduino:
để điều khiển 6 cái động cơ servo trên cánh tay , bọn mình đã sử dụng thư viện PCA9685.h
bước đầu là xác định khoảng giá trị của xung cần truyền vào servo để nó quay đúng góc mình cần
các bạn bật arduino ide , thêm thư viện vừa tải vào rồi chọn File -> Examples-> PCA9685 16-Channel PWM Driver Module->Servo Evaluator Example
các bạn để ý ở phần defend , có mấy dòng kiểu như bên dưới
PCA9685 pwmController(Wire1); // Library using Wire1 @400kHz, and default B000000 (A5-A0) i2c address // Linearly interpolates between standard 2.5%/12.5% phase length (102/512) for -90°/+90° PCA9685_ServoEval pwmServo1; // Testing our second servo has found that -90° sits at 128, 0° at 324, and +90° at 526. // Since 324 isn't precisely in the middle, a cubic spline will be used to smoothly // interpolate PWM values, which will account for said discrepancy. Additionally, since // 324 is closer to 128 than 526, there is slightly less resolution in the -90° to 0° // range while slightly more in the 0° to +90° range. PCA9685_ServoEval pwmServo2(128,324,526);đ
đoạn này là đoạn setup các giá trị xung truyền vào để lát sau mình dùng không cần phải truyền giá trị xung mà có thể dùng hàm pwmForAngle() để truyền giá tri góc trực tiếp. cái thông số này là khác nhau cho mỗi cánh tay nên bạn cần chỉnh sửa sao cho phù hợp
sau đó là phần sử lý dữ liệu từ python chuyển qua , mình dùng giao tiếp Serial trên arduino cho tiện
dưới đây là toàn bộ code của arduino của mình :
#include "PCA9685.h"
PCA9685 pwmController(Wire);
PCA9685_ServoEval pwmServo0(152,328,494);
PCA9685_ServoEval pwmServo1(338,480,550);
PCA9685_ServoEval pwmServo2(169,304,435);
PCA9685_ServoEval pwmServo3(220,380,520);
PCA9685_ServoEval pwmServo4(220,220,400);
int arr[4];
void setup() {
Serial.begin(9600); // Begin Serial and Wire1 interfaces
Wire.begin();
Serial.setTimeout(1);
pwmController.resetDevices(); // Resets all PCA9685 devices on i2c line
pwmController.init(); // Initializes module using default totem-pole driver mode, and default disabled phase balancer
pwmController.setPWMFreqServo(); // 50Hz provides standard 20ms servo phase length
pwmController.setChannelPWM(0, pwmServo0.pwmForAngle(0));
pwmController.setChannelPWM(1, pwmServo1.pwmForAngle(-45));
pwmController.setChannelPWM(2, pwmServo2.pwmForAngle(-45));
pwmController.setChannelPWM(3, pwmServo3.pwmForAngle(-45));
pwmController.setChannelPWM(4, pwmServo4.pwmForAngle(0));
}
void loop() {
while (Serial.available()>=5){
for (int i = 0; i < 5; i++){
arr[i] = Serial.read();
}
pwmController.setChannelPWM(4, pwmServo4.pwmForAngle(arr[4]-90));
pwmController.setChannelPWM(0, pwmServo0.pwmForAngle(arr[0]-90));
pwmController.setChannelPWM(1, pwmServo1.pwmForAngle(arr[1]-135));
pwmController.setChannelPWM(2, pwmServo2.pwmForAngle(arr[2]-135));
pwmController.setChannelPWM(3, pwmServo3.pwmForAngle(arr[3]-135));
}
}Tiếp theo là phần nhận diện cánh tay .
bọn mình dùng thư viện mediapipe và opencv để xử lý trên python
Sau đó . tính góc của ngón trỏ vả ngón út để xác định trạng thái nắm , mở của bàn tay
và so sánh điểm cổ tay và vai để tìm ra tỷ lệ
sau khi thử nhiệm nhiều lần thì mình tìm ra được mảng giá trị cần truyền vào arduino
bọn mình sử dụng thêm thư viên pyserial để có thể truyền dữ liệu qua cổng usb
và dưới đây là đoạn code của bọn mình :
import numpy as np
import mediapipe as mp
import cv2
from mediapipe.framework.formats import landmark_pb2
import serial
import time
import struct
prev_frame_time = 0
new_frame_time = 0
arduino = serial.Serial(port='/dev/ttyUSB0', baudrate=9600, timeout=.1)
mp_drawing = mp.solutions.drawing_utils
mp_holistic = mp.solutions.holistic
mp_drawing.DrawingSpec(color=(0, 0, 255), thickness=2, circle_radius=2)
cap = cv2.VideoCapture(0)
Wx = 1
Wy = 180
def cvx(x):
if x > 90:
return 90
if x < -90:
return -90
else:
return x
def get_angle(a, b, c):
a = np.array(a)
b = np.array(b)
c = np.array(c)
rad = np.arctan2(c[1] - b[1], c[0] - b[0]) - np.arctan2(a[1] - b[1], a[0] - b[0])
angle = np.abs(rad * 180.0 / np.pi)
if (angle > 180):
angle = 360 - angle
return angle
with mp_holistic.Holistic(min_detection_confidence=0.2, min_tracking_confidence=0.2) as holistic:
while True:
t1 = time.time()
new_frame_time = time.time()
fps = 1 / (new_frame_time - prev_frame_time)
prev_frame_time = new_frame_time
ret, frame = cap.read()
image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
results = holistic.process(image)
custom_pose_landmark = []
if results.pose_landmarks is not None:
for i in range(12, 22):
custom_pose_landmark.append(results.pose_landmarks.landmark[i])
landmark_pose_subset = landmark_pb2.NormalizedLandmarkList(landmark=custom_pose_landmark)
image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
# mp_drawing.draw_landmarks(image=image, landmark_list=landmark_pose_subset)
Xa1 = int(image.shape[1] * landmark_pose_subset.landmark[0].x) # Vai
Ya1 = int(image.shape[1] * landmark_pose_subset.landmark[0].y)
Xb1 = int(image.shape[1] * landmark_pose_subset.landmark[2].x) # Khuu Tay
Yb1 = int(image.shape[1] * landmark_pose_subset.landmark[2].y)
Xc1 = int(image.shape[1] * landmark_pose_subset.landmark[4].x) # Co Tay
Yc1 = int(image.shape[1] * landmark_pose_subset.landmark[4].y)
cv2.line(image,
(int(image.shape[1] * landmark_pose_subset.landmark[0].x),
int(image.shape[0] * landmark_pose_subset.landmark[0].y)),
(int(image.shape[1] * landmark_pose_subset.landmark[4].x),
int(image.shape[0] * landmark_pose_subset.landmark[4].y)),
(0, 255, 0), thickness=3, lineType=8)
custom_right_hand_landmark = []
if results.right_hand_landmarks is not None:
custom_right_hand_landmark.append(results.right_hand_landmarks.landmark[0])
custom_right_hand_landmark.append(results.right_hand_landmarks.landmark[4])
custom_right_hand_landmark.append(results.right_hand_landmarks.landmark[8])
landmark_right_hand_subset = landmark_pb2.NormalizedLandmarkList(landmark=custom_right_hand_landmark)
T0x = int(image.shape[1] * landmark_right_hand_subset.landmark[0].x)
T0y = int(image.shape[0] * landmark_right_hand_subset.landmark[0].y)
T1x = int(image.shape[1] * landmark_right_hand_subset.landmark[1].x)
T1y = int(image.shape[0] * landmark_right_hand_subset.landmark[1].y)
T2x = int(image.shape[1] * landmark_right_hand_subset.landmark[2].x)
T2y = int(image.shape[0] * landmark_right_hand_subset.landmark[2].y)
T0 = [landmark_right_hand_subset.landmark[0].x, landmark_right_hand_subset.landmark[0].y]
T1 = [landmark_right_hand_subset.landmark[1].x, landmark_right_hand_subset.landmark[1].y]
T2 = [landmark_right_hand_subset.landmark[2].x, landmark_right_hand_subset.landmark[2].y]
cv2.line(image,
(int(image.shape[1] * landmark_right_hand_subset.landmark[0].x),
int(image.shape[0] * landmark_right_hand_subset.landmark[0].y)),
(int(image.shape[1] * landmark_right_hand_subset.landmark[2].x),
int(image.shape[0] * landmark_right_hand_subset.landmark[2].y)),
(0, 255, 0), thickness=3, lineType=8)
cv2.line(image,
(int(image.shape[1] * landmark_right_hand_subset.landmark[0].x),
int(image.shape[0] * landmark_right_hand_subset.landmark[0].y)),
(int(image.shape[1] * landmark_right_hand_subset.landmark[1].x),
int(image.shape[0] * landmark_right_hand_subset.landmark[1].y)),
(0, 255, 0), thickness=3, lineType=8)
cv2.circle(image,
(int(image.shape[1] * landmark_pose_subset.landmark[0].x),
int(image.shape[0] * landmark_pose_subset.landmark[0].y)),
abs(Wy), (255, 0, 0), 2)
Mx = cvx(int(((Xc1 - Xa1) / Wy) * 90)) + 90
My = cvx(int(((Yc1 - Ya1) / Wy) * 90)) + 90
s0 = Mx
if (My < 90):
s1 = 90
s2 = My + 90
s3 = 90
elif (My > 90):
s1 = int(((My - 90) / 3) + 90)
s2 = 180
s3 = int(((My - 90) / 9) + 90)
else:
s1 = 90
s2 = 180
s3 = 90
if (get_angle(T1, T0, T2) > 30 ):
s4 = 90
else:
s4 = 180
print(s0,s1,s2,s3,s4)
arduino.write(struct.pack('>BBBBB', s0,s1,s2,s3,s4))
t2 = 1 / (time.time() - t1)
if cv2.waitKey(5) & 0xFF == ord('b'):
Wx = Xc1 - Xa1
Wy = Yc1 - Ya1
cv2.putText(image, str(int(fps)), (7, 70), cv2.FONT_HERSHEY_SIMPLEX, 3, (100, 255, 0), 3, cv2.LINE_AA)
cv2.imshow('frame', cv2.flip(image, 1))
# cv2.imshow('frame', image)
if cv2.waitKey(5) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
Cảm ơn các bạn đã đọc bài viết của mình