Diff: robot.cpp

Revision:

44:4b933e7409e7

Parent:

43:f7ef0f5f980c

Child:

48:519deb1d4dff

--- a/robot.cpp	Mon Mar 31 21:54:34 2014 +0000
+++ b/robot.cpp	Tue Apr 01 02:16:35 2014 +0000
@@ -488,4 +488,72 @@
     }
     switchCameraMode(0);
     return found;
+}
+int robot::nearRigCheck(){
+    if(cameraMode!=3){
+        switchCameraMode(3);
+    }
+    int gotAck=0;
+    int responseData[16];
+    stepTimer.start();
+    stepTimer.reset();
+    while(1){
+        BTLink.procBuf(0x03);
+        gotAck=BTLink.getData(0x03, responseData);
+        if(gotAck || stepTimer.read_ms()>=1000)
+            break;
+    }
+    stepTimer.stop();
+    if(responseData[6]==1 && gotAck){
+        int leftSide = (responseData[3]<<8)|responseData[2];
+        int rightSide = (responseData[5]<<8)|responseData[4];
+        int rigPos = ((leftSide+rightSide)/2) - 400;
+        int rigWidth = rightSide-leftSide;
+        DBGPRINT("Found rig at %d and width %d\n\r",rigPos,rigWidth);
+        return rigPos;
+    } else {
+        DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[3],gotAck,stepTimer.read_ms());
+        return -100000;
+    }
+}
+int robot::alignWithRig(){
+    const int cutoff = 6;
+    switchCameraMode(3);
+    BTLink.clearData();
+    cont.setSlowMode(5);
+    int found=1000;
+    while(!(found>-cutoff && found <cutoff && cont.stopped())){
+    //for(int i=0;i<5;i++){
+        found=nearRigCheck();
+        if(found!=-100000){
+            if(found>=cutoff){
+                //see the rig on the right, turn right
+                int moveFactor;
+                if(found>100){
+                    moveFactor = 3;
+                } else {
+                    moveFactor = found/40 + 1;
+                }
+                moveFactor = 1;
+                cont.rampSpeed(moveFactor,-moveFactor);
+            } else if(found<=-cutoff){
+                //see the rig on the left, turn left
+                int moveFactor;
+                if(found<-100){
+                    moveFactor = 3;
+                } else {
+                    moveFactor = -found/40 + 1;
+                }
+                moveFactor = 1;
+                cont.rampSpeed(-moveFactor,moveFactor);
+            } else {
+                cont.rampSpeed(0,0);
+            }
+        } else {
+            cont.rampSpeed(0,0);
+        }
+    }
+    cont.setSlowMode(1);
+    switchCameraMode(0);
+    return found;
 }
\ No newline at end of file