main.cpp

00001 #include "QEI.h"
00002 #include "mbed.h"
00003 #include "RawSerial.h"
00004 RawSerial pc(P5_0,P5_1); // tx, rx (ポートの指定)
00005 RawSerial pcm(USBTX,USBRX); // tx, rx(USBシリアルポートの指定)
00006 
00007 
00008 #define ROTATE_PER_REVOLUTIONS  360 //1回転でのパルス数
00009 #define JJ 0.006637 //慣性モーメント
00010 #define KT 0.184088 //モータ定数
00011 #define KD 0.001    //微分ゲイン
00012 #define KP 0.1      //比例ゲイン
00013 #define KI 0        //積分ゲイン
00014 #define T  1000     //割り込み時間(u_sec)
00015 
00016 QEI wheel(P1_0, P1_1, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);    //チャンネルAとBの初期状態を決める
00017 PwmOut Servo1(D7);          //Pwm出力Servo1の初期化
00018 PwmOut Servo2(D6);          //Pwm出力Servo2の初期化
00019 //PwmOut Servo1(D4);          //Pwm出力Servo1の初期化
00020 //PwmOut Servo2(D3);          //Pwm出力Servo2の初期化
00021 //BusOut input(D6,D5);
00022 
00023 Ticker msec;                //繰り返しタイマ割り込みmsecを定義
00024 Timer time1;                //時間タイマー
00025 
00026 double r=0;                 //目標値
00027 double y,e,old_e,u;         //現在値，エラー値，1つ前のエラー値，
00028 double t=0.001;             //割り込み周期
00029 
00030 
00031 void pid()                                                           //PID制御部
00032 {
00033 
00034     y=(double)wheel.getPulses()*360/(ROTATE_PER_REVOLUTIONS);       //エンコーダの値(角度)の取得
00035     pc.printf("(%-10f)\0",y);                                       //エンコーダ値(角度)の送信(serial)
00036 
00037     e=r-y;                                                          //誤差値の計算
00038     //u=KP*e;                                                       //P制御*******ここ書かせる*******
00039     //u=KP*e+KD*(e-old_e)/t;                                        //PD制御*******ここ書かせる*******
00040     u=KP*e+KD*(e-old_e)/t+KI*(e+old_e)*t;                           //PID計算(入力値の計算)*******ここ書かせる*******
00041     old_e=e;                                                        //誤差値の保存
00042     
00043     /***PWM値と回転方向の決定***/
00044     if(u<0) {
00045         Servo1=-u;
00046         Servo2=0;
00047         //input=0x02;
00048     } else {
00049         Servo1=0;
00050         Servo2=u;
00051         //input=0x01;
00052     }
00053 
00054 }
00055 
00056 int main()
00057 {
00058     pcm.baud(921600);                                    //ボーレートの指定(通信速度)
00059     pc.baud(921600);                                    //ボーレートの指定
00060     Servo1.period(0.00001);                                //PWMの周期の指定
00061     Servo2.period(0.00001);                                //PWMの周期の指定
00062     msec.attach_us(&pid,T);                             //タイマ割り込みの指定
00063     time1.start();
00064 
00065     while(1) {
00066 
00067         //pc.printf("(%-10f)¥0",y);                       //エンコーダ値(角度)の送信(serial)
00068         pcm.printf("%d\t%-10f\t%-10f\r\n",time1.read_ms(),y,u);//時間，エンコーダ値(角度)の送信(serial)(t,y,u)
00069 
00070         /********************************************************************/
00071         /************************シリアル通信受信部*****************************/
00072         /********************************************************************/
00073         char cmd,check_cmd;
00074         char code[13];
00075         int i;
00076 
00077 
00078         i =0;
00079 
00080         while(1) {
00081             cmd =pc.getc();
00082             check_cmd ='m';
00083             //pc.printf("%c",cmd);
00084             if(cmd=='(') {
00085                 i =0;
00086                 while(1) {
00087                     cmd =pc.getc();
00088                     if(check_cmd!=cmd) {
00089                         pc.printf("%c",cmd);
00090                         code[i] =cmd;
00091                         check_cmd ='m';
00092                         i++;
00093                     }
00094                     if(cmd==')') {
00095                         i=12;
00096                         break;
00097 
00098                     }
00099                 }
00100             }
00101             if(i>10) {
00102                 break;
00103             }
00104         }
00105 //     pc.printf("¥n%s¥n",code);
00106         r =atof(code);
00107         i=0;
00108         while(1) {
00109             i++;
00110             if(code[i]==')') {
00111                 break;
00112             }
00113         }
00114         /********************************************************************/
00115         /*********************シリアル通信受信部ここまで*************************/
00116         /********************************************************************/
00117 
00118     }
00119     time1.stop();
00120     time1.reset();
00121 }
00122 
00123