disturbance observer
Dependencies: mbed MATSUbed USBDevice
main.cpp
- Committer:
- Mtshadow
- Date:
- 2021-12-26
- Revision:
- 9:66b71a3f4ffb
- Parent:
- 8:2ae2483893f0
- Child:
- 10:21ea2a241025
File content as of revision 9:66b71a3f4ffb:
/* 外乱推定→走行面の検出を行う やること ・100Hzで離散化したプラントの逆モデルの実装 ・推定外乱から磁気吸着力変化の検出 リビジョン mbed 127:25aea2a3f4e3 */ #include "mbed.h" #include "USBSerial.h" #include "math.h" // Board settings #define LED1 P0_29 #define LED2 P0_28 #define LED3 P0_27 #define LED4 P0_26 // CANopen settings #define RxPDO1 0x220 #define RxPDO2 0x320 #define RxPDO3 0x420 #define Halt 1 #define QuickStop 2 #define ShutDown 3 #define SYNC_Enable 0 #define SYNC_Disable 1 #define node 2 // Disturbance Observer settings #define tfA 193.5 #define tfB 0.7529 #define tfC 1 #define tfD 0.5219 // 定数 #define Kv 0.092421 #define Km 0.0092 #define samplimgrate 0.01 #define bodylength 550.0 #define startdetect 10.0 #define Kn 116 USBSerial pc; //PCとのシリアル通信 char Serialdata; CANMessage canmsgTx; //送信データ処理用 CANMessage canmsgRx; //受信データ処理用 Ticker flipper; //汎用タイマー DigitalOut ENsig(P0_10); BusOut myled(LED1, LED2, LED3,LED4); CAN canPort(P0_13, P0_18); //CAN name(PinName rd, PinName td) //グローバル変数 //char Serialdata; //シリアル受信データ float Current_demand=0, Velocity_actual=0; //電流入力値,速度出力値 float Torque_estimate_tf=0; //トルク入力推定値 float Distterbance_estimate_tf=0; //外乱トルク推定値 float Velocity_estimate_z1; //推定時保持データ float Velocity_estimate=0; //推定時現在速度 float Current_estimate=0; //推定時現在電流 int Velocity_Trend=0, Current_Trend=0; //除去するトレンド float distance = 0; //走行距離[mm] float Velocity_meter; // 走行速度[mm/s] float Distterbance_estimate_tf_abs = 0; // 外乱推定値nの絶対値 float Dist_error = 0; float baseDist = 0; // 判別基準値 int cnt_ditect = 0; // しきい値を超えてからのカウント int cnt_find = 0; // 非磁性面の検出からのカウント int findnonmagnet =0; // 非磁性面検出信号 int cnt_run = 0; // 走行時間カウント bool run_eternal = false; int check = 0; bool decimal = false; //プロトタイプ宣言 // CANopen関連 void NMTPreOpn(void); //NMT PreOperationalへ移行 void NMTOpn(void); //NMT Operationalへ移行 void CtrlWord(int); //RxPDO1 void ModesOfOperation(void);//RxPDO2 void TgtVelCtrl(int); //RxPDO3 void sendCtrlRS(int); //SDO Reset void sendCtrlSD(int); //SDO Shutdown void sendCtrlEN(int); //SDO Enable //-------------------その他-------------------- void init(void); //マイコン初期化 void CANinit(void); //CANノード初期化 void SerialRX(void); //Serial受信処理 void flip(void); //タイマー割り込み void printPlantData(void); //速度指令値、出力値をPCに表示 void EstimateDisterbance(void);//指令値を推定 void detectNonmagnet(void); int main(){ init(); myled = 0b0101; while(1){ if(Serialdata == 's'){ run_eternal = false; break; }else if(Serialdata == 'e'){ run_eternal = true; break; } myled=~myled; wait(1); } Serialdata = 0; //node初期化 CANinit(); int count=0; myled=0; while(1){ if(!ENsig){ myled=count; count++; } else{ myled =~ myled; } wait(0.5); } } //マイコン初期化 void init(void){ //I/O設定 ENsig=SYNC_Disable; //CAN設定 canPort.frequency(1000000); //Bit Rate:1Mbps //割り込み設定 flipper.attach_us(&flip,5000); //汎用タイマー割り込み周期:5ms(5*10^3 us) pc.attach(SerialRX); //Serial受信割り込み開始 } //can node初期化 void CANinit(void){ //SDOコマンドで各nodeをリセット for(int nodeID=1;nodeID <= node;nodeID++){ sendCtrlRS(nodeID); sendCtrlSD(nodeID); sendCtrlEN(nodeID); } //NMTコマンドで全nodeをオペレーショナルに NMTPreOpn(); NMTOpn(); } //Serial受信割り込み処理 void SerialRX(void){ Serialdata = pc.getc(); //-------------送信コマンドを選択-------------- if (decimal) { if(Serialdata == '1'){ Velocity_Trend = 100; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } else if(Serialdata == '2'){ Velocity_Trend = 200; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } else if(Serialdata == '3'){ Velocity_Trend = 300; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } else if(Serialdata == '4'){ Velocity_Trend = 400; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } else if(Serialdata == '5'){ Velocity_Trend = 500; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } else if(Serialdata == '6'){ Velocity_Trend = 600; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } else if(Serialdata == '7'){ Velocity_Trend = 700; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } else if(Serialdata == '8'){ Velocity_Trend = 800; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } else if(Serialdata == '9'){ Velocity_Trend = 900; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } decimal = false; } else { if(Serialdata == '1'){ Velocity_Trend = 1000; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } else if(Serialdata == '2'){ Velocity_Trend = 2000; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } else if(Serialdata == '3'){ Velocity_Trend = 3000; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } else if(Serialdata == '4'){ Velocity_Trend = 4000; TgtVelCtrl(Velocity_Trend); ENsig = SYNC_Enable; Serialdata = 0; } } if(Serialdata == 'q'){ //Haltコマンド送信 ENsig = SYNC_Disable; CtrlWord(QuickStop); Serialdata = 0; } else if(Serialdata == '0'){ decimal = true; ENsig = SYNC_Disable; Serialdata = 0; } else if(Serialdata == 'h'){ //Haltコマンド送信 ENsig = SYNC_Disable; CtrlWord(Halt); Serialdata = 0; } else if(Serialdata == 'm'){ ENsig = SYNC_Disable; ModesOfOperation(); Serialdata = 0; } } //汎用タイマー void flip(void){ if(canPort.read(canmsgRx)){ cnt_run++; if(canmsgRx.id == 0x4a0) printPlantData(); // 左モータの情報を表示 } // 走行4分後に停止 if (cnt_run >= 24000 && run_eternal == false) { //Haltコマンド送信 ENsig = SYNC_Disable; CtrlWord(QuickStop); } } //速度出力値、指令値の表示 void printPlantData(void){ short Current_raw, Velocity_raw; Current_raw = canmsgRx.data[1]*0x100+canmsgRx.data[0]; //指令値の取得(電流) if (Current_raw > 10000) Current_demand = ((Current_raw - 1) ^ 0xffff) * -1; else Current_demand = Current_raw; //Velocity_actual = canmsgRx.data[5]*0x100+canmsgRx.data[4]; //出力値の取得(速度) Velocity_raw = canmsgRx.data[5]*0x100+canmsgRx.data[4]; //if (Velocity_raw > 10000) Velocity_actual = ((Velocity_raw -1) ^ 0xffff) * -1; //else Velocity_actual = Velocity_raw; Velocity_actual = Velocity_raw; EstimateDisterbance(); //外乱の推定 detectNonmagnet(); //非磁性面の判別 pc.printf("%f,%f,%f,%f,%f,%f,%d\r\n",Current_demand,Velocity_actual,Torque_estimate_tf,Distterbance_estimate_tf,baseDist,Dist_error,findnonmagnet); if ( findnonmagnet == 1 && run_eternal == false) { /*cnt_find++; if (cnt_find >= 200) { //Haltコマンド送信 ENsig = SYNC_Disable; CtrlWord(QuickStop); }*/ //ENsig = SYNC_Disable; //CtrlWord(QuickStop); } findnonmagnet = 0; } void EstimateDisterbance(void){ //出力値のトレンド除去 Velocity_estimate = (Velocity_actual-Velocity_Trend)*Kv; //電流値トレンドの計算 Current_Trend = int((Current_Trend + Current_demand)/2); Current_estimate = (Current_demand-Current_Trend)*Km; /* 伝達関数 (tfAz - tfB) tf = ------------ (tfCz - tfD) */ Torque_estimate_tf = (tfA*Velocity_estimate - tfB*Velocity_estimate_z1 + tfD*Torque_estimate_tf)/tfC; Velocity_estimate_z1 = Velocity_estimate; Distterbance_estimate_tf = Torque_estimate_tf - Current_estimate; } void detectNonmagnet(void) { Velocity_meter = Velocity_actual * Kv; //回転数から速度に変換[m/s] distance += Velocity_meter * samplimgrate; //走行距離を計算 Distterbance_estimate_tf_abs = fabsf(Distterbance_estimate_tf); if (distance <= bodylength && distance >= startdetect) { check = 0; //外乱推定値の最大値を確認 if(Distterbance_estimate_tf_abs > baseDist) baseDist = Distterbance_estimate_tf_abs; } else if (distance > bodylength) { Dist_error = Distterbance_estimate_tf_abs / baseDist; if (Dist_error < 0.3) { check = 1; cnt_ditect++; if(cnt_ditect >= 90) { findnonmagnet = 1; } } else { cnt_ditect = 0; } } } //NMT void NMTPreOpn(void){ //COB-ID:0 0x01-00-//-//-//-//-//-// canmsgTx.id = 0x0; canmsgTx.len = 2; canmsgTx.data[0] = 0x80;//0x01:enter NMT state "PreOperational" canmsgTx.data[1] = 0x00;//send All nodes canPort.write(canmsgTx); wait(0.2); } void NMTOpn(void){ //COB-ID:0 0x01-00-//-//-//-//-//-// canmsgTx.id = 0x0; canmsgTx.len = 2; canmsgTx.data[0] = 0x01;//0x01:enter NMT state "Operational" canmsgTx.data[1] = 0x00;//send All nodes canPort.write(canmsgTx); wait(0.2); } //PDO void CtrlWord(int type){ canmsgTx.id = RxPDO1; canmsgTx.len = 2; //Data Length if(type==Halt){ canmsgTx.data[0] = 0x0F;//data:0x01"0F" canmsgTx.data[1] = 0x01;//data:0x"01"0F } else if(type==QuickStop){ canmsgTx.data[0] = 0x0B;//data:0x00"0B" canmsgTx.data[1] = 0x00;//data:0x"00"0B } else if(type==ShutDown){ canmsgTx.data[0] = 0x06;//data:0x00"06" canmsgTx.data[1] = 0x00;//data:0x"00"06 } canPort.write(canmsgTx);//CANでデータ送信 } void ModesOfOperation(void){ canmsgTx.id = RxPDO2; canmsgTx.len = 1; //Data Length canmsgTx.data[0] = 0x03;//data:0x03 = "Profile Velocity Mode" canPort.write(canmsgTx);//CANでデータ送信 } void TgtVelCtrl(int rpm){ //pc.printf("%drpm|0x%08x\r\n",rpm,rpm); canmsgTx.id = RxPDO3; canmsgTx.len = 6; //Data Length //Target Velocity if (rpm > 0) { for(char cnt=0;cnt<4;cnt++){ canmsgTx.data[cnt] = rpm % 256; rpm = rpm / 256; } } else { canmsgTx.data[0] = 0x17; canmsgTx.data[1] = 0xFC; canmsgTx.data[2] = 0xFF; canmsgTx.data[3] = 0xFF; /* rpm = (rpm ^ 0xffffffff) + 1; //printf("%x",rpm); for(char cnt=0;cnt<4;cnt++){ canmsgTx.data[cnt] = rpm & 0x000000FF; //printf("%x",rpm); rpm = rpm >> 2; } */ } //CtrlWord Enable canmsgTx.data[4] = 0x0F;//data:0x00"0F" canmsgTx.data[5] = 0x00;//data:0x"00"0F canPort.write(canmsgTx);//CANでデータ送信 } //SDO void sendCtrlRS(int nodeID){ canmsgTx.id = 0x600+nodeID; canmsgTx.len = 6; //Data Length canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| canmsgTx.data[1] = 0x40;//Index LowByte canmsgTx.data[2] = 0x60;//Index HighByte canmsgTx.data[3] = 0x00;//sub-Index canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Reset)" canmsgTx.data[5] = 0x00;//data:0x"00"80 canPort.write(canmsgTx);//CANでデータ送信 wait(0.2); } void sendCtrlSD(int nodeID){ canmsgTx.id = 0x600+nodeID; canmsgTx.len = 6; //Data Length canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| canmsgTx.data[1] = 0x40;//Index LowByte canmsgTx.data[2] = 0x60;//Index HighByte canmsgTx.data[3] = 0x00;//sub-Index canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)" canmsgTx.data[5] = 0x00;//data:0x"00"06 canPort.write(canmsgTx);//CANでデータ送信 wait(0.2); } void sendCtrlEN(int nodeID){ canmsgTx.id = 0x600+nodeID; canmsgTx.len = 6; //Data Length canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| canmsgTx.data[1] = 0x40;//Index LowByte canmsgTx.data[2] = 0x60;//Index HighByte canmsgTx.data[3] = 0x00;//sub-Index canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)" canmsgTx.data[5] = 0x00;//data:0x"00"0F canPort.write(canmsgTx);//CANでデータ送信 wait(0.2); }