Diff: Master.cpp

Revision:

55:ff84fbdfd1d1

Parent:

54:17ea4b3c80de

Child:

56:7015e2e79ea7

--- a/Master.cpp	Mon Dec 02 06:54:48 2013 +0000
+++ b/Master.cpp	Mon Dec 02 23:15:15 2013 +0000
@@ -23,39 +23,20 @@
 DigitalOut Backward(p18);
 PwmOut pwmOut(p21);
 
-
-int landDetection(Kangaroo &k){
-    //Helper function to Detect If we have landed.
-    //NO Input
-    //Output:  0 =  Flight Phase.     1 = Ground Phase.
-    float th3 = k.getAngle1();
-    float a = lg*sin(th3)+h;
-    float th1 = mEnc1.getAngle();
-    float th2 = mEnc2.getAngle()-pi/2;
-    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
     //Get Length
-    float th1 = mEnc1.getAngle();
-    float th2 = mEnc2.getAngle()-pi/2;
-    double y = l1+l2*sin(th1)+l3*sin(th2-th1)+ l2*sin(th1);
-    double x = l2*cos(th1)+l3*cos(th2-th1)+ l2*cos(th1);
+    double y = kankan.getPoint().y;
+    double x = kankan.getPoint().x;
     //float length = pow(pow(x,2.0)+pow(y,2.0),0.5);
     int ks = 0.5;
     //float mag = (initLen-length)*ks;
     //Apply Forward Kinematics
+    kankan.setPoint(Point((1+ks)*x,(1+ks)*y,0));
     //Point output = Point(-mag*x/length,-mag*y/length,0);
-    Point output(-(1+ks)*x,-(1+ks)*y,0);  //try just position scaling for slip
-    Joints dtheta = invKinBody(output);
-    m1.setPos(dtheta.t1);
-    m2.setPos(dtheta.t2);
-        //Joints dtorque = invKinBody(output);
-    //Apply Motor Torque Control.
-    //m1.setTorque(dtheta.t1);
-    //m2.setTorque(dtheta.t2);
+    //Point output((1+ks)*x,(1+ks)*y,0);  //try just position scaling for slip
+    //Joints dtheta = invKinBody(output);
+    //m1.setPos(dtheta.t1);
+    //m2.setPos(dtheta.t2);
     return 1;
 }
 int FLIGHT(float initM1, float initM2){
@@ -66,28 +47,19 @@
     return 1;
 }
 int main() {
-       //kankan.start();
-       //kankan.zero();
-       
-       while(1){
-        kankan.testEncoders(pc);
-        wait(0.01);
-        if(landDetection(kankan)){
-            printf("hello ground!\n");
-        }
-        }
-       
-       /* Point p1(0,-.1,0);
+       kankan.start();
+       kankan.zero();
+
+        Point p1(0,-.1,0);
         Point p2(0,-.6,0);
         Point p3(0,-.1,0);
         Point p[3]={p1, p2, p3};
         BezCurve curve(p, 3);
         curve.startCurve();
-        m1.setPos(0);
-        m2.setPos(0);
+        
         
-        kankan.start();
-        */
+        //kankan.start();
+        /*
         
         //Initalize Control Varialbes
         int Phase = 0;//Flight Phase
@@ -101,16 +73,12 @@
                 //FLIGHT(initM1, initM2);
                 //m1.setPos(-0.5);
                 led1=0;
-                kankan.setPoint(Point(0,-.08,0));
-                if(landDetection(kankan)){
+                kankan.setPoint(Point(0,-.15,0));
+                if(kankan.landDetection()){
                     //Landed going to Phase 1
                     //Record initial Length.                   
                     Phase =1;
-                    float th1 = mEnc1.getAngle();
-                    float th2 = mEnc2.getAngle()-pi/2;
-                    double y = l1+l2*sin(th1)+l3*sin(th2-th1)+ l2*sin(th1);
-                    double x = l2*cos(th1)+l3*cos(th2-th1)+ l2*cos(th1);
-                    initLen = pow(pow(x,2.0)+pow(y,2.0),0.5);  
+                    initLen = sqrt(pow(kankan.getPoint().x,2)+pow(kankan.getPoint().y,2)); 
                 }
             }else{ //Land Phase
             led1=1; //landing detected
@@ -118,7 +86,7 @@
           
                 SLIP(initLen);
                 //m1.setPos(-1);
-                if(!landDetection(kankan)){
+                if(!kankan.landDetection()){
                     //Lift Up going to Phase 0
                     Phase =0;
                 }
@@ -126,46 +94,19 @@
             }
         }
         wait(3);
-        
-    /*while(!curve.isDone()){
+        */
+    while(!curve.isDone()){
         //kankan.testEncoders(pc);
         curve.incrementAlpha();
         Point end = curve.getPoint();
         
-        Joints motorset = invKinBody(end);
-        //pc.printf("%f, %f, %f, %f\n", motorset.t1, motorset.t2, end.x, end.y);
-        
-        m1.setPos(motorset.t1);
-        m2.setPos(motorset.t2);
+        kankan.setPoint(end);
         
         wait(.05);        
     }
-    */
     
-    /*
-    kankan.start();
-    wait(1);
-    //m1.setPos(-3.1415/6);
-    //m2.setPos(3.1415/6);
-    //m1.setTorque(2);
-    
-    //kankan.setPoint(Point(0,-.3,0));
-    wait(5);
-    */
+   
     m1.stop();
     m2.stop();
-    
-    /*Forward=1;
-        Backward=0;
-            pwmOut.period_us(500);
-
-    
-    pwmOut.write(.15);
-    
-    wait(5);
-    pwmOut.write(0);
-    
-    */
-    
-    //kankan.zero();
+ 
 }