Natsuki Kageyama / Mbed 2 deprecated MATSU-bed_detection

disturbance observer

Dependencies:   mbed MATSUbed USBDevice

main.cpp@9:66b71a3f4ffb, 2021-12-26 (annotated)

Committer:
Mtshadow
Date:
Sun Dec 26 16:17:04 2021 +0000
Revision:
9:66b71a3f4ffb
Parent:
8:2ae2483893f0
Child:
10:21ea2a241025
add low speed for motor

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