Diff: Master.cpp

Revision:

46:4497e945de6b

Parent:

45:0db0fc9f77b1

Child:

47:e01ba47d11cc

--- a/Master.cpp	Sun Dec 01 07:23:22 2013 +0000
+++ b/Master.cpp	Sun Dec 01 20:13:35 2013 +0000
@@ -28,14 +28,38 @@
     //Helper function to Detect If we have landed.
     //NO Input
     //Output:  0 =  Flight Phase.     1 = Ground Phase.
-    float th3 = bEnc1.getAngle()*6.283;
+    float th3 = bEnc1.getAngle();
     float a = lg*sin(-th3)+h;
-    float th1 = mEnc1.getAngle()*6.283;
-    float th2 = mEnc2.getAngle()*6.283-pi/2;
-    float b = l1+l2*sin(-th1)+l3*sin(-th2+th1)+ l2*sin(-th1);
+    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;
 }
 
+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);
+    float length = pow(pow(x,2.0)+pow(y,2.0),0.5);
+    int ks = 0.1;
+    float mag = (initLen-length)*ks;
+    //Apply Forward Kinematics
+    Point output = Point(-mag*x/length,-mag*y/length,0);
+    Joints dtorque = invKinBody(output);
+    //Apply Motor Torque Control.
+    m1.setTorque(dtorque.t1);
+    m2.setTorque(dtorque.t2);
+    return 1;
+}
+int FLIGHT(float initM1, float initM2){
+    //Pos Control
+    m1.setPos(initM1);
+    m2.setPos(initM2);
+    //Awesome.
+    return 1;
+}
 int main() {
         Point p1(0,-.1,0);
         Point p2(0,-.6,0);
@@ -44,20 +68,34 @@
         BezCurve curve(p, 3);
         curve.startCurve();
         m1.setPos(0);
-        m2.setPos(0);    
+        m2.setPos(0);
+        
         kankan.start();
+        //Initalize Control Varialbes
         int Phase = 0;//Flight Phase
+        int initLen = 0.17;
+        float initM1 = -0.5;
+        float initM2 = 0.5;
+        
         while(true){
             if (Phase == 0){//Flight Phase
-            //Uses Bezian Curve to Control
-                m1.setPos(-1);
+            //Uses PD to Control
+                //FLIGHT(initM1, initM2);
+                m1.setPos(-0.5);
                 if(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);
                 }
             }else{ //Land Phase
-            //Uses SLIP Model to Control.
-                m1.setPos(-0.5);
+            //Uses SLIP Model to Control
+                //SLIP(initLen);
+                m1.setPos(-1);
                 if(!landDetection()){
                     //Lift Up going to Phase 0
                     Phase =0;