From 3d4bfdae2314e4e389e6d9ef9b3e5d245f943501 Mon Sep 17 00:00:00 2001 From: Alfred Abanto <31046120+AJAbanto@users.noreply.github.com> Date: Sun, 16 May 2021 04:07:16 +0800 Subject: [PATCH 1/7] Initial changes to integrate second robot arm --- assets/imgui.ini | 4 +- src/KinectXRobotApp.cpp | 208 ++++++++++++++++++++++++++++++++++++++-- 2 files changed, 203 insertions(+), 9 deletions(-) diff --git a/assets/imgui.ini b/assets/imgui.ini index 2455e41..2f8d9c3 100644 --- a/assets/imgui.ini +++ b/assets/imgui.ini @@ -34,12 +34,12 @@ Size=465,224 Collapsed=0 [Window][Kinect output] -Pos=22,364 +Pos=27,109 Size=584,165 Collapsed=0 [Window][Robot model control panel] -Pos=529,115 +Pos=591,62 Size=566,300 Collapsed=0 diff --git a/src/KinectXRobotApp.cpp b/src/KinectXRobotApp.cpp index 78183a8..8978aef 100644 --- a/src/KinectXRobotApp.cpp +++ b/src/KinectXRobotApp.cpp @@ -69,9 +69,12 @@ class KinectXRobotApp : public App { vector gcode_string_vector; //vector of strings of points in the queue SerialPort s1; - bool port_opened; //port status bool + SerialPort s2; + bool port_opened; //port1 status bool + bool port_opened2; //port2 status bool int gcode_queue_len; //queue length - bool send_gcode; //serial communication flag + bool send_gcode; //serial communication flag for port 1 + bool send_gcode2; //serial communication flag for port 2 //gcode streaming testing tools const int S32_incre = 50.0f; @@ -82,9 +85,11 @@ class KinectXRobotApp : public App { float tot_displacement, old_tot_displacement; int x_origin, y_origin, z_origin; //Initial position of robot int mv_speed; //speed up tool - char gcode_buff[32]; //gcode container before streaming + char gcode_buff[32]; //gcode container before streaming for port 1 + char gcode_buff2[32]; //gcode container before streaming for port 2 bool apply_mvspeed; //speed testing - int loops_since_send; //loop tracker to space out between sends + int loops_since_send; //loop tracker to space out between sends for port 1 + int loops_since_send2; //loop tracker to space out between sends for port 2 //Start of Kinect stuff //Flags and handlers @@ -184,11 +189,16 @@ void KinectXRobotApp::setup() //initialize serial port status gcode_queue_len = 10; //store 10 points at a time port_opened = false; + port_opened2 = false; + send_gcode = false; + send_gcode2 = false; s1 = SerialPort(); //intantiate port object + s2 = SerialPort(); - //initialize gcode buff + //initialize gcode buff for both ports memset(gcode_buff, 0, sizeof(gcode_buff)); + memset(gcode_buff2, 0, sizeof(gcode_buff2)); //set initial position of actual robot x_origin = 0; @@ -209,6 +219,7 @@ void KinectXRobotApp::setup() apply_mvspeed = false; mv_speed = 0; loops_since_send = 0; + loops_since_send2 = 0; //initiate kinect device communication this->hr = init_kinect(); @@ -318,7 +329,8 @@ void KinectXRobotApp::update() ImGui::Spacing(); ImGui::Text("Communications"); - //Opening and closing of Serial port communication + //---------------------------ROBOT ARM CONNECTED TO COM PORT 1---------------------------------- + //Opening and closing of Serial port1 communication if (!port_opened) { //Port not yet opened @@ -466,9 +478,187 @@ void KinectXRobotApp::update() + } + //---------------------------END OF ROBOT ARM CONNECTED TO COM PORT 1----------------------------- + + //---------------------------ROBOT ARM CONNECTED TO COM PORT 2---------------------------------- + //Opening and closing of Serial port2 communication + //NOTE: Some features available with port1 will not be available with port 2 + + if (!port_opened2) { //Port not yet opened + + static char buff2[32] = ""; + ImGui::InputText("port2", buff2, 32, ImGuiInputTextFlags_CharsUppercase | ImGuiInputTextFlags_CharsNoBlank); + + //Attempt to open port + if (ImGui::Button("Open port2\n")) { + + if (s2.open(buff2) == -1) ImGui::OpenPopup("Error COM port2"); //throw error and prepare error modal window + else { + port_opened2 = true; //else raise flag to indicate port is opened + memset(buff2, 0, sizeof(buff2)); //clear textbox after opening + } + } + //if failed to open port show error modal window + if (ImGui::BeginPopupModal("Error COM port2", NULL, 0)) { + ImGui::Text("Could not open port2. Please try again"); + if (ImGui::Button("close port2")) + ImGui::CloseCurrentPopup(); + ImGui::EndPopup(); + } + } + else { + + + + + if (send_gcode2 == true) { + ImGui::Text("STREAMING GCODE.."); + + + //NOTES: Removed option to change origin coordinates + /* + //Modify initial position of actual robot and other commands + if (ImGui::TreeNode("Additional Options")) { + + ImGui::InputScalar("Initial X pos.", ImGuiDataType_S32, &x_origin, &Origin_incre); + ImGui::InputScalar("Initial Y pos.", ImGuiDataType_S32, &y_origin, &Origin_incre); + ImGui::InputScalar("Initial Z pos.", ImGuiDataType_S32, &z_origin, &Origin_incre); + + //enable and disable movement speed command in gcode + ImGui::Checkbox("Apply mv speed", &apply_mvspeed); + if (apply_mvspeed) ImGui::InputScalar("movement speed", ImGuiDataType_S32, &mv_speed, &S32_incre); + + ImGui::TreePop(); + } + */ + + //Int to track how many coordinate values have changed + int coordinates_changed2 = 0; + int displacement_filter2 = 40; + int sample_threshold2 = 2; + + + + + //Only record displacement if: + //----displacement is less than DISPLACEMENT_FILTER units long + //----new displacement has a delta of atleast SAMPLE_THRESHOLD from the previous displacement value + + + //NOTES: only record displacement if port 1 is not opened + // if it is opened, use the previously collected displacement from port 1 + // to determine if we should record displacement and write to robot arm 2 + + if (!port_opened) { + if ((displacement.x < displacement_filter2 && displacement.x > -displacement_filter2) && + ((displacement.x >= x_temp_old + sample_threshold2) || (displacement.x <= x_temp_old - sample_threshold2))) { + x_temp_old = displacement_mult * displacement.x; + coordinates_changed2++; + } + + if ((displacement.y < displacement_filter2 && displacement.y > -displacement_filter2) && + ((displacement.y >= y_temp_old + sample_threshold2) || (displacement.y <= y_temp_old - sample_threshold2))) { + y_temp_old = displacement_mult * displacement.y; + coordinates_changed2++; + } + + if ((displacement.z < displacement_filter2 && displacement.z > -displacement_filter2) && + ((displacement.z >= z_temp_old + sample_threshold2) || (displacement.z <= z_temp_old - sample_threshold2))) { + z_temp_old = displacement_mult * displacement.z; + coordinates_changed2++; + } + } + else { + //coordinates_changed2 = coordinates_changed; + } + + //NOTES: added "2" to all previously used variables for port 1 so that port 2 has independent + // variables + + //Adding displacement to current home position (multiplying displacement with a constant) + int x_dest2 = (x_temp_old * 4) + x_origin; + int y_dest2 = (y_temp_old * 2) + y_origin; + int z_dest2 = (z_temp_old * 2) + z_origin; + + //--------Coordinate edge cases----- + //minimum and maximum X coordinates + if (x_dest2 > 300) x_dest2 = 300; + if (x_dest2 < -300) x_dest2 = -300; + //minimum and maximum Y coordinates + if (y_dest2 < 250) y_dest2 = 320; + if (y_dest2 > 500) y_dest2 = 500; + //minimum and maximum Z coordinates + if (z_dest2 < 100) z_dest2 = 100; + if (z_dest2 > 320) z_dest2 = 320; + + //Apply base speed + if (apply_mvspeed) sprintf(gcode_buff2, "G1X%dY%dZ%dF%d", x_dest2, y_dest2, z_dest2, mv_speed); + else sprintf(gcode_buff2, "G1X%dY%dZ%d", x_dest2, y_dest2, z_dest2); + + + //NOTE: independent loop tracker for port 2 + + //send to robot over serial port if more than 1 coordinate has been updated + //after sending set a timer + if (coordinates_changed2 >= 1 && loops_since_send2 == 0) { + s2.write(gcode_buff); + loops_since_send2 = 10; + } + + //decriment timer till next viable gcode send + if (loops_since_send2 > 0) loops_since_send2 -= 1; + else loops_since_send2 = 0; + + + + ImGui::Text(gcode_buff2); + memset(gcode_buff2, 0, sizeof(gcode_buff2)); + + if (ImGui::Button("Stop gcode stream")) send_gcode2 = false; + } + else { + + //If not streaming gcode , allow user to send discreet gcode commands + //Note: + //Each serial message will be sent after appending both NL and CR character at the end of the gcode command string + static char writebuff2[32] = ""; + ImGui::InputText("gcode command", writebuff2, 32, ImGuiInputTextFlags_CharsUppercase | ImGuiInputTextFlags_CharsNoBlank); + ImGui::SameLine(); + + if (ImGui::Button("send")) { + s2.write(writebuff2); + memset(writebuff2, 0, sizeof(writebuff2)); + } + + //Some discreet robot commands in a hidable tree + if (ImGui::TreeNode("GCODE commands for port 2")) { + if (ImGui::Button("Enable Steppers")) s2.write("M17"); + ImGui::SameLine(); + if (ImGui::Button("Disable Steppers")) s2.write("M18"); + ImGui::TreePop(); + } + + + if (ImGui::Button("Start gcode stream")) send_gcode2 = true; + + if (ImGui::Button("Close port2 connection")) { //If port already opened give show button to close + s2.close(); //close port + port_opened2 = false; //reset flag to allow reconnection + } + + } + + + } + //---------------------------END OF ROBOT ARM CONNECTED TO COM PORT 2----------------------------- + + + + ImGui::End(); @@ -909,9 +1099,13 @@ void KinectXRobotApp::cleanup() { OutputDebugStringA("Kinect cleaned up\n"); } - //Close port if opened + //Close port1 if opened if (port_opened == true) s1.close(); + + //Close port2 if opened + if (port_opened2 == true) + s2.close(); } From bee41a34d7f8790b78cb3cd855c853021d94d70a Mon Sep 17 00:00:00 2001 From: Alfred Abanto <31046120+AJAbanto@users.noreply.github.com> Date: Sun, 16 May 2021 04:16:35 +0800 Subject: [PATCH 2/7] Some testing --- assets/imgui.ini | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/assets/imgui.ini b/assets/imgui.ini index 2f8d9c3..093c578 100644 --- a/assets/imgui.ini +++ b/assets/imgui.ini @@ -34,13 +34,13 @@ Size=465,224 Collapsed=0 [Window][Kinect output] -Pos=27,109 +Pos=-71,73 Size=584,165 Collapsed=0 [Window][Robot model control panel] Pos=591,62 -Size=566,300 +Size=599,391 Collapsed=0 [Window][Example: Log] From b383e0013bd3a063ce4bf5a4fdd625c1105662c8 Mon Sep 17 00:00:00 2001 From: Alfred Abanto <31046120+AJAbanto@users.noreply.github.com> Date: Sun, 16 May 2021 11:41:08 +0800 Subject: [PATCH 3/7] added ability to track second target --- assets/imgui.ini | 6 +- src/KinectXRobotApp.cpp | 250 ++++++++++++++++++++++++++++++++-------- 2 files changed, 202 insertions(+), 54 deletions(-) diff --git a/assets/imgui.ini b/assets/imgui.ini index 093c578..f04d29e 100644 --- a/assets/imgui.ini +++ b/assets/imgui.ini @@ -34,12 +34,12 @@ Size=465,224 Collapsed=0 [Window][Kinect output] -Pos=-71,73 -Size=584,165 +Pos=474,128 +Size=625,293 Collapsed=0 [Window][Robot model control panel] -Pos=591,62 +Pos=702,39 Size=599,391 Collapsed=0 diff --git a/src/KinectXRobotApp.cpp b/src/KinectXRobotApp.cpp index 8978aef..49ceb66 100644 --- a/src/KinectXRobotApp.cpp +++ b/src/KinectXRobotApp.cpp @@ -80,11 +80,16 @@ class KinectXRobotApp : public App { const int S32_incre = 50.0f; const int Origin_incre = 1.0f; - int x_temp, y_temp, z_temp; //Motion capturefiltering tools + int x_temp, y_temp, z_temp; //Motion capturefiltering tools for target 1 int x_temp_old, y_temp_old, z_temp_old; - float tot_displacement, old_tot_displacement; + + int x_temp2, y_temp2, z_temp2; //Motion capturefiltering tools for target 2 + int x_temp2_old, y_temp2_old, z_temp2_old; + int x_origin, y_origin, z_origin; //Initial position of robot + int mv_speed; //speed up tool + char gcode_buff[32]; //gcode container before streaming for port 1 char gcode_buff2[32]; //gcode container before streaming for port 2 bool apply_mvspeed; //speed testing @@ -106,9 +111,19 @@ class KinectXRobotApp : public App { vec3 faux_origin; bool faux_origin_set; + //Faux origin for second tracking target + vec3 faux_origin2; + bool faux_origin2_set; + + int tracking_target; //int for choosing which target to track for displacement + int tracking_target2; //int for second tracking target + bool apply_displacement; //flag for applying displacement + vec3 displacement; //displacement vector calculated relative to origin + vec3 displacement2; //displacement vector for second tracking target + float displacement_mult; //scalar multiplier to change sensitivity of displacement vector @@ -132,6 +147,9 @@ class KinectXRobotApp : public App { //function to set faux origin; void set_faux_origin(); + //function to set second faux origin + void set_faux_origin2(); + //function to draw lines as bones in 3D space void draw3D_bone(NUI_SKELETON_DATA skeletonData, NUI_SKELETON_POSITION_INDEX joint1, @@ -171,7 +189,10 @@ void KinectXRobotApp::setup() //initialize kinect tracking stuff seated_tracking = true; faux_origin_set = false; + faux_origin2_set = false; + tracking_target = 0; + tracking_target2 = 0; apply_displacement = false; displacement_mult = 1.0f; @@ -206,8 +227,8 @@ void KinectXRobotApp::setup() z_origin = 320; //initialize filtering variables - tot_displacement = 0; - old_tot_displacement = 0; + + //--port1 filters x_temp = 0; y_temp = 0; z_temp = 0; @@ -216,6 +237,15 @@ void KinectXRobotApp::setup() y_temp_old = y_temp; z_temp_old = z_temp; + //--port2 filters + x_temp2 = 0; + y_temp2 = 0; + z_temp2 = 0; + + x_temp2_old = x_temp2; + y_temp2_old = y_temp2; + z_temp2_old = z_temp2; + apply_mvspeed = false; mv_speed = 0; loops_since_send = 0; @@ -379,8 +409,6 @@ void KinectXRobotApp::update() int coordinates_changed = 0; int displacement_filter = 40; int sample_threshold = 2; - //get total length of displacement vector - tot_displacement = sqrt((displacement.x * displacement.x) + (displacement.y * displacement.y) + (displacement.z * displacement.z)); //Only record displacement if: //----displacement is less than DISPLACEMENT_FILTER units long @@ -436,8 +464,7 @@ void KinectXRobotApp::update() if (loops_since_send > 0) loops_since_send -= 1; else loops_since_send = 0; - //update record - old_tot_displacement = tot_displacement; + ImGui::Text(gcode_buff); memset(gcode_buff, 0, sizeof(gcode_buff)); @@ -538,48 +565,36 @@ void KinectXRobotApp::update() int displacement_filter2 = 40; int sample_threshold2 = 2; - - //Only record displacement if: //----displacement is less than DISPLACEMENT_FILTER units long //----new displacement has a delta of atleast SAMPLE_THRESHOLD from the previous displacement value + if ((displacement2.x < displacement_filter2 && displacement2.x > -displacement_filter2) && + ((displacement2.x >= x_temp2_old + sample_threshold2) || (displacement2.x <= x_temp2_old - sample_threshold2))) { + x_temp2_old = displacement_mult * displacement2.x; + coordinates_changed2++; + } - //NOTES: only record displacement if port 1 is not opened - // if it is opened, use the previously collected displacement from port 1 - // to determine if we should record displacement and write to robot arm 2 - - if (!port_opened) { - if ((displacement.x < displacement_filter2 && displacement.x > -displacement_filter2) && - ((displacement.x >= x_temp_old + sample_threshold2) || (displacement.x <= x_temp_old - sample_threshold2))) { - x_temp_old = displacement_mult * displacement.x; - coordinates_changed2++; - } - - if ((displacement.y < displacement_filter2 && displacement.y > -displacement_filter2) && - ((displacement.y >= y_temp_old + sample_threshold2) || (displacement.y <= y_temp_old - sample_threshold2))) { - y_temp_old = displacement_mult * displacement.y; - coordinates_changed2++; - } - - if ((displacement.z < displacement_filter2 && displacement.z > -displacement_filter2) && - ((displacement.z >= z_temp_old + sample_threshold2) || (displacement.z <= z_temp_old - sample_threshold2))) { - z_temp_old = displacement_mult * displacement.z; - coordinates_changed2++; - } + if ((displacement2.y < displacement_filter2 && displacement2.y > -displacement_filter2) && + ((displacement2.y >= y_temp2_old + sample_threshold2) || (displacement2.y <= y_temp_old - sample_threshold2))) { + y_temp2_old = displacement_mult * displacement2.y; + coordinates_changed2++; } - else { - //coordinates_changed2 = coordinates_changed; + + if ((displacement2.z < displacement_filter2 && displacement2.z > -displacement_filter2) && + ((displacement2.z >= z_temp2_old + sample_threshold2) || (displacement2.z <= z_temp2_old - sample_threshold2))) { + z_temp2_old = displacement_mult * displacement2.z; + coordinates_changed2++; } //NOTES: added "2" to all previously used variables for port 1 so that port 2 has independent // variables //Adding displacement to current home position (multiplying displacement with a constant) - int x_dest2 = (x_temp_old * 4) + x_origin; - int y_dest2 = (y_temp_old * 2) + y_origin; - int z_dest2 = (z_temp_old * 2) + z_origin; + int x_dest2 = (x_temp2_old * 4) + x_origin; + int y_dest2 = (y_temp2_old * 2) + y_origin; + int z_dest2 = (z_temp2_old * 2) + z_origin; //--------Coordinate edge cases----- //minimum and maximum X coordinates @@ -657,8 +672,6 @@ void KinectXRobotApp::update() //---------------------------END OF ROBOT ARM CONNECTED TO COM PORT 2----------------------------- - - ImGui::End(); @@ -699,31 +712,55 @@ void KinectXRobotApp::update() tracking_targets.push_back("Left foot"); tracking_targets.push_back("Right foot"); - //Displacement vector info + //Displacement vector 1 info string displacement_str = "Displacement: (X" + std::to_string(displacement.x) + " ,Y " + std::to_string(displacement.y) + " ,Z " + std::to_string(displacement.z) + " )"; - //Faux origin vector info + //Displacement vector 2 info + string displacement2_str = "Displacement 2: (X" + std::to_string(displacement2.x) + " ,Y " + std::to_string(displacement2.y) + " ,Z " + + std::to_string(displacement2.z) + " )"; + + //Faux origin 1 vector info string faux_origin_string = "Faux origin ( " + std::to_string(faux_origin.x) + ", " + std::to_string(faux_origin.y) + ", " + std::to_string(faux_origin.z) + ")"; + //Faux origin 2 vector info + string faux_origin2_string = "Faux origin 2 ( " + std::to_string(faux_origin2.x) + ", " + + std::to_string(faux_origin2.y) + ", " + std::to_string(faux_origin2.z) + ")"; + //Kinect tracking control window ImGui::Begin("Kinect output"); - - ImGui::Combo("Tracking target", &tracking_target, tracking_targets); ImGui::Checkbox("Seated Tracking", &seated_tracking); + //----------TRACKING TARGET FOR PORT 1--------------- + ImGui::Combo("Tracking target", &tracking_target, tracking_targets); + + ImGui::Text(faux_origin_string.c_str()); if (ImGui::Button("Set new faux origin")) set_faux_origin(); ImGui::Text(displacement_str.c_str()); - int dummy = 0; //dummy vairable - ImGui::ListBox("Displacement queue", &dummy, gcode_string_vector, gcode_queue_len); //view gcode queue ImGui::Spacing(); ImGui::Separator(); + + //----------TRACKING TARGET FOR PORT 2--------------- + ImGui::Combo("Tracking target", &tracking_target2, tracking_targets); + + ImGui::Text(faux_origin2_string.c_str()); + if (ImGui::Button("Set new faux origin2")) set_faux_origin2(); + + ImGui::Text(displacement2_str.c_str()); + + + ImGui::Spacing(); + ImGui::Separator(); + + + + //Camera controls for the skeleton renderer window if (ImGui::TreeNode("Renderer camera controls")) { ImGui::Spacing(); @@ -925,6 +962,7 @@ void KinectXRobotApp::set_faux_origin() Vector4 left_foot_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_FOOT_LEFT]; Vector4 right_foot_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_FOOT_RIGHT]; + //First target for tracking switch (tracking_target) { case 0: tracked_pos = head_pos; @@ -945,6 +983,8 @@ void KinectXRobotApp::set_faux_origin() tracked_pos = left_hand_pos; } + + //set faux origin temporarily via hardcoding and scale to cm from raw m values float scalar = 100.0f; faux_origin = vec3(tracked_pos.x * scalar, tracked_pos.y * scalar, tracked_pos.z * scalar); @@ -957,6 +997,63 @@ void KinectXRobotApp::set_faux_origin() +void KinectXRobotApp::set_faux_origin2() +{ + //Idea: cycle through all skeleton data, might help stabalize tracking + //Transfer to proper variable + NUI_SKELETON_DATA skeletonData = c_skeletonFrame.SkeletonData[0]; + + for (int i = 0; skeletonData.eTrackingState == NUI_SKELETON_NOT_TRACKED; i++) { + if (i >= 6) return; + else skeletonData = c_skeletonFrame.SkeletonData[i]; + } + + if (skeletonData.eTrackingState != NUI_SKELETON_NOT_TRACKED) { + + //assume that head is tracked so we get the data + Vector4 tracked_pos2 = Vector4(); + + Vector4 head_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_HEAD]; + Vector4 left_hand_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_HAND_LEFT]; + Vector4 right_hand_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT]; + Vector4 left_foot_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_FOOT_LEFT]; + Vector4 right_foot_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_FOOT_RIGHT]; + + //First target for tracking + switch (tracking_target2) { + case 0: + tracked_pos2 = head_pos; + break; + case 1: + tracked_pos2 = left_hand_pos; + break; + case 2: + tracked_pos2 = right_hand_pos; + break; + case 3: + tracked_pos2 = left_foot_pos; + break; + case 4: + tracked_pos2 = right_foot_pos; + break; + default: + tracked_pos2 = left_hand_pos; + } + + + + //set faux origin temporarily via hardcoding and scale to cm from raw m values + float scalar = 100.0f; + faux_origin2 = vec3(tracked_pos2.x * scalar, tracked_pos2.y * scalar, tracked_pos2.z * scalar); + + if (!faux_origin2_set) faux_origin2_set = true; + + return; + } +} + + + //drawing some bones here @@ -1008,17 +1105,23 @@ void KinectXRobotApp::draw_skeleton() { } + //Extracting skeleton data to use for drawing displacement vectors + Vector4 head_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_HEAD]; + Vector4 left_hand_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_HAND_LEFT]; + Vector4 right_hand_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT]; + Vector4 left_foot_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_FOOT_LEFT]; + Vector4 right_foot_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_FOOT_RIGHT]; + + + + + //--Drawing displacement vector for first tracking target //draw vector from faux_origin to tracked target (temporarily left hand) if faux origin has been set if (faux_origin_set) { //get trackig target Vector4 tracked_pos = Vector4(); - Vector4 head_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_HEAD]; - Vector4 left_hand_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_HAND_LEFT]; - Vector4 right_hand_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT]; - Vector4 left_foot_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_FOOT_LEFT]; - Vector4 right_foot_pos = skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_FOOT_RIGHT]; switch (tracking_target) { case 0: @@ -1056,7 +1159,52 @@ void KinectXRobotApp::draw_skeleton() { } } - //Draw bones here + + //--Drawing displacement vector for first tracking target + //draw vector from faux_origin to tracked target (temporarily left hand) if faux origin has been set + if (faux_origin2_set) { + //get trackig target + + Vector4 tracked_pos2 = Vector4(); + + switch (tracking_target2) { + case 0: + tracked_pos2 = head_pos; + break; + case 1: + tracked_pos2 = left_hand_pos; + break; + case 2: + tracked_pos2 = right_hand_pos; + break; + case 3: + tracked_pos2 = left_foot_pos; + break; + case 4: + tracked_pos2 = right_foot_pos; + break; + default: + tracked_pos2 = left_hand_pos; + } + + float scalar = 100.0f; //convert from m to cm + vec3 target_pos2 = vec3(tracked_pos2.x * scalar, tracked_pos2.y * scalar, tracked_pos2.z * scalar); + + + //assume faux_origin2 has been set and scaled properly + gl::color(Color(0, 1, 0)); + gl::lineWidth(8); + gl::drawLine(faux_origin2, target_pos2); + gl::color(Color(1, 1, 1)); + + //calculate and record displacement to be applied to robot + if (apply_displacement) { + displacement2 = target_pos2 - faux_origin2; + } + + } + + //PROCEED TO DRAW THE TRACKED SKELETON //3D render mode draw3D_bone(skeletonData, NUI_SKELETON_POSITION_HEAD, NUI_SKELETON_POSITION_SHOULDER_CENTER); From 05614f3c3d096cd5f934e2c4570dd1b1e0569cf5 Mon Sep 17 00:00:00 2001 From: Alfred Abanto <31046120+AJAbanto@users.noreply.github.com> Date: Sun, 16 May 2021 11:43:47 +0800 Subject: [PATCH 4/7] corrections in comments --- src/KinectXRobotApp.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/KinectXRobotApp.cpp b/src/KinectXRobotApp.cpp index 49ceb66..1bb03f7 100644 --- a/src/KinectXRobotApp.cpp +++ b/src/KinectXRobotApp.cpp @@ -1160,7 +1160,7 @@ void KinectXRobotApp::draw_skeleton() { } - //--Drawing displacement vector for first tracking target + //--Drawing displacement vector for second tracking target //draw vector from faux_origin to tracked target (temporarily left hand) if faux origin has been set if (faux_origin2_set) { //get trackig target From 6289ccf104a7cf8855cebc948b0bd897cd506f4c Mon Sep 17 00:00:00 2001 From: Alfred Abanto Date: Tue, 18 May 2021 16:47:08 +0800 Subject: [PATCH 5/7] testing --- assets/imgui.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/assets/imgui.ini b/assets/imgui.ini index f04d29e..9f41bc9 100644 --- a/assets/imgui.ini +++ b/assets/imgui.ini @@ -34,7 +34,7 @@ Size=465,224 Collapsed=0 [Window][Kinect output] -Pos=474,128 +Pos=83,170 Size=625,293 Collapsed=0 From e403d76d8ee1827e20795392e2ecdefb2db8da11 Mon Sep 17 00:00:00 2001 From: Alfred Abanto Date: Thu, 20 May 2021 03:03:39 +0800 Subject: [PATCH 6/7] first real world dual robot test --- arduino/Community_robot_firmware/robotArm_v0.41/config.h | 2 +- assets/imgui.ini | 4 ++-- src/KinectXRobotApp.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/arduino/Community_robot_firmware/robotArm_v0.41/config.h b/arduino/Community_robot_firmware/robotArm_v0.41/config.h index 214735b..b2773e4 100644 --- a/arduino/Community_robot_firmware/robotArm_v0.41/config.h +++ b/arduino/Community_robot_firmware/robotArm_v0.41/config.h @@ -65,7 +65,7 @@ #define GRIP_STEPS 1200 //FTOBLER: 1200 //COMMAND QUEUE SETTINGS -#define QUEUE_SIZE 15 +#define QUEUE_SIZE 30 //PRINT REPLY SETTING #define PRINT_REPLY false // "true" TO PRINT MSG AFTER ONE COMMAND IS PROCESSED diff --git a/assets/imgui.ini b/assets/imgui.ini index 9f41bc9..81a0b15 100644 --- a/assets/imgui.ini +++ b/assets/imgui.ini @@ -34,12 +34,12 @@ Size=465,224 Collapsed=0 [Window][Kinect output] -Pos=83,170 +Pos=423,77 Size=625,293 Collapsed=0 [Window][Robot model control panel] -Pos=702,39 +Pos=642,39 Size=599,391 Collapsed=0 diff --git a/src/KinectXRobotApp.cpp b/src/KinectXRobotApp.cpp index 1bb03f7..869e873 100644 --- a/src/KinectXRobotApp.cpp +++ b/src/KinectXRobotApp.cpp @@ -477,7 +477,7 @@ void KinectXRobotApp::update() //Note: //Each serial message will be sent after appending both NL and CR character at the end of the gcode command string static char writebuff[32] = ""; - ImGui::InputText("gcode command", writebuff, 32, ImGuiInputTextFlags_CharsUppercase | ImGuiInputTextFlags_CharsNoBlank ); + ImGui::InputText("gcode command", writebuff, 32, ImGuiInputTextFlags_CharsUppercase ); ImGui::SameLine(); if (ImGui::Button("send")) { @@ -617,7 +617,7 @@ void KinectXRobotApp::update() //send to robot over serial port if more than 1 coordinate has been updated //after sending set a timer if (coordinates_changed2 >= 1 && loops_since_send2 == 0) { - s2.write(gcode_buff); + s2.write(gcode_buff2); loops_since_send2 = 10; } @@ -638,7 +638,7 @@ void KinectXRobotApp::update() //Note: //Each serial message will be sent after appending both NL and CR character at the end of the gcode command string static char writebuff2[32] = ""; - ImGui::InputText("gcode command", writebuff2, 32, ImGuiInputTextFlags_CharsUppercase | ImGuiInputTextFlags_CharsNoBlank); + ImGui::InputText("gcode command 2", writebuff2, 32, ImGuiInputTextFlags_CharsUppercase ); ImGui::SameLine(); if (ImGui::Button("send")) { @@ -747,7 +747,7 @@ void KinectXRobotApp::update() //----------TRACKING TARGET FOR PORT 2--------------- - ImGui::Combo("Tracking target", &tracking_target2, tracking_targets); + ImGui::Combo("Tracking target 2", &tracking_target2, tracking_targets); ImGui::Text(faux_origin2_string.c_str()); if (ImGui::Button("Set new faux origin2")) set_faux_origin2(); From a9dc1e650ade102ca7c1184040650456fbb48ac3 Mon Sep 17 00:00:00 2001 From: Alfred Abanto Date: Mon, 24 May 2021 12:46:00 +0800 Subject: [PATCH 7/7] Flipped Y and Z axis as correction --- assets/imgui.ini | 4 ++-- src/KinectXRobotApp.cpp | 11 +++++++---- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/assets/imgui.ini b/assets/imgui.ini index 81a0b15..5f28a01 100644 --- a/assets/imgui.ini +++ b/assets/imgui.ini @@ -34,12 +34,12 @@ Size=465,224 Collapsed=0 [Window][Kinect output] -Pos=423,77 +Pos=611,105 Size=625,293 Collapsed=0 [Window][Robot model control panel] -Pos=642,39 +Pos=646,60 Size=599,391 Collapsed=0 diff --git a/src/KinectXRobotApp.cpp b/src/KinectXRobotApp.cpp index 869e873..22a13e5 100644 --- a/src/KinectXRobotApp.cpp +++ b/src/KinectXRobotApp.cpp @@ -450,8 +450,8 @@ void KinectXRobotApp::update() if (z_dest > 320) z_dest = 320; //Apply base speed - if (apply_mvspeed) sprintf(gcode_buff, "G1X%dY%dZ%dF%d", x_dest, y_dest, z_dest, mv_speed); - else sprintf(gcode_buff, "G1X%dY%dZ%d", x_dest, y_dest, z_dest); + if (apply_mvspeed) sprintf(gcode_buff, "G1X%dY%dZ%dF%d", x_dest, z_dest, y_dest, mv_speed); + else sprintf(gcode_buff, "G1X%dY%dZ%d", x_dest, z_dest, y_dest ); //NOTE: flip Z and Y since kinect interprets Z as "Depth" instead of "height" //send to robot over serial port if more than 1 coordinate has been updated //after sending set a timer @@ -608,8 +608,10 @@ void KinectXRobotApp::update() if (z_dest2 > 320) z_dest2 = 320; //Apply base speed - if (apply_mvspeed) sprintf(gcode_buff2, "G1X%dY%dZ%dF%d", x_dest2, y_dest2, z_dest2, mv_speed); - else sprintf(gcode_buff2, "G1X%dY%dZ%d", x_dest2, y_dest2, z_dest2); + //NOTE: Z and Y have been flipped since Z is "Depth" for the kinect and Y is "height" + //Whilst it is the converse for the Robots + if (apply_mvspeed) sprintf(gcode_buff2, "G1X%dY%dZ%dF%d", x_dest2, z_dest2, y_dest2, mv_speed); + else sprintf(gcode_buff2, "G1X%dY%dZ%d", x_dest2, z_dest2, y_dest2); //NOTE: independent loop tracker for port 2 @@ -618,6 +620,7 @@ void KinectXRobotApp::update() //after sending set a timer if (coordinates_changed2 >= 1 && loops_since_send2 == 0) { s2.write(gcode_buff2); + //ImGui::Text("SENT"); loops_since_send2 = 10; }