File content as of revision 1:798a41fa6515:

//モーターの角速度(x)とduty比(y)の関係を
//y=Cx+Dであると近似した時の、定数Cを決定するためのプログラム
//別プログラムで求めたdを使い傾きCを決定する
//入力はTera Termで行う
#include "mbed.h"
  #include "EC.h" //Encoderライブラリをインクルード
#include "SpeedController.h"
   #define RESOLUTION 2048//分解能
  #define BASIC_SPEED 20//目標角速度、一番使う速度帯におおよそ合わせるが吉
//  #define TEST_DUTY 0.3
  
Ec4multi EC_kagawa(PA_5,PC_9,RESOLUTION);
SpeedControl kagawa(PB_9,PB_8,50,EC_kagawa);
Ec4multi EC_tokusima(PA_15,PA_14,RESOLUTION);
SpeedControl tokusima(PC_7,PA_6,50,EC_tokusima);
Ec4multi EC_kouchi(PC_13,PB_7,RESOLUTION);
SpeedControl kouchi(PA_9,PB_6,50,EC_kouchi);
Ec4multi EC_ehime(PC_2,PC_3,RESOLUTION);
SpeedControl ehime(PB_3,PA_10,50,EC_ehime);
Serial pc(USBTX,USBRX);
Ticker motor_tick;
 
/*void calOmega(){
    motor.CalOmega();    //角速度計算用
}
 */
int main(void){
// motor_tick.attach(calOmega,0.05);
      kagawa.period_us(50);
      tokusima.period_us(50);
      kouchi.period_us(50);
      ehime.period_us(50);
      double kagawa_Cf=0,kagawa_Cb=0;
      double tokusima_Cf=0,tokusima_Cb=0;
      double kouchi_Cf=0,kouchi_Cb=0;
      double ehime_Cf=0,ehime_Cb=0;
      
      kagawa.setPDparam(0,0.0);  //PIDの係数を設定
      tokusima.setPDparam(0,0.0);  //PIDの係数を設定
      kouchi.setPDparam(0,0.0);  //PIDの係数を設定
      ehime.setPDparam(0,0.0);  //PIDの係数を設定
    int loop_time=0;
    double target_kagawa=0,target_tokusima=0,target_kouchi=0,target_ehime=0;
    bool print=false;
 
    while(1){
        loop_time++;
 
        if(target_kagawa==0) kagawa.stop();
        else kagawa.Sc(target_kagawa);
        if(target_tokusima==0) tokusima.stop();
        else tokusima.Sc(target_tokusima);
        if(target_kouchi==0) kouchi.stop();
        else kouchi.Sc(target_kouchi);
        if(target_ehime==0) ehime.stop();
        else ehime.Sc(target_ehime);
        
      kagawa.setEquation(kagawa_Cf,0,kagawa_Cb,0);    //求めたDの値を設定
      tokusima.setEquation(tokusima_Cf,0,tokusima_Cb,0);    //求めたDの値を設定
      kouchi.setEquation(kouchi_Cf,0,kouchi_Cb,0);    //求めたDの値を設定
      ehime.setEquation(ehime_Cf,0,ehime_Cb,0);    //求めたDの値を設定
 
        if(pc.readable()){
            char sel=pc.getc();
            if(sel=='s'){
                kagawa.stop();
                tokusima.stop();
                kouchi.stop();
                ehime.stop();
            } else if(sel=='e'){
                pc.printf("kagawa_Cf");
                if(sel=='i')kagawa_Cf+=0.002;        //e->iを押すとCfがo.o02ずつあがる
                else if(sel=='m'&&kagawa_Cf>0)kagawa_Cf-=0.002;   //e->mを押すとCfがo.o02ずつさがる
                pc.printf("duty=%f\r\n",kagawa_Cf);
            } else if(sel=='d'){
                pc.printf("kagawa_Cb");
                if(sel=='i')kagawa_Cb+=0.002;        //e->iを押すとCfがo.o02ずつあがる
                else if(sel=='m'&&kagawa_Cf>0)kagawa_Cb-=0.002;   //e->mを押すとCfがo.o02ずつさがる
                pc.printf("duty=%f\r\n",kagawa_Cf);
            }else if(sel=='r'){
                pc.printf("tokusima_Cf");
                if(sel=='i')tokusima_Cf+=0.002;        //r->iを押すとCfがo.o02ずつあがる
                else if(sel=='m'&&tokusima_Cf>0)tokusima_Cf-=0.002;   //r->mを押すとCfがo.o02ずつさがる
                pc.printf("duty=%f\r\n",tokusima_Cf);
            } else if(sel=='f'){
               pc.printf("tokusima_Cb");
                if(sel=='i')tokusima_Cb+=0.002;        //f->iを押すとCfがo.o02ずつあがる
                else if(sel=='m'&&tokusima_Cf>0)tokusima_Cb-=0.002;   //f->mを押すとCfがo.o02ずつさがる
                pc.printf("duty=%f\r\n",tokusima_Cb);
                
            }else if(sel=='t'){
               pc.printf("kouchi_Cf");
                if(sel=='i')kouchi_Cf+=0.002;        //t->iを押すとCfがo.o02ずつあがる
                else if(sel=='m'&&kouchi_Cf>0)kouchi_Cf-=0.002;   //t->mを押すとCfがo.o02ずつさがる
                pc.printf("duty=%f\r\n",kouchi_Cf);
            } else if(sel=='g'){
                pc.printf("kouchi_Cb");
                if(sel=='i')kouchi_Cb+=0.002;        //g->iを押すとCfがo.o02ずつあがる
                else if(sel=='m'&&kouchi_Cf>0)kouchi_Cb-=0.002;   //g->mを押すとCfがo.o02ずつさがる
                pc.printf("duty=%f\r\n",kouchi_Cb);
                
            }else if(sel=='y'){
                pc.printf("ehime_Cf");
                if(sel=='i')ehime_Cf+=0.002;        //t->iを押すとCfがo.o02ずつあがる
                else if(sel=='m'&&ehime_Cf>0)ehime_Cf-=0.002;   //t->mを押すとCfがo.o02ずつさがる
                pc.printf("duty=%f\r\n",ehime_Cf);
            } else if(sel=='h'){
              pc.printf("ehime_Cb");
                if(sel=='i')ehime_Cb+=0.002;        //g->iを押すとCfがo.o02ずつあがる
                else if(sel=='m'&&ehime_Cf>0)ehime_Cb-=0.002;   //g->mを押すとCfがo.o02ずつさがる
                pc.printf("duty=%f\r\n",ehime_Cb);
            }else if(sel=='p'){
                
                print=!print;                //pを押すと表示を停止/再開する
            }else if(sel=='i'){
                target_kagawa=1*BASIC_SPEED;
                target_tokusima=1*BASIC_SPEED;
                target_kouchi=-1*BASIC_SPEED;
                target_ehime=-1*BASIC_SPEED;
                
            }else if(sel=='m'){
                target_kagawa=1*BASIC_SPEED;
                target_tokusima=1*BASIC_SPEED;
                target_kouchi=-1*BASIC_SPEED;
                target_ehime=-1*BASIC_SPEED;
            }else if(sel=='k'){
                target_kagawa=1*BASIC_SPEED;
                target_tokusima=1*BASIC_SPEED;
                target_kouchi=-1*BASIC_SPEED;
                target_ehime=-1*BASIC_SPEED;
            }else if(sel=='j'){
                target_kagawa=1*BASIC_SPEED;
                target_tokusima=1*BASIC_SPEED;
                target_kouchi=-1*BASIC_SPEED;
                target_ehime=-1*BASIC_SPEED;
            }
            
        }
 
        if(loop_time%1000==0){
            
            if(print) pc.printf("%.2f %.2f %.2f %.2f\r\n",EC_kagawa.getOmega(),EC_tokusima.getOmega(),EC_kouchi.getOmega(),EC_ehime.getOmega());
        }   
    }
}