File content as of revision 50:510ce69c2a10:

#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){

}

void Kangaroo::zero(){
    m1.start();
    m2.start();
    //m1.setVel(-.2);//wait for the legs to go to stopping points
    //m2.setVel(-.2);
    
    m1.setPos(3.1415/5);
    m2.setPos(3.1415/5);//bring legs to hard stop
    
    led4=1;
    wait(1.0);
    
    //m1.zero();
    //m2.zero();
    m1.calibAngle(0);
    m2.calibAngle(2*3.1415/3);
    
    wait(1);
    
    m1.setPos(-3.1415/2);
    //m2.setPos(3.1415/2);
    
    enc1.reset();
    enc2.reset();
    led2=1;
    /*while(((enc1.getRevolutions()==0)||(enc2.getRevolutions()==0))){
        wait(0.1);
    }*/
    
    led1=1;
    
    //setPoint(Point(0,-.08,0));
    //wait(3);
    led3=0;
    led1=0;
    led2=0;
    led4=0;
    //m1.stop();
    //m2.stop();
}

void Kangaroo::testEncoders(Serial &pc){
    //pc.printf("hello world\n");
    //pc.printf("x: %f   y: %f\n", getPoint().x, getPoint().y);
    pc.printf("motor1:  %i  motor2:  %i\n",m1.getPos(), m2.getPos());
    //pc.printf("clix1:  %i, rots1: %i\n",enc1.getPulses(), enc1.getRevolutions());
    
    //pc.printf("motor2:  %i\n",);
    //pc.printf("clix2:  %i, rots2: %i\n",enc2.getPulses(), enc2.getRevolutions());
}

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

float Kangaroo::getAngle(){
    return (enc2.getAngle())+1.770973;
}

void Kangaroo::setPoint(Point p){

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

}

void Kangaroo::flightPath(){

}

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