{\rtf1\ansi\ansicpg1252\cocoartf1038\cocoasubrtf360
{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
{\colortbl;\red255\green255\blue255;}
\paperw11900\paperh16840\margl1440\margr1440\vieww9000\viewh8400\viewkind0
\pard\tx566\tx1133\tx1700\tx2267\tx2834\tx3401\tx3968\tx4535\tx5102\tx5669\tx6236\tx6803\ql\qnatural\pardirnatural

\f0\fs24 \cf0 processing:\
\
/* --------------------------------------------------------------------------\
 * SimpleOpenNI DepthMap3d Test\
 * --------------------------------------------------------------------------\
 * Processing Wrapper for the OpenNI/Kinect library\
 * http://code.google.com/p/simple-openni\
 * --------------------------------------------------------------------------\
 * prog:  Max Rheiner / Interaction Design / zhdk / http://iad.zhdk.ch/\
 * date:  02/16/2011 (m/d/y)\
 * ----------------------------------------------------------------------------\
 */\
\
import SimpleOpenNI.*;\
import processing.serial.*;\
\
//int serial_speed = 9600;\
//int serial_speed = 115200;\
\
int SERVOMIN = 200;\
int SERVOMAX = 680;\
\
Serial myPort;\
SimpleOpenNI context;\
float        zoomF =0.3f;\
float        rotX = radians(180);  // by default rotate the hole scene 180deg around the x-axis, \
                                   // the data from openni comes upside down\
float        rotY = radians(0);\
\
//dimensions\
int H = 300;\
int W = 300;\
int D = 300;\
int Dstart = 550;\
\
int maxX = W/2; int minX = maxX-W; int minY = H/2 ; int maxY = minY-H ; int minZ = Dstart; int maxZ = Dstart+D;\
float Ddep = maxZ-minZ;\
\
int sint_w = abs (W/8);\
int x0 = minX; int x1 = x0+sint_w; int x2 = x1+sint_w; int x3 = x2+sint_w; int x4 = x3+sint_w; int x5 = x4+sint_w; int x6 = x5+sint_w; int x7 = x6+sint_w; int x8 = x7+sint_w;\
\
int sint_h = abs(H/8);\
int y0 = minY; int y1 = y0-sint_h; int y2 = y1-sint_h; int y3 = y2-sint_h; int y4 = y3-sint_h; int y5 = y4-sint_h; int y6 = y5-sint_h; int y7 = y6-sint_h; int y8 = y7-sint_h;\
\
float rpz1 = 0; float rpz2 = 0; float rpz3 = 0; float rpz4 = 0; float rpz5 = 0; float rpz6 = 0; float rpz7 = 0; float rpz8 = 0;\
float rpz9 = 0;  float rpz10 = 0;  float rpz11 = 0;  float rpz12 = 0;  float rpz13 = 0;  float rpz14 = 0;  float rpz15 = 0;  float rpz16 = 0;\
float rpz17 = 0;  float rpz18 = 0;  float rpz19 = 0;  float rpz20 = 0;  float rpz21 = 0;  float rpz22 = 0;  float rpz23 = 0;  float rpz24 = 0; \
float rpz25 = 0;  float rpz26 = 0;  float rpz27 = 0;  float rpz28 = 0;  float rpz29 = 0;  float rpz30 = 0;  float rpz31 = 0;  float rpz32 = 0; \
float rpz33 = 0;  float rpz34 = 0;  float rpz35 = 0;  float rpz36 = 0;  float rpz37 = 0;  float rpz38 = 0;  float rpz39 = 0;  float rpz40 = 0; \
float rpz41 = 0;  float rpz42 = 0;  float rpz43 = 0;  float rpz44 = 0;  float rpz45 = 0;  float rpz46 = 0;  float rpz47 = 0;  float rpz48 = 0; \
float rpz49 = 0;  float rpz50 = 0;  float rpz51 = 0;  float rpz52 = 0;  float rpz53 = 0;  float rpz54 = 0;  float rpz55 = 0;  float rpz56 = 0; \
float rpz57 = 0;  float rpz58 = 0;  float rpz59 = 0;  float rpz60 = 0;  float rpz61 = 0;  float rpz62 = 0;  float rpz63 = 0;  float rpz64 = 0; \
\
int pos1;  int pos2;  int pos3;  int pos4;  int pos5;  int pos6;  int pos7;  int pos8;\
int pos9; int pos10;  int pos11;  int pos12;  int pos13;  int pos14;  int pos15;  int pos16;\
int pos17;  int pos18;  int pos19; int pos20;  int pos21;  int pos22;  int pos23;  int pos24;\
int pos25;  int pos26;  int pos27;  int pos28;  int pos29;  int pos30;  int pos31;  int pos32;\
int pos33;  int pos34;  int pos35;  int pos36;  int pos37;  int pos38;  int pos39;  int pos40;\
int pos41;  int pos42;  int pos43;  int pos44;  int pos45;  int pos46;  int pos47;  int pos48;\
int pos49;  int pos50;  int pos51;  int pos52;  int pos53;  int pos54;  int pos55;  int pos56;\
int pos57;  int pos58;  int pos59;  int pos60;  int pos61;  int pos62;  int pos63;  int pos64;\
\
\
ArrayList<PVector> pts = new ArrayList<PVector>();\
\
\
void setup()\
\{\
   frameRate(10);\
 \
  size(1024+480,768,P3D);  // strange, get drawing error in the cameraFrustum if i use P3D, in opengl there is no problem +480\
  \
  \
  //context = new SimpleOpenNI(this,SimpleOpenNI.RUN_MODE_SINGLE_THREADED);\
  context = new SimpleOpenNI(this);\
\
  // disable mirror\
  context.setMirror(true);\
\
  // enable depthMap generation \
  if(context.enableDepth() == false)\
  \{\
     println("Can't open the depthMap, maybe the camera is not connected!"); \
     exit();\
     return;\
  \}\
\
  stroke(255,255,255);\
  smooth();\
  perspective(radians(45),\
              float(width)/float(height),\
              10,150000);\
  myPort = new Serial (this, Serial.list()[0], 9600);             \
\}\
\
void draw()\
\{\
  // update the cam\
  context.update();\
\
  background(0,0,0);\
\
  translate(width/2, height/2, 0);\
  rotateX(rotX);\
  rotateY(rotY);\
  scale(zoomF);\
\
  int[]   depthMap = context.depthMap();\
  int     steps   = 5;  // to speed up the drawing, draw every third point\
  int     index;\
  PVector realWorldPoint;\
  pts.clear();\
  translate(0,0,-1000);  // set the rotation center of the scene 1000 infront of the camera\
\
  stroke(255);\
  \
  PVector[] realWorldMap = context.depthMapRealWorld();\
  \
  rpz1 = maxZ;  rpz2 = maxZ;  rpz3 = maxZ;  rpz4 = maxZ;  rpz5 = maxZ;  rpz6 = maxZ;  rpz7 = maxZ;  rpz8 = maxZ;\
  rpz9 = maxZ;   rpz10 = maxZ;   rpz11 = maxZ;   rpz12 = maxZ;   rpz13 = maxZ;   rpz14 = maxZ;   rpz15 = maxZ;   rpz16 = maxZ;\
  rpz17 = maxZ;   rpz18 = maxZ;   rpz19 = maxZ;   rpz20 = maxZ;   rpz21 = maxZ;   rpz22 = maxZ;   rpz23 = maxZ;   rpz24 = maxZ; \
  rpz25 = maxZ;   rpz26 = maxZ;   rpz27 = maxZ;   rpz28 = maxZ;   rpz29 = maxZ;   rpz30 = maxZ;   rpz31 = maxZ;   rpz32 = maxZ; \
  rpz33 = maxZ;   rpz34 = maxZ;   rpz35 = maxZ;   rpz36 = maxZ;   rpz37 = maxZ;   rpz38 = maxZ;   rpz39 = maxZ;   rpz40 = maxZ; \
  rpz41 = maxZ;   rpz42 = maxZ;   rpz43 = maxZ;   rpz44 = maxZ;   rpz45 = maxZ;   rpz46 = maxZ;   rpz47 = maxZ;   rpz48 = maxZ; \
  rpz49 = maxZ;   rpz50 = maxZ;   rpz51 = maxZ;   rpz52 = maxZ;   rpz53 = maxZ;   rpz54 = maxZ;   rpz55 = maxZ;   rpz56 = maxZ; \
  rpz57 = maxZ;   rpz58 = maxZ;   rpz59 = maxZ;   rpz60 = maxZ;   rpz61 = maxZ;   rpz62 = maxZ;   rpz63 = maxZ;   rpz64 = maxZ; \
\
  \
  for(int y=0;y < context.depthHeight();y+=steps)\
  \{\
    for(int x=0;x < context.depthWidth();x+=steps)\
    \{\
      index = x + y * context.depthWidth();\
      if(depthMap[index] > 0)\
      \{ \
        \
        // draw the projected point\
//        realWorldPoint = context.depthMapRealWorld()[index];\
        realWorldPoint = realWorldMap[index];\
        //println("x = "+x);\
        //println("y = "+y);\
        if( index == 153100)\
        \{\
          //println(realWorldPoint.z);\
          //ellipse(realWorldPoint.x,realWorldPoint.y, 10,10);\
          fill(255,0,0);\
          strokeWeight(10);\
          stroke(255,0,0);\
        \} else\{\
           stroke(255);\
        \}\
        point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);  // make realworld z negative, in the 3d drawing coordsystem +z points in the direction of the eyeint col1 = 0; int col2 = 0; int col3 = 0; int col4 = 0; int col5 = 0; int col6 = 0; int col7 = 0; int col8 = 0;\
        \
        if(realWorldPoint.x > minX && realWorldPoint.x < maxX &&\
        realWorldPoint.y < minY && realWorldPoint.y > maxY &&\
        realWorldPoint.z>minZ && realWorldPoint.z<maxZ)\
        \{ \
          float rpx = realWorldPoint.x; float rpy = realWorldPoint.y; float rpz = realWorldPoint.z;\
          \
          //pts.add(realWorldPoint.get());//store each point\
          //println(pts);\
          //col1 = int(255-((curZ - 550)*1.7));\
          \
          //row1\
          if( rpy<y0 && rpy>y1)\{\
            \
          if(rpx>x0 && rpx<x1)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz1 = rpz;\
          \}\
          \
          if(rpx>x1 && rpx<x2)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz2 = rpz;\
          \}\
          \
          if(rpx>x2 && rpx<x3)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz3 = rpz;\
          \}\
          \
          if(rpx>x3 && rpx<x4)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz4 = rpz;\
          \}\
          \
          if(rpx>x4 && rpx<x5)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz5 = rpz;\
          \}\
          \
          if(rpx>x5 && rpx<x6)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz6 = rpz;\
          \}\
          \
          if(rpx>x6 && rpx<x7)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz7 = rpz;\
          \}\
          \
          if(rpx>x7 && rpx<x8)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz8 = rpz;\
          \}\
          \
          \}\
          \
          //row2\
          if( rpy<y1 && rpy>y2)\{\
            \
          if(rpx>x0 && rpx<x1)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz9 = rpz;\
          \}\
          \
          if(rpx>x1 && rpx<x2)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz10 = rpz;\
          \}\
          \
          if(rpx>x2 && rpx<x3)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz11 = rpz;\
          \}\
          \
          if(rpx>x3 && rpx<x4)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz12 = rpz;\
          \}\
          \
          if(rpx>x4 && rpx<x5)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz13 = rpz;\
          \}\
          \
          if(rpx>x5 && rpx<x6)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz14 = rpz;\
          \}\
          \
          if(rpx>x6 && rpx<x7)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz15 = rpz;\
          \}\
          \
          if(rpx>x7 && rpx<x8)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz16 = rpz;\
          \}\
          \
          \}\
          \
          //row3\
          if( rpy<y2 && rpy>y3)\{\
            \
          if(rpx>x0 && rpx<x1)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz17 = rpz;\
          \}\
          \
          if(rpx>x1 && rpx<x2)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz18 = rpz;\
          \}\
          \
          if(rpx>x2 && rpx<x3)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz19 = rpz;\
          \}\
          \
          if(rpx>x3 && rpx<x4)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz20 = rpz;\
          \}\
          \
          if(rpx>x4 && rpx<x5)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz21 = rpz;\
          \}\
          \
          if(rpx>x5 && rpx<x6)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz22 = rpz;\
          \}\
          \
          if(rpx>x6 && rpx<x7)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz23 = rpz;\
          \}\
          \
          if(rpx>x7 && rpx<x8)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz24 = rpz;\
          \}\
          \
          \}   \
          \
          //row4\
          if( rpy<y3 && rpy>y4)\{\
            \
          if(rpx>x0 && rpx<x1)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz25 = rpz;\
          \}\
          \
          if(rpx>x1 && rpx<x2)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz26 = rpz;\
          \}\
          \
          if(rpx>x2 && rpx<x3)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz27 = rpz;\
          \}\
          \
          if(rpx>x3 && rpx<x4)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz28 = rpz;\
          \}\
          \
          if(rpx>x4 && rpx<x5)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz29 = rpz;\
          \}\
          \
          if(rpx>x5 && rpx<x6)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz30 = rpz;\
          \}\
          \
          if(rpx>x6 && rpx<x7)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz31 = rpz;\
          \}\
          \
          if(rpx>x7 && rpx<x8)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz32 = rpz;\
          \}\
          \
          \}\
          \
          //row5\
          if( rpy<y4 && rpy>y5)\{\
            \
          if(rpx>x0 && rpx<x1)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz33 = rpz;\
          \}\
          \
          if(rpx>x1 && rpx<x2)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz34 = rpz;\
          \}\
          \
          if(rpx>x2 && rpx<x3)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz35 = rpz;\
          \}\
          \
          if(rpx>x3 && rpx<x4)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz36 = rpz;\
          \}\
          \
          if(rpx>x4 && rpx<x5)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz37 = rpz;\
          \}\
          \
          if(rpx>x5 && rpx<x6)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz38 = rpz;\
          \}\
          \
          if(rpx>x6 && rpx<x7)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz39 = rpz;\
          \}\
          \
          if(rpx>x7 && rpx<x8)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz40 = rpz;\
          \}\
          \
          \}   \
          \
          //row6\
          if( rpy<y5 && rpy>y6)\{\
            \
          if(rpx>x0 && rpx<x1)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz41 = rpz;\
          \}\
          \
          if(rpx>x1 && rpx<x2)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz42 = rpz;\
          \}\
          \
          if(rpx>x2 && rpx<x3)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz43 = rpz;\
          \}\
          \
          if(rpx>x3 && rpx<x4)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz44 = rpz;\
          \}\
          \
          if(rpx>x4 && rpx<x5)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz45 = rpz;\
          \}\
          \
          if(rpx>x5 && rpx<x6)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz46 = rpz;\
          \}\
          \
          if(rpx>x6 && rpx<x7)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz47 = rpz;\
          \}\
          \
          if(rpx>x7 && rpx<x8)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz48 = rpz;\
          \}\
          \
          \}\
          \
          //row7\
          if( rpy<y6 && rpy>y7)\{\
            \
          if(rpx>x0 && rpx<x1)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz49 = rpz;\
          \}\
          \
          if(rpx>x1 && rpx<x2)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz50 = rpz;\
          \}\
          \
          if(rpx>x2 && rpx<x3)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz51 = rpz;\
          \}\
          \
          if(rpx>x3 && rpx<x4)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz52 = rpz;\
          \}\
          \
          if(rpx>x4 && rpx<x5)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz53 = rpz;\
          \}\
          \
          if(rpx>x5 && rpx<x6)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz54 = rpz;\
          \}\
          \
          if(rpx>x6 && rpx<x7)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz55 = rpz;\
          \}\
          \
          if(rpx>x7 && rpx<x8)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz56 = rpz;\
          \}\
          \
          \}   \
          \
          //row8\
          if( rpy<y7 && rpy>y8)\{\
            \
          if(rpx>x0 && rpx<x1)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz57 = rpz;\
          \}\
          \
          if(rpx>x1 && rpx<x2)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz58 = rpz;\
          \}\
          \
          if(rpx>x2 && rpx<x3)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz59 = rpz;\
          \}\
          \
          if(rpx>x3 && rpx<x4)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz60 = rpz;\
          \}\
          \
          if(rpx>x4 && rpx<x5)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz61 = rpz;\
          \}\
          \
          if(rpx>x5 && rpx<x6)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz62 = rpz;\
          \}\
          \
          if(rpx>x6 && rpx<x7)\{\
          stroke(255,0,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz63 = rpz;\
          \}\
          \
          if(rpx>x7 && rpx<x8)\{\
          stroke(0,255,0);\
          point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);\
          rpz64 = rpz;\
          \}\
          \
          \}\
             \
\
          \
        \} //if dimensions\
        \
   \
      \} //if depth\
    \} //for x loop\
  \} //for y loop\
\
  // draw the kinect cam\
  //context.drawCamFrustum();\
  \
  int q=6;\
\
pos1 = int(map(rpz1,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos2 = int(map(rpz2,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos3 = int(map(rpz3,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos4 = int(map(rpz4,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos5 = int(map(rpz5,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos6 = int(map(rpz6,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos7 = int(map(rpz7,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos8 = int(map(rpz8,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
\
pos9 = int(map(rpz9,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos10 = int(map(rpz10,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos11 = int(map(rpz11,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos12 = int(map(rpz12,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos13 = int(map(rpz13,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos14 = int(map(rpz14,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos15 = int(map(rpz15,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos16 = int(map(rpz16,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
\
pos17 = int(map(rpz17,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos18 = int(map(rpz18,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos19 = int(map(rpz19,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos20 = int(map(rpz20,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos21 = int(map(rpz21,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos22 = int(map(rpz22,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos23 = int(map(rpz23,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos24 = int(map(rpz24,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
\
pos25 = int(map(rpz25,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos26 = int(map(rpz26,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos27 = int(map(rpz27,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos28 = int(map(rpz28,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos29 = int(map(rpz29,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos30 = int(map(rpz30,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos31 = int(map(rpz31,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos32 = int(map(rpz32,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
\
pos33 = int(map(rpz33,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos34 = int(map(rpz34,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos35 = int(map(rpz35,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos36 = int(map(rpz36,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos37 = int(map(rpz37,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos38 = int(map(rpz38,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos39 = int(map(rpz39,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos40 = int(map(rpz40,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
\
pos41 = int(map(rpz41,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos42 = int(map(rpz42,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos43 = int(map(rpz43,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos44 = int(map(rpz44,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos45 = int(map(rpz45,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos46 = int(map(rpz46,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos47 = int(map(rpz47,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos48 = int(map(rpz48,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
\
pos49 = int(map(rpz49,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos50 = int(map(rpz50,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos51 = int(map(rpz51,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos52 = int(map(rpz52,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos53 = int(map(rpz53,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos54 = int(map(rpz54,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos55 = int(map(rpz55,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos56 = int(map(rpz56,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
\
pos57 = int(map(rpz57,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos58 = int(map(rpz58,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos59 = int(map(rpz59,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos60 = int(map(rpz60,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos61 = int(map(rpz61,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos62 = int(map(rpz62,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos63 = int(map(rpz63,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
pos64 = int(map(rpz64,minZ,maxZ,SERVOMIN,SERVOMAX))/q;\
\
\
\
\
\
int col1 = int(map(rpz1,minZ,maxZ,255,0));\
int col2 = int(map(rpz2,minZ,maxZ,255,0));\
int col3 = int(map(rpz3,minZ,maxZ,255,0));\
int col4 = int(map(rpz4,minZ,maxZ,255,0));\
int col5 = int(map(rpz5,minZ,maxZ,255,0));\
int col6 = int(map(rpz6,minZ,maxZ,255,0));\
int col7 = int(map(rpz7,minZ,maxZ,255,0));\
int col8 = int(map(rpz8,minZ,maxZ,255,0));\
\
int col9 = int(map(rpz9,minZ,maxZ,255,0));\
int col10 = int(map(rpz10,minZ,maxZ,255,0));\
int col11 = int(map(rpz11,minZ,maxZ,255,0));\
int col12 = int(map(rpz12,minZ,maxZ,255,0));\
int col13 = int(map(rpz13,minZ,maxZ,255,0));\
int col14 = int(map(rpz14,minZ,maxZ,255,0));\
int col15 = int(map(rpz15,minZ,maxZ,255,0));\
int col16 = int(map(rpz16,minZ,maxZ,255,0));\
\
int col17 = int(map(rpz17,minZ,maxZ,255,0));\
int col18 = int(map(rpz18,minZ,maxZ,255,0));\
int col19 = int(map(rpz19,minZ,maxZ,255,0));\
int col20 = int(map(rpz20,minZ,maxZ,255,0));\
int col21 = int(map(rpz21,minZ,maxZ,255,0));\
int col22 = int(map(rpz22,minZ,maxZ,255,0));\
int col23 = int(map(rpz23,minZ,maxZ,255,0));\
int col24 = int(map(rpz24,minZ,maxZ,255,0));\
\
int col25 = int(map(rpz25,minZ,maxZ,255,0));\
int col26 = int(map(rpz26,minZ,maxZ,255,0));\
int col27 = int(map(rpz27,minZ,maxZ,255,0));\
int col28 = int(map(rpz28,minZ,maxZ,255,0));\
int col29 = int(map(rpz29,minZ,maxZ,255,0));\
int col30 = int(map(rpz30,minZ,maxZ,255,0));\
int col31 = int(map(rpz31,minZ,maxZ,255,0));\
int col32 = int(map(rpz32,minZ,maxZ,255,0));\
\
int col33 = int(map(rpz33,minZ,maxZ,255,0));\
int col34 = int(map(rpz34,minZ,maxZ,255,0));\
int col35 = int(map(rpz35,minZ,maxZ,255,0));\
int col36 = int(map(rpz36,minZ,maxZ,255,0));\
int col37 = int(map(rpz37,minZ,maxZ,255,0));\
int col38 = int(map(rpz38,minZ,maxZ,255,0));\
int col39 = int(map(rpz39,minZ,maxZ,255,0));\
int col40 = int(map(rpz40,minZ,maxZ,255,0));\
\
int col41 = int(map(rpz41,minZ,maxZ,255,0));\
int col42 = int(map(rpz42,minZ,maxZ,255,0));\
int col43 = int(map(rpz43,minZ,maxZ,255,0));\
int col44 = int(map(rpz44,minZ,maxZ,255,0));\
int col45 = int(map(rpz45,minZ,maxZ,255,0));\
int col46 = int(map(rpz46,minZ,maxZ,255,0));\
int col47 = int(map(rpz47,minZ,maxZ,255,0));\
int col48 = int(map(rpz48,minZ,maxZ,255,0));\
\
int col49 = int(map(rpz49,minZ,maxZ,255,0));\
int col50 = int(map(rpz50,minZ,maxZ,255,0));\
int col51 = int(map(rpz51,minZ,maxZ,255,0));\
int col52 = int(map(rpz52,minZ,maxZ,255,0));\
int col53 = int(map(rpz53,minZ,maxZ,255,0));\
int col54 = int(map(rpz54,minZ,maxZ,255,0));\
int col55 = int(map(rpz55,minZ,maxZ,255,0));\
int col56 = int(map(rpz56,minZ,maxZ,255,0));\
\
int col57 = int(map(rpz57,minZ,maxZ,255,0));\
int col58 = int(map(rpz58,minZ,maxZ,255,0));\
int col59 = int(map(rpz59,minZ,maxZ,255,0));\
int col60 = int(map(rpz60,minZ,maxZ,255,0));\
int col61 = int(map(rpz61,minZ,maxZ,255,0));\
int col62 = int(map(rpz62,minZ,maxZ,255,0));\
int col63 = int(map(rpz63,minZ,maxZ,255,0));\
int col64 = int(map(rpz64,minZ,maxZ,255,0));\
\
\
\
stroke(255,0,0);\
\
fill(col1);\
rect(-950,350,50,50);\
fill(col2);\
rect(-900,350,50,50);\
fill(col3);\
rect(-850,350,50,50);\
fill(col4);\
rect(-800,350,50,50);\
fill(col5);\
rect(-750,350,50,50);\
fill(col6);\
rect(-700,350,50,50);\
fill(col7);\
rect(-650,350,50,50);\
fill(col8);\
rect(-600,350,50,50);\
\
fill(col9);\
rect(-950,300,50,50);\
fill(col10);\
rect(-900,300,50,50);\
fill(col11);\
rect(-850,300,50,50);\
fill(col12);\
rect(-800,300,50,50);\
fill(col13);\
rect(-750,300,50,50);\
fill(col14);\
rect(-700,300,50,50);\
fill(col15);\
rect(-650,300,50,50);\
fill(col16);\
rect(-600,300,50,50);\
\
fill(col17);\
rect(-950,250,50,50);\
fill(col18);\
rect(-900,250,50,50);\
fill(col19);\
rect(-850,250,50,50);\
fill(col20);\
rect(-800,250,50,50);\
fill(col21);\
rect(-750,250,50,50);\
fill(col22);\
rect(-700,250,50,50);\
fill(col23);\
rect(-650,250,50,50);\
fill(col24);\
rect(-600,250,50,50);\
\
fill(col25);\
rect(-950,200,50,50);\
fill(col26);\
rect(-900,200,50,50);\
fill(col27);\
rect(-850,200,50,50);\
fill(col28);\
rect(-800,200,50,50);\
fill(col29);\
rect(-750,200,50,50);\
fill(col30);\
rect(-700,200,50,50);\
fill(col31);\
rect(-650,200,50,50);\
fill(col32);\
rect(-600,200,50,50);\
\
fill(col33);\
rect(-950,150,50,50);\
fill(col34);\
rect(-900,150,50,50);\
fill(col35);\
rect(-850,150,50,50);\
fill(col36);\
rect(-800,150,50,50);\
fill(col37);\
rect(-750,150,50,50);\
fill(col38);\
rect(-700,150,50,50);\
fill(col39);\
rect(-650,150,50,50);\
fill(col40);\
rect(-600,150,50,50);\
\
fill(col41);\
rect(-950,100,50,50);\
fill(col42);\
rect(-900,100,50,50);\
fill(col43);\
rect(-850,100,50,50);\
fill(col44);\
rect(-800,100,50,50);\
fill(col45);\
rect(-750,100,50,50);\
fill(col46);\
rect(-700,100,50,50);\
fill(col47);\
rect(-650,100,50,50);\
fill(col48);\
rect(-600,100,50,50);\
\
fill(col49);\
rect(-950,50,50,50);\
fill(col50);\
rect(-900,50,50,50);\
fill(col51);\
rect(-850,50,50,50);\
fill(col52);\
rect(-800,50,50,50);\
fill(col53);\
rect(-750,50,50,50);\
fill(col54);\
rect(-700,50,50,50);\
fill(col55);\
rect(-650,50,50,50);\
fill(col56);\
rect(-600,50,50,50);\
\
fill(col57);\
rect(-950,0,50,50);\
fill(col58);\
rect(-900,0,50,50);\
fill(col59);\
rect(-850,0,50,50);\
fill(col60);\
rect(-800,0,50,50);\
fill(col61);\
rect(-750,0,50,50);\
fill(col62);\
rect(-700,0,50,50);\
fill(col63);\
rect(-650,0,50,50);\
fill(col64);\
rect(-600,0,50,50);\
\
\
\
\
byte dataArray[] = new byte[64];\
\
      dataArray[0] = byte(pos1);\
      dataArray[1] = byte(pos2);\
      dataArray[2] = byte(pos3);\
      dataArray[3] = byte(pos4);\
      dataArray[4] = byte(pos5);\
      dataArray[5] = byte(pos6);\
      dataArray[6] = byte(pos7);\
      dataArray[7] = byte(pos8);\
      \
      dataArray[8] = byte(pos9);\
      dataArray[9] = byte(pos10);\
      dataArray[10] = byte(pos11);\
      dataArray[11] = byte(pos12);\
      dataArray[12] = byte(pos13);\
      dataArray[13] = byte(pos14);\
      dataArray[14] = byte(pos15);\
      dataArray[15] = byte(pos16);\
      \
      dataArray[16] = byte(pos17);\
      dataArray[17] = byte(pos18);\
      dataArray[18] = byte(pos19);\
      dataArray[19] = byte(pos20);\
      dataArray[20] = byte(pos21);\
      dataArray[21] = byte(pos22);\
      dataArray[22] = byte(pos23);\
      dataArray[23] = byte(pos24);\
\
      dataArray[24] = byte(pos25);\
      dataArray[25] = byte(pos26);\
      dataArray[26] = byte(pos27);\
      dataArray[27] = byte(pos28);\
      dataArray[28] = byte(pos29);\
      dataArray[29] = byte(pos30);\
      dataArray[30] = byte(pos31);\
      dataArray[31] = byte(pos32);\
      \
      dataArray[32] = byte(pos33);\
      dataArray[33] = byte(pos34);\
      dataArray[34] = byte(pos35);\
      dataArray[35] = byte(pos36);\
      dataArray[36] = byte(pos37);\
      dataArray[37] = byte(pos38);\
      dataArray[38] = byte(pos39);\
      dataArray[39] = byte(pos40);\
      \
      dataArray[40] = byte(pos41);\
      dataArray[41] = byte(pos42);\
      dataArray[42] = byte(pos43);\
      dataArray[43] = byte(pos44);\
      dataArray[44] = byte(pos45);\
      dataArray[45] = byte(pos46);\
      dataArray[46] = byte(pos47);\
      dataArray[47] = byte(pos48);\
      \
      dataArray[48] = byte(pos49);\
      dataArray[49] = byte(pos50);\
      dataArray[50] = byte(pos51);\
      dataArray[51] = byte(pos52);\
      dataArray[52] = byte(pos53);\
      dataArray[53] = byte(pos54);\
      dataArray[54] = byte(pos55);\
      dataArray[55] = byte(pos56);\
      \
      dataArray[56] = byte(pos57);\
      dataArray[57] = byte(pos58);\
      dataArray[58] = byte(pos59);\
      dataArray[59] = byte(pos60);\
      dataArray[60] = byte(pos61);\
      dataArray[61] = byte(pos62);\
      dataArray[62] = byte(pos63);\
      dataArray[63] = byte(pos64);\
      \
          int sum =\
          dataArray[0]+\
          dataArray[1]+\
          dataArray[2]+\
          dataArray[3]+\
          dataArray[4]+\
          dataArray[5]+\
          dataArray[6]+\
          dataArray[7]+\
          \
          dataArray[8]+\
          dataArray[9]+\
          dataArray[10]+\
          dataArray[11]+\
          dataArray[12]+\
          dataArray[13]+\
          dataArray[14]+\
          dataArray[15]+\
          \
          dataArray[16]+\
          dataArray[17]+\
          dataArray[18]+\
          dataArray[19]+\
          dataArray[20]+\
          dataArray[21]+\
          dataArray[22]+\
          dataArray[23]+\
          \
          dataArray[24]+\
          dataArray[25]+\
          dataArray[26]+\
          dataArray[27]+\
          dataArray[28]+\
          dataArray[29]+\
          dataArray[30]+\
          dataArray[31]+\
          \
          dataArray[32]+\
          dataArray[33]+\
          dataArray[34]+\
          dataArray[35]+\
          dataArray[36]+\
          dataArray[37]+\
          dataArray[38]+\
          dataArray[39]+\
          \
          dataArray[40]+\
          dataArray[41]+\
          dataArray[42]+\
          dataArray[43]+\
          dataArray[44]+\
          dataArray[45]+\
          dataArray[46]+\
          dataArray[47]+\
          \
          dataArray[48]+\
          dataArray[49]+\
          dataArray[50]+\
          dataArray[51]+\
          dataArray[52]+\
          dataArray[53]+\
          dataArray[54]+\
          dataArray[55]+\
          \
          dataArray[56]+\
          dataArray[57]+\
          dataArray[58]+\
          dataArray[59]+\
          dataArray[60]+\
          dataArray[61]+\
          dataArray[62]+\
          dataArray[63];\
\
          \
if (sum<7232)\{\
\
\
myPort.write(dataArray);\}\
\
//println(dataArray);\
\
delay(2500);\
\
\}\
\
\
\
void keyPressed()\
\{\
  switch(key)\
  \{\
  case ' ':\
    context.setMirror(!context.mirror());\
    break;\
  \}\
\
  switch(keyCode)\
  \{\
  case LEFT:\
    rotY += 0.1f;\
    break;\
  case RIGHT:\
    // zoom out\
    rotY -= 0.1f;\
    break;\
  case UP:\
    if(keyEvent.isShiftDown())\
      zoomF += 0.02f;\
    else\
      rotX += 0.1f;\
    break;\
  case DOWN:\
    if(keyEvent.isShiftDown())\
    \{\
      zoomF -= 0.02f;\
      if(zoomF < 0.01)\
        zoomF = 0.01;\
    \}\
    else\
      rotX -= 0.1f;\
    break;\
  \}\
\}\
\
\
\
arduino:\
\
#include <Wire.h>\
#include <Adafruit_PWMServoDriver.h>\
\
Adafruit_PWMServoDriver pwm_0 = Adafruit_PWMServoDriver();\
Adafruit_PWMServoDriver pwm_1 = Adafruit_PWMServoDriver(0x41);\
Adafruit_PWMServoDriver pwm_2 = Adafruit_PWMServoDriver(0x42);\
Adafruit_PWMServoDriver pwm_3 = Adafruit_PWMServoDriver(0x43);\
\
#define SERVOMIN  200\
#define SERVOMAX  650\
\
int serial_speed = 9600;\
int relay1 = 2;\
\
int degrees;\
\
int cV = 0;\
int val[] = \{0,0,0,0,0,0,0,0,\
             0,0,0,0,0,0,0,0,\
             0,0,0,0,0,0,0,0,\
             0,0,0,0,0,0,0,0,\
             0,0,0,0,0,0,0,0,\
             0,0,0,0,0,0,0,0,\
             0,0,0,0,0,0,0,0,\
             0,0,0,0,0,0,0,0\};       \
          \
void setup()                \
\{ \
  pwm_0.begin();\
  pwm_1.begin();\
  pwm_2.begin();\
  pwm_3.begin();\
  \
  pwm_0.setPWMFreq(60);\
  pwm_1.setPWMFreq(60);\
  pwm_2.setPWMFreq(60);\
  pwm_3.setPWMFreq(60);\
  \
  Serial.begin(serial_speed);\
  //pinMode(relay1, OUTPUT);\
\}\
\
\
void loop() \{   \
  if(Serial.available())\{\
    //digitalWrite(relay1, LOW);\
    int incV = Serial.read();\
    \
    val[cV] = incV;\
 \
    cV++;\
    if(cV > 63)\{\
      cV = 0;\
    \}\
 \
    \
    \
    int q = 6;\
    \
int    pos1 = val[0]*q;\
int    pos2 = val[1]*q;\
int    pos3 = val[2]*q;\
int    pos4 = val[3]*q;\
int    pos5 = val[4]*q;\
int    pos6 = val[5]*q;\
int    pos7 = val[6]*q;\
int    pos8 = val[7]*q;\
    \
int    pos9 = val[8]*q;\
int    pos10 = val[9]*q;\
int    pos11 = val[10]*q;\
int    pos12 = val[11]*q;\
int    pos13 = val[12]*q;\
int    pos14 = val[13]*q;\
int    pos15 = val[14]*q;\
int    pos16 = val[15]*q;\
    \
int    pos17 = val[16]*q;\
int    pos18 = val[17]*q;\
int    pos19 = val[18]*q;\
int    pos20 = val[19]*q;\
int    pos21 = val[20]*q;\
int    pos22 = val[21]*q;\
int    pos23 = val[22]*q;\
int    pos24 = val[23]*q;\
    \
int    pos25 = val[24]*q;\
int    pos26 = val[25]*q;\
int    pos27 = val[26]*q;\
int    pos28 = val[27]*q;\
int    pos29 = val[28]*q;\
int    pos30 = val[29]*q;\
int    pos31 = val[30]*q;\
int    pos32 = val[31]*q;\
    \
int    pos33 = val[32]*q;\
int    pos34 = val[33]*q;\
int    pos35 = val[34]*q;\
int    pos36 = val[35]*q;\
int    pos37 = val[36]*q;\
int    pos38 = val[37]*q;\
int    pos39 = val[38]*q;\
int    pos40 = val[39]*q;\
    \
int    pos41 = val[40]*q;\
int    pos42 = val[41]*q;\
int    pos43 = val[42]*q;\
int    pos44 = val[43]*q;\
int    pos45 = val[44]*q;\
int    pos46 = val[45]*q;\
int    pos47 = val[46]*q;\
int    pos48 = val[47]*q;\
    \
int    pos49 = val[48]*q;\
int    pos50 = val[49]*q;\
int    pos51 = val[50]*q;\
int    pos52 = val[51]*q;\
int    pos53 = val[52]*q;\
int    pos54 = val[53]*q;\
int    pos55 = val[54]*q;\
int    pos56 = val[55]*q;\
    \
int    pos57 = val[56]*q;\
int    pos58 = val[57]*q;\
int    pos59 = val[58]*q;\
int    pos60 = val[59]*q;\
int    pos61 = val[60]*q;\
int    pos62 = val[61]*q;\
int    pos63 = val[62]*q;\
int    pos64 = val[63]*q;\
    \
    \
    \
    pwm_0.setPWM(0, 0, pos1);\
    pwm_0.setPWM(1, 0, pos2);\
    pwm_0.setPWM(2, 0, pos3);\
    pwm_0.setPWM(3, 0, pos4);\
    pwm_0.setPWM(4, 0, pos5);\
    pwm_0.setPWM(5, 0, pos6);\
    pwm_0.setPWM(6, 0, pos7);\
    pwm_0.setPWM(7, 0, pos8);\
    \
    pwm_0.setPWM(8, 0, pos9);\
    pwm_0.setPWM(9, 0, pos10);\
    pwm_0.setPWM(10, 0, pos11);\
    pwm_0.setPWM(11, 0, pos12);\
    pwm_0.setPWM(12, 0, pos13);\
    pwm_0.setPWM(13, 0, pos14);\
    pwm_0.setPWM(14, 0, pos15);\
    pwm_0.setPWM(15, 0, pos16);\
    \
    pwm_1.setPWM(0, 0, pos17);\
    pwm_1.setPWM(1, 0, pos18);\
    pwm_1.setPWM(2, 0, pos19);\
    pwm_1.setPWM(3, 0, pos20);\
    pwm_1.setPWM(4, 0, pos21);\
    pwm_1.setPWM(5, 0, pos22);\
    pwm_1.setPWM(6, 0, pos23);\
    pwm_1.setPWM(7, 0, pos24);\
    \
    pwm_1.setPWM(8, 0, pos25);\
    pwm_1.setPWM(9, 0, pos26);\
    pwm_1.setPWM(10, 0, pos27);\
    pwm_1.setPWM(11, 0, pos28);\
    pwm_1.setPWM(12, 0, pos29);\
    pwm_1.setPWM(13, 0, pos30);\
    pwm_1.setPWM(14, 0, pos31);\
    pwm_1.setPWM(15, 0, pos32);\
    \
    pwm_2.setPWM(0, 0, pos33);\
    pwm_2.setPWM(1, 0, pos34);\
    pwm_2.setPWM(2, 0, pos35);\
    pwm_2.setPWM(3, 0, pos36);\
    pwm_2.setPWM(4, 0, pos37);\
    pwm_2.setPWM(5, 0, pos38);\
    pwm_2.setPWM(6, 0, pos39);\
    pwm_2.setPWM(7, 0, pos40);\
    \
    pwm_2.setPWM(8, 0, pos41);\
    pwm_2.setPWM(9, 0, pos42);\
    pwm_2.setPWM(10, 0, pos43);\
    pwm_2.setPWM(11, 0, pos44);\
    pwm_2.setPWM(12, 0, pos45);\
    pwm_2.setPWM(13, 0, pos46);\
    pwm_2.setPWM(14, 0, pos47);\
    pwm_2.setPWM(15, 0, pos48);\
    \
    pwm_3.setPWM(0, 0, pos49);\
    pwm_3.setPWM(1, 0, pos50);\
    pwm_3.setPWM(2, 0, pos51);\
    pwm_3.setPWM(3, 0, pos52);\
    pwm_3.setPWM(4, 0, pos53);\
    pwm_3.setPWM(5, 0, pos54);\
    pwm_3.setPWM(6, 0, pos55);\
    pwm_3.setPWM(7, 0, pos56);\
    \
    pwm_3.setPWM(8, 0, pos57);\
    pwm_3.setPWM(9, 0, pos58);\
    pwm_3.setPWM(10, 0, pos59);\
    pwm_3.setPWM(11, 0, pos60);\
    pwm_3.setPWM(12, 0, pos61);\
    pwm_3.setPWM(13, 0, pos62);\
    pwm_3.setPWM(14, 0, pos63);\
    pwm_3.setPWM(15, 0, pos64);\
    \
    \
  \} else\{\
    \
    int i=600;\
    \
    pwm_0.setPWM(0, 0, i);\
    pwm_0.setPWM(1, 0, i);\
    pwm_0.setPWM(2, 0, i);\
    pwm_0.setPWM(3, 0, i);\
    pwm_0.setPWM(4, 0, i);\
    pwm_0.setPWM(5, 0, i);\
    pwm_0.setPWM(6, 0, i);\
    pwm_0.setPWM(7, 0, i);\
    \
    pwm_0.setPWM(8, 0, i);\
    pwm_0.setPWM(9, 0, i);\
    pwm_0.setPWM(10, 0, i);\
    pwm_0.setPWM(11, 0, i);\
    pwm_0.setPWM(12, 0, i);\
    pwm_0.setPWM(13, 0, i);\
    pwm_0.setPWM(14, 0, i);\
    pwm_0.setPWM(15, 0, i);\
    \
    pwm_1.setPWM(0, 0, i);\
    pwm_1.setPWM(1, 0, i);\
    pwm_1.setPWM(2, 0, i);\
    pwm_1.setPWM(3, 0, i);\
    pwm_1.setPWM(4, 0, i);\
    pwm_1.setPWM(5, 0, i);\
    pwm_1.setPWM(6, 0, i);\
    pwm_1.setPWM(7, 0, i);\
    \
    pwm_1.setPWM(8, 0, i);\
    pwm_1.setPWM(9, 0, i);\
    pwm_1.setPWM(10, 0, i);\
    pwm_1.setPWM(11, 0, i);\
    pwm_1.setPWM(12, 0, i);\
    pwm_1.setPWM(13, 0, i);\
    pwm_1.setPWM(14, 0, i);\
    pwm_1.setPWM(15, 0, i);\
    \
    pwm_2.setPWM(0, 0, i);\
    pwm_2.setPWM(1, 0, i);\
    pwm_2.setPWM(2, 0, i);\
    pwm_2.setPWM(3, 0, i);\
    pwm_2.setPWM(4, 0, i);\
    pwm_2.setPWM(5, 0, i);\
    pwm_2.setPWM(6, 0, i);\
    pwm_2.setPWM(7, 0, i);\
    \
    pwm_2.setPWM(8, 0, i);\
    pwm_2.setPWM(9, 0, i);\
    pwm_2.setPWM(10, 0, i);\
    pwm_2.setPWM(11, 0, i);\
    pwm_2.setPWM(12, 0, i);\
    pwm_2.setPWM(13, 0, i);\
    pwm_2.setPWM(14, 0, i);\
    pwm_2.setPWM(15, 0, i);\
    \
    pwm_3.setPWM(0, 0, i);\
    pwm_3.setPWM(1, 0, i);\
    pwm_3.setPWM(2, 0, i);\
    pwm_3.setPWM(3, 0, i);\
    pwm_3.setPWM(4, 0, i);\
    pwm_3.setPWM(5, 0, i);\
    pwm_3.setPWM(6, 0, i);\
    pwm_3.setPWM(7, 0, i);\
    \
    pwm_3.setPWM(8, 0, i);\
    pwm_3.setPWM(9, 0, i);\
    pwm_3.setPWM(10, 0, i);\
    pwm_3.setPWM(11, 0, i);\
    pwm_3.setPWM(12, 0, i);\
    pwm_3.setPWM(13, 0, i);\
    pwm_3.setPWM(14, 0, i);\
    pwm_3.setPWM(15, 0, i);\
    \
    \
\
\}\
\}\
  \
    \
    \
    \
    \
    \
    \
}