Diff: Master.cpp

Revision:

45:0db0fc9f77b1

Parent:

44:3566c5699ba6

Child:

46:4497e945de6b

--- a/Master.cpp	Wed Nov 27 02:51:39 2013 +0000
+++ b/Master.cpp	Sun Dec 01 07:23:22 2013 +0000
@@ -1,15 +1,16 @@
 #include "mbed.h"
-
 #include "Master.hpp"
 
 Serial pc(USBTX, USBRX);
-
+//Tick = 1200
 QEI mEnc1(p25, p26, NC, 1200, QEI::X4_ENCODING); //hip
 QEI mEnc2(p23, p24, NC, 1200, QEI::X4_ENCODING);//knee
 
 Motor m1(p15,p17,p18,p21,mEnc2);//hip p17 high is CCW
 Motor m2(p16,p19,p20,p22,mEnc1);//knee p19 high is CCW
 
+
+//Tick = 500
 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
 
@@ -18,22 +19,52 @@
 Ticker t;
 
 AnalogIn aIn(p15);
-        DigitalOut Forward(p17);
-        DigitalOut Backward(p18);
-        PwmOut pwmOut(p21);
+DigitalOut Forward(p17);
+DigitalOut Backward(p18);
+PwmOut pwmOut(p21);
+
+
+int landDetection(){
+    //Helper function to Detect If we have landed.
+    //NO Input
+    //Output:  0 =  Flight Phase.     1 = Ground Phase.
+    float th3 = bEnc1.getAngle()*6.283;
+    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);
+    return a<b;
+}
 
 int main() {
-
         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.setTorque(3);
-        m2.setPos(0);        
+        m1.setPos(0);
+        m2.setPos(0);    
         kankan.start();
+        int Phase = 0;//Flight Phase
+        while(true){
+            if (Phase == 0){//Flight Phase
+            //Uses Bezian Curve to Control
+                m1.setPos(-1);
+                if(landDetection()){
+                    //Landed going to Phase 1
+                    Phase =1;
+                }
+            }else{ //Land Phase
+            //Uses SLIP Model to Control.
+                m1.setPos(-0.5);
+                if(!landDetection()){
+                    //Lift Up going to Phase 0
+                    Phase =0;
+                }
+            
+            }
+        }
         wait(3);
         
     /*while(!curve.isDone()){