empc_sts_10k1.h

2812例程以及 控制直流无刷电机程序

0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
0,
1,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
1,
0,
0,
0,
1,
0,
};


  static long mpt_searchTree(const float *X)
{
    int ix, iu;
	Uint16 U0[MPT_NU],U[MPT_NU];
    long node = 1, row;
    float hx, k;
    
    /* initialize U to zero*/
   // for (iu=0; iu<MPT_NU; iu++) {
   //     U[iu] = 0;
   // }
    
    /* find region which contains the state x0 */
    while (node > 0) {
        hx = 0;
        row = (node-1)*(MPT_NX+3);
        for (ix=0; ix<MPT_NX; ix++) {
            hx = hx + MPT_ST[row+ix]*X[ix];
        }
        k = MPT_ST[row+MPT_NX];
        
        if ((hx - k) < 0) {
            /* x0 on plus-side of the hyperplane */
            node = (long)MPT_ST[row+MPT_NX+2];
        } else {
            /* x0 on minus-side of the hyperplane */
            node = (long)MPT_ST[row+MPT_NX+1];
        }
    }
    
    node = -node;
    
    /* compute control action associated to state x0 */
    for (iu=0; iu<MPT_NU; iu++) {
     //   for (ix=0; ix<MPT_NX; ix++) {
     //       U[iu] = U[iu] + MPT_F[(node-1)*MPT_NX*MPT_NU + iu*MPT_NX + ix]*X[ix];
     //   }
        U[iu] = MPT_G[(node-1)*MPT_NU + iu];
    }

	GpioDataRegs.GPADAT.bit.GPIOA1 = U0[1] = U[0] || U[3];
	GpioDataRegs.GPADAT.bit.GPIOA0 = U0[0] = U[0] && U0[1];
	GpioDataRegs.GPADAT.bit.GPIOA3 = U0[3] = U[1] || U[4];
	GpioDataRegs.GPADAT.bit.GPIOA2 = U0[2] = U[1] && U0[3];
	GpioDataRegs.GPADAT.bit.GPIOA5 = U0[5] = U[2] || U[5];
	GpioDataRegs.GPADAT.bit.GPIOA4 = U0[4] = U[2] && U0[5];

    return node;
}