int main(int argc, char** argv) {
// Ensure corrent number of arguments provided
if (argc != ARG_COUNT) {
cout << "Incorrect argument count" << endl;
return -1;
}
// Handle shutdown
signal(SIGINT, sigint_handler);
// Delta Robot
DeltaRobot delta(serial, 1, 2, 3, 4);
delta.ctrlWindow();
bool torque = true;
// RGBD Camera
if (!camera.connect(1)) {
cout << "Failed to connect to camera" << endl;
return -3;
}
filter.ctrlWindow();
// Detector
Detector detect(detectorTraining);
detect.ctrlWindow();
// Bandaid orientation
Bandaid bandaid(camera.getFX(), camera.getFY());
// TODO: Clean
vector<Rect> rects;
Pathing path(&camera, translate);
while (state != Shutdown) {
// Get camera frame
camera.getFrames();
rgb = camera.getRGB();
depth = camera.getDepth();
grey = camera.getGrey();
// Filter out end effector
// if (!filter.started()) {
// filter.start(rgb, depth);
// }
// Vector2i effectorPos2D = camera.to2D(
// translate.deltaToCamera(delta.getPos()));
// filter.clean(rgb, depth, effectorPos2D);
// Update pathing depth map
Vector3f effectorPos3D = translate.deltaToReal(delta.getPos());
if (effectorPos3D[1] > 200) {
path.setDepth(depth);
}
imshow("path", path.depth);
path.setPosition(effectorPos3D);
Mat mask = Mat::zeros(depth.size(), CV_8UC1);
bool found = false;
// Display current images
switch (state) {
case Idle:
imshow("main", rgb);
imshow("depth", depth / 4500.0f);
setMouseCallback("main", onMouse, 0 );
setMouseCallback("depth", onMouse, 0 );
break;
case HandDetect:
// Detect hand
found = detect.detectHand(depth, mask);
if (found) {
Mat red(rgb.rows, rgb.cols, CV_8UC4);
red.setTo(Scalar(0, 0, 255));
red.copyTo(rgb, mask);
}
imshow("main", rgb);
imshow("depth", depth / 4500.0f);
break;
case Shutdown:
// Nothing
break;
}
Vector3f pos, end;
Vector2i point, point2;
float angle;
char input[32];
vector<Vector3i> t;
vector<Vector3f> f, f2;
vector<uint8_t> send;
switch (waitKey(1) & 0xFF) {
case 'h': // Hide the end effector
delta.setPosition(Vector3f(0, -180, -600));
break;
case 'f': // Find hand
if (state == HandDetect) {
state = Idle;
} else {
// Move effector out of the way
delta.setPosition(Vector3f(0, -180, -600));
state = HandDetect;
}
break;
case 'm': // Move to position
// Enter point
cout << "Enter X (mm): ";
cin >> input;
pos[0] = atof(input);
cout << "Enter Y (mm): ";
cin >> input;
pos[1] = atof(input);
cout << "Enter Z (mm): ";
cin >> input;
pos[2] = atof(input);
delta.move(path.move(pos));
break;
case '1': // Complete task 1
// Check if hand mask available
if (!found) {
break;
}
// Enter start x + y
cout << "Enter Start X (mm): ";
cin >> input;
pos[0] = atof(input);
cout << "Enter Start Y (mm): ";
cin >> input;
pos[1] = atof(input);
// // Enter end x + y
cout << "Enter End X (mm): ";
cin >> input;
end[0] = atof(input);
cout << "Enter End Y (mm): ";
cin >> input;
end[1] = atof(input);
// Complete task 1
delta.move(path.task1(pos, end, mask));
state = Idle;
break;
case '2':
// Enter point
cout << "Enter X (mm): ";
cin >> input;
point[0] = atof(input);
cout << "Enter Y (mm): ";
cin >> input;
point[1] = atof(input);
// Compelete task 2
delta.move(path.task2(point));
state = Idle;
break;
case '3':
// Check if hand mask available
if (!found) {
break;
}
// Enter point
cout << "Enter X (mm): ";
cin >> input;
pos[0] = atof(input);
cout << "Enter Y (mm): ";
cin >> input;
pos[1] = atof(input);
pos[2] = 0;
// Convert to camera
point = camera.to2D(translate.realToCamera(pos));
angle = bandaid.find(mask, point, depth.at<float>(point[1], point[0]));
delta.setBandaid(angle * 180 / M_PI);
// Complete task 3
point2 = Vector2i(pos[0], pos[1]);
f = path.task2(point2);
f2.clear();
for (Vector3f vec: f) {
vec[2] += 10;
f2.push_back(vec);
}
f2.rbegin()[1][2] -= 100;
end = f2.rbegin()[1];
f2.pop_back();
for (int x = 0; x < 10; x++) {
f2.push_back(end);
}
end[2] += 200;
f2.push_back(end);
delta.move(f2);
state = Idle;
break;
case '4': // Task 4
// Accept multiple coordinates
f.clear();
while (true) {
// Enter point
cout << "Enter X (mm): ";
cin >> input;
pos[0] = atof(input);
cout << "Enter Y (mm): ";
cin >> input;
pos[1] = atof(input);
pos[2] = 0;
if (pos[0] == 0 && pos[1] == 0) {
break;
} else {
f.push_back(pos);
}
}
// Complete task 4
delta.move(path.task4(f));
state = Idle;
break;
// circle
// fill
case 't': // Toggle Torque
torque = !torque;
delta.setTorque(torque);
break;
case 'p': // Print all register data
delta.printAll();
break;
case 'i': // Print position data
delta.printPosition();
break;
case 27:
state = Shutdown;
break;
}
}
delta.setTorque(false);
delta.disconnect();
camera.disconnect();
return 0;
}
