I am getting data in from a Kinect and am using it to move the head of an XY plotter back and forth, up and down, etc.

The way I am doing it, is I am assigning certain characters to certain directions and having Processing send them to the Arduino as commands.

The problem is, if I send it a bunch of data, and then the situation changes, it continues to execute the whole sequence of data input from before. For example, if it's mirroring me, and I stand to the left, it sends the plotter head to the left. But if I move right, it keeps sending commands to go left for a time, THEN sends it right.

Is there a way to clear this backlog? 

Thanks in advance!!!!!!!!!!

Processing Code:

2. 
   
3. //---------------------------------------------
4. //Thanks to  thomas diewald for dLibs processing library for Kinect  
5. //Antonio Molinaro for blobscanner library
6. 
   
7. import java.util.Arrays; // This may not be needed
8. import Blobscanner.*;
9. 
   
10. //freenect stuff
11. import dLibs.freenect.toolbox.*;
12. import dLibs.freenect.constants.*;
13. import dLibs.freenect.interfaces.*;
14. import dLibs.freenect.*;
15. 
    
16. //SERIAL
17. 
    
18. import processing.serial.*;
19. Serial myPort;                       
20. char keyIndex;
21. 
    
22. 
    
23. //-----------------------KINECT STUFF-------------------------------------
24. Kinect kinect_;
25. KinectFrameVideo kinect_video_;
26. KinectFrameDepth kinect_depth_;
27. 
    
28. int kinectFrame_size_x = VIDEO_FORMAT._RGB_.getWidth();
29. int kinectFrame_size_y = VIDEO_FORMAT._RGB_.getHeight();
30. 
    
31. PImage video_frame_, depth_frame_;  // images
32. 
    
33. // raw depth values of the kinect depth camera  ( copy by reference or deep copy)
34. int[] raw_depth; 
35. 
    
36. int tooFar, tooClose;
37. float personX, personY;
38. boolean reaching = false;
39. boolean isPresent;
40. int presCounter = 0;
41. int absenceCounter = 0;
42. 
    
43. //mood variables
44. int mood;
45. 
    
46. //----------------------------------------------------------------------------
47. //-----------------------BLOB SCANNER STUFF-----------------------------------
48. 
    
49. 
    
50. Detector bd;
51. 
    
52. PImage img;
53. PFont f = createFont("", 10);
54. 
    
55. //-----------------------------------------------------------------------------
56. 
    
57. 
    
58. void setup() {
59.   println(Serial.list());
60.   String portName = Serial.list()[0];
61.   println(portName);
62.   myPort = new Serial(this, portName, 9600);
63. 
    
64.   size(kinectFrame_size_x, kinectFrame_size_y*2);
65.     
66.       kinect_ = new Kinect(0);
67.       kinect_video_ = new KinectFrameVideo(VIDEO_FORMAT._RGB_  );
68.       kinect_depth_ = new KinectFrameDepth(DEPTH_FORMAT._11BIT_);
69.       kinect_depth_.setColorMode(2); // no generation of depth-map by default
70.       kinect_video_.setFrameRate(30);
71.       kinect_depth_.setFrameRate(30);    
72.       kinect_video_.connect(kinect_);
73.       kinect_depth_.connect(kinect_); 
74. 
    
75.   // create a PImage for video/depth
76.   video_frame_ = createImage(VIDEO_FORMAT._RGB_  .getWidth(), VIDEO_FORMAT._RGB_  .getHeight(), RGB);
77.   depth_frame_ = createImage(DEPTH_FORMAT._11BIT_.getWidth(), DEPTH_FORMAT._11BIT_.getHeight(), RGB);
78. 
    
79. 
    
80.   //blob detection
81.   bd = new Detector( this, 0, 0, kinectFrame_size_x, kinectFrame_size_y, 255 );
82. 
    
83.   mood = 0;
84.   isPresent = false;
85. 
    
86. }
87. 
    
88. //-------------------------------------------------------------------
89. void draw() {
90. 
    
91.   background(0);
92.   // println(frameRate);
93. 
    
94.   //---------------------------------------
95.   // DEPTH VALUES
96.   //---------------------------------------
97.   raw_depth = kinect_depth_.getRawDepth(); // just copy by reference
98. 
    
99.   // custom mapping from raw-depth values, to a gray depthmap
100.   depth_frame_.loadPixels();
101.   for (int i = 0; i < raw_depth.length; i++) {
102.     // 2047 is the depth-value-"code" for kinect-shadows
103.     /*
104.      } else
105.      { */    // valid depth values
106.     // raw depth values have a range of ~330 to ~1150 and are mapped to a gray-values from 255 to 0
107.     //int gray = ((int) map(raw_depth[i], 330, 1150, 255, 0)) & 0xFF ; 
108.     if (raw_depth[i] > 350 && raw_depth[i] < 720 ) {
109.       depth_frame_.pixels[i] = 0xFF0000;
110.     } 
111.     else {
112.       depth_frame_.pixels[i] = 0x000000;
113.     }
114.   }
115. 
     
116.   //FLAG if anyone reaches ahead
117. 
     
118.   depth_frame_.updatePixels();
119. 
     
120.   //---------------------------------------
121.   // VIDEO VALUES
122.   //---------------------------------------
123.   // copy rgb-video-pixels to image (by reference!!)
124.   video_frame_.loadPixels();
125.   video_frame_.pixels = kinect_video_.getPixels(); 
126.   video_frame_.updatePixels();
127. 
     
128. 
     
129.   //set up blobtracker thresholding of image
130.   bd.imageFindBlobs(depth_frame_);
131.   bd.loadBlobsFeatures();
132.   image(depth_frame_, 0, 0);
133.   //image(depth_frame_, 0, DEPTH_FORMAT._11BIT_.getHeight());
134. 
     
135. 
     
136.   //Draws the bounding box for all the blobs in the image.
137.   int id = getBiggestBlobIndex(bd);
138. 
     
139.   bd.findCentroids(false, false);
140.   
141.   if (id > -1) {
142.     personX = bd.getCentroidX(id);
143.     personY = bd.getCentroidY(id);
144.     presCounter++;
145.     mood++;
146.     absenceCounter=0;
147.     isPresent = true;
148.   }
149.   if (id < 0){
150.     absenceCounter++; 
151.     presCounter = 0;
152.     isPresent = false;
153.     mood = 0;
154.   }
155. if(isPresent && mood <= 200){
156.   shyPresent();  
157.  }
158. else if(isPresent && mood > 200){
159.   cheekyPresent();  
160.  }
161. 
     
162. 
     
163. 
     
164. //show us where the centroid is
165.   ellipse(personX, personY, 20, 20);
166. 
     
167. 
     
168. }
169. 
     
170. 
     
171. void shyPresent(){
172.   println("Someone is present, but I am shy!");
173.     if (personX>=300) {
174.       myPort.write('J'); // Send out serial msg
175.     }
176.     if (personX<300) {
177.       myPort.write('X'); // Send out serial msg
178.     }
179. 
     
180. }
181. void cheekyPresent(){
182.   println("Someone is present, and I am excited!");
183.     if (personX>=300) {
184.       myPort.write('Z'); // Send out serial msg
185.     }
186.     if (personX<300) {
187.       myPort.write('H'); // Send out serial msg
188.     }
189. 
     
190. }
191. void absent(){
192. }
193. 
     
194. 
     
195. //thanks to Quarks http://www.lagers.org.uk for this bit of code 
196. public int getBiggestBlobIndex(Detector d) {
197.   int nbrBlobs = d.getBlobsNumber();
198.   // If we have no blobs return
199.   if (nbrBlobs ==0)
200.     return -1;
201.   d.weightBlobs(false);
202.   BlobWeight[] blobs = new BlobWeight[nbrBlobs];
203.   for (int i = 0; i < blobs.length; i++)
204.     blobs[i] = new BlobWeight(i, d.getBlobWeight(i));
205.   Arrays.sort(blobs);
206.   return blobs[0].id;
207. }
208. 
     
209. public class BlobWeight implements Comparable<BlobWeight> {
210.   public int id;
211.   public Integer weight;
212. 
     
213.   public BlobWeight(int id, Integer weight) {
214.     this.id = id;
215.     this.weight = weight;
216.   }
217. 
     
218.   public int compareTo(BlobWeight b) {
219.     // Reverse so heaviest are first in list
220.     return new Integer(b.weight).compareTo(weight);
221.   }
222. }
223. 
     
224. 
     
225. void dispose() {
226.   Kinect.shutDown(); 
227.   super.dispose();
228. }

Arduino Code:

2. 
   
3. 
   
4. 
   
5. 
   
6. /* Stepper Motor Controller ;  language: Wiring/Arduino
7.  This program drives a unipolar or bipolar stepper motor.  by Tom Igoe 
8.  and the Spline library by Kerinin
9.  */
10.  
11. /* significant help from arduino forum superstars: sirbow2, stimmer
12.  */
13. #include <AFMotor.h>
14. #include <Servo.h>
15. 
    
16. 
    
17. AF_Stepper xAxis(200, 1);
18. AF_Stepper yAxis(200, 2);
19. 
    
20. 
    
21. int reading = 0;
22. int previous = LOW;
23. long time = 0;         // the last time the output pin was toggled
24. long debounce = 50;   // the debounce time, increase if the output flickers
25. int state = HIGH;    
26. 
    
27. //set pin numbers
28. 
    
29. int xStopMin = A0;
30. int xStopMax = A1;
31. int yStopMin = A2;
32. int yStopMax = A3;
33. 
    
34. 
    
35. int xPos;
36. int yPos;
37. boolean xIs0;
38. boolean xIsMax;
39. boolean yIs0;
40. boolean yIsMax;
41. 
    
42. boolean interruption;
43. 
    
44. void setup() { 
45. 
    
46.   xAxis.setSpeed(60);
47. 
    
48.   yAxis.setSpeed(60);
49. 
    
50. 
    
51.   //set limit switches to analog inputs
52.   pinMode(xStopMin, INPUT);
53.   pinMode(xStopMax, INPUT);
54.   pinMode(yStopMin, INPUT); 
55.   pinMode(yStopMax, INPUT);
56. 
    
57. 
    
58.   interruption = false;
59. 
    
60.   xPos = 0;
61.   yPos = 0;
62. 
    
63.   // Initialize the Serial port:
64.   Serial.begin(9600);
65. }
66. 
    
67. 
    
68. void loop() {
69.   //check interrupt
70.   if(interruption == true){
71.     int counter;
72.     counter ++;
73.     Serial.println("pause for a sec");
74.     delay(1000);
75.     Serial.println("ok, do new stuff now");
76.     interruption = false; 
77.   }
78. 
    
79. 
    
80.   //check limit switches
81.   if(digitalRead(xStopMin) == HIGH){
82.     xIs0 = true;  
83.     xPos = 0;
84.     Serial.println("xMin");
85.   }
86.   else xIs0 = false;
87. 
    
88. 
    
89.   if(digitalRead(xStopMax) == HIGH){
90.     xIsMax = true;  
91.     Serial.println("xMax");
92.   }
93.   else xIsMax = false;
94. 
    
95. 
    
96.   if(digitalRead(yStopMin) == HIGH){
97.     yIs0 = true;  
98.     Serial.println("yMin");
99.     yPos = 0;
100. 
     
101.   }
102.   else yIs0 = false;
103. 
     
104.   if(digitalRead(yStopMax) == HIGH){
105.     yIsMax = true;  
106.     Serial.println("yMax");
107.   }
108.   else yIsMax = false;
109. 
     
110. 
     
111. 
     
112.   char val=0;
113.   if(Serial.available()) val = Serial.read();
114. 
     
115.   //keyboard control  
116.   switch(val){
117. 
     
118.   case 'A':
119.     Serial.read();
120.     interruption = true;
121.     break;  
122. 
     
123.   case 'N':
124.     Serial.read();
125.     moveN(200);
126.     break;
127. 
     
128.   case 'S':
129.     Serial.read();
130.     moveS(200);
131.     break;
132. 
     
133.   case 'E':
134.     Serial.read();
135.     moveE(200);
136.     break;
137. 
     
138.   case 'W':
139.     Serial.read();
140.     moveW(200);
141.     break; 
142. 
     
143. 
     
144. 
     
145. 
     
146.     //diagonal (moving motors almost at the same time)      
147.   case 'J': 
148.     Serial.read();
149.     moveNE(400);
150.     Serial.println("I am moving North East");
151.     break;
152. 
     
153. 
     
154.   case 'H': //NW
155.     Serial.read();
156.     moveNW(400);
157.     break;
158. 
     
159.   case 'X': //SE
160.     Serial.read();
161.     moveSE(400);
162.     break;
163. 
     
164. 
     
165.   case 'Z': //SW
166.     Serial.read();
167.     moveSW(400);
168.     break;
169. 
     
170.   case 'C': //
171.     Serial.read();
172.     moveCirc();
173.     break;
174. 
     
175. 
     
176. 
     
177. 
     
178.   case 'B': //
179.     Serial.read();
180.     moveTo (1100,3150);
181.     break;
182. 
     
183. 
     
184.     //resets
185.   case 'Q': //reset to 0,0 
186.     Serial.read();
187.     resetFully();
188.     break;
189. 
     
190.   case 'P':
191.     Serial.read();
192.     Serial.print("x position: ");
193.     Serial.println(xPos);
194.     Serial.println("y position: ");
195.     Serial.println(yPos);
196.     break;
197. 
     
198.   default:
199.     Serial.read();
200.     delay(10);
201.     break;
202.   }
203. }
204. //movement functions
205. 
     
206. //first, str  aight up down left right
207. void moveE(int numSteps){
208.   int s = 0;
209.   while(!digitalRead(xStopMax)&& s<numSteps)//checks the button each step, also checks to see if it has moved s amount of steps
210.   {
211.     Serial.read();
212.     if(!interruption){      
213.       xAxis.step(1,FORWARD,DOUBLE);
214.       s++;
215.     }
216.   }  
217.   //delay(100);    
218. 
     
219.   xPos += numSteps;
220. 
     
221.   //check where we are
222.   Serial.print("x position: ");
223.   Serial.println(xPos);
224.   Serial.print("y position: ");
225.   Serial.println(yPos);
226. 
     
227. }
228. 
     
229. 
     
230. void moveW(int numSteps){
231.   int s = 0;
232.   while(!digitalRead(xStopMin)&& s<numSteps)//checks the button each step, also checks to see if it has moved s amount of steps
233.   {
234.     Serial.read();
235.     if(!interruption){
236.       xAxis.step(1,BACKWARD,DOUBLE);
237.       s++;
238.     }
239.   }  
240.   //delay(100);    
241.   xPos -= numSteps;
242. 
     
243.   //check where we are
244.   Serial.print("x position: ");
245.   Serial.println(xPos);
246.   Serial.print("y position: ");
247.   Serial.println(yPos);
248. }
249. 
     
250. 
     
251. void moveN(int numSteps){
252.   int s = 0;
253.   while(!digitalRead(yStopMin)&& s<numSteps)//checks the button each step, also checks to see if it has moved s amount of steps
254.   {
255.     Serial.read();
256.     if(!interruption){
257.       yAxis.step(1,BACKWARD,DOUBLE);
258.       s++;
259.     }
260.   }  
261.   //delay(100);    
262.   yPos -= numSteps;
263. 
     
264.   //check where we are
265.   Serial.print("x position: ");
266.   Serial.println(xPos);
267.   Serial.print("y position: ");
268.   Serial.println(yPos);
269. 
     
270. }  
271. 
     
272. void moveS(int numSteps){
273.   int s = 0;
274.   while(!digitalRead(yStopMax)&& s<numSteps)//checks the button each step, also checks to see if it has moved s amount of steps
275.   {
276.     Serial.read();
277.     if(!interruption){
278.       yAxis.step(1,FORWARD,DOUBLE);
279.       s++;
280.     }
281.   }  
282.   //delay(100);    
283.   yPos += numSteps;
284. 
     
285.   //check where we are
286.   Serial.print("x position: ");
287.   Serial.println(xPos);
288.   Serial.print("y position: ");
289.   Serial.println(yPos);  
290. }
291. //diagonals
292. void moveNE(int numSteps){    
293.   int s=0; 
294.   if (!digitalRead(xStopMax) && !digitalRead(yStopMin) && s<numSteps){ 
295.     for(int s=0; s<numSteps; s++)
296.     {
297.       if(!interruption){  
298.         xAxis.step(1,FORWARD,DOUBLE);
299.         yAxis.step(1,BACKWARD,DOUBLE);
300.       }
301.     }
302.     xPos += numSteps;
303.     yPos -= numSteps;
304. 
     
305.     //check where we are
306.     Serial.print("x position: ");
307.     Serial.println(xPos);
308.     Serial.print("y position: ");
309.     Serial.println(yPos);
310.   }
311. }  
312. 
     
313. 
     
314. void moveSE(int numSteps){
315.   int s=0; 
316.   if(!digitalRead(xStopMax) && !digitalRead(yStopMax) && s<numSteps){ 
317.     for(int s=0; s<numSteps; s++)
318.     {
319.       if(!interruption){
320.         xAxis.step(1,FORWARD,DOUBLE);
321.         yAxis.step(1,FORWARD,DOUBLE);
322.       }
323.     }
324.     xPos += numSteps;
325.     yPos += numSteps;
326. 
     
327.     //check where we are
328.     Serial.print("x position: ");
329.     Serial.println(xPos);
330.     Serial.print("y position: ");
331.     Serial.println(yPos);
332.   }  
333. }
334. 
     
335. void moveSW(int numSteps){   
336.   int s=0; 
337.   if(!digitalRead(xStopMin) && !digitalRead(yStopMax) && s<numSteps){ 
338.     for(int s=0; s<numSteps; s++)
339.     {
340.       if(!interruption){
341.         xAxis.step(1,BACKWARD,DOUBLE);
342.         yAxis.step(1,FORWARD,DOUBLE);
343.       }
344.     }
345.     xPos -= numSteps;
346.     yPos += numSteps;
347. 
     
348.     //check where we are
349.     Serial.print("x position: ");
350.     Serial.println(xPos);
351.     Serial.print("y position: ");
352.     Serial.println(yPos);
353.   }
354. }
355. 
     
356. void moveNW(int numSteps){
357.   int s=0; 
358.   if(!digitalRead(xStopMin) && !digitalRead(yStopMin) && s<numSteps){ 
359.     for(int s=0; s<numSteps; s++)
360.     {
361.       if(!interruption){
362.         xAxis.step(1,BACKWARD,DOUBLE);
363.         yAxis.step(1,BACKWARD,DOUBLE);
364.       }
365.     }
366.     xPos -= numSteps;
367.     yPos -= numSteps;
368. 
     
369.     //check where we are
370.     Serial.print("x position: ");
371.     Serial.println(xPos);
372.     Serial.print("y position: ");
373.     Serial.println(yPos);
374.   }
375. } 
376. 
     
377. 
     
378. void resetFully(){
379. 
     
380.   bool bMoveX = true;
381.   bool bMoveY = true;
382.   bool doReset = true;
383.   int count = 0;
384.   while (doReset == true){
385. 
     
386.     if (digitalRead(xStopMin)) bMoveX = false;
387.     if (digitalRead(yStopMin)) bMoveY = false;
388. 
     
389.     count ++;
390. 
     
391.     if (bMoveX == true) xAxis.step(1,BACKWARD,DOUBLE);
392.     if (bMoveY == true) yAxis.step(1,BACKWARD,DOUBLE);
393. 
     
394.     if (bMoveX == false && bMoveY == false){
395.       doReset = false;
396.     }
397.     if (count == 8000){
398.       doReset = false;
399.     }
400.   }
401.   Serial.println("done!");
402.   xPos = 0;
403.   yPos = 0;
404. 
     
405. }
406. 
     
407. void moveTo(int newX, int newY){
408. 
     
409.   int xSteps = newX - xPos;
410.   int ySteps = newY - yPos;
411.   int stepDirectionX = (newX > xPos) ? FORWARD : BACKWARD;
412.   int stepDirectionY = (newY > yPos) ? FORWARD : BACKWARD;
413. 
     
414.   int nStepsX = abs(xSteps);
415.   int nStepsY = abs(ySteps);
416. 
     
417.   int maxSteps = abs(xSteps) > abs(ySteps) ? abs(xSteps) : abs(ySteps);
418. 
     
419.   for (int i = 0; i < maxSteps; i++){
420. 
     
421.     if (i < nStepsX){
422.       bool bMoveX = true;
423.       if (digitalRead(xStopMin)) bMoveX = false;
424.       if (digitalRead(xStopMax)) bMoveX = false;
425.       if (bMoveX == true){
426.         if(!interruption){
427.           xAxis.step(1, stepDirectionX, DOUBLE);
428.           xPos += stepDirectionX;
429.         } 
430.       }
431.     }
432.     if (i < nStepsY){
433.       bool bMoveY = true;
434.       if (digitalRead(yStopMin)) bMoveY = false;
435.       if (digitalRead(yStopMax)) bMoveY = false;
436.       if (bMoveY == true){
437.         if(!interruption){
438.           yAxis.step(1, stepDirectionY, DOUBLE);
439.           yPos += stepDirectionY;
440.         }
441.       } 
442.     }
443.   }
444. }
445. 
     
446. 
     
447. 
     
448. void moveCirc(){
449.   if(!interruption){     
450.     moveNE(200); 
451.   }
452.   if(!interruption){     
453.     moveE(100);  
454.   }
455.   if(!interruption){     
456.     moveSE(200); 
457.   }
458.   if(!interruption){     
459.     moveS(200);  
460.   }
461.   if(!interruption){     
462.     moveSW(200); 
463.   }
464.   if(!interruption){     
465.     moveW(100);  
466.   }
467.   if(!interruption){     
468.     moveNW(200); 
469.   }
470. 
     
471. }
472.