void closeDevice()
{
g_Player.Release();
g_Device.Release();
g_Depth.Release();
g_Image.Release();
g_IR.Release();
g_Audio.Release();
g_scriptNode.Release();
g_Context.Release();
}
void CleanUpExit() {
recorder.Release();
g_player.Release();
g_image.Release();
g_scriptNode.Release();
g_context.Release();
g_hands.Release();
g_gesture.Release();
free(g_pTexMap);
exit(1);
}
int main()
{
XnStatus nRetVal = XN_STATUS_OK;
Context context;
ScriptNode scriptNode;
EnumerationErrors errors;
const char *fn = NULL;
if (fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH;
else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL;
else {
printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL);
return XN_STATUS_ERROR;
}
printf("Reading config from: '%s'\n", fn);
nRetVal = context.InitFromXmlFile(fn, scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
return (nRetVal);
}
DepthGenerator depth;
nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
CHECK_RC(nRetVal, "Find depth generator");
XnFPSData xnFPS;
nRetVal = xnFPSInit(&xnFPS, 180);
CHECK_RC(nRetVal, "FPS Init");
DepthMetaData depthMD;
while (!xnOSWasKeyboardHit())
{
nRetVal = context.WaitOneUpdateAll(depth);
if (nRetVal != XN_STATUS_OK)
{
printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal));
continue;
}
xnFPSMarkFrame(&xnFPS);
depth.GetMetaData(depthMD);
printf("Frame %d Middle point is: %u. FPS: %f\n", depthMD.FrameID(), depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2), xnFPSCalc(&xnFPS));
}
depth.Release();
scriptNode.Release();
context.Release();
return 0;
}
int main(int argc, char* argv[])
{
XnStatus nRetVal = XN_STATUS_OK;
Context context;
EnumerationErrors errors;
Mode mode;
// default mode
#if XN_PLATFORM == XN_PLATFORM_WIN32
mode = MODE_PLAY;
#else
mode = MODE_RECORD;
#endif
// check if mode was provided by user
if (argc > 1)
{
if (strcmp(argv[1], "play") == 0)
{
mode = MODE_PLAY;
}
else if (strcmp(argv[1], "record") == 0)
{
mode = MODE_RECORD;
}
else
{
printUsage(argv[0]);
return -1;
}
}
// make sure mode is valid
#if XN_PLATFORM != XN_PLATFORM_WIN32
if (mode == MODE_PLAY)
{
printf("Playing is not supported on this platform!\n");
return -1;
}
#endif
ScriptNode scriptNode;
nRetVal = context.InitFromXmlFile(SAMPLE_XML_PATH, scriptNode);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
return (nRetVal);
}
// find audio nodes
AudioGenerator gens[nSupportedNodes];
XnUInt32 nNodes = 0;
NodeInfoList list;
nRetVal = context.EnumerateExistingNodes(list, XN_NODE_TYPE_AUDIO);
CHECK_RC(nRetVal, "Enumerate audio nodes");
for (NodeInfoList::Iterator it = list.Begin(); it != list.End(); ++it)
{
NodeInfo info = *it;
nRetVal = info.GetInstance(gens[nNodes]);
CHECK_RC(nRetVal, "Get audio node");
nNodes++;
}
if (nNodes == 0)
{
printf("No audio node was found!\n");
return -1;
}
if (mode == MODE_PLAY)
{
nRetVal = play(context, gens, nNodes);
}
else if (mode == MODE_RECORD)
{
nRetVal = record(context, gens, nNodes);
}
scriptNode.Release();
for (int i = 0; i < nSupportedNodes; ++i)
gens[i].Release();
context.Release();
return nRetVal;
}
/*
* Function: ~KinectMonitor (Destructor)
*
* Releases all the production nodes and the context.
*/
KinectMonitor::~KinectMonitor() {
scriptNode.Release();
depthGenerator.Release();
userGenerator.Release();
context.Release();
}
int main()
{
XnStatus nRetVal = XN_STATUS_OK;
Context context;
ScriptNode scriptNode;
EnumerationErrors errors;
XnUInt32 min_z1, min_z2, min_z3, maxGrad, distVal;
const char *fn = NULL;
if (fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH;
else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL;
else {
printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL);
return XN_STATUS_ERROR;
}
printf("Reading config from: '%s'\n", fn);
nRetVal = context.InitFromXmlFile(fn, scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
return (nRetVal);
}
DepthGenerator depth;
nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
CHECK_RC(nRetVal, "Find depth generator");
XnFPSData xnFPS;
nRetVal = xnFPSInit(&xnFPS, 180);
CHECK_RC(nRetVal, "FPS Init");
DepthMetaData depthMD;
//Initialize WiringPi
if(wiringPiSetup() == -1)
exit(1);
//Enable SoftPWM on pin 1,2 and 3
softPwmCreate(1, 0, RANGE);
softPwmCreate(2, 0, RANGE);
softPwmCreate(3, 0, RANGE);
while (!xnOSWasKeyboardHit())
{
nRetVal = context.WaitOneUpdateAll(depth);
if (nRetVal != XN_STATUS_OK)
{
printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal));
continue;
}
xnFPSMarkFrame(&xnFPS);
depth.GetMetaData(depthMD);
const XnDepthPixel* pDepthMap = depthMD.Data();
int XRes = depthMD.XRes();
int YRes = depthMD.YRes();
//To find closest pixel value in Zone 1, Zone 2 and Zone 3
min_z1 = getClosestPixel( 0 , 0, (XRes / 2) , YRes, depthMD);
min_z2 = getClosestPixel( (XRes / 4), 0, (3 * XRes / 4), YRes, depthMD);
min_z3 = getClosestPixel( (XRes / 2), 0, XRes , YRes, depthMD);
double in_low = 600;
double in_high = 2000;
double in_diff = in_high - in_low;
double out_low = 51;
double out_high = 973;
double out_diff = out_high - out_low;
distVal = min_z1;
XnUInt32 pwm_val1 = ( (out_diff) / ((in_diff)*(in_diff)*(in_diff)) ) * ((in_high - distVal) * (in_high - distVal) * (in_high - distVal)) + out_low;
distVal = min_z2;
XnUInt32 pwm_val2 = ( (out_diff) / ((in_diff)*(in_diff)*(in_diff)) ) * ((in_high - distVal) * (in_high - distVal) * (in_high - distVal)) + out_low;
distVal = min_z3;
XnUInt32 pwm_val3 = ( (out_diff) / ((in_diff)*(in_diff)*(in_diff)) ) * ((in_high - distVal) * (in_high - distVal) * (in_high - distVal)) + out_low;
// Zone 1 - Left side (pin )
if (pwm_val1 < out_low)
pwm_val1 = 0; // if object too far, set DUTY CYCLE to 0
if (min_z1 == 9000.0)
pwm_val1 = out_high; //if object too close, set DUTY CYCLE to max (here, 95%)
if (min_z1 < 600)
pwm_val1 = out_high;
// Zone 2 - Center (pin )
if (pwm_val2 < out_low)
pwm_val2 = 0; // if object too far, set DUTY CYCLE to 0
if (min_z2 == 9000.0)
pwm_val2 = out_high; //if object too close, set DUTY CYCLE to max (here, 95%)
if (min_z2 < 600)
pwm_val2 = out_high;
// Zone 3 - Right side (pin )
if (pwm_val3 < out_low)
pwm_val3 = 0; // if object too far, set DUTY CYCLE to 0
if (min_z3 == 9000.0)
pwm_val3 = out_high; //if object too close, set DUTY CYCLE to max (here, 95%)
if (min_z3 < 600)
pwm_val3 = out_high;
pwm_val1 = ((pwm_val1 - out_low) / (1.0 * out_diff)) * 100.0;
pwm_val2 = ((pwm_val2 - out_low) / (1.0 * out_diff)) * 100.0;
pwm_val3 = ((pwm_val3 - out_low) / (1.0 * out_diff)) * 100.0;
softPwmWrite(1,(int)pwm_val1);
softPwmWrite(2,(int)pwm_val2);
softPwmWrite(3,(int)pwm_val3);
if ( (depthMD.FrameID() % 30) == 0)
{
printf("Frame %d", depthMD.FrameID());
printf("\n");
printf("Zone 1 value is %u \t", pwm_val1);
printf("Zone 2 value is %u \t", pwm_val2);
printf("Zone 3 value is %u \n", pwm_val3);
printf("Zone1 min_dis %u \t", min_z1);
printf("Zone2 min_dis %u \t", min_z2);
printf("Zone3 min_dis %u \n", min_z3);
//To find a gradient value for the floor
//maxGrad = getGradient( 5, 0, (YRes/2) + 1, depthMD.XRes(), depthMD.YRes(), depthMD);
//printf("Frame %d max gradient for Floor is: %u. FPS: %f\n\n", depthMD.FrameID(), maxGrad, xnFPSCalc(&xnFPS));
}
}
softPwmWrite(1,0);
softPwmWrite(2,0);
softPwmWrite(3,0);
//release the nodes
depth.Release();
scriptNode.Release();
context.Release();
return 0;
}
