Natsuki Kageyama
disturbance observer
Dependencies: mbed MATSUbed USBDevice
Diff: main.cpp
- Revision:
- 8:2ae2483893f0
- Parent:
- 7:49afd76d633d
- Child:
- 9:66b71a3f4ffb
--- a/main.cpp Tue Nov 10 08:36:16 2020 +0000
+++ b/main.cpp Tue Dec 01 06:10:15 2020 +0000
@@ -45,8 +45,7 @@
USBSerial pc; //PCとのシリアル通信
char Serialdata;
CANMessage canmsgTx; //送信データ処理用
-CANMessage canmsgRxR; //受信データ処理用
-CANMessage canmsgRxL; //受信データ処理用
+CANMessage canmsgRx; //受信データ処理用
Ticker flipper; //汎用タイマー
DigitalOut ENsig(P0_10);
@@ -55,13 +54,13 @@
//グローバル変数
//char Serialdata; //シリアル受信データ
-float Current_demand_L=0, Current_demand_R=0, Velocity_actual=0; //電流入力値，速度出力値
+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_R=0, Current_estimate_L=0; //推定時現在電流
-int Velocity_Trend=0, Current_Trend_R=0, Current_Trend_L=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の絶対値
@@ -71,9 +70,6 @@
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;
@@ -159,7 +155,7 @@
Serialdata = 0;
}
else if(Serialdata == '1'){
- Velocity_Trend = 1000;
+ Velocity_Trend = -1000;
TgtVelCtrl(Velocity_Trend);
ENsig = SYNC_Enable;
Serialdata = 0;
@@ -197,39 +193,9 @@
}
//汎用タイマー
void flip(void){
- short Current_raw_R, Current_raw_L;
-
- if (ENsig == SYNC_Enable) {
+ if(canPort.read(canmsgRx)){
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();
-
- //}
- }
+ if(canmsgRx.id == 0x4a0) printPlantData();
}
// 走行2分後に停止
if (cnt_run >= 12000 && run_eternal == false) {
@@ -243,28 +209,39 @@
}
//速度出力値、指令値の表示
void printPlantData(void){
- Velocity_actual = canmsgRxR.data[5]*0x100+canmsgRxR.data[4]; //出力値の取得(速度)
+ 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;
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++;
+ 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)/Kn;
+ Velocity_estimate = (Velocity_actual-Velocity_Trend)*Kv;
//電流値トレンドの計算
- Current_Trend_R = int((Current_Trend_R + Current_demand_R)/2);
- Current_estimate_R = (Current_demand_R-Current_Trend_R)*Km;
+ Current_Trend = int((Current_Trend + Current_demand)/2);
+ Current_estimate = (Current_demand-Current_Trend)*Km;
/*
伝達関数
(tfAz - tfB)
@@ -273,7 +250,7 @@
*/
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;
+ Distterbance_estimate_tf = Torque_estimate_tf - Current_estimate;
}
void detectNonmagnet(void) {
@@ -289,7 +266,7 @@
if (Dist_error < 0.3) {
check = 1;
cnt_ditect++;
- if(cnt_ditect >= 150) {
+ if(cnt_ditect >= 90) {
findnonmagnet = 1;
}
} else {
@@ -346,9 +323,25 @@
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;
+ 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"