Diff: main.cpp

Revision:

0:61f234ae4194

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Nov 04 08:34:54 2018 +0000
@@ -0,0 +1,461 @@
+
+#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);
+        
+    }
+}