项目介绍

研究生新生开学，偷得两日闲，记录一下本科期间自己参与的一个项目。本f项目是在实验室的人型机器人的基础上进行的创新性开发。硬件部分为Kinect二代体感器，多自由度桌面小型机械臂两部分。软件同样分为两部分。本人主要负责上位机的代码。

上位机代码说明

上位机代码可以分为识别和发送两部分：

识别部分：调用了Kinect体感器的API进行人体骨架识别并获骨骼点坐标。
发送部分：该部分使用了蓝牙串口通讯。

1.识别部分

1.1GetPosition.cpp

这部分代码是直接与下位机进行通讯，主要思路是，先对桌面机械臂每舵机进行编号，同时对一条手臂上的关节点与舵机对应。在发送关节角度时，先对计算所得角度的合理性进行判断。通讯格式是以舵机编号开头，后接关节角度，存为字符型数组。

//
///
/// @file    GetPosition.cpp
///
///
/// 本文件为获取骨骼坐标并输出角度的实现代码  /// @author:  KC
/// @date :   2020/10/27
///
///
//  #include"pch.h"
#include<iostream>
#include<windows.h>
#include<kinect.h>
#include<DirectXMath.h>
#include"SerialPort.h"
#include"kinectJointFilter.h"using namespace std;
using namespace Sample;
using namespace DirectX;template <class Interface> //释放指针模板类
inline void SafeRelease(Interface *& pInterfaceToRelease)
{if (pInterfaceToRelease){pInterfaceToRelease->Release();pInterfaceToRelease = NULL;}
}/*角度计算*/
FLOAT Angle(const DirectX::XMVECTOR* vec, JointType jointA, JointType jointB, JointType jointC)
{float angle = 0.0f;XMVECTOR vBA = XMVectorSubtract(vec[jointB], vec[jointA]);XMVECTOR vBC = XMVectorSubtract(vec[jointB], vec[jointC]);XMVECTOR vAngle = XMVector3AngleBetweenVectors(vBA, vBC);angle = XMVectorGetX(vAngle) * 180.0 * XM_1DIVPI;    // XM_1DIVPI: An optimal representation of 1 / πreturn angle;
}/*串口发送*/
void PortSend(int flag, int t)
{char aa[15] = "#001";CSerialPort mySerialPort;if (!mySerialPort.InitPort(7)){cout << "initPort fail !" << endl;}else{cout << "initPort success !" << endl;}if (!mySerialPort.OpenListenThread()){cout << "OpenListenThread fail !" << endl;}else{cout << "OpenListenThread success !" << endl;}if (flag == 1){char aa[15] = "#001P";aa[5] = (char)('0' + (t / 1000));aa[6] = (char)('0' + (t % 1000) / 100);aa[7] = (char)('0' + (t % 100) / 10);aa[8] = (char)('0' + (t % 10));aa[9] = 'T';aa[10] = '1';aa[11] = '0';aa[12] = '0';aa[13] = '0';aa[14] = '!';mySerialPort.WriteData((unsigned char *)&aa, 15);}else if (flag == 2){char aa[15] = "#002P";aa[5] = (char)('0' + (t / 1000));aa[6] = (char)('0' + (t % 1000) / 100);aa[7] = (char)('0' + (t % 100) / 10);aa[8] = (char)('0' + (t % 10));aa[9] = 'T';aa[10] = '1';aa[11] = '0';aa[12] = '0';aa[13] = '0';aa[14] = '!';mySerialPort.WriteData((unsigned char *)&aa, 15);}else if (flag == 3){char aa[15] = "#003P";aa[5] = (char)('0' + (t / 1000));aa[6] = (char)('0' + (t % 1000) / 100);aa[7] = (char)('0' + (t % 100) / 10);aa[8] = (char)('0' + (t % 10));aa[9] = 'T';aa[10] = '1';aa[11] = '0';aa[12] = '0';aa[13] = '0';aa[14] = '!';mySerialPort.WriteData((unsigned char *)&aa, 15);}else if (flag == 4){char aa[15] = "#004P";aa[5] = (char)('0' + (t / 1000));aa[6] = (char)('0' + (t % 1000) / 100);aa[7] = (char)('0' + (t % 100) / 10);aa[8] = (char)('0' + (t % 10));aa[9] = 'T';aa[10] = '1';aa[11] = '0';aa[12] = '0';aa[13] = '0';aa[14] = '!';mySerialPort.WriteData((unsigned char *)&aa, 15);}
}void delay(float x)
{LARGE_INTEGER litmp;LONGLONG QPart1, QPart2;double dfMinus, dfFreq, dfTim;QueryPerformanceFrequency(&litmp);dfFreq = (double)litmp.QuadPart;// 获得计数器的时钟频率QueryPerformanceCounter(&litmp);QPart1 = litmp.QuadPart;// 获得初始值do{QueryPerformanceCounter(&litmp);QPart2 = litmp.QuadPart;//获得中止值dfMinus = (double)(QPart2 - QPart1);dfTim = dfMinus / dfFreq;// 获得对应的时间值，单位为秒} while (dfTim < x);
}// output operator for CameraSpacePoint
ostream& operator<<(ostream& rOS, const CameraSpacePoint& rPos)
{rOS << "(" << rPos.X << "/" << rPos.Y << "/" << rPos.Z << ")";return rOS;
}
// output operator for Vector4
ostream& operator<<(ostream& rOS, const Vector4& rVec)
{rOS << "(" << rVec.x << "/" << rVec.y << "/" << rVec.z << "/" << rVec.w << ")";return rOS;
}int main(int argc, char** argv[])
{// 1a. Get default Sensorcout << "Try to get default sensor" << endl;IKinectSensor* pSensor = nullptr;if (GetDefaultKinectSensor(&pSensor) != S_OK){cerr << "Get Sensor failed" << endl;return -1;}// 1b. Open sensorcout << "Try to open sensor" << endl;if (pSensor->Open() != S_OK){cerr << "Can't open sensor" << endl;return -1;}// 2a. Get frame sourcecout << "Try to get body source" << endl;IBodyFrameSource* pFrameSource = nullptr;if (pSensor->get_BodyFrameSource(&pFrameSource) != S_OK){cerr << "Can't get body frame source" << endl;return -1;}// 2b. Get the number of bodyINT32 iBodyCount = 0;if (pFrameSource->get_BodyCount(&iBodyCount) != S_OK){cerr << "Can't get body count" << endl;return -1;}cout << " > Can trace " << iBodyCount << " bodies" << endl;IBody** aBody = new IBody*[iBodyCount];for (int i = 0; i < iBodyCount; ++i)aBody[i] = nullptr;// 3a. get frame readercout << "Try to get body frame reader" << endl;IBodyFrameReader* pFrameReader = nullptr;if (pFrameSource->OpenReader(&pFrameReader) != S_OK){cerr << "Can't get body frame reader" << endl;return -1;}/******************************************************************************//*holt双指数滤波器*/FilterDoubleExponential filter[BODY_COUNT];// 设置平滑参数for (int count = 0; count < iBodyCount; count++){float smoothing = 0.25f;          // [0..1], lower values closer to raw datafloat correction = 0.25f;         // [0..1], lower values slower to correct towards the raw datafloat prediction = 0.25f;         // [0..n], the number of frames to predict into the futurefloat jitterRadius = 0.03f;       // The radius in meters for jitter reductionfloat maxDeviationRadius = 0.05f; // The maximum radius in meters that filtered positions are allowed to deviate from raw datafilter[count].Init(smoothing, correction, prediction, jitterRadius, maxDeviationRadius);}// 2b. release Frame sourcecout << "Release frame source" << endl;SafeRelease(pFrameSource);// Enter main loopint iStep = 0;while (iStep < 10000){// 4a. Get last frameIBodyFrame* pFrame = nullptr;if (pFrameReader->AcquireLatestFrame(&pFrame) == S_OK){++iStep;// 4b. get Body dataif (pFrame->GetAndRefreshBodyData(iBodyCount, aBody) == S_OK){int iTrackedBodyCount = 0;// 4c. for each bodyfor (int i = 0; i < iBodyCount; i++){IBody* pBody = aBody[i];// check if is trackedBOOLEAN bTracked = false;if ((pBody->get_IsTracked(&bTracked) == S_OK) && bTracked){++iTrackedBodyCount;cout << "User " << i << " is under tracking" << endl;// get joint positionJoint aJoints[JointType::JointType_Count];if (pBody->GetJoints(JointType::JointType_Count, aJoints) != S_OK){cerr << "Get joints fail" << endl;}// get joint orientationJointOrientation aOrientations[JointType::JointType_Count];if (pBody->GetJointOrientations(JointType::JointType_Count, aOrientations) != S_OK){cerr << "Get joints fail" << endl;}// 滤波接口filter[i].Update(aJoints);const DirectX::XMVECTOR* vec_1 = filter[i].GetFilteredJoints();//肘关节角度const DirectX::XMVECTOR* vec_2 = filter[i].GetFilteredJoints();//肩关节角度const DirectX::XMVECTOR* vec_3 = filter[i].GetFilteredJoints();//右手平动角//输出坐标/*右腕*/JointType WristRightJointType = JointType::JointType_WristRight;const Joint& WristRightJointPos = aJoints[WristRightJointType];const JointOrientation& WristRightJointOri = aOrientations[WristRightJointType];cout << " > WristRight is ";if (WristRightJointPos.TrackingState == TrackingState_NotTracked){cout << "not tracked" << endl;}if (WristRightJointPos.TrackingState == TrackingState_NotTracked){cout << "not tracked" << endl;}else{if (WristRightJointPos.TrackingState == TrackingState_Inferred){cout << "inferred ";}else if (WristRightJointPos.TrackingState == TrackingState_Tracked){cout << "tracked ";}cout << "at" << WristRightJointPos.Position << endl;}/*右肩*/JointType ShoulderRightJointType = JointType::JointType_ShoulderRight;const Joint& ShoulderRightJointPos = aJoints[ShoulderRightJointType];const JointOrientation& ShoulderRightJointOri = aOrientations[ShoulderRightJointType];cout << " > ShoulderRight is ";if (ShoulderRightJointPos.TrackingState == TrackingState_NotTracked){cout << "not tracked" << endl;}if (ShoulderRightJointPos.TrackingState == TrackingState_NotTracked){cout << "not tracked" << endl;}else{if (ShoulderRightJointPos.TrackingState == TrackingState_Inferred){cout << "inferred ";}else if (ShoulderRightJointPos.TrackingState == TrackingState_Tracked){cout << "tracked ";}cout << "at " << ShoulderRightJointPos.Position << endl;}/*右肘*/JointType ElbowRightJointType = JointType::JointType_ElbowRight;const Joint& ElbowRightJointPos = aJoints[ElbowRightJointType];const JointOrientation& ElbowRightJointOri = aOrientations[ElbowRightJointType];cout << " > ElbowRight is ";if (ElbowRightJointPos.TrackingState == TrackingState_NotTracked){cout << "not tracked" << endl;}if (ElbowRightJointPos.TrackingState == TrackingState_NotTracked){cout << "not tracked" << endl;}else{if (ElbowRightJointPos.TrackingState == TrackingState_Inferred){cout << "inferred ";}else if (ElbowRightJointPos.TrackingState == TrackingState_Tracked){cout << "tracked ";}cout << "at " << ElbowRightJointPos.Position << endl;}  /*左肘*/JointType ElbowLeftightJointType = JointType::JointType_ElbowLeft;const Joint& ElbowLeftJointPos = aJoints[ElbowLeftightJointType];const JointOrientation& ElbowLeftJointOri = aOrientations[ElbowLeftightJointType];cout << " > ElbowLeft is ";if (ElbowLeftJointPos.TrackingState == TrackingState_NotTracked){cout << "not tracked" << endl;}if (ElbowLeftJointPos.TrackingState == TrackingState_NotTracked){cout << "not tracked" << endl;}else{if (ElbowLeftJointPos.TrackingState == TrackingState_Inferred){cout << "inferred ";}else if (ElbowLeftJointPos.TrackingState == TrackingState_Tracked){cout << "tracked ";}cout << "at " << ElbowLeftJointPos.Position << endl;}/*左肩*/JointType ShoulderLeftJointType = JointType::JointType_ShoulderRight;const Joint& ShoulderLeftJointPos = aJoints[ShoulderLeftJointType];const JointOrientation& ShoulderLeftJointOri = aOrientations[ShoulderLeftJointType];cout << " > ShoulderRight is ";if (ShoulderLeftJointPos.TrackingState == TrackingState_NotTracked){cout << "not tracked" << endl;}if (ShoulderLeftJointPos.TrackingState == TrackingState_NotTracked){cout << "not tracked" << endl;}else{if (ShoulderLeftJointPos.TrackingState == TrackingState_Inferred){cout << "inferred ";}else if (ShoulderLeftJointPos.TrackingState == TrackingState_Tracked){cout << "tracked ";}cout << "at " << ShoulderLeftJointPos.Position << endl;}/*计算角度*//*肘*/int AngleForRightElbow = Angle(vec_1, JointType_WristRight, JointType_ElbowRight, JointType_ShoulderRight);/*肩*/int AngleForRightShoulder = Angle(vec_2, JointType_ElbowRight, JointType_ShoulderRight, JointType_ShoulderLeft);/*右手平动*/int Angle1_3 = Angle(vec_3, JointType_ElbowLeft, JointType_ShoulderLeft, JointType_ShoulderRight);/*显示与发送*/int P = 0;if ((AngleForRightElbow) && (AngleForRightElbow <= 180)){P = AngleForRightElbow * 11.1;P = P + 500;cout << "右手肘的角度：" << AngleForRightElbow << endl;cout << "对应舵机参数：" << P << endl;PortSend(3, P);Sleep(50);P = 0;}if ((AngleForRightShoulder) && (AngleForRightShoulder <= 180)){P = AngleForRightShoulder * 11.1;P = P + 500;cout << "右肩膀的角度：" << AngleForRightShoulder << endl;cout << "对应舵机参数：" << P << endl;PortSend(2, P);Sleep(50);P = 0;}if ((Angle1_3) && (Angle1_3 <= 180)){P = Angle1_3 * 11.1;P = P + 500;cout << "左手平动的角度：" << Angle1_3 << endl;cout << "对应舵机参数：" << P << endl;PortSend(1, P);Sleep(80);P = 0;}}}if (iTrackedBodyCount > 0)cout << "Total " << iTrackedBodyCount << " bodies in this time\n" << endl;}else{cerr << "Can't read body data" << endl;}// 4e. release frameSafeRelease(pFrame);}}// delete body data arraydelete[] aBody;// 3b. release frame readercout << "Release frame reader" << endl;SafeRelease(pFrameReader);// 1c. Close Sensorcout << "close sensor" << endl;pSensor->Close();// 1d. Release Sensorcout << "Release sensor" << endl;SafeRelease(pSensor);return 0;
}

1.2 KinetJiointFilter.cpp

对骨骼点的坐标进行平滑滤波，减少出现数据急剧变化的情况。


#include "pch.h"
#include "KinectJointFilter.h"using namespace Sample;
using namespace DirectX;/* 两个浮点间的线性插值*/
inline FLOAT Lerp(FLOAT f1, FLOAT f2, FLOAT fBlend)
{return f1 + (f2 - f1) * fBlend;
}// if joint is 0 it is not valid.
inline BOOL JointPositionIsValid(XMVECTOR vJointPosition)
{return (XMVectorGetX(vJointPosition) != 0.0f ||XMVectorGetY(vJointPosition) != 0.0f ||XMVectorGetZ(vJointPosition) != 0.0f);
}
/* Holt双指数平滑滤波器的实现双指数*/void FilterDoubleExponential::Update(IBody* const pBody)
{assert(pBody);// Check for divide by zero. Use an epsilon of a 10th of a millimeterm_fJitterRadius = XMMax(0.0001f, m_fJitterRadius);TRANSFORM_SMOOTH_PARAMETERS SmoothingParams;UINT jointCapacity = 0;Joint joints[JointType_Count];pBody->GetJoints(jointCapacity, joints);for (INT i = 0; i < JointType_Count; i++){SmoothingParams.fSmoothing = m_fSmoothing;SmoothingParams.fCorrection = m_fCorrection;SmoothingParams.fPrediction = m_fPrediction;SmoothingParams.fJitterRadius = m_fJitterRadius;SmoothingParams.fMaxDeviationRadius = m_fMaxDeviationRadius;// If inferred, we smooth a bit more by using a bigger jitter radiusJoint joint = joints[i];if (joint.TrackingState == TrackingState::TrackingState_Inferred){SmoothingParams.fJitterRadius *= 2.0f;SmoothingParams.fMaxDeviationRadius *= 2.0f;}Update(joints, i, SmoothingParams);}
}void FilterDoubleExponential::Update(Joint joints[])
{// Check for divide by zero. Use an epsilon of a 10th of a millimeterm_fJitterRadius = XMMax(0.0001f, m_fJitterRadius);TRANSFORM_SMOOTH_PARAMETERS SmoothingParams;for (INT i = 0; i < JointType_Count; i++){SmoothingParams.fSmoothing = m_fSmoothing;SmoothingParams.fCorrection = m_fCorrection;SmoothingParams.fPrediction = m_fPrediction;SmoothingParams.fJitterRadius = m_fJitterRadius;SmoothingParams.fMaxDeviationRadius = m_fMaxDeviationRadius;// If inferred, we smooth a bit more by using a bigger jitter radiusJoint joint = joints[i];if (joint.TrackingState == TrackingState::TrackingState_Inferred){SmoothingParams.fJitterRadius *= 2.0f;SmoothingParams.fMaxDeviationRadius *= 2.0f;}Update(joints, i, SmoothingParams);}}void FilterDoubleExponential::Update(Joint joints[], UINT JointID, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
{XMVECTOR vPrevRawPosition;XMVECTOR vPrevFilteredPosition;XMVECTOR vPrevTrend;XMVECTOR vRawPosition;XMVECTOR vFilteredPosition;XMVECTOR vPredictedPosition;XMVECTOR vDiff;XMVECTOR vTrend;XMVECTOR vLength;FLOAT fDiff;BOOL bJointIsValid;const Joint joint = joints[JointID];vRawPosition = XMVectorSet(joint.Position.X, joint.Position.Y, joint.Position.Z, 0.0f);vPrevFilteredPosition = m_pHistory[JointID].m_vFilteredPosition;vPrevTrend = m_pHistory[JointID].m_vTrend;vPrevRawPosition = m_pHistory[JointID].m_vRawPosition;bJointIsValid = JointPositionIsValid(vRawPosition);// If joint is invalid, reset the filterif (!bJointIsValid){m_pHistory[JointID].m_dwFrameCount = 0;}// Initial start valuesif (m_pHistory[JointID].m_dwFrameCount == 0){vFilteredPosition = vRawPosition;vTrend = XMVectorZero();m_pHistory[JointID].m_dwFrameCount++;}else if (m_pHistory[JointID].m_dwFrameCount == 1){vFilteredPosition = XMVectorScale(XMVectorAdd(vRawPosition, vPrevRawPosition), 0.5f);vDiff = XMVectorSubtract(vFilteredPosition, vPrevFilteredPosition);vTrend = XMVectorAdd(XMVectorScale(vDiff, smoothingParams.fCorrection), XMVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));m_pHistory[JointID].m_dwFrameCount++;}else{// First apply jitter filtervDiff = XMVectorSubtract(vRawPosition, vPrevFilteredPosition);vLength = XMVector3Length(vDiff);fDiff = fabs(XMVectorGetX(vLength));if (fDiff <= smoothingParams.fJitterRadius){vFilteredPosition = XMVectorAdd(XMVectorScale(vRawPosition, fDiff / smoothingParams.fJitterRadius),XMVectorScale(vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius));}else{vFilteredPosition = vRawPosition;}// Now the double exponential smoothing filtervFilteredPosition = XMVectorAdd(XMVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing),XMVectorScale(XMVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing));vDiff = XMVectorSubtract(vFilteredPosition, vPrevFilteredPosition);vTrend = XMVectorAdd(XMVectorScale(vDiff, smoothingParams.fCorrection), XMVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));}// Predict into the future to reduce latencyvPredictedPosition = XMVectorAdd(vFilteredPosition, XMVectorScale(vTrend, smoothingParams.fPrediction));// Check that we are not too far away from raw datavDiff = XMVectorSubtract(vPredictedPosition, vRawPosition);vLength = XMVector3Length(vDiff);fDiff = fabs(XMVectorGetX(vLength));if (fDiff > smoothingParams.fMaxDeviationRadius){vPredictedPosition = XMVectorAdd(XMVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff),XMVectorScale(vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));}// Save the data from this framem_pHistory[JointID].m_vRawPosition = vRawPosition;m_pHistory[JointID].m_vFilteredPosition = vFilteredPosition;m_pHistory[JointID].m_vTrend = vTrend;// Output the datam_pFilteredJoints[JointID] = vPredictedPosition;m_pFilteredJoints[JointID] = XMVectorSetW(m_pFilteredJoints[JointID], 1.0f);
}

1.3

这里是使用了别人的串并口通信类（作者信息见代码块）

//
/// COPYRIGHT NOTICE
/// Copyright (c) 2009, 华中科技大学tickTick Group  （版权声明）
/// All rights reserved.
///
/// @file    SerialPort.cpp
/// @brief   串口通信类的实现文件
///
/// 本文件为串口通信类的实现代码
///
/// @version 1.0
/// @author  卢俊
/// @E-mail：lujun.hust@gmail.com
/// @date    2010/03/19
///
///
///  修订说明：
//  #include "pch.h"
#include "SerialPort.h"
#include <process.h>
#include <iostream>  /** 线程退出标志 */
bool CSerialPort::s_bExit = false;
/** 当串口无数据时,sleep至下次查询间隔的时间,单位:秒 */
const UINT SLEEP_TIME_INTERVAL = 5;CSerialPort::CSerialPort(void): m_hListenThread(INVALID_HANDLE_VALUE)
{m_hComm = INVALID_HANDLE_VALUE;m_hListenThread = INVALID_HANDLE_VALUE;InitializeCriticalSection(&m_csCommunicationSync);//std::cout << "Hello World1";
}CSerialPort::~CSerialPort(void)
{CloseListenTread();ClosePort();DeleteCriticalSection(&m_csCommunicationSync);//std::cout << "Hello World2";
}bool CSerialPort::InitPort(UINT portNo/*= 1*/, UINT baud /*= CBR_9600*/, char parity /*= 'N'*/,UINT databits /*= 8*/, UINT stopsbits/*= 1*/, DWORD dwCommEvents /*= EV_RXCHAR*/)
{/** 临时变量,将制定参数转化为字符串形式,以构造DCB结构 */char szDCBparam[50];sprintf_s(szDCBparam, "baud=%d parity=%c data=%d stop=%d", baud, parity, databits, stopsbits);/** 打开指定串口,该函数内部已经有临界区保护,上面请不要加保护 */if (!openPort(portNo)){return false;}/** 进入临界段 */EnterCriticalSection(&m_csCommunicationSync);/** 是否有错误发生 */BOOL bIsSuccess = TRUE;/** 在此可以设置输入输出的缓冲区大小,如果不设置,则系统会设置默认值.*  自己设置缓冲区大小时,要注意设置稍大一些,避免缓冲区溢出*//*if (bIsSuccess ){bIsSuccess = SetupComm(m_hComm,10,10);}*//** 设置串口的超时时间,均设为0,表示不使用超时限制 */COMMTIMEOUTS  CommTimeouts;CommTimeouts.ReadIntervalTimeout = 0;CommTimeouts.ReadTotalTimeoutMultiplier = 0;CommTimeouts.ReadTotalTimeoutConstant = 0;CommTimeouts.WriteTotalTimeoutMultiplier = 0;CommTimeouts.WriteTotalTimeoutConstant = 0;if (bIsSuccess){bIsSuccess = SetCommTimeouts(m_hComm, &CommTimeouts);}DCB  dcb;if (bIsSuccess){// 将ANSI字符串转换为UNICODE字符串  DWORD dwNum = MultiByteToWideChar(CP_ACP, 0, szDCBparam, -1, NULL, 0);wchar_t *pwText = new wchar_t[dwNum];if (!MultiByteToWideChar(CP_ACP, 0, szDCBparam, -1, pwText, dwNum)){bIsSuccess = TRUE;}/** 获取当前串口配置参数,并且构造串口DCB参数 */bIsSuccess = GetCommState(m_hComm, &dcb) && BuildCommDCB(pwText, &dcb);/** 开启RTS flow控制 */dcb.fRtsControl = RTS_CONTROL_ENABLE;/** 释放内存空间 */delete[] pwText;}if (bIsSuccess){/** 使用DCB参数配置串口状态 */bIsSuccess = SetCommState(m_hComm, &dcb);}/**  清空串口缓冲区 */PurgeComm(m_hComm, PURGE_RXCLEAR | PURGE_TXCLEAR | PURGE_RXABORT | PURGE_TXABORT);/** 离开临界段 */LeaveCriticalSection(&m_csCommunicationSync);
//std::cout << "Hello World";return bIsSuccess == TRUE;}bool CSerialPort::InitPort(UINT portNo, const LPDCB& plDCB)
{/** 打开指定串口,该函数内部已经有临界区保护,上面请不要加保护 */if (!openPort(portNo)){return false;}/** 进入临界段 */EnterCriticalSection(&m_csCommunicationSync);/** 配置串口参数 */if (!SetCommState(m_hComm, plDCB)){return false;}/**  清空串口缓冲区 */PurgeComm(m_hComm, PURGE_RXCLEAR | PURGE_TXCLEAR | PURGE_RXABORT | PURGE_TXABORT);/** 离开临界段 */LeaveCriticalSection(&m_csCommunicationSync);return true;
}void CSerialPort::ClosePort()
{/** 如果有串口被打开，关闭它 */if (m_hComm != INVALID_HANDLE_VALUE){CloseHandle(m_hComm);m_hComm = INVALID_HANDLE_VALUE;}
}bool CSerialPort::openPort(UINT portNo)
{/** 进入临界段 */EnterCriticalSection(&m_csCommunicationSync);/** 把串口的编号转换为设备名 */char szPort[50];sprintf_s(szPort, "COM%d", portNo);/** 打开指定的串口 */m_hComm = CreateFileA(szPort,  /** 设备名,COM1,COM2等 */GENERIC_READ | GENERIC_WRITE, /** 访问模式,可同时读写 */0,                            /** 共享模式,0表示不共享 */NULL,                         /** 安全性设置,一般使用NULL */OPEN_EXISTING,                /** 该参数表示设备必须存在,否则创建失败 */0,0);/** 如果打开失败，释放资源并返回 */if (m_hComm == INVALID_HANDLE_VALUE){LeaveCriticalSection(&m_csCommunicationSync);return false;}/** 退出临界区 */LeaveCriticalSection(&m_csCommunicationSync);return true;
}bool CSerialPort::OpenListenThread()
{/** 检测线程是否已经开启了 */if (m_hListenThread != INVALID_HANDLE_VALUE){/** 线程已经开启 */return false;}s_bExit = false;/** 线程ID */UINT threadId;/** 开启串口数据监听线程 */m_hListenThread = (HANDLE)_beginthreadex(NULL, 0, ListenThread, this, 0, &threadId);if (!m_hListenThread){return false;}/** 设置线程的优先级,高于普通线程 */if (!SetThreadPriority(m_hListenThread, THREAD_PRIORITY_ABOVE_NORMAL)){return false;}return true;
}bool CSerialPort::CloseListenTread()
{if (m_hListenThread != INVALID_HANDLE_VALUE){/** 通知线程退出 */s_bExit = true;/** 等待线程退出 */Sleep(10);/** 置线程句柄无效 */CloseHandle(m_hListenThread);m_hListenThread = INVALID_HANDLE_VALUE;}return true;
}UINT CSerialPort::GetBytesInCOM()
{DWORD dwError = 0;  /** 错误码 */COMSTAT  comstat;   /** COMSTAT结构体,记录通信设备的状态信息 */memset(&comstat, 0, sizeof(COMSTAT));UINT BytesInQue = 0;/** 在调用ReadFile和WriteFile之前,通过本函数清除以前遗留的错误标志 */if (ClearCommError(m_hComm, &dwError, &comstat)){BytesInQue = comstat.cbInQue; /** 获取在输入缓冲区中的字节数 */}return BytesInQue;
}UINT WINAPI CSerialPort::ListenThread(void* pParam)
{/** 得到本类的指针 */CSerialPort *pSerialPort = reinterpret_cast<CSerialPort*>(pParam);// 线程循环,轮询方式读取串口数据  while (!pSerialPort->s_bExit){UINT BytesInQue = pSerialPort->GetBytesInCOM();/** 如果串口输入缓冲区中无数据,则休息一会再查询 */if (BytesInQue == 0){Sleep(SLEEP_TIME_INTERVAL);continue;}/** 读取输入缓冲区中的数据并输出显示 */char cRecved = 0x00;do{cRecved = 0x00;if (pSerialPort->ReadChar(cRecved) == true){std::cout << cRecved;continue;}} while (--BytesInQue);}return 0;
}bool CSerialPort::ReadChar(char &cRecved)
{BOOL  bResult = TRUE;DWORD BytesRead = 0;if (m_hComm == INVALID_HANDLE_VALUE){return false;}/** 临界区保护 */EnterCriticalSection(&m_csCommunicationSync);/** 从缓冲区读取一个字节的数据 */bResult = ReadFile(m_hComm, &cRecved, 1, &BytesRead, NULL);if ((!bResult)){/** 获取错误码,可以根据该错误码查出错误原因 */DWORD dwError = GetLastError();/** 清空串口缓冲区 */PurgeComm(m_hComm, PURGE_RXCLEAR | PURGE_RXABORT);LeaveCriticalSection(&m_csCommunicationSync);return false;}/** 离开临界区 */LeaveCriticalSection(&m_csCommunicationSync);return (BytesRead == 1);}bool CSerialPort::WriteData(unsigned char* pData, unsigned int length)
{BOOL   bResult = TRUE;DWORD  BytesToSend = 0;if (m_hComm == INVALID_HANDLE_VALUE){return false;}/** 临界区保护 */EnterCriticalSection(&m_csCommunicationSync);/** 向缓冲区写入指定量的数据 */bResult = WriteFile(m_hComm, pData, length, &BytesToSend, NULL);if (!bResult){DWORD dwError = GetLastError();/** 清空串口缓冲区 */PurgeComm(m_hComm, PURGE_RXCLEAR | PURGE_RXABORT);LeaveCriticalSection(&m_csCommunicationSync);return false;}/** 离开临界区 */LeaveCriticalSection(&m_csCommunicationSync);return true;
}

1.4 SerialPort.h


#ifndef SERIALPORT_H_
#define SERIALPORT_H_  #include <Windows.h>  /** 串口通信类
*
*  本类实现了对串口的基本操作
*  例如监听发到指定串口的数据、发送指定数据到串口
*/
class CSerialPort
{public:CSerialPort(void);~CSerialPort(void);public:/** 初始化串口函数**  @param:  UINT portNo 串口编号,默认值为1,即COM1,注意,尽量不要大于9*  @param:  UINT baud   波特率,默认为9600*  @param:  char parity 是否进行奇偶校验,'Y'表示需要奇偶校验,'N'表示不需要奇偶校验*  @param:  UINT databits 数据位的个数,默认值为8个数据位*  @param:  UINT stopsbits 停止位使用格式,默认值为1*  @param:  DWORD dwCommEvents 默认为EV_RXCHAR,即只要收发任意一个字符,则产生一个事件*  @return: bool  初始化是否成功*  @note:   在使用其他本类提供的函数前,请先调用本函数进行串口的初始化*　　　　　   /n本函数提供了一些常用的串口参数设置,若需要自行设置详细的DCB参数,可使用重载函数*           /n本串口类析构时会自动关闭串口,无需额外执行关闭串口*  @see:*/bool InitPort(UINT  portNo = 1, UINT  baud = CBR_9600, char  parity = 'N', UINT  databits = 8, UINT  stopsbits = 1, DWORD dwCommEvents = EV_RXCHAR);/** 串口初始化函数**  本函数提供直接根据DCB参数设置串口参数*  @param:  UINT portNo*  @param:  const LPDCB & plDCB*  @return: bool  初始化是否成功*  @note:   本函数提供用户自定义地串口初始化参数*  @see:*/bool InitPort(UINT  portNo, const LPDCB& plDCB);/** 开启监听线程**  本监听线程完成对串口数据的监听,并将接收到的数据打印到屏幕输出*  @return: bool  操作是否成功*  @note:   当线程已经处于开启状态时,返回flase*  @see:*/bool OpenListenThread();/** 关闭监听线程***  @return: bool  操作是否成功*  @note:   调用本函数后,监听串口的线程将会被关闭*  @see:*/bool CloseListenTread();/** 向串口写数据**  将缓冲区中的数据写入到串口*  @param:  unsigned char * pData 指向需要写入串口的数据缓冲区*  @param:  unsigned int length 需要写入的数据长度*  @return: bool  操作是否成功*  @note:   length不要大于pData所指向缓冲区的大小*  @see:*/bool WriteData(unsigned char* pData, unsigned int length);/** 获取串口缓冲区中的字节数***  @return: UINT  操作是否成功*  @note:   当串口缓冲区中无数据时,返回0*  @see:*/UINT GetBytesInCOM();/** 读取串口接收缓冲区中一个字节的数据***  @param:  char & cRecved 存放读取数据的字符变量*  @return: bool  读取是否成功*  @note:*  @see:*/bool ReadChar(char &cRecved);private:/** 打开串口***  @param:  UINT portNo 串口设备号*  @return: bool  打开是否成功*  @note:*  @see:*/bool openPort(UINT  portNo);/** 关闭串口***  @return: void  操作是否成功*  @note:*  @see:*/void ClosePort();/** 串口监听线程**  监听来自串口的数据和信息*  @param:  void * pParam 线程参数*  @return: UINT WINAPI 线程返回值*  @note:*  @see:*/static UINT WINAPI ListenThread(void* pParam);private:/** 串口句柄 */HANDLE  m_hComm;/** 线程退出标志变量 */static bool s_bExit;/** 线程句柄 */volatile HANDLE    m_hListenThread;/** 同步互斥,临界区保护 */CRITICAL_SECTION   m_csCommunicationSync;       //!< 互斥操作串口  };#endif //SERIALPORT_H_ #pragma once

1.5 kinectJointFilter.h

/*双指数平滑滤波*/
#pragma once#include<windows.h>
#include<kinect.h>
#include<DirectXmath.h>   //图形应用程序数学库
#include<queue>          //队列namespace Sample
{typedef struct _TRANSFORM_SMOOTH_PARAMETERS{FLOAT fSmoothing;  /*越低与越接近原始数据*/FLOAT fCorrection; /*平滑参数，设置处理骨骼数据帧时的平滑量。值越大，平滑的越多，0表示不进行平滑；*/FLOAT fPrediction; /*预测超前期数，增大该值能减小滤波的延迟，但是会对突然的运动更敏感，容易造成过冲*/FLOAT fJitterRadius;/*抖动半径参数，设置修正的半径.如果关节点“抖动”超过了设置的这个半径，将会被纠正到这个半径之内*/FLOAT fMaxDeviationRadius;/*最大偏离半径参数，用来和抖动半径一起来设置抖动半径的最大边界*/}TRANSFORM_SMOOTH_PARAMETERS;// 霍尔特双指数平滑滤波器class FilterDoubleExponentialData{public:DirectX::XMVECTOR m_vRawPosition;DirectX::XMVECTOR m_vFilteredPosition;DirectX::XMVECTOR m_vTrend;DWORD    m_dwFrameCount;};class FilterDoubleExponential{public:FilterDoubleExponential() { Init(); }~FilterDoubleExponential() { Shutdown(); }void Init(FLOAT fSmoothing = 0.25f, FLOAT fCorrection = 0.25f, FLOAT fPrediction = 0.25f, FLOAT fJitterRadius = 0.03f, FLOAT fMaxDeviationRadius = 0.05f){Reset(fSmoothing, fCorrection, fPrediction, fJitterRadius, fMaxDeviationRadius);}void Shutdown(){}void Reset(FLOAT fSmoothing = 0.25f, FLOAT fCorrection = 0.25f, FLOAT fPrediction = 0.25f, FLOAT fJitterRadius = 0.03f, FLOAT fMaxDeviationRadius = 0.05f){assert(m_pFilteredJoints);assert(m_pHistory);m_fMaxDeviationRadius = fMaxDeviationRadius; // 最大预测半径的大小在过高时可以恢复到有噪声的数据m_fSmoothing = fSmoothing;                   // 平滑程度，过高会有明显延迟m_fCorrection = fCorrection;                 // 从预测中修正m_fPrediction = fPrediction;                 // 预测未来使用的数量。过高时过冲m_fJitterRadius = fJitterRadius;             // 消除抖动的半径的大小。过多的平滑会过高memset(m_pFilteredJoints, 0, sizeof(DirectX::XMVECTOR) * JointType_Count);memset(m_pHistory, 0, sizeof(FilterDoubleExponentialData) * JointType_Count);}void Update(IBody* const pBody);void Update(Joint joints[]);inline const DirectX::XMVECTOR* GetFilteredJoints() const { return &m_pFilteredJoints[0]; }private:DirectX::XMVECTOR m_pFilteredJoints[JointType_Count];FilterDoubleExponentialData m_pHistory[JointType_Count];FLOAT m_fSmoothing;FLOAT m_fCorrection;FLOAT m_fPrediction;FLOAT m_fJitterRadius;FLOAT m_fMaxDeviationRadius;void Update(Joint joints[], UINT JointID, TRANSFORM_SMOOTH_PARAMETERS smoothingParams);};
}

最后

上位机代码全部完成，cpp文件与h文件对应使用。最终效果实现简单的随动。

基于Kinect体感器控制的机械臂项目记录相关推荐

基于Kinect体感的仿真对抗游戏系统
项目意义: 1.Kinect的研究和应用在国内外都呈现出比较高的热潮,相关技术,如人脸识别.动态跟踪.手势识别等均出现了比较优越的实际效果.让机器人能够实时跟踪人的动作功能并完成与人的沟通互动是机器人 ...
制作Kinect体感控制小车教程＜一＞
转载请注明出处:http://blog.csdn.net/lxk7280 Kinect体感控制小车 Kine ...
制作Kinect体感控制小车教程 lt;一gt;
转载请注明出处:http://blog.csdn.net/lxk7280 Kinect体感控制小车 Kine ...
Kinect体感机器人（三）—— 空间向量法计算关节角度
Kinect体感机器人(三)-- 空间向量法计算关节角度 By 马冬亮(凝霜 Loki) 一个人的战争(http://blog.csdn.net/MDL13412) 终于写到体感机器人的核心代码了, ...
Kinect体感互动解决方案——体感炫舞互动系统
儿童乐园.商场人流较少.缺乏活力.死气沉沉? 可能你需要的是Kinect体感炫舞互动游戏. 佩京体感炫舞互动游戏,为儿童乐园.商场注入更多互动活力,您带来从未有过的超级酷炫体验,其精彩纷呈的动态舞台场 ...
（转）Kinect体感机器人—— 空间向量法计算关节角度
Kinect体感机器人(三)-- 空间向量法计算关节角度 By 马冬亮(凝霜 Loki) 一个人的战争(http://blog.csdn.net/MDL13412) 终于写到体感机器人的核心代码了, ...
用多个Kinect体感摄像头实现真正360度运动捕捉系统
以前我在这里写过博客文章,研究用微软的Kinect体感摄像头做运动捕捉,当时设计了两种方案,一种是用NiTE中间件,在它的基础上改进了一点点,但处理360度转身主要靠插值,说白了就是靠猜测,效果不是很 ...
基于adams与simulink的七自由度机械臂模型与控制仿真
基于adams与simulink的七自由度机械臂模型与控制仿真最近在搞adams与simulink联合仿真,发现网上关于高自由度机械臂的建模与仿真中文资料很少,也没有开源模型.因此将我的学习成果开源 ...
Kinect体感机器人（二）—— 体感识别
Kinect体感机器人(二)-- 体感识别 By 马冬亮(凝霜 Loki) 一个人的战争(http://blog.csdn.net/MDL13412) 背景知识体感技术属于NUI(自然人机界面)的 ...

基于Kinect体感器控制的机械臂项目记录

基于Kinect体感器控制的机械臂