Diff: main.cpp

Revision:

0:67b5faeb3720

Child:

1:2d5ccfddbe36

diff -r 000000000000 -r 67b5faeb3720 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 09 03:56:27 2018 +0000
@@ -0,0 +1,118 @@
+#include "QEI.h"
+#include "mbed.h"
+#include "RawSerial.h"
+RawSerial pc(P5_0,P5_1); // tx, rx (ポートの指定)
+//RawSerial pc(USBTX,USBRX); // tx, rx(USBシリアルポートの指定)
+
+
+#define ROTATE_PER_REVOLUTIONS  360 //1回転でのパルス数
+#define JJ 0.006637 //慣性モーメント
+#define KT 0.184088 //モータ定数
+#define KD 0.001    //微分ゲイン
+#define KP 0.1      //比例ゲイン
+#define KI 0        //積分ゲイン
+#define T  1000     //割り込み時間(u_sec)
+
+QEI wheel(P1_0, P1_1, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);    //チャンネルAとBの初期状態を決める
+PwmOut Servo1(D7);          //Pwm出力Servo1の初期化
+PwmOut Servo2(D6);          //Pwm出力Servo2の初期化
+//BusOut input(D6,D5);
+
+Ticker msec;                //繰り返しタイマ割り込みmsecを定義
+
+double r=0;                 //目標値
+double y,e,old_e,u;         //現在値，エラー値，1つ前のエラー値，
+double t=0.001;             //割り込み周期
+
+
+void pid()                                                           //PID制御部
+{
+
+    y=(double)wheel.getPulses()*360/(ROTATE_PER_REVOLUTIONS);       //エンコーダの値(角度)の取得
+    pc.printf("(%-10f)¥0",y);                                       //エンコーダ値(角度)の送信(serial)
+
+    e=r-y;                                                          //誤差値の計算
+    //u=KP*e;                                                       //P制御*******ここ書かせる*******
+    //u=KP*e+KD*(e-old_e)/t;                                        //PD制御*******ここ書かせる*******
+    u=KP*e+KD*(e-old_e)/t+KI*(e+old_e)*t;                           //PID計算(入力値の計算)*******ここ書かせる*******
+    old_e=e;                                                        //誤差値の保存
+
+    /***PWM値と回転方向の決定***/
+    if(u<0) {
+        Servo1=-u;
+        Servo2=0;
+        //input=0x02;
+    } else {
+        Servo1=0;
+        Servo2=u;
+        //input=0x01;
+    }
+
+}
+
+int main()
+{
+    //pc.baud(115200);                                    //ボーレートの指定(通信速度)
+    pc.baud(921600);                                    //ボーレートの指定
+    Servo1.period(0.00001);                                //PWMの周期の指定
+    Servo2.period(0.00001);                                //PWMの周期の指定
+    msec.attach_us(&pid,T);                             //タイマ割り込みの指定
+
+    while(1) {
+
+        //pc.printf("(%-10f)¥0",y);                       //エンコーダ値(角度)の送信(serial)
+
+
+
+        /********************************************************************/
+        /************************シリアル通信受信部*****************************/
+        /********************************************************************/
+        char cmd,check_cmd;
+        char code[13];
+        int i;
+
+
+        i =0;
+
+        while(1) {
+            cmd =pc.getc();
+            check_cmd ='m';
+            //pc.printf("%c",cmd);
+            if(cmd=='(') {
+                i =0;
+                while(1) {
+                    cmd =pc.getc();
+                    if(check_cmd!=cmd) {
+                        pc.printf("%c",cmd);
+                        code[i] =cmd;
+                        check_cmd ='m';
+                        i++;
+                    }
+                    if(cmd==')') {
+                        i=12;
+                        break;
+
+                    }
+                }
+            }
+            if(i>10) {
+                break;
+            }
+        }
+//     pc.printf("¥n%s¥n",code);
+        r =atof(code);
+        i=0;
+        while(1) {
+            i++;
+            if(code[i]==')') {
+                break;
+            }
+        }
+        /********************************************************************/
+        /*********************シリアル通信受信部ここまで*************************/
+        /********************************************************************/
+
+    }
+}
+
+