File content as of revision 58:c0b09adb2997:

#include "mbed.h"
#include "include/kangaroo.hpp"



Kangaroo::Kangaroo(Motor &M1, Motor &M2, QEI &e1, QEI &e2):enc1(e1),enc2(e2),m1(M1),m2(M2),led1(LED1),led2(LED2),led3(LED3),led4(LED4){

    joint_h=0;
    leg_y = 0;

}

void Kangaroo::zero(){
    m1.start();
    m2.start();
    
    m1.setVel(.5);//use in final
    m2.setVel(.3);
    //m2.setPos(3.1415/5);//bring legs to hard stop
    
    led4=1;
    wait(5.0);
    
    //m1.zero();
    //m2.zero();
    m1.calibAngle(0.0);
    m2.calibAngle(2.25);//2.44
    
    wait(.1);
    
    
    m1.setPos(-3.1415/12);
    m2.setPos(3.1415/2);
    
    enc1.reset();
    enc2.reset();
    led2=1;
    while(((enc1.getRevolutions()==0)||(enc2.getRevolutions()==0))){
        wait(0.1);
    }
    
    landDetection();
    led1=1;
    
    //setPoint(Point(0,-.08,0));
    wait(3);
    led3=0;
    led1=0;
    led2=0;
    led4=0;
    //m1.stop();
    //m2.stop();
}

float Kangaroo::jointHeight(){
    return joint_h;
}

float Kangaroo::legY(){
    return leg_y;
}

void Kangaroo::testEncoders(Serial &pc){
    //pc.printf("hello world\n");
    pc.printf("x: %f   y: %f  b: %f\n", getPoint().x, getPoint().y, getAngle2());
    
    //pc.printf("t1: %f  t2: %f b1: %f  b2: %f \n",m1.getAngle(), m2.getAngle(), getAngle1(), getAngle2());
    
    //pc.printf("m1: %f  m2:  %f\n",dt1, dt2);
}

int Kangaroo::landDetection(){
    //Helper function to Detect If we have landed.
    //NO Input
    //Output:  0 =  Flight Phase.     1 = Ground Phase.
    float th3 = getAngle1();
    joint_h = lg*sin(th3)+h;
    float th4 = getAngle2();
    leg_y = -1*(-l1+getPoint().y)*cos(th4);
    return joint_h<(leg_y+.021);//.013
}

Point Kangaroo::getPoint(){
    float x = (l2+l4)*cos(m1.getAngle())+(l3)*cos(m1.getAngle()+m2.getAngle()-3.1415);
    float y = (l2+l4)*sin(m1.getAngle())+(l3)*sin(m1.getAngle()+m2.getAngle()-3.1415);
    
    return Point(x,y,0);
}

float Kangaroo::getAngle2(){
    return (enc2.getAngle())-1.45;//-1.65
}


float Kangaroo::getAngle1(){
    return -(enc1.getAngle()-0.5372);
}

void Kangaroo::setPoint(Point p){

    Joints motors = invKinBody(p);
    m1.setPos(motors.t1);
    m2.setPos(motors.t2);

}

Joints Kangaroo::jacobianT(Point p){
    float t1=m1.getAngle();
    float t2=m2.getAngle();
    
    float tor1 = ((l2+l4)*(-sin(t1))+l3*(-sin(t1+t2-3.1415)))*p.x+((l2+l4)*(cos(t1))+l3*(cos(t1+t2-3.1415)))*p.y;
    float tor2 = (l3*(-sin(t1+t2-3.1415)))*p.x+(l3*(cos(t1+t2-3.1415)))*p.y;
    return Joints(tor1,tor2);
}

void Kangaroo::slip(float k, Point desir, Serial &pc){
    Point now = getPoint();
    //pc.printf("nx: %f  ny: %f  dx: %f  dy: %f  ", now.x, now.y, desir.x, desir.y);
    float fy = (now.y-desir.y)*-k;
    float fx = (now.x-desir.x)*-k;
    
    Point force = Point(fx, fy, 0);
    
    Joints torq = jacobianT(force);
    //pc.printf("t1: %f  t2: %f\n", torq.t1, torq.t2);
    m1.setTorque(torq.t1);
    m2.setTorque(torq.t2);
    //dt1=torq.t1;
    //dt2=torq.t2;
}

void Kangaroo::flightPath(){

}

void Kangaroo::start(){
    m1.start();
    m2.start();
}