main.cpp

00001 /***************************************
00002 MotorDriver Ver4 or Ver5 or Ver6 select
00003 ***************************************/
00004 //<注意>プログラム上でのバージョンです 書き込む基板に合わせてここだけ変更してください．
00005 //Ver4 2019Bチーム手動機で使っていたMD(CAN通信のコネクタとPWM入力用のコネクタとロータリーエンコーダ入力用のコネクタがあります　全部でコネクタが４つ)
00006 //Ver5 2019Bチーム自動機で使っていたMD(CAN通信のコネクタとロータリーエンコーダ入力用のコネクタがあります　全部でコネクタが３つ)
00007 //Ver6 センサ入力に対応したMD
00008 #define MOTOR_DRIVER_VERSION 5
00009 /***************************************
00010         SMB or LAP select
00011 ***************************************/
00012 //駆動方式の選択 選択する方のコメントアウトを外してください．
00013 //<default> SMBモード
00014 //<option>  LAPモード
00015 #define MOTOR_DRIVER_MODE_SMB
00016 //#define MOTOR_DRIVER_MODE_LAP
00017 /*************************************************************************************************************************/
00018 //                                              souce code
00019 /***************************
00020      include file
00021 ***************************/
00022 #include "mbed.h"
00023 #include "can_network_motordriver_definition.h"
00024 #include "Can_network_definition.h"
00025 #include "bitCommunication.h"
00026 #include "buzzer.h"
00027 #include "eeprom.h"
00028 #include "eeprom_initalize.h"
00029 //#include "QEI.h"
00030 #include "qeihw.h"
00031 #include "Pid.h"
00032 #include "motorDrive_SMB.h"
00033 #include "motorDrive_LAP.h"
00034 #include "MedianFilter.h"
00035 #include "definition.h"
00036 /******************************
00037     definition instance
00038 ******************************/
00039 CAN can(CAN_PIN[CAN_RD],CAN_PIN[CAN_TD]);
00040 Serial pc(SERIAL_PIN[SERIAL_TX],SERIAL_PIN[SERIAL_RX],communication_baud::PC);
00041 BusIn idSwitch(ID_PIN[SWITCH1],ID_PIN[SWITCH2],ID_PIN[SWITCH3],ID_PIN[SWITCH4]);
00042 //QEI encoder(ENCODER_PIN[ENCODER_A],ENCODER_PIN[ENCODER_B],NC,ENCODER_PPR);
00043 QEIHW qei(QEI_DIRINV_NONE,QEI_SIGNALMODE_QUAD,QEI_CAPMODE_4X,QEI_INVINX_NONE);      //QEI_hw
00044 buzzer led(LED_PIN[FREE]);
00045 PwmOut pwmLed[2] = {
00046     PwmOut(LED_PIN[CW]),
00047     PwmOut(LED_PIN[CCW]),
00048 };
00049 Pid pid;
00050 Ticker controlCycle;
00051 Timer errorTimer;
00052 #ifdef MOTOR_DRIVER_MODE_LAP
00053 motorDriveLAP motor(A3921_PIN[PWMH],A3921_PIN[PHASE]);
00054 #else
00055 motorDriveSMB motor(A3921_PIN[PWMH],A3921_PIN[PHASE]);
00056 #endif
00057 #if MOTOR_DRIVER_VERSION == 4
00058 DigitalOut pwml(PWML_PIN,true);
00059 #endif
00060 /******************************
00061       global variable
00062 ******************************/
00063 float parameter[PARAMETER_NUMBER] = {0.0f,0.0f,0.0f,0.0f};
00064 float outputDuty = 0.0f;
00065 float targetRpm = 0.0f;
00066 float rps = 0.0f;
00067 float rpm = 0.0f;
00068 float rotation[2] = {0.0f,0.0f};
00069 int powerFilterCount;
00070 int powerFilterArray[10];
00071 bool enable_motor_output = false;    //モーターへの出力許可を管理(true : 出力可能 , false : 出力を強制停止)
00072 bool outputMode = false;
00073 bool printfMode = false;
00074 bool receiveParameterStatus[4] = {false,false,false,false};
00075 /******************************
00076     definition function
00077 ******************************/
00078 void initalize();
00079 void mainLoop();
00080 void outputIndicator(float control_duty);
00081 void safetyControl(float duty);
00082 void canReceiveHandler();
00083 void debag();
00084 /******************************
00085       function content
00086 ******************************/
00087 int main() {
00088     initalize();
00089     while(1){
00090     }
00091 }
00092 
00093 void initalize() {
00094     /***************************EEPROM　読み取り処理*******************************/
00095     can_flame eepromData;
00096     Chip_EEPROM_init();
00097     __disable_irq();
00098     read_eeprom((char*)0, eepromData.c, 4);
00099     parameter[KP] = eepromData.f;
00100     read_eeprom((char*)4, eepromData.c, 4);
00101     parameter[KI] = eepromData.f;
00102     read_eeprom((char*)8, eepromData.c, 4);
00103     parameter[KD] = eepromData.f;
00104     __enable_irq();
00105     /***************************************************************************/
00106     
00107     /******************************割り込み優先度設定******************************/
00108     NVIC_SetPriority(C_CAN0_IRQn,0);
00109     NVIC_SetPriority(QEI_IRQn,1);
00110     /***************************************************************************/
00111     
00112     /*******************************CAN通信の設定*********************************/
00113     can.frequency(communication_baud::CAN);
00114     can.attach(&canReceiveHandler,CAN::RxIrq);
00115     /***************************************************************************/
00116     
00117     /***************************MotorDrive 設定**********************************/
00118     motor.setupFrequency(motor_option::FREQUENCY);
00119     motor.setupLimitDuty(motor_option::LOWER_LIMIT,motor_option::UPPER_LIMIT);
00120     /****************************************************************************/
00121     
00122     /*********************************SpeedPID設定*******************************/
00123     pid.setup(parameter[KP],parameter[KI],parameter[KD],SPEED_PID,period::CONTROL_CYCLE);
00124     /***************************************************************************/
00125     
00126     /**********************************その他************************************/
00127     qei.SetDigiFilter(480UL);                   //QEI_hw filter設定
00128     qei.SetMaxPosition(0xFFFFFFFF);             //QEI_hw filter設定
00129     qei.SetVelocityTimerReload_us(10000);       //QEI_hw filter設定
00130     
00131     errorTimer.start();                                     //CAN通信 タイムアウト検知用計測開始
00132     pc.printf("\r\nInitalize MotorDriver!\r\n");            //initalize終了出力
00133     pc.printf("Firmware:robocon2021_TBCMotorDriver( https://os.mbed.com/users/YutaTogashi/code/robocon2021_TBCMotorDriver/ )\r\n");  //Firmwareをシリアル出力
00134     pc.printf("UsingLibrary:can_network_motordriver ( https://os.mbed.com/users/YutaTogashi/code/can_network_motordriver/ )\r\n");   //MDを使用しやすくしたライブラリ　シリアル出力
00135     #if MOTOR_DRIVER_VERSION == 4
00136     pc.printf("MotorDriverVersion:4\r\n");
00137     #elif MOTOR_DRIVER_VERSION == 5
00138     pc.printf("MotorDriverVersion:5\r\n");
00139     #elif MOTOR_DRIVER_VERSION == 6
00140     pc.printf("MotorDriverVersion:6\r\n");
00141     #endif
00142     controlCycle.attach(&mainLoop,period::CONTROL_CYCLE);   //制御ループ設定
00143     /***************************************************************************/
00144 }
00145 
00146 void mainLoop() {   
00147     //RPS,RPMの計算
00148     rotation[0] = static_cast<float>(static_cast<int32_t>(qei.GetPosition())) / (static_cast<float>(ENCODER_PPR) * 4.0f);
00149     rps = (rotation[0] - rotation[1]) / period::CONTROL_CYCLE;
00150     rpm = rps * 60.0f;
00151     rotation[1] = rotation[0];
00152     
00153     //速度PIDモードの挙動
00154     if(outputMode) {
00155         pid.setup(parameter[KP],parameter[KI],parameter[KD],SPEED_PID,period::CONTROL_CYCLE);
00156         pid.calculate(targetRpm,/*encoder.getRPM()*/rpm);
00157         outputDuty = pid.getDuty();
00158     }
00159     
00160     //TIMEOUT判定
00161     if(errorTimer.read() > TIMEOUT_VALUE)   enable_motor_output = false;
00162     else                                    enable_motor_output = true;
00163     
00164     //出力処理
00165     if(enable_motor_output) {
00166         led.output(outputMode);     //速度PIDモードの場合は，LEDインジゲータが点灯
00167     } else {
00168         led.output(0.5f);           //TIMEOUT時は，LEDインジゲータが点滅
00169         safetyControl(0.05f);       //徐々に出力を下げる
00170         pid.reset();                //速度PIDの計算をリセット
00171     }
00172     
00173     outputIndicator(outputDuty);    //Duty,Phaseインジゲータを制御
00174     motor.output(outputDuty);       //A3921制御
00175 
00176     debag();                        //デバッグ関数
00177 }
00178 
00179 void debag() {
00180     if(!(printfMode)) {
00181         pc.printf("SWITCH:%d\t",idSwitch.read());
00182         pc.printf("ID:%d\t", (idSwitch.read()));
00183         pc.printf("DUTY:%.2f\t",outputDuty);
00184         pc.printf("RPM:%.2f\t",/*encoder.getRPM()*/rpm);
00185         
00186         if(outputMode) {
00187             pc.printf("TARGET:%.1f\t",targetRpm);
00188             pc.printf("Kp:%f\t",parameter[KP]);
00189             pc.printf("Ki:%f\t",parameter[KI]);
00190             pc.printf("Kd:%f\t",parameter[KD]);
00191         }
00192         pc.printf("TIME:%f",errorTimer.read());
00193         pc.printf("\r\n");
00194     } else {
00195         pc.printf("%f,",targetRpm);
00196         pc.printf("%f,",/*encoder.getRPM()*/rpm);
00197         pc.printf("%f",outputDuty);
00198         pc.printf("\r\n");
00199     }
00200 }
00201 
00202 void outputIndicator(float duty) {
00203     if(fabs(duty) > 0.0f) {
00204         if(duty > 0.0f) {
00205             pwmLed[CW].write(fabs(duty));
00206             pwmLed[CCW].write(0.0f);
00207         } else {
00208             pwmLed[CW].write(0.0f);
00209             pwmLed[CCW].write(fabs(duty));
00210         }
00211     } else {
00212         pwmLed[CW].write(0.0f);
00213         pwmLed[CCW].write(0.0f);
00214     }
00215 }
00216 
00217 void safetyControl(float control_duty) {
00218     if(fabs(outputDuty - 0.0f) > 0.10f) {
00219         if(outputDuty > 0.0f)   outputDuty -= control_duty;
00220         else                    outputDuty += control_duty;
00221     } else {
00222         outputDuty = 0.0f;   
00223     }
00224 }
00225 
00226 void canReceiveHandler() {
00227     can_flame message,returnMessage;
00228     CANMessage sendMsg,receiveMessage;
00229     
00230     float value = 0.0f;
00231     CANMessage msg;
00232     can.read(msg);
00233     
00234     if((getCanIdData(msg.id,0) == can_protcol::device_type::MOTOR_DRIVER) && (getCanIdData(msg.id,1) == idSwitch.read())) {           //MD基板宛のメッセージのみ抽出
00235         errorTimer.reset();                                                    //TIMEOUT時間管理用タイマーをリセット
00236         switch (((static_cast<uint8_t>(msg.data[0])) >> 4)) {
00237             case can_protcol::device_type::CONTROL_MCU:                        //メイン制御基板からきたデータを抽出
00238             case can_protcol::device_type::PC:                                 //PCからきたデータを抽出
00239                 switch (getCanIdData(msg.id,2)) {       //DataTypeを確認
00240                     case can_protcol::data_type::motor_driver::DEBAG_MODE:                  //debag mode
00241                         break;
00242                     case can_protcol::data_type::motor_driver::PARAMETER:                   //パラメータ受信
00243                         switch (static_cast<uint8_t>(msg.data[1])) {
00244                             case 0:             //PositionPid Kp
00245                                 break;
00246                             case 1:             //PositoinPid Ki
00247                                 break;
00248                             case 2:             //PositionPid Kd
00249                                 break;
00250                             case 3:             //VelocityPid Kp
00251                                 memcpy(&value,msg.data+2,4);
00252                                 parameter[KP] = value;
00253                                 receiveParameterStatus[0] = true;
00254                                 break;
00255                             case 4:             //VelocityPid Ki
00256                                 memcpy(&value,msg.data+2,4);
00257                                 parameter[KI] = value;
00258                                 receiveParameterStatus[1] = true;
00259                                 break;
00260                             case 5:             //VelocityPid Kd
00261                                 memcpy(&value,msg.data+2,4);
00262                                 parameter[KD] = value;
00263                                 receiveParameterStatus[2] = true;
00264                                 break;
00265                             case 6:             //Acceleration
00266                                 break;
00267                             case 7:             //Timeout Time
00268                                 break;
00269                             case 8:             //Max Duty
00270                                 break;
00271                             case 9:             //Max Velocity(RPM)
00272                                 break;
00273                             case 10:            //Encoder PPR
00274                                 break;
00275                             case 11:            //AbsEncoder Reset
00276                                 break;
00277                             default:
00278                                 break;   
00279                         }
00280                         break;
00281                     case can_protcol::data_type::motor_driver::CONTROL_INFORMATION:         //制御量受信
00282                         switch (static_cast<uint8_t>(msg.data[1])) {
00283                             case 0:                     //duty駆動
00284                                 memcpy(&outputDuty,msg.data+2,4);
00285                                 outputMode = false;
00286                                 break;
00287                             case 1:                     //速度制御                                
00288                                 if((static_cast<uint8_t>(msg.data[6]) & 0b1) == 0b1) {      //駆動電源のフィードバック情報より駆動電源が入力されている場合の挙動
00289                                     memcpy(&targetRpm,msg.data+2,4);
00290                                     outputMode = true;
00291                                 } else {                                                    //駆動電源のフィードバック情報より駆動電源が入力されていない場合の挙動
00292                                     targetRpm = 0.0f;
00293                                     outputDuty = 0.0f;
00294                                     pid.reset();
00295                                     outputMode = false;
00296                                 }
00297                                 break;
00298                             default:
00299                                 break;   
00300                         }
00301                         break;
00302                     case can_protcol::data_type::motor_driver::STEER_CONTROL_INFORMATION:   //(ステア用)制御量受信
00303                         break;
00304                     case can_protcol::data_type::motor_driver::PRINTF_FORMAT:               //printf出力形式の指定情報を受信
00305                         break;
00306                     default:
00307                         break;
00308                 }
00309                 break;
00310             default:
00311                 break;
00312         }
00313     }
00314     
00315     
00316     //sendMsg.id = MAIN_MCU;
00317 
00318     /*if((MD_0_MCU + idSwitch.read()) == receiveMessage.id) {
00319         errorTimer.reset();
00320         switch ((int)receiveMessage.data[0]) {
00321             case md_can_flame::SEND_KP:
00322                 for(int i=2;i<6;i++)    message.c[i-2] = receiveMessage.data[i];
00323                 __disable_irq();
00324                 write_eeprom(message.c,(char*)0,4);
00325                 __enable_irq();
00326                 parameter[KP] = message.f;
00327                 receiveParameterStatus[0] = true;
00328                 break;
00329             case md_can_flame::SEND_KI:
00330                 for(int i=2;i<6;i++)    message.c[i-2] = receiveMessage.data[i];
00331                 __disable_irq();
00332                 write_eeprom(message.c,(char*)4,4);
00333                 __enable_irq();
00334                 parameter[KI] = message.f;
00335                 receiveParameterStatus[1] = true;
00336                 break;
00337             case md_can_flame::SEND_KD:
00338                 for(int i=2;i<6;i++)    message.c[i-2] = receiveMessage.data[i];
00339                 __disable_irq();
00340                 write_eeprom(message.c,(char*)8,4);
00341                 __enable_irq();
00342                 parameter[KD] = message.f;
00343                 receiveParameterStatus[2] = true;
00344                 break;
00345             case md_can_flame::SEND_ACCELERATION:
00346                 for(int i=2;i<6;i++)    message.c[i-2] = receiveMessage.data[i];
00347                 parameter[ACCELERATION] = message.f;
00348                 receiveParameterStatus[3] = true;
00349                 break;
00350             case md_can_flame::SEND_CONTROL_DUTY:
00351                 for(int i=2;i<6;i++)    message.c[i-2] = receiveMessage.data[i];
00352                 outputDuty = message.f;
00353                 outputMode = false;
00354                 break;
00355             case md_can_flame::SEND_CONTROL_RPM:
00356                 if(bitRead((int)receiveMessage.data[1],0)) {
00357                     for(int i=2;i<6;i++)    message.c[i-2] = receiveMessage.data[i];
00358                     targetRpm = message.f;
00359                     outputMode = true;
00360                 } else {
00361                     targetRpm = 0.0f;
00362                     outputDuty = 0.0f;
00363                     pid.reset();
00364                     outputMode = false;
00365                 }
00366                 break;
00367             case md_can_flame::REQUEST_RECEIVE_CURRENT:
00368                 //no code
00369                 break;
00370             case md_can_flame::REQUEST_RECEIVE_ERROR:
00371                 int parameterStatus = 0;
00372                 for(int i=0;i<4;i++) bitWrite(&parameterStatus,i,receiveParameterStatus[i]);
00373                 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_ERROR;
00374                 sendMsg.data[2] = parameterStatus;
00375                 can.write(sendMsg);
00376                 break;
00377             case md_can_flame::REQUEST_RECEIVE_POSITION:        //dont useing
00378                 //returnMessage.i = encoder.getPosition();
00379                 returnMessage.i = qei.GetPosition();
00380                 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_POSITION;
00381                 for(int i=0;i<4;i++)    sendMsg.data[i+2] = returnMessage.c[i];
00382                 can.write(sendMsg);
00383                 break;
00384             case md_can_flame::REQUEST_RECEIVE_ROTATION:
00385                 //returnMessage.f = static_cast<float>(encoder.getRotation());
00386                 returnMessage.f = rotation[0];
00387                 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_ROTATION;
00388                 for(int i=0;i<4;i++)    sendMsg.data[i+2] = returnMessage.c[i];
00389                 can.write(sendMsg);
00390                 break;
00391             case md_can_flame::REQUEST_RECEIVE_RPS:
00392                 //returnMessage.f = encoder.getRPS();
00393                 returnMessage.f = rps;
00394                 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_RPS;
00395                 for(int i=0;i<4;i++)    sendMsg.data[i+2] = returnMessage.c[i];
00396                 can.write(sendMsg);
00397                 break;
00398             case md_can_flame::REQUEST_RECEIVE_RPM:
00399                 //returnMessage.f = encoder.getRPM();
00400                 returnMessage.f = rpm;
00401                 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_RPM;
00402                 for(int i=0;i<4;i++)    sendMsg.data[i+2] = returnMessage.c[i];
00403                 can.write(sendMsg);
00404                 break;
00405             case md_can_flame::REQUEST_PRINTF_DEBAG:
00406                 printfMode = false;
00407                 break;
00408             case md_can_flame::REQUEST_PRINTF_FORMAT_CSV:
00409                 printfMode = true;
00410                 break;
00411             case md_can_flame::REQUEST_RESET_QEI:
00412                 //encoder.reset();
00413                 qei.Reset(QEI_RESET_POS);
00414                 break;
00415             case md_can_flame::REQUEST_RESET_MCU:
00416                 NVIC_SystemReset();
00417                 break;
00418             default:
00419                 break;
00420         }
00421     } else if(ALL_MCU == receiveMessage.id) {
00422         NVIC_SystemReset();
00423     } else {
00424         
00425     }
00426     */
00427 }