File content as of revision 0:13ba01a70b4a:

#include "mbed.h"
#include "Moter3_L298N.h"
#include "QEI.h"
#define LOW 0
#define maxv 344.0    //电机最高转速

float setspeed[2]={150,150}, error_current[2]={0,0}, error_last[2]={0,0}, error_sum[2]={0,0}, kp[2]={0.9,0.9}, ki[2]={0,0}, kd[2]={0,0};
float time_encoder_pid=0.2;     //编码器读取周期,单位s,也是pid控制计算周期

Motor_3 motor[] = {Motor_3(PB_12,PB_13,PA_11),Motor_3(PB_14,PB_15,PA_8)};
//Motor_3 motor2(PB_14,PB_15,PA_8);

//调试串口
Serial pc(PB_10, PB_11);
Serial bt(PA_2, PA_3);

Ticker toggle_time_ticker;
//编码器1-4
QEI wheel[] = {QEI(PB_5, PB_6, NC, 11, QEI::X4_ENCODING),QEI(PB_7, PB_8, NC, 11, QEI::X4_ENCODING)};                //买来的电机自带编码器,说明书上有电机每转一圈编码器发生11个信号
//QEI wheel2(PB_7, PB_8, NC, 11, QEI::X4_ENCODING);

float a[2]= {150, 150};      //初始速度
void time_ticker();
void serialread();
void pidcontroller();

void pidcontroller(){
    float target[2];
    for (int i=0; i<=1;i++){
        error_sum[i] += error_current[i];
        target[i]=(a[i] + kp[i]*error_current[i] + ki[i]*(error_current[i] - error_last[i]) +kd[i]*(error_sum[i]));
        motor[i].mv(target[i]/maxv*100);        //mv函数的参数取值范围是-100~100
//    motor1.mv(a[0] + kp[0]*error_current[0] + ki*(error_current[0] - error_last[0]) +kd*(error_sum[0]));
//    motor2.mv(a[1] + kp[1]*error_current[1] + ki*(error_current[1] - error_last[1]) +kd*(error_sum[1]));
        error_last[i]=error_current[i];
        i++;
    }
    
    pc.printf("target x:%f y:%f\r\n",target[0],target[1]);
}


void time_ticker(){
    for (int i=0; i<=1; i++){
    //获取一个轮子的编码器读到的数和旋转方向
    error_current[i] = setspeed[i] - wheel[i].getPulses();
    //wheel[i].getState();   //获取旋转方向,返回值为1--顺时针, -1--逆时针
    wheel[i].reset();
    }
    
    pc.printf("error x:%f y:%f\r\n",error_current[0],error_current[1]);
    pidcontroller();
    
    
}

void pcserialread(){
    switch (pc.getc())
  {
    case '1':
          setspeed[0]=100;
          setspeed[1]=100;          
        break;
    case '2':
          setspeed[0]=300;
          setspeed[1]=300;          
        break;
    default:
        break;
  }
}

    typedef enum
{
    CheckS,CheckW,Checka,Checkb,Checkc,Checkd,Checke,Checkf,CheckSum
}STATE;

void btserialread(){
    char u1Rec; 
    static STATE State = CheckS;//初始状态待检查
    static char PositionTemp[7]={0,0,0,0,0,0,0};//装载数据值a(左右转) b(前后退) c(机械手高度) d(机械手旋转角度) Sum(求和校验)
    u1Rec = bt.getc(); 
    switch(State)
    {
        case CheckS:
            if(u1Rec==(char)'S')
                State=CheckW;//准备跳转至数据读取
            else
                State=CheckS;
            break;
        case CheckW:
            if(u1Rec==(char)'W')
            State=Checka;
            else if(u1Rec==(char)'S')
                State=CheckW;
            else 
                State=CheckS;
            break;
        case Checka:
            PositionTemp[0]=u1Rec;
            State=Checkb;
            break;
        case Checkb:
            PositionTemp[1]=u1Rec;
            State=Checkc;
            break;
        case Checkc:
            PositionTemp[2]=u1Rec;
            State=Checkd;
            break;
        case Checkd:
            PositionTemp[3]=u1Rec;
            State=Checke;
            break;
        case Checke:
            PositionTemp[4]=u1Rec;
            State=Checkf;
            break;
        case Checkf:
            PositionTemp[5]=u1Rec;
            State=CheckSum;
            break;
        case CheckSum:
            PositionTemp[6]=u1Rec;
            if(PositionTemp[6]==(char)(PositionTemp[0]+PositionTemp[1]+PositionTemp[2]+PositionTemp[3]+PositionTemp[4]+PositionTemp[5]))
            {
                kp[0] = PositionTemp[0];
                ki[0] = PositionTemp[1];
                kd[0] = PositionTemp[2];
                kp[1] = PositionTemp[3];
                ki[1] = PositionTemp[4];
                kd[1] = PositionTemp[5];
            }
            State=CheckS;
            break;
        default:
            State=CheckS;
            break;
    }
}



int main()
{
    toggle_time_ticker.attach(&time_ticker, time_encoder_pid);       //使能ticker中断,0.2s产生一次.中断函数用于获取每个轮子的计数值
    
    pc.attach(&pcserialread);
    bt.attach(&btserialread);
    //for (int speed=10;speed<1000;speed+=10){        
//        pc.printf("set to %d\r\n",speed);
//        for (int i=0; i<=1; i++){
//            motor[i].mv(speed/maxv*100);
//        } 
//        wait(2);
//    }
    for (int i=0; i<=1; i++){
        motor[i].mv(a[i]/maxv*100);
    } 
    
    while(1);
}