Natsuki Kageyama
disturbance observer
Dependencies: mbed MATSUbed USBDevice
Diff: main.cpp
- Revision:
- 7:49afd76d633d
- Parent:
- 6:238d52235a29
- Child:
- 8:2ae2483893f0
diff -r 238d52235a29 -r 49afd76d633d main.cpp
--- a/main.cpp Sun Dec 02 18:08:02 2018 +0000
+++ b/main.cpp Tue Nov 10 08:36:16 2020 +0000
@@ -1,38 +1,394 @@
+/*
+外乱推定→走行面の検出を行う
+やること
+・100Hzで離散化したプラントの逆モデルの実装
+・推定外乱から磁気吸着力変化の検出
+リビジョン
+mbed 127:25aea2a3f4e3
+*/
+
#include "mbed.h"
#include "USBSerial.h"
-#include "MATSUbed.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 0.9981
+#define tfB 0.7529
+#define tfC 0.002457
+#define tfD 0.000004588
+
+// 定数
+#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 canmsgRxR; //受信データ処理用
+CANMessage canmsgRxL; //受信データ処理用
+Ticker flipper; //汎用タイマー
-USBSerial pc(0x1f00,0x2012, 0x0001,false);
+DigitalOut ENsig(P0_10);
+BusOut myled(LED1, LED2, LED3,LED4);
+CAN canPort(P0_13, P0_18); //CAN name(PinName rd, PinName td)
-BusOut myled(LED1, LED2, LED3, LED4);
+//グローバル変数
+//char Serialdata; //シリアル受信データ
+float Current_demand_L=0, Current_demand_R=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_R=0, Current_estimate_L=0; //推定時現在電流
+int Velocity_Trend=0, Current_Trend_R=0, Current_Trend_L=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; // 走行時間カウント
+int beforecnt = 0;
+short firstid, secondid;
+short beforeID = 0x4a1;
+bool run_eternal = false;
+int check = 0;
+
+//プロトタイプ宣言
+// 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);
-//DigitalIn sw1(ISP0);
-//DigitalIn sw2(ISP1);
-
-InterruptIn USB_ISP_bt(ISP0);
+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(Serialdata == 'q'){
+ //Haltコマンド送信
+ ENsig = SYNC_Disable;
+ CtrlWord(QuickStop);
+ Serialdata = 0;
+ }
+ 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;
+ }
+ else if(Serialdata == 'h'){
+ //Haltコマンド送信
+ ENsig = SYNC_Disable;
+ CtrlWord(Halt);
+ Serialdata = 0;
+ }
+ else if(Serialdata == 'm'){
+ ENsig = SYNC_Disable;
+ ModesOfOperation();
+ Serialdata = 0;
+ }
-// ISP0ボタンを押すとUSB ISPモードに入る設定
-void enter_USB_ISP(){
- pc.printf("enter ISP\n");
- NVIC_SystemReset();
+}
+//汎用タイマー
+void flip(void){
+ short Current_raw_R, Current_raw_L;
+
+ if (ENsig == SYNC_Enable) {
+ cnt_run++;
+ if(canPort.read(canmsgRxR)){
+ if(canmsgRxR.id == 0x4a0) {
+ pc.printf("%d\r\n",cnt_run-beforecnt);
+ //pc.printf("%x ",canmsgRxR.id);
+ beforecnt = cnt_run;
+ }
+ /*
+ if(canPort.read(canmsgRxL)){
+ if(canmsgRxL.id == 0x4a1) {
+ pc.printf("%x\r\n",canmsgRxL.id);
+ }
+ }*/
+ /*
+ Current_raw_L = canmsgRx.data[1]*0x100+canmsgRx.data[0]; //指令値の取得(電流)
+ if (Current_raw_L > 10000) Current_demand_L = ((Current_raw_L - 1) ^ 0xffff) * -1;
+ else Current_demand_L = Current_raw_L;
+ */
+ //if(canPort.read(canmsgRx)){
+ //if(canmsgRx.id == 0x4a1) {
+ //pc.printf("%x\r\n",canmsgRx.id);
+ /*
+ Current_raw_R = canmsgRx.data[1]*0x100+canmsgRx.data[0]; //指令値の取得(電流)
+ if (Current_raw_R > 10000) Current_demand_R = ((Current_raw_R - 1) ^ 0xffff) * -1;
+ else Current_demand_R = Current_raw_R;
+ */
+ //printPlantData();
+
+ //}
+ }
+ }
+ // 走行2分後に停止
+ if (cnt_run >= 12000 && run_eternal == false) {
+ //Haltコマンド送信
+ ENsig = SYNC_Disable;
+ CtrlWord(QuickStop);
+ }
+
+
+
+}
+//速度出力値、指令値の表示
+void printPlantData(void){
+ Velocity_actual = canmsgRxR.data[5]*0x100+canmsgRxR.data[4]; //出力値の取得(速度)
+
+ EstimateDisterbance(); //外乱の推定
+ detectNonmagnet(); //非磁性面の判別
+ pc.printf("%f,%f,%f,%f,%f,%f,%d\r\n",Current_demand_L,Current_demand_R,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);
+ }
+ }*/
+ findnonmagnet = 0;
}
-int main() {
- USB_ISP_bt.mode(PullUp);
- USB_ISP_bt.fall(&enter_USB_ISP);
-
- myled = 0b0001;
-
- while(1){
- if(myled < 0b1000){
- wait(0.5);
- myled = myled << 1;
- }else{
- while(myled != 0b0001) {
- wait(0.5);
- myled = myled >> 1;
+void EstimateDisterbance(void){
+ //出力値のトレンド除去
+ Velocity_estimate = (Velocity_actual-Velocity_Trend)/Kn;
+ //電流値トレンドの計算
+ Current_Trend_R = int((Current_Trend_R + Current_demand_R)/2);
+ Current_estimate_R = (Current_demand_R-Current_Trend_R)*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_R;
+}
+
+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 >= 150) {
+ findnonmagnet = 1;
}
+ } else {
+ cnt_ditect = 0;
}
- pc.printf("hello world \r\n");
+ }
+
+}
+//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
+ for(char cnt=0;cnt<4;cnt++){
+ canmsgTx.data[cnt] = rpm % 256;
+ rpm = rpm / 256;
+ }
+ //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);
+}