File content as of revision 7:942fd77d5e19:

#include "ControllerLoop.h"
using namespace std;



ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096), dout1(PB_9)
{
    this->Ts = Ts;
    diff1.reset(0.0f,0);
    diff2.reset(0.0f,0);
    Kv = 150;
    is_initialized = false;
    ti.reset();
    ti.start();
    data.laser_on = false;
    }


ControllerLoop::~ControllerLoop() {}


void ControllerLoop::loop(void){
    float A=.6;
    uint32_t k=0;
    while(1)
        {
        ThisThread::flags_wait_any(threadFlag);
        // THE LOOP ------------------------------------------------------------
        if(++k%500==0)
            {
//            short c1 = counter1;            // get counts from Encoder
//            short c2 = counter2;            // get counts from Encoder
            
  //          printf("1: %d %d, 2: %d %d\r\n",index1.positionAtIndexPulse,c1-index1.positionAtIndexPulse-mc.inc_offset[0],index2.positionAtIndexPulse,c2-index2.positionAtIndexPulse-mc.inc_offset[1]);
            //led1=!led1;
            }
        if(!is_initialized)
            {
            find_index();
            if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) 
                is_initialized=true;
            }
        else
            {
            short c1 = counter1 - index1.positionAtIndexPulse - mk.inc_offset[0]- mk.inc_additional_offset[0];            // get counts from Encoder
            short c2 = counter2 - index2.positionAtIndexPulse - mk.inc_offset[1]- mk.inc_additional_offset[1];            // get counts from Encoder
            data.sens_phi[0] = uw2pi1(2*3.1415927/4000.0*(float)c1);
            data.sens_Vphi[0] = diff1(c1);                 // motor velocity 
            data.sens_phi[1] = uw2pi2(2*3.1415927/4000.0*(float)c2);
            data.sens_Vphi[1] = diff2(c2);                 // motor velocity
            // ------------------------ do the control first
            float v_des1 = pos_cntrl(data.cntrl_phi_des[0]-data.sens_phi[0]);
            float v_des2 = pos_cntrl(data.cntrl_phi_des[1]-data.sens_phi[1]);
            data.i_des[0] = v_cntrl[0](v_des1 - data.sens_Vphi[0] ) ;
            data.i_des[1] = v_cntrl[1](v_des2 - data.sens_Vphi[1] ) ;
            // ------------------------ write outputs
            i_des1.write(i2u(data.i_des[0]));
            i_des2.write(i2u(data.i_des[1]));
            // now do trafos etc

            float w01=2*3.1415927 * .5;
            float w02=2*3.1415927 * 1.5;
            float xy[2];
            if(mk.external_control) // get desired values from external source (GUI)
                {
                if(mk.trafo_is_on)  // use desired xy values from xternal source and transform
                                    // otherwise external source delivers phi1, phi2 values directly
                    {
                    bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
                    }
                }
            else
                {
                if(mk.trafo_is_on)
                    {
                    data.cntrl_xy_des[0] = 50.0f*cosf(w01*glob_ti.read());
                    data.cntrl_xy_des[1] = 50.0f*sinf(w01*glob_ti.read());
                    bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
                    }
                else
                    {
                    data.cntrl_phi_des[0] = .50f*cosf(w01*glob_ti.read());
                    data.cntrl_phi_des[1] = .50f*sinf(w01*glob_ti.read());
                    }
                }
            //bool dum = mk.P2X(data.cntrl_phi_des,data.cntrl_xy_des)
            //current_path->get_x_v(glob_ti.read(),&phi_des,&v_des);
            
            //data.cntrl_phi_des[0] = 0.0f*A*sinf(w01*ti.read());
            //data.cntrl_phi_des[1] = 0.0f*A*sinf(w02*ti.read());
            //data.est_xy[0]=data.cntrl_phi_des[0]; // temporary
            //data.est_xy[1]=data.cntrl_phi_des[1];
            //laser_on = 1;
            laser_on = data.laser_on;
            }
        }
}

void ControllerLoop::sendSignal() {
    thread.flags_set(threadFlag);
}
void ControllerLoop::start_loop(void)
{
    thread.start(callback(this, &ControllerLoop::loop));
    ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts);
}

float ControllerLoop::pos_cntrl(float d_phi)
{
    return Kv*mk.mot_inc_to_rad*round(mk.mot_rad_to_inc*(d_phi));
    }

void ControllerLoop::init_controllers(void)
{
    float Kp = 2000 * 4.89e-7/0.094f;   // XX * J/km
    float TroV = 1.0f / (2.0f * 3.1415f * 330.0f);
    float Tn = .005f;
    v_cntrl[0].setCoefficients(Kp,Kp/Tn,0.0f,1.0,TroV,Ts,-.8,.8);
    v_cntrl[1].setCoefficients(Kp,Kp/Tn,0.0f,1.0,TroV,Ts,-.8,.8);
}
void ControllerLoop::find_index(void)
{
    float Kp = 0.005;
    short counts1 = counter1;            // get counts from Encoder
    float vel1 = diff1(counts1);                 // motor velocity 
    short counts2 = counter2;            // get counts from Encoder
    float vel2 = diff2(counts2);                 // motor velocity 
    float i1 = Kp*(25.0f - vel1 );
    float i2 = Kp*(25.0f - vel2 ) ;
    //float dum = (float)(++u_test%16)/16.0f;
    //i_des1.write(i2u(.25f*sin(2*3.14f*1.0f)));
    i_des1.write(i2u(i1));
    i_des2.write(i2u(i2));
    }