File content as of revision 57:dfea5d24d650:

#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

Motor m1(p15,p17,p18,p21,mEnc2);//hip p17 high is CCW
Motor m2(p16,p19,p20,p22,mEnc1);//knee p19 high is CCW


QEI bEnc1(p27, p28, p29, 2000, QEI::X4_ENCODING);  //track  offset:-2.236814
QEI bEnc2(p5, p6, p7, 2000, 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 SLIP(float initLen){//SlIP Model
    //Get Length
    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
    Point slip_p = Point((1+ks)*x,(1+ks)*y,0);
    kankan.setPoint(slip_p);

    return 1;
}
int FLIGHT(float initM1, float initM2){
    //Pos Control
    m1.setPos(initM1);
    m2.setPos(initM2);
    //Awesome.
    return 1;
}
int main() {
       kankan.start();
       kankan.zero();
       /*m1.stop();
       m2.stop();
       while(1){
        kankan.testEncoders(pc);
        wait(.1);
       }*/

        //Initalize Control Varialbes
        int Phase = 0;//Flight Phase
        
        while(true){
            if (Phase == 0){//Flight Phase
            //Uses PD to Control
                //FLIGHT(initM1, initM2);
                //m1.setPos(-0.5);
                led1=0;
                kankan.setPoint(Point(0.12,-.15,0));
                if(kankan.landDetection()){
                    //Landed going to Phase 1
                    //Record initial Length.                   
                    Phase =1;
                }
            }else{ //Land Phase
            led1=1; //landing detected
            //Uses SLIP Model to Control
          
                kankan.slip(1000,Point(-.08,-1.0,0), pc);
                //kankan.testEncoders(pc);
                //m1.setPos(-1);
                if(!kankan.landDetection()){
                    //Lift Up going to Phase 0
                    Phase =0;
                }
            
            }
        }
    
   
    m1.stop();
    m2.stop();
 
}