Natsuki Kageyama / Mbed 2 deprecated MATSU-bed_detection

disturbance observer

Dependencies:   mbed MATSUbed USBDevice

main.cpp@7:49afd76d633d, 2020-11-10 (annotated)

Committer:
Mtshadow
Date:
Tue Nov 10 08:36:16 2020 +0000
Revision:
7:49afd76d633d
Parent:
6:238d52235a29
Child:
8:2ae2483893f0
EPOS4 of Right and Left prepare change

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