Diff: Master.cpp

Revision:

48:8f0e007bd305

Parent:

47:e01ba47d11cc

Child:

49:3aaa790800ad

--- a/Master.cpp	Sun Dec 01 20:34:31 2013 +0000
+++ b/Master.cpp	Sun Dec 01 22:19:48 2013 +0000
@@ -1,6 +1,8 @@
 #include "mbed.h"
 #include "Master.hpp"
 
+DigitalOut led1(LED1);
+
 Serial pc(USBTX, USBRX);
 QEI mEnc1(p25, p26, NC, 1200, QEI::X4_ENCODING); //hip
 QEI mEnc2(p23, p24, NC, 1200, QEI::X4_ENCODING);//knee
@@ -40,15 +42,19 @@
     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;
+    //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
-    Point output = Point(-mag*x/length,-mag*y/length,0);
-    Joints dtorque = invKinBody(output);
+    //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(dtorque.t1);
-    m2.setTorque(dtorque.t2);
+    //m1.setTorque(dtheta.t1);
+    //m2.setTorque(dtheta.t2);
     return 1;
 }
 int FLIGHT(float initM1, float initM2){
@@ -59,7 +65,10 @@
     return 1;
 }
 int main() {
-        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};
@@ -69,31 +78,37 @@
         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;
+        int initLen = 0.08;//0.17;
+        //float initM1 = -0.5;
+        //float initM2 = 0.5;
         
         while(true){
             if (Phase == 0){//Flight Phase
             //Uses PD to Control
                 //FLIGHT(initM1, initM2);
-                m1.setPos(-0.5);
+                //m1.setPos(-0.5);
+                led1=0;
+                kankan.setPoint(Point(0,-.08,0));
                 if(landDetection()){
                     //Landed going to Phase 1
-                    //Record initial Length.
+                    //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 = pow(pow(x,2.0)+pow(y,2.0),0.5);  
                 }
             }else{ //Land Phase
+            led1=1; //landing detected
             //Uses SLIP Model to Control
-                //SLIP(initLen);
-                m1.setPos(-1);
+          
+                SLIP(initLen);
+                //m1.setPos(-1);
                 if(!landDetection()){
                     //Lift Up going to Phase 0
                     Phase =0;