Разработка системы управления механической системы в режиме мягкого реального времени

putoutbuf( temp | phases_signals_x[ phase -1]);

if ((indexstep_x==Vm_step_x)&&((abs(dx)-1)>indexstep_x)) {sost=CONST_Sx;}

if ((abs(dx)-1)<=indexstep_x) {sost=DEACCEL_Sx;}

if ((indexstep_x<Vm_step_x)&&((abs(dx)-1)>indexstep_x)) {sost=ACCEL_Sx;}

NEXTSTATE=WAIT_Sx;

}

break;

case WAIT_Sx:

// секция входа

if (RUNENTRY) {

TendStep=ClockCycles()+dt;

}

if (ClockCycles() >=TendStep ) {

NEXTSTATE =sost;

if (dx>0) {Xt=Xt+1; Vt_x=mainmass_x[indexstep_x].V;}

else {Xt=Xt-1; Vt_x=-mainmass_x[indexstep_x].V;}

// if (Yt==300) { task_x=1;}

//printf("Vt_y\n%f\n",Vt_y);

//printf("x\n%d\n",Yt);

}

break;

case DEACCEL_Sx:

// секция входа

if (RUNENTRY) {

//printf("DEACCEL\n%d\n");

};

dx=(Xc-Xt);

if ((((dx>0)&&(Vt_x>0)) || ((dx<0)&&(Vt_x<0)))&& (abs(dx)>indexstep_x)) {

if (indexstep_x<Vm_step_x) {NEXTSTATE=ACCEL_Sx; } else {NEXTSTATE=CONST_Sx;} }

else {

indexstep_x=indexstep_x-1;

dt=mainmass_x[indexstep_x].dt; //v taktah

if ( dx>0 ) { if ( ++phase > 4) { phase = 1; }; };

if ( dx<0 ) { if ( --phase < 1 ) { phase = 4; }; };

temp = getoutbuf() & phases_mask_x;

putoutbuf( temp | phases_signals_x[ phase -1]);

if (indexstep_x==0) {sost=BRAKE_Sx;} else {sost=DEACCEL_Sx;}

NEXTSTATE=WAIT_Sx;

//printf("dt_ms\n%d\n",mainmass_y[indexstep_y].dt_ms);

}

break;

case CONST_Sx:

// секция входа

if (RUNENTRY) {

//printf("CONST\n%d\n");

};

dx=(Xc-Xt);

if ((((dx>0)&&(Vt_x<0)) || ((dx<0)&&(Vt_x>0)))|| (abs(dx)<=indexstep_x)) {NEXTSTATE=DEACCEL_Sx; }

else {

if ((abs(dx)-1)<=indexstep_x) {sost=DEACCEL_Sx;} else {sost=CONST_Sx;}

if ( dx>0 ) { if ( ++phase > 4) { phase = 1; }; };

if ( dx<0 ) { if ( --phase < 1 ) { phase = 4; }; };

temp = getoutbuf() & phases_mask_x;

putoutbuf( temp | phases_signals_x[ phase -1]);

NEXTSTATE=WAIT_Sx;

}

break;

case STEP_Sx:

// секция входа

if (RUNENTRY) {

//printf("CONST\n%d\n");

};

//dx=(Xc-Xt);

// if ((((dx>0)&&(Vt_x<0)) || ((dx<0)&&(Vt_x>0)))|| (abs(dx)<=indexstep_x)) {NEXTSTATE=DEACCEL_Sx; }

// else {

// if ((abs(dx)-1)<=indexstep_x) {sost=DEACCEL_Sx;} else {sost=CONST_Sx;}

indexstep_x=1;

dt=mainmass_x[indexstep_x].dt; //v taktah

if ( dx>0 ) { if ( ++phase > 4) { phase = 1; }; };

if ( dx<0 ) { if ( --phase < 1 ) { phase = 4; }; };

temp = getoutbuf() & phases_mask_x;

putoutbuf( temp | phases_signals_x[ phase -1]);

sost=BRAKE_Sx;

NEXTSTATE=WAIT_Sx;

//}

break;

default: work =0;

} // end swith

};

//

intsled_x_init(void)

{

Ttormoz = SYSPAGE_ENTRY(qtime)->cycles_per_sec / 1000 * Ttormoz_ms;

//printf("sled_x_init\n");

return 0;

};

А.3 Листинг кода задачи Sled_Y

int sled_y (void)

{

unsigned short int temp;

// переменные состояний

static int STATE = INITSTATE; // текущее состояние задачи

static int NEXTSTATE = STOP_S;// следующее состояние задачи (если задача не изменила своё состояние, то =-1 Если на данном просмотре она не меняет своего состояния, то NextStatee=-1*/;

static int RUNENTRY;// признак выполнения секции входа для данного состояния

static int phase=1;

//интервалы

static uint64_t dt;// временной интервал между шагами в тактах

// Переменные

static uint64_t TendStep;// время окончания шага в тактах

static uint64_t TendT;// время окончания срабатывания тормоза в тактах

static int sost; //

static int BSM_x;

static int64_t dy;

if (NEXTSTATE != -1) // задача изменила своё состояние

{

logE (TASKID,STATE,NEXTSTATE);// зарегистрируем это событие

STATE = NEXTSTATE;

NEXTSTATE = -1;

RUNENTRY = 1;

}

else RUNENTRY = 0;

switch (STATE)

{

case STOP_S:

// секция входа

if (RUNENTRY) {

//printf("STOP_y\n%d\n");

//printf("STOP_yt\n%d\n",Yt);

};

if ((vihod_0X==1) && (vihod_0Y==1)) {

// //printf("xt\n%d\n",Xt);

// //printf("xc\n%d\n",Xc);

dy=Yc-Yt;

if (dy!=0) {

Vt_y=0;

NEXTSTATE = BRAKE_S;

sost=STOP_S;

}

}

break;

case BRAKE_S:

// секция входа

if (RUNENTRY) {

//printf("BRAKE\n%d\n");

if (sost==STOP_S) {

setpin(num_Ty);

temp = getoutbuf() & phases_mask_y;

putoutbuf( temp | phases_signals_y[0]);

} else {

clrpin(num_Ty);

temp = getoutbuf() & phases_mask_y;

putoutbuf( temp | 0);

indexstep_y=0;

}

TendT=ClockCycles()+Ttormoz;

//printf("x\n%d\n",Yt);

}

if (ClockCycles() >=TendT) {

//printf("x\n%d\n",Yt);

if (sost==STOP_S) {

if (dy==1) {NEXTSTATE=STEP_S;} else {NEXTSTATE =ACCEL_S;}

} else {NEXTSTATE =STOP_S;}

}

break;

case ACCEL_S:

// секция входа

if (RUNENTRY) {

//printf("ACCEL\n%d\n");

};

// //printf("Vt_x\n%f\n",Vt_x);

// //printf("dx\n%f\n",dx);

// //printf("st\n%f\n",St);

// //printf("xt\n%llu\n",Xt);

// //printf("xc\n%llu\n",Xc);

dy=(Yc-Yt);

if (((dy>0) && (Vt_y<0)) || ((dy<0) && (Vt_y>0))) {NEXTSTATE=DEACCEL_S; }

if (abs(dy)<=indexstep_y) {NEXTSTATE=DEACCEL_S; }

else {

indexstep_y=indexstep_y+1;

dt=mainmass_y[indexstep_y].dt; //v taktah

if ( dy>0 ) { if ( ++phase > 4) { phase = 1; }; };

if ( dy<0 ) { if ( --phase < 1 ) { phase = 4; }; };

temp = getoutbuf() & phases_mask_y;

putoutbuf( temp | phases_signals_y[ phase -1]);

if ((indexstep_y==Vm_step_y)&&((abs(dy)-1)>indexstep_y)) {sost=CONST_S;}

if ((abs(dy)-1)<=indexstep_y) {sost=DEACCEL_S;}

if ((indexstep_y<Vm_step_y)&&((abs(dy)-1)>indexstep_y)) {sost=ACCEL_S;}

NEXTSTATE=WAIT_S;

//printf("dt\n%d\n",dt);

//printf("dt_ms\n%d\n",mainmass_y[indexstep_y].dt_ms);

}

break;

case WAIT_S:

// секция входа

if (RUNENTRY) {

//printf("WAIT\n%d\n");

//printf("dt\n%d\n",dt);

TendStep=ClockCycles()+dt;

// //printf("clock\n%d\n",ClockCycles());

// //printf("TendStep\n%d\n",TendStep);

}

if (ClockCycles() >=TendStep ) {

NEXTSTATE =sost;

if (dy>0) {Yt=Yt+1; Vt_y=mainmass_y[indexstep_y].V;}

else {Yt=Yt-1; Vt_y=-mainmass_y[indexstep_y].V;}

// if (Yt==300) { task_x=1;}

//printf("Vt_y\n%f\n",Vt_y);

//printf("x\n%d\n",Yt);

}

break;

case DEACCEL_S:

// секция входа

if (RUNENTRY) {

//printf("DEACCEL\n%d\n");

};

dy=(Yc-Yt);

if ((((dy>0)&&(Vt_y>0)) || ((dy<0)&&(Vt_y<0)))&& (abs(dy)>indexstep_y)) {

if (indexstep_y<Vm_step_y) {NEXTSTATE=ACCEL_S; } else {NEXTSTATE=CONST_S;} }

else {

indexstep_y=indexstep_y-1;

dt=mainmass_y[indexstep_y].dt; //v taktah

if ( dy>0 ) { if ( ++phase > 4) { phase = 1; }; };

if ( dy<0 ) { if ( --phase < 1 ) { phase = 4; }; };

temp = getoutbuf() & phases_mask_y;

putoutbuf( temp | phases_signals_y[ phase -1]);

if (indexstep_y==0) {sost=BRAKE_S;} else {sost=DEACCEL_S;}

NEXTSTATE=WAIT_S;

//printf("dt_ms\n%d\n",mainmass_y[indexstep_y].dt_ms);

}

break;

case CONST_S:

// секция входа

if (RUNENTRY) {

//printf("CONST\n%d\n");

};

dy=(Yc-Yt);

if ((((dy>0)&&(Vt_y<0)) || ((dy<0)&&(Vt_y>0)))|| (abs(dy)<=indexstep_y)) {NEXTSTATE=DEACCEL_S; }

else {

if ((abs(dy)-1)<=indexstep_y) {sost=DEACCEL_S;} else {sost=CONST_S;}

if ( dy>0 ) { if ( ++phase > 4) { phase = 1; }; };

if ( dy<0 ) { if ( --phase < 1 ) { phase = 4; }; };

temp = getoutbuf() & phases_mask_y;

putoutbuf( temp | phases_signals_y[ phase -1]);

NEXTSTATE=WAIT_S;

}

break;

case STEP_S:

// секция входа

if (RUNENTRY) {

//printf("CONST\n%d\n");

};

//dx=(Xc-Xt);

// if ((((dx>0)&&(Vt_x<0)) || ((dx<0)&&(Vt_x>0)))|| (abs(dx)<=indexstep_x)) {NEXTSTATE=DEACCEL_Sx; }

// else {

// if ((abs(dx)-1)<=indexstep_x) {sost=DEACCEL_Sx;} else {sost=CONST_Sx;}

indexstep_y=1;

dt=mainmass_y[indexstep_y].dt; //v taktah

if ( dy>0 ) { if ( ++phase > 4) { phase = 1; }; };

if ( dy<0 ) { if ( --phase < 1 ) { phase = 4; }; };

temp = getoutbuf() & phases_mask_y;

putoutbuf( temp | phases_signals_y[ phase -1]);

sost=BRAKE_S;

NEXTSTATE=WAIT_S;

//}

break;

default: work =0;

} // end swith

};

//

intsled_y_init(void)

{

Ttormoz = SYSPAGE_ENTRY(qtime)->cycles_per_sec / 1000 * Ttormoz_ms;

//printf("sled_y_init\n");

return 0;

};

Размещено на Allbest.ru