File content as of revision 5:768e10f6d372:

#include "ControllerLoop.h"
using namespace std;



ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096)
{
    thread.start(callback(this, &ControllerLoop::loop));
    ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts);
    diff1.reset(0.0f,0);
    diff2.reset(0.0f,0);
    }


ControllerLoop::~ControllerLoop() {}


void ControllerLoop::loop(void){
    float Kp = 1500 * 4.89e-7/0.094;   // XX * J/km
    float Tn = .0035;
    float Kv=50;
    float phi_des = 0;
    float v_des = 0;
    PID_Cntrl v_cntrl(Kp,Tn,0,1.0,Ts,-.95,.95);
    Unwrapper_2pi uw2pi;
    uint8_t k = 0;
    while(1)
        {
        ThisThread::flags_wait_any(threadFlag);
        // THE LOOP ------------------------------------------------------------
        if(++k%64==0)
            {
                printf("%f %f\r\n",phi_des,v_des);
            }
        short c1 = counter1;            // get counts from Encoder
        short c2 = counter2;            // get counts from Encoder
        float phi1 = uw2pi(2*3.1415927/4000.0*(float)c1);
        float vel1 = diff1(c1);                 // motor velocity 
        float phi2 = uw2pi(2*3.1415927/4000.0*(float)c2);
        float vel2 = diff2(c2);                 // motor velocity 
        float w0=2*3.1415927 * 3;
        //phi_des = 1.0*sinf(w0*ti.read());
        current_path->get_x_v(glob_ti.read(),&phi_des,&v_des);
        float i1 = v_cntrl(Kv * (phi_des-phi2)+v_des-vel2 );
        //float i1 = v_cntrl(v_des-vel ) + exc;
        i_des2.write(i2pwm(-i1));
        }
}

void ControllerLoop::sendSignal() {
    thread.flags_set(threadFlag);
}