Natsuki Kageyama / Mbed 2 deprecated MATSU-bed_detection

disturbance observer

Dependencies:   mbed MATSUbed USBDevice

main.cpp@8:2ae2483893f0, 2020-12-01 (annotated)

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