（1）PID算法程序

void pid_set_t(PID *pid, float kp, float ti, float td, short t, float smax, float e_isp, short tp)

{

      pid->kp = kp;

      pid->ki = kp * t / ti;

      pid->kd = kp * td / t;

      pid->smax = smax;

      pid->e_isp = e_isp;

      pid->type = tp;

}

void pid_process(PID *pid)

{          

      if(pid->type & PID_TYPE_ISP)

      {

             if(pid->e < pid->e_isp && pid->e > -pid->e_isp)

                    pid->e_s += pid->e;

             else

                    pid->e_s /= 2;

      }

      else

      {

             pid->e_s += pid->e;

      }

    //积分限幅

      if(pid->e_s > pid->smax)

             pid->e_s = pid->smax;

      if(pid->e_s < -pid->smax)

             pid->e_s = -pid->smax;

      if(!(pid->type & PID_TYPE_DFB))

      {

             pid->e_d = pid->e - pid->p_e;

      }

      pid->p_e = pid->e;

      if(pid->type & PID_TYPE_DDL)

      {

             pid->e_ds += pid->e_d;

             pid->e_ds /= 2;

      }

      else

      {

             pid->e_ds = pid->e_d;

      }    

      pid->o = (float)(pid->kp * pid->e

             + pid->kd * pid->e_ds

             + pid->ki * pid->e_s);

}

（2）画长度大于50cm的直线程序

Q1:

      motor_set(200, 200, 200, 200);

      while(1)

      {

             int a = tm1638_get_int();

             motor_set(a, 0, 0, 0);

             a = tm1638_get_int();

             motor_set(0, a, 0, 0);

             a = tm1638_get_int();

             motor_set(0, 0, a, 0);

             a = tm1638_get_int();

             motor_set(0, 0, 0, a);

      }

       （3）画设置角度、设置长度的直线程序

Q2:

      a_sv = tm1638_get_int();

      amp = tm1638_get_int();

      tm1638_int2(a_sv, amp);

      tm1638_disp();

      a_sv = 3.1415926f * 2 - a_sv * 3.1415926f / 180.0f;

      x_sv = 0.4f * arm_cos_f32(a_sv);

      y_sv = 0.4f * arm_sin_f32(a_sv);

      while(1)

      {

             delay_ms_set(1);              

             //Read Position

             spi_start();

             spi_send(0x0C);

             for(int i = 0; i < 16; i++)

                    ((unsigned char *)(position_q))[i] = spi_send(0xFF);

             spi_stop();          

             quart_multi_q(position_q, position_q_correct);

             quart_to_eular(position_q, position_e);       

             pid_sta_x.e = x_sv - position_e[0];

             if(pid_sta_x.e <0.01f&& pid_sta_x.e >-0.01f)

                    pid_sta_x.e = 0;

             else if(pid_sta_x.e > 0)

                    pid_sta_x.e -= 0.01f;

             else

                    pid_sta_x.e += 0.01f;

             pid_process(&pid_sta_x);

             motor_ctrl_x = limit(pid_sta_x.o, -1000, 1000);

             pid_sta_y.e = y_sv - position_e[1];

             if(pid_sta_y.e <0.01f&& pid_sta_y.e >-0.01f)

                    pid_sta_y.e = 0;

             else if(pid_sta_y.e > 0)

                    pid_sta_y.e -= 0.01f;

             else

                    pid_sta_y.e += 0.01f;

             pid_process(&pid_sta_y);

             motor_ctrl_y = limit(pid_sta_y.o, -1000, 1000);

             if(time % 20 == 0)

             {

                    motor_setx(motor_ctrl_x);

                    motor_sety(motor_ctrl_y);

             }

             time++; 

             if(time == 1500)

                    goto Q2T;           

             delay_wait();

       }

（4）快速制动静止程序

Q5:

      oled_disp_6x8str(80, 6, "Time:");

      x_sv = 0;

      y_sv = 0;

      while(1)

      {

             delay_ms_set(1);              

             //Read Position

             spi_start();

             spi_send(0x0C);

             for(int i = 0; i < 16; i++)

                    ((unsigned char *)(position_q))[i] = spi_send(0xFF);

             spi_stop();

             quart_multi_q(position_q, position_q_correct);

             quart_to_eular(position_q, position_e);

             pid_sta_x.e = x_sv - position_e[0];

             if(pid_sta_x.e <0.01f&& pid_sta_x.e >-0.01f)

                    pid_sta_x.e = 0;

             else if(pid_sta_x.e > 0)

                    pid_sta_x.e -= 0.01f;

             else

                    pid_sta_x.e += 0.01f;

             pid_process(&pid_sta_x);

             motor_ctrl_x = limit(pid_sta_x.o, -1000, 1000);

             pid_sta_y.e = y_sv - position_e[1];

             if(pid_sta_y.e <0.01f&& pid_sta_y.e >-0.01f)

                    pid_sta_y.e = 0;

             else if(pid_sta_y.e > 0)

                    pid_sta_y.e -= 0.01f;

             else

                    pid_sta_y.e += 0.01f;

             pid_process(&pid_sta_y);

             motor_ctrl_y = limit(pid_sta_y.o, -1000, 1000); 

             if(time % 20 == 0)

             {

                    motor_setx(motor_ctrl_x);

                    motor_sety(motor_ctrl_y);

             }           

             if(time % 50 == 0)

                    judge(__sqrtf(pid_sta_y.e * pid_sta_y.e + pid_sta_x.e * pid_sta_x.e), 0, 0.03f, 0.001f, 50);

             //oled_disp_int(80, 0, position_e[0] * 1000);

             //oled_disp_int(80, 1, position_e[1] * 1000);             

             time++; 

             delay_wait();

       }

（5）作半径可设置的圆周运动程序

Q9:

      oled_disp_6x8str(0, 0, "Radius:");

      oled_disp_6x8str(80, 6, "Time:");

      disp_en = tm1638_get_int();  

      pid_init_k(&pid_cir_rr, 250, 0.9f, 50, 20, 0, PID_TYPE_POS);   

      pid_cir_rr.e_s = -5;    

      for(int i = 0; i < 8; i++)

              pid_init_k(&pid_cir_r[i], 820, 1.2f, 1600, 8, 0, PID_TYPE_POS);

      r_sv = tm1638_get_int();

      gg = 1;

      if(r_sv > 31)

             gg = 2;

      while(1)

      {

             delay_ms_set(20);     

             //Read Position

             spi_start();

             spi_send(0x0C);

             for(int i = 0; i < 16; i++)

                    ((unsigned char *)(position_q))[i] = spi_send(0xFF);

             spi_stop();          

             quart_multi_q(position_q, position_q_correct);         

             Quarternion p;

             quart_set(p, 0, 0, 0, -1);           

             Quarternion qb;

             qb[0] = position_q[0];

             qb[1] = position_q[1];

             qb[2] = position_q[2];

             qb[3] = position_q[3];

             quart_inverse(qb);

             quart_multi_q(position_q, p);

             quart_multi_q(position_q, qb);             

             float vetex[3];

             vetex[0] = position_q[1];

             vetex[1] = position_q[2];

             vetex[2] = position_q[3];

             quart_multi_q(position_q, p);

             quart_to_eular(position_q, position_e);       

             float dist = __sqrtf(vetex[0] * vetex[0] + vetex[1] * vetex[1]);

             float prot = asinf(dist);

             float zrot = atan2f(vetex[1], vetex[0]);

             float validv;        

             sliding_filter_in(&sf_r, prot);

             sliding_filter_in(&sf_rr, prot);

             float now_r = sliding_filter_out(&sf_r);             

             for(int i = 0; i < 8; i++)

             {

                    if(- 3.1415926f + i * 3.1415926f / 4 - 3.1415926f / 8 < zrot && zrot <= - 3.1415926f + (i + 1) * 3.1415926f / 4 + 3.1415926f / 8)

                    {

                           pid_cir_r[i].e = now_r - prot;//rsv

                           pid_process(&pid_cir_r[i]);

                    }

                    if(- 3.1415926f + i * 3.1415926f / 4 < zrot && zrot <= - 3.1415926f + (i + 1) * 3.1415926f / 4)

                           validv = pid_cir_r[i].o;

             }

             pid_cir_rr.e = x2ang(r_sv) - sliding_filter_out(&sf_rr);

             pid_process(&pid_cir_rr);

             pid_cir_rr.o = limit(pid_cir_rr.o + 10, -300, 300);

             validv = limit(validv, -800, 800);

             oled_disp_int(80, 0, prot * 1000);

             oled_disp_int(80, 1, zrot * 1000); 

             float motor_val = __sqrtf(validv * validv + pid_cir_rr.o * pid_cir_rr.o);

             float theta = zrot + 1.5708f + atan2f(validv, pid_cir_rr.o);            

             if(theta <-3.1415926f)

                    theta += 3.1415926f * 2;        

             if(theta >3.1415926f)

                    theta -= 3.1415926f * 2;         

             oled_disp_int(80, 7, time);

             motor_theta(theta, motor_val);

             time++;

             if(time % 10 == 0)

                    judge(prot, r_sv, 0.02f * gg, 0.001f, 200);

             oled_disp_int(0, 1, tanf(now_r) * HEIGHT + 0.5f);

             oled_disp_int(80, 0, prot * 1000);

             oled_disp_int(80, 1, zrot * 1000);

             if(disp_en)

             {

                    int oledx, oledy;

                    oledx = 30 - limit(vetex[0] * 60, -30, 30);

                    oledy = 24 + limit(vetex[1] * 60, -24, 24);

                    past_x[disp_idx] = oledx;

                    past_y[disp_idx] = oledy;

                    oled_bmp_set(past_x[disp_idx], past_y[disp_idx], 1);

                    disp_idx++;

                    if(disp_idx == 70)

                           disp_idx = 0;

                    oled_bmp_set(past_x[disp_idx], past_y[disp_idx], 0);

                    oled_disp_bmp(0, 2, 60, 7);

             }

             GPIO_SetBits(GPIOD, GPIO_Pin_12);

             delay_wait();

             GPIO_ResetBits(GPIOD, GPIO_Pin_12);

      }

}

（6）发挥部分上、下、左、右以一定步长移动程序

Q3:

      x_sv = 0;

      y_sv = 0;

      tm1638_int2(x_sv, y_sv);

      tm1638_disp();

      pid_init_k(&pid_sta_x, 850, 38, 70000, 500, 0, PID_TYPE_POS);

      pid_init_k(&pid_sta_y, 850, 38, 70000, 500, 0, PID_TYPE_POS);

      while(1)

      {

             delay_ms_set(20);

             //Read Position

             spi_start();

             spi_send(0x0C);

             for(int i = 0; i < 16; i++)

                    ((unsigned char *)(position_q))[i] = spi_send(0xFF);

             spi_stop();          

             quart_multi_q(position_q, position_q_correct);

             quart_to_eular(position_q, position_e);              

             pid_sta_x.e = x2ang(x_sv) - position_e[0];

             pid_process(&pid_sta_x);

             motor_ctrl_x = limit(pid_sta_x.o, -1000, 1000);

             pid_sta_y.e = x2ang(y_sv) - position_e[1];

             pid_process(&pid_sta_y);

             motor_ctrl_y = limit(pid_sta_y.o, -1000, 1000);

             motor_setx(motor_ctrl_x);

             motor_sety(motor_ctrl_y)                            

             if(ttl_x)

                    ttl_x--;

             if(ttl_x == 0)

             {

                    unsigned char key = tm1638_key();

                    if(key != 0xFF)

                           ttl_x = 15;

                    if(key == 7)

                           y_sv--;

                    else if(key == 15)

                           y_sv++;

                    else if(key == 10)

                           x_sv++;

                    else if(key == 12)

                           x_sv--;

             }

             if(ttl_x == 10)

                    tm1638_int2(x_sv, y_sv);

             if(ttl_x == 5)

                    tm1638_disp();

             time++;

             delay_wait();

       }