Diff: main.cpp

Revision:

0:13ba01a70b4a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun May 06 06:01:45 2018 +0000
@@ -0,0 +1,162 @@
+#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);
+}
\ No newline at end of file