Diff: Master.cpp

Revision:

54:17ea4b3c80de

Parent:

53:978b7fa74080

Child:

55:ff84fbdfd1d1

diff -r 978b7fa74080 -r 17ea4b3c80de Master.cpp
--- a/Master.cpp	Mon Dec 02 05:16:26 2013 +0000
+++ b/Master.cpp	Mon Dec 02 06:54:48 2013 +0000
@@ -11,8 +11,8 @@
 Motor m2(p16,p19,p20,p22,mEnc1);//knee p19 high is CCW
 
 
-QEI bEnc1(p27, p28, p29, 500, QEI::X4_ENCODING);  //track  offset:-2.236814
-QEI bEnc2(p5, p6, p7, 500, QEI::X4_ENCODING); //body   offset:1.770973
+QEI bEnc1(p27, p28, p29, 2000, QEI::X4_ENCODING);  //track  offset:-2.236814
+QEI bEnc2(p5, p6, p7, 2000, QEI::X4_ENCODING); //body   offset:1.770973
 
 Kangaroo kankan(m1,m2,bEnc1,bEnc2);
 
@@ -24,16 +24,17 @@
 PwmOut pwmOut(p21);
 
 
-int landDetection(){
+int landDetection(Kangaroo &k){
     //Helper function to Detect If we have landed.
     //NO Input
     //Output:  0 =  Flight Phase.     1 = Ground Phase.
-    float th3 = bEnc1.getAngle();
-    float a = lg*sin(-th3)+h;
+    float th3 = k.getAngle1();
+    float a = lg*sin(th3)+h;
     float th1 = mEnc1.getAngle();
     float th2 = mEnc2.getAngle()-pi/2;
-    float b = l1+l2*sin(th1)+l3*sin(th2-th1)+ l2*sin(th1);
-    return a<b;
+    float th4 = k.getAngle2();
+    float b = (l1+(l2+l4)*sin(th1)+l3*sin(th2+th1))*sin(th4);
+    return a<(b+.005);
 }
 
 int SLIP(float initLen){//SlIP Model
@@ -65,12 +66,15 @@
     return 1;
 }
 int main() {
-       kankan.start();
-       kankan.zero();
+       //kankan.start();
+       //kankan.zero();
        
        while(1){
         kankan.testEncoders(pc);
         wait(0.01);
+        if(landDetection(kankan)){
+            printf("hello ground!\n");
+        }
         }
        
        /* Point p1(0,-.1,0);
@@ -98,7 +102,7 @@
                 //m1.setPos(-0.5);
                 led1=0;
                 kankan.setPoint(Point(0,-.08,0));
-                if(landDetection()){
+                if(landDetection(kankan)){
                     //Landed going to Phase 1
                     //Record initial Length.                   
                     Phase =1;
@@ -114,7 +118,7 @@
           
                 SLIP(initLen);
                 //m1.setPos(-1);
-                if(!landDetection()){
+                if(!landDetection(kankan)){
                     //Lift Up going to Phase 0
                     Phase =0;
                 }