Natsuki Kageyama / Mbed 2 deprecated MATSU-bed_detection

disturbance observer

Dependencies:   mbed MATSUbed USBDevice

main.cpp@10:21ea2a241025, 2021-12-27 (annotated)

Committer:
Mtshadow
Date:
Mon Dec 27 06:56:54 2021 +0000
Revision:
10:21ea2a241025
Parent:
9:66b71a3f4ffb 
success detect nonmagnet at horizon

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mtshadow 7:49afd76d633d 1 /*
Mtshadow 7:49afd76d633d 2 外乱推定→走行面の検出を行う
Mtshadow 7:49afd76d633d 3 やること
Mtshadow 7:49afd76d633d 4 ・100Hzで離散化したプラントの逆モデルの実装
Mtshadow 7:49afd76d633d 5 ・推定外乱から磁気吸着力変化の検出
Mtshadow 7:49afd76d633d 6 リビジョン
Mtshadow 7:49afd76d633d 7 mbed 127:25aea2a3f4e3
Mtshadow 7:49afd76d633d 8 */
Mtshadow 7:49afd76d633d 9
hardtail 0:d696cd1aea56 10 #include "mbed.h"
hardtail 1:68364bcdd696 11 #include "USBSerial.h"
Mtshadow 7:49afd76d633d 12 #include "math.h"
Mtshadow 7:49afd76d633d 13
Mtshadow 7:49afd76d633d 14 // Board settings
Mtshadow 7:49afd76d633d 15 #define LED1 P0_29
Mtshadow 7:49afd76d633d 16 #define LED2 P0_28
Mtshadow 7:49afd76d633d 17 #define LED3 P0_27
Mtshadow 7:49afd76d633d 18 #define LED4 P0_26
Mtshadow 7:49afd76d633d 19
Mtshadow 7:49afd76d633d 20 // CANopen settings
Mtshadow 7:49afd76d633d 21 #define RxPDO1 0x220
Mtshadow 7:49afd76d633d 22 #define RxPDO2 0x320
Mtshadow 7:49afd76d633d 23 #define RxPDO3 0x420
Mtshadow 7:49afd76d633d 24 #define Halt 1
Mtshadow 7:49afd76d633d 25 #define QuickStop 2
Mtshadow 7:49afd76d633d 26 #define ShutDown 3
Mtshadow 7:49afd76d633d 27 #define SYNC_Enable 0
Mtshadow 7:49afd76d633d 28 #define SYNC_Disable 1
Mtshadow 7:49afd76d633d 29 #define node 2
Mtshadow 7:49afd76d633d 30
Mtshadow 7:49afd76d633d 31 // Disturbance Observer settings
Mtshadow 9:66b71a3f4ffb 32 #define tfA 193.5
Mtshadow 9:66b71a3f4ffb 33 #define tfB 0.7529
Mtshadow 9:66b71a3f4ffb 34 #define tfC 1
Mtshadow 9:66b71a3f4ffb 35 #define tfD 0.5219
Mtshadow 7:49afd76d633d 36
Mtshadow 7:49afd76d633d 37 // 定数
Mtshadow 7:49afd76d633d 38 #define Kv 0.092421
Mtshadow 7:49afd76d633d 39 #define Km 0.0092
Mtshadow 7:49afd76d633d 40 #define samplimgrate 0.01
Mtshadow 10:21ea2a241025 41 #define startdetect 20.0
Mtshadow 10:21ea2a241025 42 #define enddetect 200.0
Mtshadow 7:49afd76d633d 43 #define Kn 116
Mtshadow 7:49afd76d633d 44
Mtshadow 7:49afd76d633d 45 USBSerial pc; //PCとのシリアル通信
Mtshadow 7:49afd76d633d 46 char Serialdata;
Mtshadow 7:49afd76d633d 47 CANMessage canmsgTx; //送信データ処理用
Mtshadow 8:2ae2483893f0 48 CANMessage canmsgRx; //受信データ処理用
Mtshadow 7:49afd76d633d 49 Ticker flipper; //汎用タイマー
hardtail 0:d696cd1aea56 50
Mtshadow 7:49afd76d633d 51 DigitalOut ENsig(P0_10);
Mtshadow 7:49afd76d633d 52 BusOut myled(LED1, LED2, LED3,LED4);
Mtshadow 7:49afd76d633d 53 CAN canPort(P0_13, P0_18); //CAN name(PinName rd, PinName td)
hardtail 0:d696cd1aea56 54
Mtshadow 7:49afd76d633d 55 //グローバル変数
Mtshadow 7:49afd76d633d 56 //char Serialdata; //シリアル受信データ
Mtshadow 8:2ae2483893f0 57 float Current_demand=0, Velocity_actual=0; //電流入力値，速度出力値
Mtshadow 7:49afd76d633d 58 float Torque_estimate_tf=0; //トルク入力推定値
Mtshadow 7:49afd76d633d 59 float Distterbance_estimate_tf=0; //外乱トルク推定値
Mtshadow 7:49afd76d633d 60 float Velocity_estimate_z1; //推定時保持データ
Mtshadow 7:49afd76d633d 61 float Velocity_estimate=0; //推定時現在速度
Mtshadow 8:2ae2483893f0 62 float Current_estimate=0; //推定時現在電流
Mtshadow 8:2ae2483893f0 63 int Velocity_Trend=0, Current_Trend=0; //除去するトレンド
Mtshadow 7:49afd76d633d 64 float distance = 0; //走行距離[mm]
Mtshadow 7:49afd76d633d 65 float Velocity_meter; // 走行速度[mm/s]
Mtshadow 7:49afd76d633d 66 bool run_eternal = false;
Mtshadow 7:49afd76d633d 67 int check = 0;
Mtshadow 9:66b71a3f4ffb 68 bool decimal = false;
Mtshadow 7:49afd76d633d 69
Mtshadow 10:21ea2a241025 70 // 判別用
Mtshadow 10:21ea2a241025 71 float Distterbance_estimate_tf_abs = 0; // 外乱推定値nの絶対値
Mtshadow 10:21ea2a241025 72 float Dist_error = 0; // 低下割合
Mtshadow 10:21ea2a241025 73 float baseDist = 0; // 判別基準値
Mtshadow 10:21ea2a241025 74 int cnt_ditect = 0; // しきい値を超えてからのカウント
Mtshadow 10:21ea2a241025 75 int cnt_find = 0; // 非磁性面の検出からのカウント
Mtshadow 10:21ea2a241025 76 int findnonmagnet =0; // 非磁性面検出信号
Mtshadow 10:21ea2a241025 77 int cnt_run = 0; // 走行時間カウント
Mtshadow 10:21ea2a241025 78
Mtshadow 7:49afd76d633d 79 //プロトタイプ宣言
Mtshadow 7:49afd76d633d 80 // CANopen関連
Mtshadow 7:49afd76d633d 81 void NMTPreOpn(void); //NMT PreOperationalへ移行
Mtshadow 7:49afd76d633d 82 void NMTOpn(void); //NMT Operationalへ移行
Mtshadow 7:49afd76d633d 83 void CtrlWord(int); //RxPDO1
Mtshadow 7:49afd76d633d 84 void ModesOfOperation(void);//RxPDO2
Mtshadow 7:49afd76d633d 85 void TgtVelCtrl(int); //RxPDO3
Mtshadow 7:49afd76d633d 86 void sendCtrlRS(int); //SDO Reset
Mtshadow 7:49afd76d633d 87 void sendCtrlSD(int); //SDO Shutdown
Mtshadow 7:49afd76d633d 88 void sendCtrlEN(int); //SDO Enable
Mtshadow 7:49afd76d633d 89 //-------------------その他--------------------
Mtshadow 7:49afd76d633d 90 void init(void); //マイコン初期化
Mtshadow 7:49afd76d633d 91 void CANinit(void); //CANノード初期化
Mtshadow 7:49afd76d633d 92 void SerialRX(void); //Serial受信処理
Mtshadow 7:49afd76d633d 93 void flip(void); //タイマー割り込み
Mtshadow 7:49afd76d633d 94 void printPlantData(void); //速度指令値、出力値をPCに表示
Mtshadow 7:49afd76d633d 95 void EstimateDisterbance(void);//指令値を推定
Mtshadow 7:49afd76d633d 96 void detectNonmagnet(void);
hardtail 0:d696cd1aea56 97
Mtshadow 7:49afd76d633d 98 int main(){
Mtshadow 7:49afd76d633d 99 init();
Mtshadow 7:49afd76d633d 100 myled = 0b0101;
Mtshadow 7:49afd76d633d 101 while(1){
Mtshadow 7:49afd76d633d 102 if(Serialdata == 's'){
Mtshadow 7:49afd76d633d 103 run_eternal = false;
Mtshadow 7:49afd76d633d 104 break;
Mtshadow 7:49afd76d633d 105 }else if(Serialdata == 'e'){
Mtshadow 7:49afd76d633d 106 run_eternal = true;
Mtshadow 7:49afd76d633d 107 break;
Mtshadow 7:49afd76d633d 108 }
Mtshadow 7:49afd76d633d 109 myled=~myled;
Mtshadow 7:49afd76d633d 110 wait(1);
Mtshadow 7:49afd76d633d 111 }
Mtshadow 7:49afd76d633d 112 Serialdata = 0;
Mtshadow 7:49afd76d633d 113 //node初期化
Mtshadow 7:49afd76d633d 114 CANinit();
Mtshadow 7:49afd76d633d 115 int count=0;
Mtshadow 7:49afd76d633d 116 myled=0;
Mtshadow 7:49afd76d633d 117 while(1){
Mtshadow 7:49afd76d633d 118 if(!ENsig){
Mtshadow 7:49afd76d633d 119 myled=count;
Mtshadow 7:49afd76d633d 120 count++;
Mtshadow 7:49afd76d633d 121 }
Mtshadow 7:49afd76d633d 122 else{
Mtshadow 7:49afd76d633d 123 myled =~ myled;
Mtshadow 7:49afd76d633d 124 }
Mtshadow 7:49afd76d633d 125 wait(0.5);
Mtshadow 7:49afd76d633d 126 }
Mtshadow 7:49afd76d633d 127 }
Mtshadow 7:49afd76d633d 128 //マイコン初期化
Mtshadow 7:49afd76d633d 129 void init(void){
Mtshadow 7:49afd76d633d 130 //I/O設定
Mtshadow 7:49afd76d633d 131 ENsig=SYNC_Disable;
Mtshadow 7:49afd76d633d 132 //CAN設定
Mtshadow 7:49afd76d633d 133 canPort.frequency(1000000); //Bit Rate:1Mbps
Mtshadow 7:49afd76d633d 134 //割り込み設定
Mtshadow 7:49afd76d633d 135 flipper.attach_us(&flip,5000); //汎用タイマー割り込み周期:5ms(5*10^3 us)
Mtshadow 7:49afd76d633d 136 pc.attach(SerialRX); //Serial受信割り込み開始
Mtshadow 7:49afd76d633d 137 }
Mtshadow 7:49afd76d633d 138 //can node初期化
Mtshadow 7:49afd76d633d 139 void CANinit(void){
Mtshadow 7:49afd76d633d 140 //SDOコマンドで各nodeをリセット
Mtshadow 7:49afd76d633d 141 for(int nodeID=1;nodeID <= node;nodeID++){
Mtshadow 7:49afd76d633d 142 sendCtrlRS(nodeID);
Mtshadow 7:49afd76d633d 143 sendCtrlSD(nodeID);
Mtshadow 7:49afd76d633d 144 sendCtrlEN(nodeID);
Mtshadow 7:49afd76d633d 145 }
Mtshadow 7:49afd76d633d 146 //NMTコマンドで全nodeをオペレーショナルに
Mtshadow 7:49afd76d633d 147 NMTPreOpn();
Mtshadow 7:49afd76d633d 148 NMTOpn();
Mtshadow 7:49afd76d633d 149 }
Mtshadow 7:49afd76d633d 150 //Serial受信割り込み処理
Mtshadow 7:49afd76d633d 151 void SerialRX(void){
Mtshadow 7:49afd76d633d 152 Serialdata = pc.getc();
Mtshadow 7:49afd76d633d 153 //-------------送信コマンドを選択--------------
Mtshadow 9:66b71a3f4ffb 154 if (decimal) {
Mtshadow 9:66b71a3f4ffb 155 if(Serialdata == '1'){
Mtshadow 9:66b71a3f4ffb 156 Velocity_Trend = 100;
Mtshadow 9:66b71a3f4ffb 157 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 158 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 159 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 160 }
Mtshadow 9:66b71a3f4ffb 161 else if(Serialdata == '2'){
Mtshadow 9:66b71a3f4ffb 162 Velocity_Trend = 200;
Mtshadow 9:66b71a3f4ffb 163 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 164 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 165 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 166 }
Mtshadow 9:66b71a3f4ffb 167 else if(Serialdata == '3'){
Mtshadow 9:66b71a3f4ffb 168 Velocity_Trend = 300;
Mtshadow 9:66b71a3f4ffb 169 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 170 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 171 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 172 }
Mtshadow 9:66b71a3f4ffb 173 else if(Serialdata == '4'){
Mtshadow 9:66b71a3f4ffb 174 Velocity_Trend = 400;
Mtshadow 9:66b71a3f4ffb 175 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 176 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 177 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 178 }
Mtshadow 9:66b71a3f4ffb 179 else if(Serialdata == '5'){
Mtshadow 9:66b71a3f4ffb 180 Velocity_Trend = 500;
Mtshadow 9:66b71a3f4ffb 181 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 182 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 183 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 184 }
Mtshadow 9:66b71a3f4ffb 185 else if(Serialdata == '6'){
Mtshadow 9:66b71a3f4ffb 186 Velocity_Trend = 600;
Mtshadow 9:66b71a3f4ffb 187 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 188 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 189 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 190 }
Mtshadow 9:66b71a3f4ffb 191 else if(Serialdata == '7'){
Mtshadow 9:66b71a3f4ffb 192 Velocity_Trend = 700;
Mtshadow 9:66b71a3f4ffb 193 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 194 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 195 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 196 }
Mtshadow 9:66b71a3f4ffb 197 else if(Serialdata == '8'){
Mtshadow 9:66b71a3f4ffb 198 Velocity_Trend = 800;
Mtshadow 9:66b71a3f4ffb 199 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 200 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 201 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 202 }
Mtshadow 9:66b71a3f4ffb 203 else if(Serialdata == '9'){
Mtshadow 9:66b71a3f4ffb 204 Velocity_Trend = 900;
Mtshadow 9:66b71a3f4ffb 205 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 206 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 207 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 208 }
Mtshadow 9:66b71a3f4ffb 209 decimal = false;
Mtshadow 9:66b71a3f4ffb 210 } else {
Mtshadow 9:66b71a3f4ffb 211 if(Serialdata == '1'){
Mtshadow 9:66b71a3f4ffb 212 Velocity_Trend = 1000;
Mtshadow 9:66b71a3f4ffb 213 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 214 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 215 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 216 }
Mtshadow 9:66b71a3f4ffb 217 else if(Serialdata == '2'){
Mtshadow 9:66b71a3f4ffb 218 Velocity_Trend = 2000;
Mtshadow 9:66b71a3f4ffb 219 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 220 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 221 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 222 }
Mtshadow 9:66b71a3f4ffb 223 else if(Serialdata == '3'){
Mtshadow 9:66b71a3f4ffb 224 Velocity_Trend = 3000;
Mtshadow 9:66b71a3f4ffb 225 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 226 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 227 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 228 }
Mtshadow 9:66b71a3f4ffb 229 else if(Serialdata == '4'){
Mtshadow 9:66b71a3f4ffb 230 Velocity_Trend = 4000;
Mtshadow 9:66b71a3f4ffb 231 TgtVelCtrl(Velocity_Trend);
Mtshadow 9:66b71a3f4ffb 232 ENsig = SYNC_Enable;
Mtshadow 9:66b71a3f4ffb 233 Serialdata = 0;
Mtshadow 9:66b71a3f4ffb 234 }
Mtshadow 9:66b71a3f4ffb 235 }
Mtshadow 9:66b71a3f4ffb 236
Mtshadow 7:49afd76d633d 237 if(Serialdata == 'q'){
Mtshadow 7:49afd76d633d 238 //Haltコマンド送信
Mtshadow 7:49afd76d633d 239 ENsig = SYNC_Disable;
Mtshadow 7:49afd76d633d 240 CtrlWord(QuickStop);
Mtshadow 7:49afd76d633d 241 Serialdata = 0;
Mtshadow 7:49afd76d633d 242 }
Mtshadow 9:66b71a3f4ffb 243 else if(Serialdata == '0'){
Mtshadow 9:66b71a3f4ffb 244 decimal = true;
Mtshadow 9:66b71a3f4ffb 245 ENsig = SYNC_Disable;
Mtshadow 7:49afd76d633d 246 Serialdata = 0;
Mtshadow 7:49afd76d633d 247 }
Mtshadow 7:49afd76d633d 248 else if(Serialdata == 'h'){
Mtshadow 7:49afd76d633d 249 //Haltコマンド送信
Mtshadow 7:49afd76d633d 250 ENsig = SYNC_Disable;
Mtshadow 7:49afd76d633d 251 CtrlWord(Halt);
Mtshadow 7:49afd76d633d 252 Serialdata = 0;
Mtshadow 7:49afd76d633d 253 }
Mtshadow 7:49afd76d633d 254 else if(Serialdata == 'm'){
Mtshadow 7:49afd76d633d 255 ENsig = SYNC_Disable;
Mtshadow 7:49afd76d633d 256 ModesOfOperation();
Mtshadow 7:49afd76d633d 257 Serialdata = 0;
Mtshadow 7:49afd76d633d 258 }
hardtail 4:a6882e08058c 259
Mtshadow 7:49afd76d633d 260 }
Mtshadow 7:49afd76d633d 261 //汎用タイマー
Mtshadow 7:49afd76d633d 262 void flip(void){
Mtshadow 8:2ae2483893f0 263 if(canPort.read(canmsgRx)){
Mtshadow 7:49afd76d633d 264 cnt_run++;
Mtshadow 9:66b71a3f4ffb 265 if(canmsgRx.id == 0x4a0) printPlantData(); // 左モータの情報を表示
Mtshadow 7:49afd76d633d 266 }
Mtshadow 9:66b71a3f4ffb 267 // 走行4分後に停止
Mtshadow 9:66b71a3f4ffb 268 if (cnt_run >= 24000 && run_eternal == false) {
Mtshadow 7:49afd76d633d 269 //Haltコマンド送信
Mtshadow 7:49afd76d633d 270 ENsig = SYNC_Disable;
Mtshadow 7:49afd76d633d 271 CtrlWord(QuickStop);
Mtshadow 7:49afd76d633d 272 }
Mtshadow 7:49afd76d633d 273
Mtshadow 7:49afd76d633d 274
Mtshadow 7:49afd76d633d 275
Mtshadow 7:49afd76d633d 276 }
Mtshadow 7:49afd76d633d 277 //速度出力値、指令値の表示
Mtshadow 7:49afd76d633d 278 void printPlantData(void){
Mtshadow 8:2ae2483893f0 279 short Current_raw, Velocity_raw;
Mtshadow 8:2ae2483893f0 280
Mtshadow 8:2ae2483893f0 281 Current_raw = canmsgRx.data[1]*0x100+canmsgRx.data[0]; //指令値の取得(電流)
Mtshadow 8:2ae2483893f0 282 if (Current_raw > 10000) Current_demand = ((Current_raw - 1) ^ 0xffff) * -1;
Mtshadow 8:2ae2483893f0 283 else Current_demand = Current_raw;
Mtshadow 8:2ae2483893f0 284
Mtshadow 8:2ae2483893f0 285 //Velocity_actual = canmsgRx.data[5]*0x100+canmsgRx.data[4]; //出力値の取得(速度)
Mtshadow 8:2ae2483893f0 286 Velocity_raw = canmsgRx.data[5]*0x100+canmsgRx.data[4];
Mtshadow 9:66b71a3f4ffb 287 //if (Velocity_raw > 10000) Velocity_actual = ((Velocity_raw -1) ^ 0xffff) * -1;
Mtshadow 9:66b71a3f4ffb 288 //else Velocity_actual = Velocity_raw;
Mtshadow 9:66b71a3f4ffb 289 Velocity_actual = Velocity_raw;
Mtshadow 7:49afd76d633d 290
Mtshadow 7:49afd76d633d 291 EstimateDisterbance(); //外乱の推定
Mtshadow 7:49afd76d633d 292 detectNonmagnet(); //非磁性面の判別
Mtshadow 8:2ae2483893f0 293 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);
Mtshadow 9:66b71a3f4ffb 294
Mtshadow 10:21ea2a241025 295 // ロボット停止処理
Mtshadow 8:2ae2483893f0 296 if ( findnonmagnet == 1 && run_eternal == false) {
Mtshadow 8:2ae2483893f0 297 /*cnt_find++;
Mtshadow 7:49afd76d633d 298 if (cnt_find >= 200) {
Mtshadow 7:49afd76d633d 299 //Haltコマンド送信
Mtshadow 7:49afd76d633d 300 ENsig = SYNC_Disable;
Mtshadow 7:49afd76d633d 301 CtrlWord(QuickStop);
Mtshadow 8:2ae2483893f0 302 }*/
Mtshadow 10:21ea2a241025 303 ENsig = SYNC_Disable;
Mtshadow 10:21ea2a241025 304 CtrlWord(QuickStop);
Mtshadow 8:2ae2483893f0 305 }
Mtshadow 7:49afd76d633d 306 findnonmagnet = 0;
hardtail 4:a6882e08058c 307 }
hardtail 4:a6882e08058c 308
Mtshadow 7:49afd76d633d 309 void EstimateDisterbance(void){
Mtshadow 7:49afd76d633d 310 //出力値のトレンド除去
Mtshadow 8:2ae2483893f0 311 Velocity_estimate = (Velocity_actual-Velocity_Trend)*Kv;
Mtshadow 7:49afd76d633d 312 //電流値トレンドの計算
Mtshadow 8:2ae2483893f0 313 Current_Trend = int((Current_Trend + Current_demand)/2);
Mtshadow 8:2ae2483893f0 314 Current_estimate = (Current_demand-Current_Trend)*Km;
Mtshadow 7:49afd76d633d 315 /*
Mtshadow 7:49afd76d633d 316 伝達関数
Mtshadow 7:49afd76d633d 317 (tfAz - tfB)
Mtshadow 7:49afd76d633d 318 tf = ------------
Mtshadow 7:49afd76d633d 319 (tfCz - tfD)
Mtshadow 7:49afd76d633d 320 */
Mtshadow 7:49afd76d633d 321 Torque_estimate_tf = (tfA*Velocity_estimate - tfB*Velocity_estimate_z1 + tfD*Torque_estimate_tf)/tfC;
Mtshadow 7:49afd76d633d 322 Velocity_estimate_z1 = Velocity_estimate;
Mtshadow 8:2ae2483893f0 323 Distterbance_estimate_tf = Torque_estimate_tf - Current_estimate;
Mtshadow 7:49afd76d633d 324 }
Mtshadow 7:49afd76d633d 325
Mtshadow 7:49afd76d633d 326 void detectNonmagnet(void) {
Mtshadow 7:49afd76d633d 327 Velocity_meter = Velocity_actual * Kv; //回転数から速度に変換[m/s]
Mtshadow 7:49afd76d633d 328 distance += Velocity_meter * samplimgrate; //走行距離を計算
Mtshadow 7:49afd76d633d 329 Distterbance_estimate_tf_abs = fabsf(Distterbance_estimate_tf);
Mtshadow 10:21ea2a241025 330 if (distance <= enddetect && distance >= startdetect) {
Mtshadow 7:49afd76d633d 331 check = 0;
Mtshadow 7:49afd76d633d 332 //外乱推定値の最大値を確認
Mtshadow 7:49afd76d633d 333 if(Distterbance_estimate_tf_abs > baseDist) baseDist = Distterbance_estimate_tf_abs;
Mtshadow 10:21ea2a241025 334 } else if (distance > enddetect) {
Mtshadow 10:21ea2a241025 335 // 低下割合算出
Mtshadow 7:49afd76d633d 336 Dist_error = Distterbance_estimate_tf_abs / baseDist;
Mtshadow 10:21ea2a241025 337
Mtshadow 10:21ea2a241025 338 // しきい値判断
Mtshadow 10:21ea2a241025 339 if (Dist_error < 0.5) {
Mtshadow 7:49afd76d633d 340 check = 1;
Mtshadow 7:49afd76d633d 341 cnt_ditect++;
Mtshadow 10:21ea2a241025 342 // 低下時間判断
Mtshadow 10:21ea2a241025 343 if(cnt_ditect >= 50) {
Mtshadow 7:49afd76d633d 344 findnonmagnet = 1;
Tiryoh 2:f94af4a543bd 345 }
Mtshadow 7:49afd76d633d 346 } else {
Mtshadow 7:49afd76d633d 347 cnt_ditect = 0;
hardtail 0:d696cd1aea56 348 }
Mtshadow 7:49afd76d633d 349 }
Mtshadow 7:49afd76d633d 350 }
Mtshadow 7:49afd76d633d 351 //NMT
Mtshadow 7:49afd76d633d 352 void NMTPreOpn(void){
Mtshadow 7:49afd76d633d 353 //COB-ID:0 0x01-00-//-//-//-//-//-//
Mtshadow 7:49afd76d633d 354 canmsgTx.id = 0x0;
Mtshadow 7:49afd76d633d 355 canmsgTx.len = 2;
Mtshadow 7:49afd76d633d 356 canmsgTx.data[0] = 0x80;//0x01:enter NMT state "PreOperational"
Mtshadow 7:49afd76d633d 357 canmsgTx.data[1] = 0x00;//send All nodes
Mtshadow 7:49afd76d633d 358 canPort.write(canmsgTx);
Mtshadow 7:49afd76d633d 359 wait(0.2);
Mtshadow 7:49afd76d633d 360 }
Mtshadow 7:49afd76d633d 361 void NMTOpn(void){
Mtshadow 7:49afd76d633d 362 //COB-ID:0 0x01-00-//-//-//-//-//-//
Mtshadow 7:49afd76d633d 363 canmsgTx.id = 0x0;
Mtshadow 7:49afd76d633d 364 canmsgTx.len = 2;
Mtshadow 7:49afd76d633d 365 canmsgTx.data[0] = 0x01;//0x01:enter NMT state "Operational"
Mtshadow 7:49afd76d633d 366 canmsgTx.data[1] = 0x00;//send All nodes
Mtshadow 7:49afd76d633d 367 canPort.write(canmsgTx);
Mtshadow 7:49afd76d633d 368 wait(0.2);
Mtshadow 7:49afd76d633d 369 }
Mtshadow 7:49afd76d633d 370 //PDO
Mtshadow 7:49afd76d633d 371 void CtrlWord(int type){
Mtshadow 7:49afd76d633d 372 canmsgTx.id = RxPDO1;
Mtshadow 7:49afd76d633d 373 canmsgTx.len = 2; //Data Length
Mtshadow 7:49afd76d633d 374 if(type==Halt){
Mtshadow 7:49afd76d633d 375 canmsgTx.data[0] = 0x0F;//data:0x01"0F"
Mtshadow 7:49afd76d633d 376 canmsgTx.data[1] = 0x01;//data:0x"01"0F
Mtshadow 7:49afd76d633d 377 }
Mtshadow 7:49afd76d633d 378 else if(type==QuickStop){
Mtshadow 7:49afd76d633d 379 canmsgTx.data[0] = 0x0B;//data:0x00"0B"
Mtshadow 7:49afd76d633d 380 canmsgTx.data[1] = 0x00;//data:0x"00"0B
Mtshadow 7:49afd76d633d 381 }
Mtshadow 7:49afd76d633d 382 else if(type==ShutDown){
Mtshadow 7:49afd76d633d 383 canmsgTx.data[0] = 0x06;//data:0x00"06"
Mtshadow 7:49afd76d633d 384 canmsgTx.data[1] = 0x00;//data:0x"00"06
hardtail 0:d696cd1aea56 385 }
Mtshadow 7:49afd76d633d 386 canPort.write(canmsgTx);//CANでデータ送信
Tiryoh 2:f94af4a543bd 387 }
Mtshadow 7:49afd76d633d 388 void ModesOfOperation(void){
Mtshadow 7:49afd76d633d 389 canmsgTx.id = RxPDO2;
Mtshadow 7:49afd76d633d 390 canmsgTx.len = 1; //Data Length
Mtshadow 7:49afd76d633d 391 canmsgTx.data[0] = 0x03;//data:0x03 = "Profile Velocity Mode"
Mtshadow 7:49afd76d633d 392 canPort.write(canmsgTx);//CANでデータ送信
Mtshadow 7:49afd76d633d 393 }
Mtshadow 7:49afd76d633d 394 void TgtVelCtrl(int rpm){
Mtshadow 7:49afd76d633d 395 //pc.printf("%drpm|0x%08x\r\n",rpm,rpm);
Mtshadow 7:49afd76d633d 396 canmsgTx.id = RxPDO3;
Mtshadow 7:49afd76d633d 397 canmsgTx.len = 6; //Data Length
Mtshadow 7:49afd76d633d 398 //Target Velocity
Mtshadow 8:2ae2483893f0 399 if (rpm > 0) {
Mtshadow 8:2ae2483893f0 400 for(char cnt=0;cnt<4;cnt++){
Mtshadow 8:2ae2483893f0 401 canmsgTx.data[cnt] = rpm % 256;
Mtshadow 8:2ae2483893f0 402 rpm = rpm / 256;
Mtshadow 8:2ae2483893f0 403 }
Mtshadow 8:2ae2483893f0 404 } else {
Mtshadow 8:2ae2483893f0 405 canmsgTx.data[0] = 0x17;
Mtshadow 8:2ae2483893f0 406 canmsgTx.data[1] = 0xFC;
Mtshadow 8:2ae2483893f0 407 canmsgTx.data[2] = 0xFF;
Mtshadow 8:2ae2483893f0 408 canmsgTx.data[3] = 0xFF;
Mtshadow 8:2ae2483893f0 409 /*
Mtshadow 8:2ae2483893f0 410 rpm = (rpm ^ 0xffffffff) + 1;
Mtshadow 8:2ae2483893f0 411 //printf("%x",rpm);
Mtshadow 8:2ae2483893f0 412 for(char cnt=0;cnt<4;cnt++){
Mtshadow 8:2ae2483893f0 413 canmsgTx.data[cnt] = rpm & 0x000000FF;
Mtshadow 8:2ae2483893f0 414 //printf("%x",rpm);
Mtshadow 8:2ae2483893f0 415 rpm = rpm >> 2;
Mtshadow 8:2ae2483893f0 416 }
Mtshadow 8:2ae2483893f0 417 */
Mtshadow 7:49afd76d633d 418 }
Mtshadow 7:49afd76d633d 419 //CtrlWord Enable
Mtshadow 7:49afd76d633d 420 canmsgTx.data[4] = 0x0F;//data:0x00"0F"
Mtshadow 7:49afd76d633d 421 canmsgTx.data[5] = 0x00;//data:0x"00"0F
Mtshadow 7:49afd76d633d 422 canPort.write(canmsgTx);//CANでデータ送信
Mtshadow 7:49afd76d633d 423 }
Mtshadow 7:49afd76d633d 424 //SDO
Mtshadow 7:49afd76d633d 425 void sendCtrlRS(int nodeID){
Mtshadow 7:49afd76d633d 426 canmsgTx.id = 0x600+nodeID;
Mtshadow 7:49afd76d633d 427 canmsgTx.len = 6; //Data Length
Mtshadow 7:49afd76d633d 428 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
Mtshadow 7:49afd76d633d 429 canmsgTx.data[1] = 0x40;//Index LowByte
Mtshadow 7:49afd76d633d 430 canmsgTx.data[2] = 0x60;//Index HighByte
Mtshadow 7:49afd76d633d 431 canmsgTx.data[3] = 0x00;//sub-Index
Mtshadow 7:49afd76d633d 432 canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Reset)"
Mtshadow 7:49afd76d633d 433 canmsgTx.data[5] = 0x00;//data:0x"00"80
Mtshadow 7:49afd76d633d 434 canPort.write(canmsgTx);//CANでデータ送信
Mtshadow 7:49afd76d633d 435 wait(0.2);
Mtshadow 7:49afd76d633d 436 }
Mtshadow 7:49afd76d633d 437 void sendCtrlSD(int nodeID){
Mtshadow 7:49afd76d633d 438 canmsgTx.id = 0x600+nodeID;
Mtshadow 7:49afd76d633d 439 canmsgTx.len = 6; //Data Length
Mtshadow 7:49afd76d633d 440 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
Mtshadow 7:49afd76d633d 441 canmsgTx.data[1] = 0x40;//Index LowByte
Mtshadow 7:49afd76d633d 442 canmsgTx.data[2] = 0x60;//Index HighByte
Mtshadow 7:49afd76d633d 443 canmsgTx.data[3] = 0x00;//sub-Index
Mtshadow 7:49afd76d633d 444 canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)"
Mtshadow 7:49afd76d633d 445 canmsgTx.data[5] = 0x00;//data:0x"00"06
Mtshadow 7:49afd76d633d 446 canPort.write(canmsgTx);//CANでデータ送信
Mtshadow 7:49afd76d633d 447 wait(0.2);
Mtshadow 7:49afd76d633d 448 }
Mtshadow 7:49afd76d633d 449 void sendCtrlEN(int nodeID){
Mtshadow 7:49afd76d633d 450 canmsgTx.id = 0x600+nodeID;
Mtshadow 7:49afd76d633d 451 canmsgTx.len = 6; //Data Length
Mtshadow 7:49afd76d633d 452 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
Mtshadow 7:49afd76d633d 453 canmsgTx.data[1] = 0x40;//Index LowByte
Mtshadow 7:49afd76d633d 454 canmsgTx.data[2] = 0x60;//Index HighByte
Mtshadow 7:49afd76d633d 455 canmsgTx.data[3] = 0x00;//sub-Index
Mtshadow 7:49afd76d633d 456 canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)"
Mtshadow 7:49afd76d633d 457 canmsgTx.data[5] = 0x00;//data:0x"00"0F
Mtshadow 7:49afd76d633d 458 canPort.write(canmsgTx);//CANでデータ送信
Mtshadow 7:49afd76d633d 459 wait(0.2);
Mtshadow 7:49afd76d633d 460 }