Diff: src/kangaroo.cpp

Revision:

55:ff84fbdfd1d1

Parent:

54:17ea4b3c80de

Child:

56:7015e2e79ea7

--- a/src/kangaroo.cpp	Mon Dec 02 06:54:48 2013 +0000
+++ b/src/kangaroo.cpp	Mon Dec 02 23:15:15 2013 +0000
@@ -5,14 +5,17 @@
 
 Kangaroo::Kangaroo(Motor &M1, Motor &M2, QEI &e1, QEI &e2):enc1(e1),enc2(e2),m1(M1),m2(M2),led1(LED1),led2(LED2),led3(LED3),led4(LED4){
 
+    joint_h=0;
+    leg_y = 0;
+
 }
 
 void Kangaroo::zero(){
     m1.start();
     m2.start();
     
-    //m1.setVel(.3);//use in final
-    m2.setVel(5);
+    m1.setVel(.5);//use in final
+    m2.setVel(.3);
     //m2.setPos(3.1415/5);//bring legs to hard stop
     
     led4=1;
@@ -23,23 +26,24 @@
     m1.calibAngle(0.0);
     m2.calibAngle(2.44);
     
-    wait(1);
+    wait(.1);
+    
     
-    //m1.setPos(-3.1415/2);
-    //m2.setPos(3.1415/2);
+    m1.setPos(-3.1415/12);
+    m2.setPos(3.1415/2);
     
     enc1.reset();
     enc2.reset();
     led2=1;
-    /*while(((enc1.getRevolutions()==0)||(enc2.getRevolutions()==0))){
+    while(((enc1.getRevolutions()==0)||(enc2.getRevolutions()==0))){
         wait(0.1);
-    }*/
+    }
     
-    
+    landDetection();
     led1=1;
     
     //setPoint(Point(0,-.08,0));
-    //wait(3);
+    wait(3);
     led3=0;
     led1=0;
     led2=0;
@@ -48,25 +52,44 @@
     //m2.stop();
 }
 
+float Kangaroo::jointHeight(){
+    return joint_h;
+}
+
+float Kangaroo::legY(){
+    return leg_y;
+}
+
 void Kangaroo::testEncoders(Serial &pc){
     //pc.printf("hello world\n");
     //pc.printf("x: %f   y: %f\n", getPoint().x, getPoint().y);
-    pc.printf("me:  %f  beam:  %f\n",getAngle2(), getAngle1());
+    pc.printf("me:  %f  joint: %f length: %f\n",getAngle2(), jointHeight(), legY());
     //pc.printf("clix1:  %i, rots1: %i\n",enc1.getPulses(), enc1.getRevolutions());
     
     //pc.printf("motor2:  %i\n",);
     //pc.printf("clix2:  %i, rots2: %i\n",enc2.getPulses(), enc2.getRevolutions());
 }
 
+int Kangaroo::landDetection(){
+    //Helper function to Detect If we have landed.
+    //NO Input
+    //Output:  0 =  Flight Phase.     1 = Ground Phase.
+    float th3 = getAngle1();
+    joint_h = lg*sin(th3)+h;
+    float th4 = getAngle2();
+    leg_y = -1*(-l1+getPoint().y)*cos(th4);
+    return joint_h<(leg_y+.013);
+}
+
 Point Kangaroo::getPoint(){
-    float x = (l2+l4)*cos(m1.getAngle())-(l3)*cos(m1.getAngle()+m2.getAngle());
-    float y = (l2+l4)*sin(m1.getAngle())+(l3)*sin(m1.getAngle()+m2.getAngle());
+    float x = (l2+l4)*cos(m1.getAngle())+(l3)*cos(m1.getAngle()+m2.getAngle()-3.1415);
+    float y = (l2+l4)*sin(m1.getAngle())+(l3)*sin(m1.getAngle()+m2.getAngle()-3.1415);
     
     return Point(x,y,0);
 }
 
 float Kangaroo::getAngle2(){
-    return (enc2.getAngle())-1.56;
+    return (enc2.getAngle())-1.65;
 }