File content as of revision 0:61f234ae4194:

#include "mbed.h"

#define PWM_FC1_1 PC_3
#define PWM_FC2_1 PC_2
#define PWM_FC1_2 PA_0
#define PWM_FC2_2 PA_1
#define PWM_FC1_3 PC_8
#define PWM_FC2_3 PC_6
#define PWM_FC1_4 PB_0
#define PWM_FC2_4 PB_1
#define N 19
/*#define L1 PB_5
#define L2 PB_6
#define F1 PB_9
#define F2 PB_10
#define B1 PB_7
#define B2 PB_8*/
#define uchar unsigned char 
#define uint unsigned int
Ticker timer;
InterruptIn event1(PC_1);
InterruptIn event2(PC_0);
InterruptIn event3(PA_8);
InterruptIn event4(PC_7);
/*InterruptIn event_L1(PB_5);
InterruptIn event_L2(PB_6);
InterruptIn event_F1(PB_9);
InterruptIn event_F2(PB_10);
InterruptIn event_B1(PB_7);
InterruptIn event_B2(PB_8);*/
DigitalOut PWM1_1(PWM_FC1_1);
DigitalOut PWM2_1(PWM_FC2_1);
DigitalOut PWM1_2(PWM_FC1_2);
DigitalOut PWM2_2(PWM_FC2_2);
DigitalOut PWM1_3(PWM_FC1_3);
DigitalOut PWM2_3(PWM_FC2_3);
DigitalOut PWM1_4(PWM_FC1_4);
DigitalOut PWM2_4(PWM_FC2_4);
/*DigitalIn L_1(L1);
DigitalIn L_2(L2);
DigitalIn F_1(F1);
DigitalIn F_2(F2);
DigitalIn B_1(B1);
DigitalIn B_2(B2);*/
Serial pc(PA_9, PA_10,9600);

int count = 0;
int JS=0;
int sum1=0,sum2=0,sum3=0,sum4=0;
int sum1_1=0,sum2_1=0,sum3_1=0,sum4_1=0;
int sum1_2=0,sum2_2=0,sum3_2=0,sum4_2=0;
int PWM=0;
int turn=1;
int e_1=0,e1_1=0,e2_1=0;          //pid 偏差
float uk_1=0,uk1_1=0.0,duk_1=0.0; //pid输出值
int e_2=0,e1_2=0,e2_2=0;          //pid 偏差
float uk_2=0,uk1_2=0.0,duk_2=0.0; //pid输出值
int e_3=0,e1_3=0,e2_3=0;          //pid 偏差
float uk_3=0,uk1_3=0.0,duk_3=0.0; //pid输出值
int e_4=0,e1_4=0,e2_4=0;          //pid 偏差
float uk_4=0,uk1_4=0.0,duk_4=0.0; //pid输出值
float Kp=2,Ki=2,Kd=0.9;

int out1=0,out2=0,out3=0,out4=0;;
int X=0,Y=0;
uint SpeedSet1=15;
uint SpeedSet2=15;
uint SpeedSet3=15;
uint SpeedSet4=15;
uint cnt=0;
uint Inpluse1=0,Inpluse2=0,Inpluse3=0,Inpluse4=0,num1=0,num2=0,num3=0,num4=0;       //脉冲计数
uint PWMTime1=0,PWMTime2=0,PWMTime3=10,PWMTime4=10 ;


void PIDControl1();
void PIDControl2();
void PIDControl3();
void PIDControl4();
void PWMOUT();
   
void trigger1() {
    //pc.printf("triggered!\n");
    
    Inpluse1++;
   // pc.printf("%d\n",n);
}
void trigger2() {
   
    Inpluse2++;
}
void trigger3() {
   
    Inpluse3++;
}
void trigger4() {
   
    Inpluse4++;
}
/*void trigger_L1_up() {          //    F1      L1 L2        B1
                                //     |                    |
                                //    F2                   B2
   if(turn==3||turn==4)
    { SpeedSet1=15;
      SpeedSet2=15;
      SpeedSet3=15;
      SpeedSet4=15;}
   else if(turn==1)
     {X++;}
   else if(turn==2)
     {X--;}
}
void trigger_L1_down() {
   if(turn==4)
   SpeedSet1=N;
   else if(turn==3)
   SpeedSet2=N;
}
void trigger_L2_up() {
  if(turn==3||turn==4)
   {  SpeedSet1=15;
      SpeedSet2=15;
      SpeedSet3=15;
      SpeedSet4=15;}
}
void trigger_L2_down() {
   if(turn==4)
   SpeedSet2=N;
   else if(turn==3)
   SpeedSet1=N;
      
}
void trigger_F1_up() {
    if(turn==1||turn==2)
    { SpeedSet1=15;
      SpeedSet2=15;
      SpeedSet3=15;
      SpeedSet4=15;}
   esle if(turn==3)
      {Y--;}
   else if(turn==4)
      {Y++;}
    }
void trigger_F1_down() {
    if(turn==1)
    SpeedSet4=N;
    else if(turn==2)
    SpeedSet1=N;
    
    }
void trigger_F2_up() {
     if(turn==1||turn==2)
    { SpeedSet1=15;
      SpeedSet2=15;
      SpeedSet3=15;
      SpeedSet4=15;}
    
    }
void trigger_F2_down() {
    if(turn==1)
    SpeedSet1=N;
    else if(turn==2)
    SpeedSet4=N;
    
    }
void trigger_B1_up() {
     if(turn==1||turn==2)
    { SpeedSet1=15;
      SpeedSet2=15;
      SpeedSet3=15;
      SpeedSet4=15;}
    
    }
void trigger_B1_down() {
    if(turn==1)
    SpeedSet2=N;
    else if(turn==2)
    SpeedSet3=N;
    
    }
void trigger_B2_up() {
     if(turn==1||turn==2)
    { SpeedSet1=15;
      SpeedSet2=15;
      SpeedSet3=15;
      SpeedSet4=15;}
    
    }
void trigger_B2_down() {
    if(turn==1)
    SpeedSet3=N;
    else if(turn==2)
    SpeedSet2=N;
    
    }
    */
void attach(){
   
   
    cnt++;
    count++;
    if(count==20)//每0.1s处理一次脉冲
    {    
        count=0;
        JS++;
        num1=Inpluse1;
        num2=Inpluse2;
        num3=Inpluse3;
        num4=Inpluse4;
        sum1=Inpluse1+sum1;
        sum2=Inpluse2+sum2;
        sum3=Inpluse3+sum3;
        sum4=Inpluse4+sum4;
 /*if(JS==100)
       {
        pc.printf("1DJ%d\r\n",PWMTime1);
        pc.printf("2DJ%d\r\n",PWMTime2);
        pc.printf("3DJ%d\r\n",PWMTime3);
        pc.printf("4DJ%d\r\n",PWMTime4);
        pc.printf("1DJsum%d\r\n",sum1);
        pc.printf("2DJsum%d\r\n",sum2);
        pc.printf("3DJsum%d\r\n",sum3);
        pc.printf("4DJsum%d\r\n",sum4);
        JS=0;}*/
        Inpluse1=0;     //清零，为下一次计数做准备
        Inpluse2=0; 
        Inpluse3=0; 
        Inpluse4=0; 
        PIDControl1();  //调用PID控制程序
        PIDControl2();
        PIDControl3();
        PIDControl4();
    }

}
/*void Tracking()                 //    F1      L1 L2        B1
{                                       |                     |
                                  //    F2                   B2
    if(turn==1&&F_1==1&&F_2==1&&B_1==1&&B_2==1)
      
    
    }*/
void motor1(int i)
{
    if(i==1)
    {PWM1_1=PWM;
    PWM2_1=0;}
    else if(i==2)
    {PWM2_1=PWM;
    PWM1_1=0;}
    
    else if(i==3)
    {PWM1_1=PWM;
    PWM2_1=0; }
    else if(i==4)
    {PWM2_1=PWM;
    PWM1_1=0;}
    }
void motor2(int i)
{
    if(i==1)
    {PWM2_2=PWM;
    PWM1_2=0;}
    else if(i==2)
    {PWM1_2=PWM;
    PWM2_2=0;}
    else if(i==3)
    {PWM1_2=PWM;
    PWM2_2=0;}
    else if(i==4)
    {PWM2_2=PWM;
    PWM1_2=0;}
    }
void motor3(int i)
{
    if(i==1)
    {PWM1_3=PWM;
    PWM2_3=0;}
    else if(i==2)
    {PWM2_3=PWM;
    PWM1_3=0;}
    else if(i==4)
    {PWM2_3=PWM;
    PWM1_3=0;}
    else if(i==3)
    {PWM1_3=PWM;
    PWM2_3=0;}
    }
void motor4(int i)
{
    if(i==1)
    {PWM1_4=PWM;
    PWM2_4=0;}
    else if(i==2)
    {PWM2_4=PWM;
    PWM1_4=0;}
     else if(i==3)
    {PWM2_4=PWM;
    PWM1_4=0;}
    else if(i==4)
    {PWM1_4=PWM;
    PWM2_4=0;}
    }
void PWMOUT()       //50ms pwm
{
    if(cnt<PWMTime4)//若小于PWM的设定时间，则输出高电平
        {PWM=1;
        motor4(turn);
        }
    else           //否则输出低电平
        {PWM=0;
        motor4(turn);
        }
    if(cnt<PWMTime2)//若小于PWM的设定时间，则输出高电平
        {PWM=1;
        motor2(turn);
        }
    else           //否则输出低电平
        {PWM=0;
        motor2(turn);
        }
    if(cnt<PWMTime3)//若小于PWM的设定时间，则输出高电平
        {PWM=1;
        motor3(turn);
        }
    else           //否则输出低电平
        {PWM=0;
        motor3(turn);
        }
    if(cnt<PWMTime1)//若小于PWM的设定时间，则输出高电平
        {PWM=1;
        motor1(turn);
        }
    else           //否则输出低电平
        {PWM=0;
        motor1(turn);
        }
    if(cnt>25)    //超过限制清零
        cnt=0;
}

void PIDControl1()        //pid偏差计算
{
    e_1=SpeedSet1-num1;//设置速度-实际速度，两者的差值 
    //对应于增量式PID的公式Δuk=uk-u（k-1）
//  duk=(Kp*(e-e1))/100;//只调节P
    duk_1=(Kp*e_1+Ki*(e1_1+e_1)*0.01+Kd*(e_1-e1_1)/0.01);//只调节PI
//  duk=(Kp*(e-e1)+Ki*e+Kd*(e-2*e1+e2))/100;//调节PID
    //uk=uk1+duk;//uk=u(k-1)+Δuk
    uk_1=duk_1;
    out1=(int)uk_1;//取整后输出
   // pc.printf("wc1 %d\r\n",out1);
    if(out1>25) //设置最大限制
        {out1=25;}
    else if(out1<0)//设置最小限制
       { out1=0;}
    uk1_1=uk_1;       //为下一次增量做准备
    e2_1=e1_1;
    e1_1=e_1;
    PWMTime1=out1;  //out对应于PWM高电平的时间
  //pc.printf("pwm %d\r\n",out1);
}

void PIDControl2()        //pid偏差计算
{
    e_2=SpeedSet2-num2;//设置速度-实际速度，两者的差值 
    //对应于增量式PID的公式Δuk=uk-u（k-1）
//  duk=(Kp*(e-e1))/100;//只调节P
    duk_2=(Kp*e_2+Ki*(e1_2+e_2)*0.01+Kd*(e_2-e1_2)/0.01);//只调节PI
//  duk=(Kp*(e-e1)+Ki*e+Kd*(e-2*e1+e2))/100;//调节PID
    //uk=uk1+duk;//uk=u(k-1)+Δuk
    uk_2=duk_2;
    out2=(int)uk_2;//取整后输出
   // pc.printf("wc2 %d\r\n",out2);
    if(out2>25) //设置最大限制
        {out2=25;}
    else if(out2<0)//设置最小限制
       { out2=0;}
    uk1_2=uk_2;       //为下一次增量做准备
    e2_2=e1_2;
    e1_2=e_2;
   /* if((sum1-sum2)>2)
    {out2=out2+3;}
    else if((sum2-sum1)>2)
    {out2=out2-3;}*/
    PWMTime2=out2;  //out对应于PWM高电平的时间
   //pc.printf("pwm %d\r\n",out);
}
void PIDControl3()        //pid偏差计算
{
    e_3=SpeedSet3-num3;//设置速度-实际速度，两者的差值 
    //对应于增量式PID的公式Δuk=uk-u（k-1）
//  duk=(Kp*(e-e1))/100;//只调节P
    duk_3=(Kp*e_3+Ki*(e1_3+e_3)*0.01+Kd*(e_3-e1_3)/0.01);//只调节PI
//  duk=(Kp*(e-e1)+Ki*e+Kd*(e-2*e1+e2))/100;//调节PID
    //uk=uk1+duk;//uk=u(k-1)+Δuk
    uk_3=duk_3;
    out3=(int)uk_3;//取整后输出
   // pc.printf("wc2 %d\r\n",out2);
    if(out3>25) //设置最大限制
        {out3=25;}
    else if(out3<0)//设置最小限制
       { out3=0;}
    uk1_3=uk_3;       //为下一次增量做准备
    e2_3=e1_3;
    e1_3=e_3;
  /*  if((sum3-sum2)>2)
    {out3=out3-3;}
    else if((sum2-sum3)>2)
    {out3=out3+5;}*/
    PWMTime3=out3;  //out对应于PWM高电平的时间
   //pc.printf("pwm %d\r\n",out);
}
void PIDControl4()        //pid偏差计算
{
    e_4=SpeedSet4-num4;//设置速度-实际速度，两者的差值 
    //对应于增量式PID的公式Δuk=uk-u（k-1）
//  duk=(Kp*(e-e1))/100;//只调节P
    duk_4=(Kp*e_4+Ki*(e1_4+e_4)*0.01+Kd*(e_4-e1_4)/0.01);//只调节PI
//  duk=(Kp*(e-e1)+Ki*e+Kd*(e-2*e1+e2))/100;//调节PID
    //uk=uk1+duk;//uk=u(k-1)+Δuk
    uk_4=duk_4;
    out4=(int)uk_4;//取整后输出
   // pc.printf("wc2 %d\r\n",out2);
    if(out4>25) //设置最大限制
        {out4=25;}
    else if(out4<0)//设置最小限制
       { out4=0;}
    uk1_4=uk_4;       //为下一次增量做准备
    e2_4=e1_4;
    e1_4=e_4;
    /*if((sum4-sum1)>2)
    {out4=out4-3;}
    else if((sum1-sum4)>2)
    {out4=out4+5;}*/
    PWMTime4=out4;  //out对应于PWM高电平的时间
   //pc.printf("pwm %d\r\n",out);
}
int main() {
    event1.rise(&trigger1);
    event2.rise(&trigger2);
    event3.rise(&trigger3);
    event4.rise(&trigger4);
    /*event_L1.rise(&trigger_L1_up);
      event_L1.fall(&trigger_L1_down);
      event_L2.rise(&trigger_L2_up);
      event_L2.fall(&trigger_L2_down);
      event_F2.rise(&trigger_F2_up);
      event_F2.fall(&trigger_F2_down);
      event_B2.rise(&trigger_B2_up);
      event_B2.fall(&trigger_B2_down);*/
    timer.attach(&attach, 0.0005);
   
    while(1) 
    {
        PWMOUT();
  
       // pc.printf("%d\r\n",n);
        
    }
}