alperen.isik
/
It_Robotics_Pong_Robot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update

This commit is contained in:
Hasan Emre AYDIN 2025-04-22 20:16:46 +02:00
parent e4c2972995
commit cd2130a571
1 changed files with 61 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

166
src/RobotServoing.cpp
View File

@ -1,10 +1,11 @@
#include <chrono>
#include <chrono>
#include <thread>
#include <iostream>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <cmath>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
@ -21,7 +22,6 @@
#define ROBOT_L1 5
#define ROBOT_L2 5.5
#define ROBOT_L3 6
#define PUSH_THRESHOLD_CM 4.0f // İtme eşiği (cm)
using namespace std;
using namespace cv;
@ -31,8 +31,35 @@ string _robotDxlPortName = "/dev/ttyUSB0";
float _robotDxlProtocol = 2.0;
int _robotDxlBaudRate = 1000000;
void initRobot(DynamixelHandler& dxlHandler, string portName, float protocol, int baudRate)
{
struct Vec2 {
double x;
double y;
};
Vec2 computePushTarget(const Vec2& ee_pos, const Vec2& ball_pos) {
Vec2 direction = {
ball_pos.x - ee_pos.x,
ball_pos.y - ee_pos.y
};
double length = std::sqrt(direction.x * direction.x + direction.y * direction.y);
if (length == 0) return ee_pos;
Vec2 unit_dir = {
direction.x / length,
direction.y / length
};
double push_distance = 0.05;
Vec2 push_target = {
ball_pos.x + unit_dir.x * push_distance,
ball_pos.y + unit_dir.y * push_distance
};
return push_target;
}
void initRobot(DynamixelHandler& dxlHandler, string portName, float protocol, int baudRate) {
cout << "===Initialization of the Dynamixel Motor communication====" << endl;
dxlHandler.setDeviceName(portName);
dxlHandler.setProtocolVersion(protocol);
@ -42,14 +69,7 @@ void initRobot(DynamixelHandler& dxlHandler, string portName, float protocol, in
cout << endl;
}
void closeRobot(DynamixelHandler& dxlHandler)
{
dxlHandler.enableTorque(false);
dxlHandler.closePort();
}
bool readCameraParameters(string filename, Mat &camMatrix, Mat & distCoeffs)
{
bool readCameraParameters(string filename, Mat &camMatrix, Mat & distCoeffs) {
FileStorage fs(filename, FileStorage::READ);
if (!fs.isOpened()) {
cout << "[ERROR] Could not open the camera parameter file storage: " << filename << " !" << endl;
@ -60,8 +80,7 @@ bool readCameraParameters(string filename, Mat &camMatrix, Mat & distCoeffs)
return true;
}
bool readColorParameters(string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV)
{
bool readColorParameters(string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV) {
FileStorage fs(filename, FileStorage::READ);
if (!fs.isOpened()) {
cout << "[ERROR] Could not open the color paramter file storage: " << filename << " !" << endl;
@ -73,60 +92,21 @@ bool readColorParameters(string filename, int& iLowH, int& iHighH, int& iLowS, i
return true;
}
// Yeni fonksiyon: Rastgele pozisyon göndererek topu itmek
void pushBallRandomly(DynamixelHandler &dxlHandler) {
float randAngle1 = deg2rad(rand() % 90 - 45); // -45 ile 45 arasında rastgele açı
float randAngle2 = deg2rad(rand() % 30 + 60); // 60 ile 90 arasında rastgele açı (kol havada kalmalı)
float randAngle3 = deg2rad(rand() % 90 - 45);
float randAngle4 = deg2rad(rand() % 180 - 90);
vector<float> qi = {randAngle1, randAngle2, randAngle3, randAngle4};
computeForwardKinematics(qi[0], qi[1], qi[2], qi[3]);
dxlHandler.sendTargetJointPosition(qi);
std::this_thread::sleep_for(std::chrono::milliseconds(500)); // biraz bekle
}
int main(int argc, char** argv)
{
float q2 = deg2rad(80);
string sCameraParamFilename = CAM_PARAMS_FILENAME;
string sColorParamFilename = COLOR_PARAMS_FILENAME;
int main(int argc, char** argv) {
float fFPS = FPS;
int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
int iAreaThresold = AREA_THRESOLD;
int opt;
while ((opt = getopt(argc, argv, ":c:f:s:a:i:")) != -1) {
switch (opt) {
case 'c': sColorParamFilename = optarg; break;
case 'f': fFPS = atof(optarg); break;
case 's': iStructuralElementSize = atoi(optarg); break;
case 'a': iAreaThresold = atoi(optarg); break;
case 'i': sCameraParamFilename = optarg; break;
case '?':
fprintf(stderr, "Unknown option -%c.\n", optopt);
return 1;
default:
abort();
}
}
string sCameraParamFilename = CAM_PARAMS_FILENAME;
string sColorParamFilename = COLOR_PARAMS_FILENAME;
initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
int iLowH, iHighH, iLowS, iHighS, iLowV, iHighV;
bool isColorParamsSet = readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
if (!isColorParamsSet) {
cout << "[ERROR] Color parameters could not be loaded!" << endl;
return -1;
}
if (!readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV)) return -1;
bool bIsImageUndistorted = true;
Mat cameraMatrix, distCoeffs;
bool isCamParamsSet = readCameraParameters(sCameraParamFilename, cameraMatrix, distCoeffs);
if (!isCamParamsSet) {
cout << "[WARNING] Camera intrinsic parameters could not be loaded!" << endl;
}
VideoCapture cap(0, cv::CAP_V4L2);
if (!cap.isOpened()) {
@ -137,26 +117,20 @@ int main(int argc, char** argv)
int iLastX = -1, iLastY = -1;
Mat imgTmp;
cap.read(imgTmp);
bool bIsImageUndistorted = true;
while (true) {
Mat imgLines = Mat::zeros(imgTmp.size(), CV_8UC3);
Mat imgOriginal;
bool bSuccess = cap.read(imgOriginal);
if (!bSuccess) {
cout << "[WARNING] Could not read a new frame from video stream" << endl;
break;
}
if (!cap.read(imgOriginal)) break;
if (bIsImageUndistorted && isCamParamsSet) {
Mat temp = imgOriginal.clone();
undistort(temp, imgOriginal, cameraMatrix, distCoeffs);
}
Mat imgHSV;
Mat imgHSV, imgThresholded;
cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV);
Mat imgThresholded;
inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded);
erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)));
@ -176,59 +150,41 @@ int main(int argc, char** argv)
posX = dM10 / dArea;
posY = dM01 / dArea;
if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0) {
line(imgLines, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2);
}
iLastX = posX;
iLastY = posY;
float pixelToCmX = 640.0f / 60.0f;
float pixelToCmY = 480.0f / 45.0f;
float pixelToCmX = 640.0f / 30.0f;
float pixelToCmY = 480.0f / 22.5f;
x = (posX - 320.0f) / pixelToCmX;
y = (240.0f - posY) / pixelToCmY;
vector<float> current_q = {deg2rad(0), deg2rad(80), deg2rad(0), deg2rad(0)};
computeForwardKinematics(current_q[0], current_q[1], current_q[2], current_q[3]);
Vec2 ee_pos = { getEndEffectorX(), getEndEffectorY() }; // Burada robotun ucu pozisyonu
Vec2 ball_pos = { x, y };
Vec2 push_target = computePushTarget(ee_pos, ball_pos);
vector<float> qi = computeInverseKinematics(push_target.x, push_target.y);
computeForwardKinematics(qi[0], qi[1], qi[2], qi[3]);
if (qi.size() == 4) {
_oDxlHandler.sendTargetJointPosition(qi);
}
} else {
// Top görünmüyorsa başlangıç pozisyonuna dön
vector<float> qi = {deg2rad(0), deg2rad(80), deg2rad(0), deg2rad(0)};
computeForwardKinematics(qi[0], qi[1], qi[2], qi[3]);
_oDxlHandler.sendTargetJointPosition(qi);
continue; // bu frame'i geç, aşağıdaki inverse kinematic vs. çalışmasın
}
// Yeşil çizgi ekle
line(imgOriginal, Point(0, imgOriginal.rows / 2 + PUSH_THRESHOLD_CM * (480.0f / 22.5f)),
Point(imgOriginal.cols, imgOriginal.rows / 2 + PUSH_THRESHOLD_CM * (480.0f / 22.5f)),
Scalar(0, 255, 0), 2);
if (y < PUSH_THRESHOLD_CM) { // top çizgiye yaklaştı mı kontrolü
pushBallRandomly(_oDxlHandler);
continue; // itme sonrası bu frame'i atla
continue;
}
imshow("Thresholded Image", imgThresholded);
drawMarker(imgOriginal, Point(imgTmp.size().width / 2, imgTmp.size().height / 2), 10, MARKER_CROSS, LINE_8);
imgOriginal = imgOriginal + imgLines;
imshow("Original", imgOriginal);
vector<float> qi = computeInverseKinematics(x, y);
computeForwardKinematics(qi[0], qi[1], qi[2], qi[3]);
if (qi.size() == 4) {
vector<float> vTargetJointPosition = {qi[0], qi[1], qi[2], qi[3]};
_oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
}
char key = (char)waitKey(1000.0 / fFPS);
if (key == 27) {
cout << "[INFO] esc key is pressed by user -> Shutting down!" << endl;
break;
}
if (key == 'u') {
bIsImageUndistorted = !bIsImageUndistorted;
cout << "[INFO] Image undistorted: " << bIsImageUndistorted << endl;
}
if (key == 27) break;
if (key == 'u') bIsImageUndistorted = !bIsImageUndistorted;
}
_oDxlHandler.closePort();
return 0;
}
}