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 2455e41..5f28a01 100644 --- a/assets/imgui.ini +++ b/assets/imgui.ini @@ -34,13 +34,13 @@ Size=465,224 Collapsed=0 [Window][Kinect output] -Pos=22,364 -Size=584,165 +Pos=611,105 +Size=625,293 Collapsed=0 [Window][Robot model control panel] -Pos=529,115 -Size=566,300 +Pos=646,60 +Size=599,391 Collapsed=0 [Window][Example: Log] diff --git a/src/KinectXRobotApp.cpp b/src/KinectXRobotApp.cpp index 78183a8..22a13e5 100644 --- a/src/KinectXRobotApp.cpp +++ b/src/KinectXRobotApp.cpp @@ -69,22 +69,32 @@ 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; 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 + + 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 @@ -101,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 @@ -127,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, @@ -166,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; @@ -184,11 +210,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; @@ -196,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; @@ -206,9 +237,19 @@ 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; + loops_since_send2 = 0; //initiate kinect device communication this->hr = init_kinect(); @@ -318,7 +359,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 @@ -367,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 @@ -410,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 @@ -424,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)); @@ -438,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")) { @@ -466,9 +505,176 @@ 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 + + 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++; + } + + 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++; + } + + 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_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 + 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 + //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 + + //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_buff2); + //ImGui::Text("SENT"); + 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 2", writebuff2, 32, ImGuiInputTextFlags_CharsUppercase ); + 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(); @@ -509,31 +715,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 2", &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(); @@ -735,6 +965,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; @@ -755,6 +986,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); @@ -767,6 +1000,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 @@ -818,17 +1108,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: @@ -866,7 +1162,52 @@ void KinectXRobotApp::draw_skeleton() { } } - //Draw bones here + + //--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 + + 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); @@ -909,9 +1250,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(); }