File content as of revision 46:4497e945de6b:

#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

Kangaroo kankan(m1,m2,bEnc1,bEnc2);

Ticker t;

AnalogIn aIn(p15);
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();
    float a = lg*sin(-th3)+h;
    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);
        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();
        //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 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
                //SLIP(initLen);
                m1.setPos(-1);
                if(!landDetection()){
                    //Lift Up going to Phase 0
                    Phase =0;
                }
            
            }
        }
        wait(3);
        
    /*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);
        
        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();
}