Diff: robot.cpp

Revision:

26:ade7c813538f

Parent:

17:e247d58d9f42

Child:

27:688409727452

--- a/robot.cpp	Thu Mar 13 06:25:31 2014 +0000
+++ b/robot.cpp	Fri Mar 14 00:38:51 2014 +0000
@@ -17,12 +17,8 @@
     const int *referenceCounts = bigenc.getReferences();
     motors.setSetPoint(referenceCounts[0],referenceCounts[1]);
     
-    //control system constants
-    pfac=0.00035;
-    ifac=0.00000001;
-    dfac=0.000001;
-    
-    angfac=0.0000016;
+    //storage for shape positions
+    circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
 }
 
 //driveforward, but set up so that 
@@ -395,4 +391,65 @@
     //right.brake();
     //left.brake();
     return (gyroticks-nowz)*dir;
-}*/
\ No newline at end of file
+}*/
+
+int robot::pollForShapes(){
+    int response=0;
+    int gotAck=0;
+    int responseData[16];
+    int testdata[]={'H','e','l','l','o'};
+    stepTimer.start();
+    while(1){ //put the phone in mode 1 to start finding shapes
+        response=BTLink.sendCmd(0x01,testdata,5);
+        stepTimer.reset();
+        while(1){
+            gotAck=BTLink.getAck(response);
+            if(gotAck || stepTimer.read_ms()>=500)
+                break;
+        }
+        DBGPRINT("Mode1=%d, %d, %d [%d]\n\r",response,BTLink.bufSize(), gotAck, stepTimer.read_ms());
+        if(gotAck)
+            break;
+    }
+    while(1){ //scan the information we get back to find valid data
+        stepTimer.reset();
+        gotAck=0;
+        while(1){
+            BTLink.procBuf(0x01);
+            gotAck=BTLink.getData(0x01, responseData);
+            if(gotAck || stepTimer.read_ms()>=4000)
+                break;
+        }
+        if(responseData[15]==1&&gotAck){
+            for(int i=0;i<16;i++)
+                DBGPRINT("%d,",responseData[i]);
+            DBGPRINT("\n\r",1);
+            circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
+            rectX=(responseData[4]<<8)|responseData[3];
+            rectY=(responseData[6]<<8)|responseData[5];
+            rectRot=responseData[2];
+            circleX=(responseData[12]<<8)|responseData[11];
+            circleY=(responseData[14]<<8)|responseData[13];
+            triX=(responseData[8]<<8)|responseData[7];
+            triY=(responseData[10]<<8)|responseData[9];
+            triRot=responseData[15];
+            DBGPRINT("Rect(%d, %d, %d) Tri(%d, %d, %d) Cir(%d, %d)\n\r",rectX,rectY,rectRot,triX,triY,triRot,circleX,circleY);
+            break;
+        } else {
+            DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[15],gotAck,stepTimer.read_ms());
+        }
+    }
+    while(1){ //put the phone back in default state
+        response=BTLink.sendCmd(0x00,testdata,5);
+        stepTimer.reset();
+        while(1){
+            gotAck=BTLink.getAck(response);
+            if(gotAck || stepTimer.read_ms()>=500)
+                break;
+        }
+        DBGPRINT("BackToNormal=%d, %d, %d [%d]\n\r",response,BTLink.bufSize(), gotAck, stepTimer.read_ms());
+        if(gotAck)
+            break;
+    }
+    stepTimer.stop();
+}
\ No newline at end of file