SpeedControl Class Reference

 //サンプルプログラム 差動二輪駆動ロボットを想定
 //直進・後退・右旋回・左旋回が可能
 #include "mbed.h"
 #include "SpeedController.h"

 #define BASIC_SPEED 20  //モーターはこの角速度で駆動させる

 SpeedControl motorR(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5);      //右輪
 SpeedControl motorL(PF_1,PA_11,NC,1048,0.05,PA_9,PA_10);    //左輪
 Ticker motor_tick;  //角速度計算用ticker
 Serial pc(USBTX,USBRX);

 void calOmega() //角速度計算関数
 {
     motorR.CalOmega();
     motorL.CalOmega();
 }

 int main(void){
     motor_tick.attach(&calOmega,0.05);
     //Cの値を設定
     motorR.setDOconstant(46.3);
     motorL.setDOconstant(47.2);
     //pd係数を設定
     motorR.setPDparam(0.1,0);
     motorR.setPDparam(0.12,0);

     int kai=0;
     double target_R=0,target_L=0;
     char status[5][10]={"STOP","FORWARD","BACKWARD","RIGHT","LEFT"};
     int status_num=0;         //初期状態は停止

     while(1) {
         motorR.Sc(target_R);
         motorL.Sc(target_L);       //target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う

         if(kai>=500) {
             pc.printf("status=%s omega_R=%f omega_L=%f \r\n",status[status_num],motorR.getOmega(),motorL.getOmega());
             //機体の進む方向、右モーターの角速度、左モーターの角速度を表示
             kai=0;
         }

         if(pc.readable()) {
             char sel=pc.getc();

             switch (sel) {
                 case 's':           //停止
                     motorR.stop();
                     motorL.stop();
                     target_R=0;
                     target_L=0;
                     status_num=0;
                     break;
                 case 'i':           //前進
                     target_R=BASIC_SPEED;
                     target_L=BASIC_SPEED;
                     status_num=1;
                     break;
                 case 'm':           //後退
                     target_R=(-1)*BASIC_SPEED;
                     target_L=(-1)*BASIC_SPEED;
                     status_num=2;
                     break;
                 case 'k':           //右旋回
                     motorR.stop();
                     target_R=0;
                     target_L=BASIC_SPEED;
                     status_num=3;
                     break;
                 case 'j':           //左旋回
                     motorL.stop();
                     target_R=BASIC_SPEED;
                     target_L=0;
                     status_num=4;
                     break;
                 default:
                     break;
             }
             if(sel=='q'){
                 break; //qを押したら左右のモーターを停止
             }
         }
         kai++;
     }
     motorR.stop();
     motorL.stop();

     return 0;
 }

 //2,3で使えるサンプルコード
 //入力はTera Termで行う
 #include "mbed.h"
 #include "SpeedController.h"

 SpeedControl motor(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5);
 Serial pc(USBTX,USBRX);
 Ticker motor_tick;

 void calOmega(){
     motor.CalOmega();    //角速度計算用
 }

 int main(void){
     motor_tick.attach(calOmega,0.05);
     int loop_time=0;
     float in_duty=0;
     bool print=false;

     while(1){
         loop_time++;

         if(in_duty>0) motor.turnF(in_duty); //in_dutyが正のとき正回転（このプログラムではPA_8で駆動）
         else motor.turnB((-1)*in_duty);     //in_dutyが負のとき逆回転（このプログラムではPB_5で駆動）


         if(pc.readable()){
             char sel=pc.getc();
             if(sel=='s'){
                 motor.stop();
             } else if(sel=='f'){
                 in_duty+=(float)0.05;        //fを押すとduty比がo.o5ずつあがる
                 pc.printf("duty=%f\r\n",in_duty);
             } else if(sel=='b'){
                 in_duty-=(float)0.05;        //bを押すとduty比がo.o5ずつさがる
                 pc.printf("duty=%f\r\n",in_duty);
             } else if(sel=='p'){
                 print=!print;                //pを押すと表示を停止/再開する
             }
         }

         if(loop_time%1000==0){
             if(print) pc.printf("count=%d omega=%f dutyF=%f dutyB=%f\r\n",motor.getCount(),motor.getOmega(),(double)motor.pwm_F_,(double)motor.pwm_B_);
         }   //左からエンコーダのカウント、角速度、正方向出力duty比、負方向出力duty比を表示
     }
 }

 //PDの係数を探したいときに使えるサンプル
 //入力はTera Termで行う
 #include "mbed.h"
 #include "SpeedController.h"

 #define TARGET_OMEGA 20 //目標角速度を20rad/sに設定

 SpeedControl motor(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5); //エンコーダ分解能1024、角速度計算間隔を0.05秒に設定
 Ticker motor_tick;  //角速度計算用ticker
 Serial pc(USBTX,USBRX);

 void calOmega() //角速度計算関数
 {
     motor.CalOmega();
 }

 int main()
 {
     motor_tick.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
     motor.setDOconstant(46.3);         //求めたCの値を設定

     int kai=0;
     double target_omega=0;
     double P=0.0,D=0.0;

     while(1) {
         motor.Sc(target_omega);     //速度制御のコア関数
         if(kai>=500) {
             pc.printf("count=%d omega=%f target_omega=%f dutyF=%f dutyB=%f P=%f D=%f\r\n",motor.getCount(),motor.getOmega(),target_omega,(double)motor.pwm_F_,(double)motor.pwm_B_,P,D);
             //エンコーダのカウント、角速度、目標角速度、正方向出力duty比、負方向出力duty比、Pの係数、Dの係数を表示
             kai=0;
         }
         //係数探しの流れとしては、係数変更→速度制御開始→収束具合を観察・比較→モーター停止の繰り返し
         if(pc.readable()) {
         char sel=pc.getc();

         switch (sel) {
             case 'k':   //kでpの係数に0.01加算
                 P+=0.01;
                 break;
             case 'm':   //mでpの係数に0.01減算
                 P-=0.01;
                 break;
             case 'j':   //jでdの係数に0.01加算
                 D+=0.01;
                 break;
             case 'n':   //nでdの係数に0.01減算
                 D-=0.01;
                 break;
             case 's':
                 motor.stop();   //sでモーター停止
                 target_omega=0;
                 break;
             case 'x':
                 target_omega=TARGET_OMEGA;  //xで速度制御開始
                 break;
             default:
                 break;
         }

         if(sel=='q'){
             break; //qを押したら終了
         }

         motor.setPDparam(P,D);  //変更したパラメータをセット
     }
         kai++;
     }
     motor1.stop();
 }