Diff: main.cpp

Revision:

0:3e0c71851fee

Child:

1:47c28ece54ea

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Aug 03 03:20:27 2020 +0000
@@ -0,0 +1,257 @@
+/*
+    Motacon 20200803 ver.2
+    ver.1: ブラシレスモータ(BLDC), USBメモリ書き込み, LM61(インバータ温度測定), INA226(インバータ電流電圧測定), CAN
+    ver.2 まともなプログラムになるように努力中
+*/
+#include "mbed.h"
+#include "MSCFileSystem.h"  //USBメモリ操作のため
+#include "INA226.hpp"       //INA226操作のため
+#include "RateLimiter.h"    //モータ加減速調整のため
+#include "BLDCmotorDriver.h"//BLDC操作のため
+
+/*定数の設定*/
+#define Vin1_LENGTH 20   //要素数(個数指定用の定数)
+#define Cin1_LENGTH 20   //要素数(個数指定用の定数)
+#define Vout1_LENGTH 20   //要素数(個数指定用の定数)
+#define Cout1_LENGTH 20   //要素数(個数指定用の定数)
+#define temp1_LENGTH 20   //要素数(個数指定用の定数)
+#define temp2_LENGTH 20   //要素数(個数指定用の定数)
+
+/*基本の設定*/
+Serial pc(USBTX, USBRX);
+DigitalOut myled(LED1); //
+DigitalOut led2(LED2);  //
+DigitalOut led3(LED3);  //
+bool flagPrintf = 0;    //main関数でのprintf処理のためのフラグ
+Ticker ticker1;     //割り込み設定用
+
+/*BLDC設定*/
+Timer timer;        //回転数計算用タイマ設定
+BLDCmotorDriver M(p26, p24, p22, p25, p23, p21, p14, p17, p18, LED1);
+AnalogIn Pot(p20);
+InterruptIn HS_p16(p16);    
+InterruptIn DIRECTION_SW(p8);
+void initBLDC();  //BLDCの初期設定を行う関数
+float rpm = 0, speed = 0;
+double dc = 0.0;
+bool direction = 0;//方向を決める
+int begin, end; //
+int HS_timer = 0;   //速度検出のために使う変数 
+int HS_timer1 = 0;  //速度検出のために使う変数 
+int reset_cnt = 0;  //Timerをリセットするために使うカウンタ
+
+/*CANの設定*/
+CAN canSlave(p30, p29); //CANのピン設定
+void initCAN();  //CANの初期設定を行う関数
+int forSend = 0;        //データ送信時に一時的に使用する変数
+bool CANsendOK = 0;  //CAN受信完了時,セットする(mainでのprintfのため)
+CANMessage msgSlave;    //CAN送信用
+unsigned int  canSlaveID = 0x10;   //canSlaveの初期IDを0x10に設定  (モタコン:0x10, )
+void Handler_canSlaveSend();
+
+/*INA226設定*/    //GNDGND(G-G):0x80, Vs+Vs+(1-1):0x8A, SDASDA(D-D):0x94, SCLSCL(C-C):0x9E, GNDVs+(G-1):0x82
+I2C i2c(p28,p27);
+INA226 VCmonitor1(i2c,0x9E);        //GNDGND(G-G)に設定したINA226がVCmonitor1(1台目)
+INA226 VCmonitor2(i2c,0x94);        //Vs+Vs+(1-1)に設定したINA226がVCmonitor2(2台目)
+void initINA226();  //INA226の初期設定を行う関数
+unsigned short val;
+double V,C,Vin1,Cin1,Vout1,Cout1;
+int count = 1;      //INA226動作確認用カウンタ
+float data_Vin1[Vin1_LENGTH] = {0.0, 0.0, 0.0, 0.0, 0.0};        //一時的データ格納配列
+float data_Cin1[Cin1_LENGTH] = {0.0, 0.0, 0.0, 0.0, 0.0};        //一時的データ格納配列
+float data_Vout1[Vout1_LENGTH] = {0.0, 0.0, 0.0, 0.0, 0.0};        //一時的データ格納配列
+float data_Cout1[Cout1_LENGTH] = {0.0, 0.0, 0.0, 0.0, 0.0};        //一時的データ格納配列
+void get_VCin1(void);
+void get_VCout1(void);
+
+/*温度測定(LM61)設定*/
+AnalogIn LM61(p15); ////set p15 to analog input to read LM61 sensor's voltage output
+float temp1 = 0.0, temp2 = 0.0;
+void get_temp(void);    //温度取得する関数
+float data_temp1[temp1_LENGTH] = {0.0, 0.0, 0.0, 0.0, 0.0};        //一時的データ格納配列
+float data_temp2[temp2_LENGTH] = {0.0, 0.0, 0.0, 0.0, 0.0};        //一時的データ格納配列
+
+/*ADCのピン設定*/    //also setting unused analog input pins to digital outputs reduces A/D noise a bit    //see http://mbed.org/users/chris/notebook/Getting-best-ADC-performance/
+DigitalOut P19(p19);
+    
+ /*USBメモリの設定*/
+MSCFileSystem msc("usb"); // Mount flash drive under the name "msc"
+void write_toMSC(void); //MSC=USBメモリ
+FILE *fp;   //ファイルの設定
+
+//Timer timer;  //処理時間測定用タイマ デバッグに使用する
+int n = 0;  //移動平均算出に使うカウンタ
+
+/*時間割り込み関数*/
+void handler_0t2s(){   //0.2秒毎の割り込み
+    if( direction == 0 ){    //正回転            
+        dc = Pot.read(); 
+    }
+    if( direction == 1 ){    //逆回転            
+        dc = -1 * Pot.read(); 
+    } 
+    reset_cnt++;
+    if( reset_cnt >= 6000 ){
+        timer.reset();
+        reset_cnt = 0;
+    }
+    //flagPrintf = 1;
+}
+/*ホールセンサの立ち上がりから立ち上がりまでの時間計測する関数*/
+void handler_HS(){  
+    if( HS_timer == 0 ){
+        begin = timer.read_us();
+        HS_timer1 = 0;
+    }
+    if( HS_timer == 1 ){
+        end = timer.read_us();
+        HS_timer1 = 1;
+    }
+    if( HS_timer1 == 0 ){
+        HS_timer = 1;
+    }
+    if( HS_timer1 == 1 ){
+        HS_timer = 0;
+        rpm = 3750 / (abs(end - begin) * 0.001);
+        speed = rpm * 0.10518;
+    }
+}
+/*Directionスイッチの割り込み 進行方向切り替え*/
+void handler_directionSW(){
+    if( dc <= 0 ){
+        direction = !direction;
+    }
+}
+/*CANでデータを送信する関数*/
+void Handler_canSlaveSend() { 
+    forSend = (int)(V/100);
+    msgSlave.data[0] = forSend / 100;    //forSend / 100 = 15あまり43 となり,答えの方がデータ格納される
+    msgSlave.data[1] = forSend % 100;    //forSend / 100 = 15あまり43 となり,あまりの方がデータ格納される
+    
+    forSend = (int)(C/10);  //mAで取得した値を10で割り、char2分割で送り切れるようにする。(例: 測定電流が199.99Aのとき,C=199900.00となるため,forSend=19990となり,199と90で分割することで送信できる)
+    msgSlave.data[2] = forSend / 100;   //forSend / 100 = 5あまり54 となり,答えの方がデータ格納される
+    msgSlave.data[3] = forSend % 100;   //forSend % 100 = 5あまり54 となり,あまりの方がデータ格納される
+    
+    forSend = (int)(speed*10);  //speed * 10 = 856.5だが,intにするため,856が代入される 
+    msgSlave.data[4] = forSend / 100;   //forSend / 100 = 8あまり56 となり,8がデータ格納される
+    msgSlave.data[5] = forSend % 100;   //forSend % 100 = 8あまり56 となり,56がデータ格納される
+    
+    forSend = (int)(dc*100);  //dc * 100 = 95
+    msgSlave.data[6] = forSend;   //charの最大値である255以下になるため,そのままデータ格納される
+    
+    if(canSlave.write(msgSlave)){   //格納したデータを送信する
+        CANsendOK = 1;
+    }
+}
+/*CAN通信相手から送信要求がきたとき実行される*/
+void Handler_canSlaveRecieve(){ //CANの通信相手から,送信要求がきたときに実行される関数
+    if( canSlave.read( msgSlave ) ){ //msgに送られたデータが入る   
+        led2 = !led2;
+        if( msgSlave.id == canSlaveID ){ //IDがcanSlaveIDであれば処理する   
+            Handler_canSlaveSend(); //送信処理をする関数に飛ぶ
+        }
+    }
+}
+/*電圧と電流測定する関数*/
+void get_VCin1(){
+    if((VCmonitor1.getVoltage(&V) == 0) && (VCmonitor1.getCurrent(&C) == 0)){     //1台目INA226の電圧電流測定
+        Vin1 -= data_Vin1[n];    //移動平均処理
+        Cin1 -= data_Cin1[n];    //移動平均処理
+        data_Vin1[n] = V/Vin1_LENGTH;   //移動平均処理
+        data_Cin1[n] = C/Cin1_LENGTH;   //移動平均処理
+        Vin1 += data_Vin1[n];   //移動平均処理
+        Cin1 += data_Cin1[n];   //移動平均処理
+        pc.printf("\n%d, Vin1: %f, Cin1: %f\n",count,Vin1,Cin1);
+    }
+}
+/*温度測定する関数*/
+void get_temp(){
+    temp1 -= data_temp1[n];     //移動平均処理
+    data_temp1[n] = ( ((LM61*3.3)-0.600)*100.0 ) / temp1_LENGTH;    //移動平均処理
+    temp1 += data_temp1[n];     //移動平均処理
+    pc.printf("temp1:%5.2F C temp2:%5.2F C \n\r", temp1, temp2);
+}
+void write_toMSC(){
+    if ( (fp= fopen( "/usb/MPPT.csv", "a")) == NULL ){  //ファイルを開く, aは上書きの命令(ファイルが存在しなければ新規作成する)
+        pc.printf("USB error\r\n");
+        exit(1);
+    }
+    pc.printf("USB file write!\r\n");
+    //timer.start();    //書き込み時間測定開始
+    fprintf(fp,"%f,%f,%f,%f,%f,%f\n", Vin1, Cin1, Vout1, Vout1, temp1, temp2);  //ファイル書き込み
+    //timer.stop();     //書き込み時間測定終了    
+    fclose(fp);         //ファイルを閉じる  
+    //pc.printf("write time :%f\n\n\n",timer.read());  
+}
+
+/*INA226の初期設定*/
+void initINA226(){
+    //  1台目の設定
+    if ( !VCmonitor1.isExist() ){
+        pc.printf("VCmonitor1 INA226 Not Found.\n");
+    }
+    val = 0;
+    if ( VCmonitor1.rawRead(0x00,&val) != 0){
+        pc.printf("VCmonitor1 INA226 Read Error\n");
+    }
+    pc.printf("VCmonitor1 Reg 0x00 : 0x%04x\r\n",val);
+    VCmonitor1.setCurrentCalibration();
+}
+/*CANの初期設定*/
+void initCAN(){
+    canSlave.attach(&Handler_canSlaveRecieve, CAN::RxIrq);   //CAN受信割り込みの設定
+    msgSlave.id = canSlaveID; //CAN送信側(slave)のIDを決定
+    msgSlave.len = 7;   //CAN送信側で送るデータのバイト数
+}
+void initBLDC(){
+    timer.start();      //回転数計算用タイマスタート
+    DIRECTION_SW.mode(PullUp);                //directionスイッチのモード設定
+    DIRECTION_SW.fall(&handler_directionSW);  //directionスイッチの立ち上がり割り込み設定
+    DIRECTION_SW.rise(&handler_directionSW);  //directionスイッチの立ち下がり割り込み設定   どちらも同じ関数に飛ぶ
+    led3 = DIRECTION_SW.read();
+//    HS_p16.rise(&handler_HS);       //ホールセンサ立ち上がり割り込み設定
+}
+
+/*メイン関数*/
+int main()
+{
+    initINA226();        //INA226の初期設定
+    initCAN();
+    initBLDC();     //BLDC制御のための初期設定
+    ticker1.attach(&handler_0t2s, 0.3); //0.3秒毎に飛ぶ「handler_0t3s」関数
+    
+    while(1) {
+        get_VCin1();    //Vipm(IPMへの入力電圧)とCipm(IPMへの入力電流)を取得
+        get_temp();     //温度測定
+//        write_toMSC();  //データをUSBメモリに保存
+
+        n++;
+        if( n >= Vin1_LENGTH ){ //カウンタがサンプル数を超えたら,0に戻り,移動平均算出を続ける           
+            n = 0;
+        }
+        
+        M.setDutyCycle(dc);     //モータを駆動する関数    引数dcは,PWMのデューティ比
+        flagPrintf = 1;
+        if( flagPrintf ){   //flagPrntfが1なら,以下を処理する
+            flagPrintf = 0;
+            pc.printf("Duty Cycle: %1.2f, Sector: %d\n\r",dc, M.getSector());
+            pc.printf("Toggle the led takes %d us\n", abs(end - begin));
+            pc.printf("%f rpm\n",rpm);
+            pc.printf("%f km/h\n",speed);
+            pc.printf("V,%f,C,%f\n",V,C);
+        }
+        /*
+        if( CANsendOK ) {
+            CANsendOK = 0;     
+            printf("Data in msgSlave.data[0] : %d\n\r", msgSlave.data[0]);    //CANで送信したデータをそのまま表示
+            printf("Data in msgSlave.data[1] : %d\n\r", msgSlave.data[1]);    //上に同じ
+            printf("Data in msgSlave.data[2] : %d\n\r", msgSlave.data[2]);    //上に同じ
+            printf("Data in msgSlave.data[3] : %d\n\r", msgSlave.data[3]);    //上に同じ
+            printf("Data in msgSlave.data[4] : %d\n\r", msgSlave.data[4]);    //上に同じ
+            printf("Data in msgSlave.data[5] : %d\n\r", msgSlave.data[5]);    //上に同じ
+            printf("Data in msgSlave.data[6] : %d\n\r", msgSlave.data[6]);    //上に同じ
+            printf("\n\r");
+        }
+        */  
+    }
+}