Kanta Takasu / Mbed 2 deprecated YUKA_tks

yuka_tks

Dependencies:   mbed HCSR04

main.cpp@1:b2bd1511307e, 2021-12-19 (annotated)

Committer:
Tomo1213
Date:
Sun Dec 19 09:10:50 2021 +0000
Revision:
1:b2bd1511307e
Parent:
0:b4b94eb28093
Child:
2:93d72af1b94c
kansuu sakusei

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ngokystk 0:b4b94eb28093 1 //2021/12/18更新
ngokystk 0:b4b94eb28093 2 //YUKA本番機用プログラム
ngokystk 0:b4b94eb28093 3 //入力切替確認済み
ngokystk 0:b4b94eb28093 4 //一定時間の入力なしで動作切替
ngokystk 0:b4b94eb28093 5 //ジグザグ動作実装前
ngokystk 0:b4b94eb28093 6 //エンコーダ読み取りによる入力
ngokystk 0:b4b94eb28093 7 //加減速度調整パラメータ実装
ngokystk 0:b4b94eb28093 8 //PID制御による機体角度補正_
Tomo1213 1:b2bd1511307e 9 //aoki
Tomo1213 1:b2bd1511307e 10
ngokystk 0:b4b94eb28093 11 #include "mbed.h"
ngokystk 0:b4b94eb28093 12 #include "hcsr04.h"
ngokystk 0:b4b94eb28093 13
Tomo1213 1:b2bd1511307e 14 #define ACC_B 1000
Tomo1213 1:b2bd1511307e 15 #define DEC_B 700
Tomo1213 1:b2bd1511307e 16 #define ACC_C 1000
Tomo1213 1:b2bd1511307e 17 #define DEC_C 1000
ngokystk 0:b4b94eb28093 18
Tomo1213 1:b2bd1511307e 19 #define KICK 2000
Tomo1213 1:b2bd1511307e 20
Tomo1213 1:b2bd1511307e 21
Tomo1213 1:b2bd1511307e 22
ngokystk 0:b4b94eb28093 23 DigitalOut led1(LED1);
ngokystk 0:b4b94eb28093 24 DigitalOut led2(LED2);
ngokystk 0:b4b94eb28093 25 DigitalOut led3(LED3);
ngokystk 0:b4b94eb28093 26 DigitalOut led4(LED4);
Tomo1213 1:b2bd1511307e 27
ngokystk 0:b4b94eb28093 28 Timer t;
ngokystk 0:b4b94eb28093 29 double Time = 0;
ngokystk 0:b4b94eb28093 30 #define Standby_Time 10
ngokystk 0:b4b94eb28093 31 Serial pc(USBTX, USBRX);
ngokystk 0:b4b94eb28093 32 char Serialdata;
ngokystk 0:b4b94eb28093 33 BusOut myled(LED1, LED2, LED3, LED4);
Tomo1213 1:b2bd1511307e 34
ngokystk 0:b4b94eb28093 35 //HCSR04 u1(p13, p14), u2(p11, p12), u3(p23, p24), u4(p25, p26); // Trigger(DO), Echo(PWMIN); LPC1768
ngokystk 0:b4b94eb28093 36 HCSR04 u1(PA_0, PA_1),u2(PA_5, PA_6),u3(PB_0, PA_10); // Trigger(DO), Echo(PWMIN); f303k8
Tomo1213 1:b2bd1511307e 37
ngokystk 0:b4b94eb28093 38 CANMessage canmsgTx;
ngokystk 0:b4b94eb28093 39 CANMessage canmsgRx;
ngokystk 0:b4b94eb28093 40 //CAN canPort(p30, p29); //CAN name(PinName rd, PinName td) LPC1768
ngokystk 0:b4b94eb28093 41 CAN canPort(PA_11, PA_12); //CAN name(PinName rd, PinName td) F303k8
Tomo1213 1:b2bd1511307e 42
ngokystk 0:b4b94eb28093 43 //プロトタイプ宣言
ngokystk 0:b4b94eb28093 44 //------------------send関数-------------------
ngokystk 0:b4b94eb28093 45 //mode Setting
ngokystk 0:b4b94eb28093 46 void sendOPModeT(int); //Operating Mode
ngokystk 0:b4b94eb28093 47 void sendOPModeV(int); //Operating Mode
ngokystk 0:b4b94eb28093 48 //Control Word
ngokystk 0:b4b94eb28093 49 void sendCtrlRS(int); //Reset
ngokystk 0:b4b94eb28093 50 void sendCtrlSD(int); //Shutdown
ngokystk 0:b4b94eb28093 51 void sendCtrlEN(int); //Switch on & Enable
ngokystk 0:b4b94eb28093 52 void sendCtrlQS(int); //Quick Stop
ngokystk 0:b4b94eb28093 53 void sendCtrlHL(int); //Halt
ngokystk 0:b4b94eb28093 54 //Velocity Setting
ngokystk 0:b4b94eb28093 55 void sendTgtVel(int,int); //Target Velocity
ngokystk 0:b4b94eb28093 56 //Torque Setting
ngokystk 0:b4b94eb28093 57 void sendTgtTrq(int,int); //Target Torque
ngokystk 0:b4b94eb28093 58 //Acceleration Setting
ngokystk 0:b4b94eb28093 59 void sendProAcc(int,int); //Plof Acceleration
ngokystk 0:b4b94eb28093 60 void sendProDec(int,int); //Plof Deceleration
ngokystk 0:b4b94eb28093 61 //------------------read関数-------------------
ngokystk 0:b4b94eb28093 62 void readActVel(int); //Actual Velocity
ngokystk 0:b4b94eb28093 63 void readActPos(int); //Actual Position
ngokystk 0:b4b94eb28093 64 //-------------------その他--------------------
ngokystk 0:b4b94eb28093 65 void printCANTX(void); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 66 void printCANRX(void); //CAN受信データをPCに表示
ngokystk 0:b4b94eb28093 67 void CANdataRX(void); //CAN受信処理
ngokystk 0:b4b94eb28093 68 void SerialRX(void); //Serial受信処理
Tomo1213 1:b2bd1511307e 69 //---------------------------------------------
Tomo1213 1:b2bd1511307e 70 void set_ACC(int);
Tomo1213 1:b2bd1511307e 71 void set_DEC(int);
Tomo1213 1:b2bd1511307e 72 void set_MODE_V(void);
Tomo1213 1:b2bd1511307e 73 void set_MODE_T(void);
ngokystk 0:b4b94eb28093 74
ngokystk 0:b4b94eb28093 75 int nodeall=4;
Tomo1213 1:b2bd1511307e 76
ngokystk 0:b4b94eb28093 77 int main(){
ngokystk 0:b4b94eb28093 78 //Serial
ngokystk 0:b4b94eb28093 79 pc.attach(SerialRX);
ngokystk 0:b4b94eb28093 80 //pc.baud(115200);
ngokystk 0:b4b94eb28093 81
ngokystk 0:b4b94eb28093 82 //CAN
ngokystk 0:b4b94eb28093 83 canPort.frequency(1000000); //Bit Rate:1MHz
ngokystk 0:b4b94eb28093 84 canPort.attach(CANdataRX,CAN::RxIrq);
ngokystk 0:b4b94eb28093 85 int node1 = 1; //CAN node Setting
ngokystk 0:b4b94eb28093 86 int node2 = 2;
ngokystk 0:b4b94eb28093 87 int node3 = 3;
ngokystk 0:b4b94eb28093 88 int node4 = 4;
ngokystk 0:b4b94eb28093 89
ngokystk 0:b4b94eb28093 90 //モータ回転数
ngokystk 0:b4b94eb28093 91 int rpm = 600;
ngokystk 0:b4b94eb28093 92
ngokystk 0:b4b94eb28093 93 //各モータ回転数
ngokystk 0:b4b94eb28093 94 int rpm1 = rpm; //Velocity Setting[rpm]
ngokystk 0:b4b94eb28093 95 int rpm2 = rpm; //Velocity Setting[rpm]
ngokystk 0:b4b94eb28093 96 int rpm3 = rpm; //Velocity Setting[rpm]
ngokystk 0:b4b94eb28093 97 int rpm4 = rpm; //Velocity Setting[rpm]
ngokystk 0:b4b94eb28093 98
ngokystk 0:b4b94eb28093 99 //エンコーダ関係
ngokystk 0:b4b94eb28093 100 int ActPos = 0;
ngokystk 0:b4b94eb28093 101 int Init_Pos = 0;
ngokystk 0:b4b94eb28093 102
ngokystk 0:b4b94eb28093 103 //超音波センサ関係パラメータ
ngokystk 0:b4b94eb28093 104 int max_rpm = rpm + 200;
ngokystk 0:b4b94eb28093 105 int min_rpm = rpm -200;
ngokystk 0:b4b94eb28093 106 int wall_distance = 0;
ngokystk 0:b4b94eb28093 107 int dist1, dist2,dist3, dist4;
ngokystk 0:b4b94eb28093 108
ngokystk 0:b4b94eb28093 109
ngokystk 0:b4b94eb28093 110 //PID制御関係
ngokystk 0:b4b94eb28093 111 //角度調整パラメータ
ngokystk 0:b4b94eb28093 112 int p1, i1, d1;
ngokystk 0:b4b94eb28093 113 int error_before1 = 0;
ngokystk 0:b4b94eb28093 114 int error_now1 = 0;
ngokystk 0:b4b94eb28093 115 int feedback_val1;
ngokystk 0:b4b94eb28093 116 int integral1;
ngokystk 0:b4b94eb28093 117 int pid_sum1;
ngokystk 0:b4b94eb28093 118 #define DELTA_T1 0.1
ngokystk 0:b4b94eb28093 119 #define target_val1 0
ngokystk 0:b4b94eb28093 120 #define Kp1 3
ngokystk 0:b4b94eb28093 121 #define Ki1 0
ngokystk 0:b4b94eb28093 122 #define Kd1 0
Tomo1213 1:b2bd1511307e 123
ngokystk 0:b4b94eb28093 124 pc.printf("YUKA PROGRAM START\r\n");
ngokystk 0:b4b94eb28093 125 wait(0.1);
ngokystk 0:b4b94eb28093 126 //-------------起動時に必ず送信---------------
ngokystk 0:b4b94eb28093 127 //オペレーティングモードを送信
ngokystk 0:b4b94eb28093 128 sendOPModeT(node1);
ngokystk 0:b4b94eb28093 129 sendOPModeT(node2);
ngokystk 0:b4b94eb28093 130 sendOPModeT(node3);
ngokystk 0:b4b94eb28093 131 sendOPModeT(node4);
ngokystk 0:b4b94eb28093 132
ngokystk 0:b4b94eb28093 133 //Shutdown,Enableコマンド送信｜リセット
ngokystk 0:b4b94eb28093 134 sendCtrlSD(node1);
ngokystk 0:b4b94eb28093 135 sendCtrlSD(node2);
ngokystk 0:b4b94eb28093 136 sendCtrlSD(node3);
ngokystk 0:b4b94eb28093 137 sendCtrlSD(node4);
Tomo1213 1:b2bd1511307e 138
ngokystk 0:b4b94eb28093 139 sendCtrlEN(node1);
ngokystk 0:b4b94eb28093 140 sendCtrlEN(node2);
ngokystk 0:b4b94eb28093 141 sendCtrlEN(node3);
ngokystk 0:b4b94eb28093 142 sendCtrlEN(node4);
ngokystk 0:b4b94eb28093 143
ngokystk 0:b4b94eb28093 144 //初期加減速度
ngokystk 0:b4b94eb28093 145 int Acc = 2000;
ngokystk 0:b4b94eb28093 146 int Dec = 2000;
ngokystk 0:b4b94eb28093 147
ngokystk 0:b4b94eb28093 148 sendProAcc(1,Acc);
ngokystk 0:b4b94eb28093 149 sendProAcc(2,Acc);
ngokystk 0:b4b94eb28093 150 sendProAcc(3,Acc);
ngokystk 0:b4b94eb28093 151 sendProAcc(4,Acc);
ngokystk 0:b4b94eb28093 152
ngokystk 0:b4b94eb28093 153 sendProDec(1,Dec);
ngokystk 0:b4b94eb28093 154 sendProDec(2,Dec);
ngokystk 0:b4b94eb28093 155 sendProDec(3,Dec);
ngokystk 0:b4b94eb28093 156 sendProDec(4,Dec);
Tomo1213 1:b2bd1511307e 157
ngokystk 0:b4b94eb28093 158 //トルク設定
ngokystk 0:b4b94eb28093 159 int trq = 100; //torque Setting[mA]
ngokystk 0:b4b94eb28093 160
ngokystk 0:b4b94eb28093 161 sendTgtTrq(1,trq);
ngokystk 0:b4b94eb28093 162 sendTgtTrq(2,trq);
ngokystk 0:b4b94eb28093 163 sendTgtTrq(3,trq);
ngokystk 0:b4b94eb28093 164 sendTgtTrq(4,trq);
Tomo1213 1:b2bd1511307e 165
Tomo1213 1:b2bd1511307e 166 while(1){
Tomo1213 1:b2bd1511307e 167
Tomo1213 1:b2bd1511307e 168 ActPos = 0;
Tomo1213 1:b2bd1511307e 169 Init_Pos = 0;
Tomo1213 1:b2bd1511307e 170 readActPos(1);
Tomo1213 1:b2bd1511307e 171 ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
Tomo1213 1:b2bd1511307e 172 if(ActPos > 8388608){
Tomo1213 1:b2bd1511307e 173 ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
ngokystk 0:b4b94eb28093 174 }
Tomo1213 1:b2bd1511307e 175 Init_Pos = ActPos;//起動時の角度を保存
ngokystk 0:b4b94eb28093 176 t.reset();
ngokystk 0:b4b94eb28093 177 t.start();
Tomo1213 1:b2bd1511307e 178
ngokystk 0:b4b94eb28093 179 while(1){
ngokystk 0:b4b94eb28093 180
Tomo1213 1:b2bd1511307e 181 Time = t.read();
ngokystk 0:b4b94eb28093 182
Tomo1213 1:b2bd1511307e 183 readActPos(1);
Tomo1213 1:b2bd1511307e 184 ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
Tomo1213 1:b2bd1511307e 185 if(ActPos > 8388608){
Tomo1213 1:b2bd1511307e 186 ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
ngokystk 0:b4b94eb28093 187 //printf("check\r\n");
Tomo1213 1:b2bd1511307e 188 }
Tomo1213 1:b2bd1511307e 189 printf("dsit1 %d\r\n",dist1);
Tomo1213 1:b2bd1511307e 190 /*--------------------------*/
Tomo1213 1:b2bd1511307e 191 //前方向に入力があった時
Tomo1213 1:b2bd1511307e 192 if(ActPos < Init_Pos - KICK){
ngokystk 0:b4b94eb28093 193
Tomo1213 1:b2bd1511307e 194 set_ACC(ACC_B);
Tomo1213 1:b2bd1511307e 195 set_DEC(ACC_C);
Tomo1213 1:b2bd1511307e 196
Tomo1213 1:b2bd1511307e 197 //速度制御モード送信
Tomo1213 1:b2bd1511307e 198 set_MODE_V();
ngokystk 0:b4b94eb28093 199
Tomo1213 1:b2bd1511307e 200 while(1){
ngokystk 0:b4b94eb28093 201
Tomo1213 1:b2bd1511307e 202 u1.start();
Tomo1213 1:b2bd1511307e 203 u2.start();
Tomo1213 1:b2bd1511307e 204 dist1 = u1.get_dist_cm();
Tomo1213 1:b2bd1511307e 205 dist2 = u2.get_dist_cm();
ngokystk 0:b4b94eb28093 206
Tomo1213 1:b2bd1511307e 207 //速度を指定
Tomo1213 1:b2bd1511307e 208 sendTgtVel(1,rpm*(-1));
Tomo1213 1:b2bd1511307e 209 sendTgtVel(2,rpm*(-1));
Tomo1213 1:b2bd1511307e 210 sendTgtVel(3,rpm);
Tomo1213 1:b2bd1511307e 211 sendTgtVel(4,rpm);
Tomo1213 1:b2bd1511307e 212 //指令値を送信
Tomo1213 1:b2bd1511307e 213 for(int i=1;i<= 4;i++){
Tomo1213 1:b2bd1511307e 214 sendCtrlEN(i);
Tomo1213 1:b2bd1511307e 215 }
ngokystk 0:b4b94eb28093 216
Tomo1213 1:b2bd1511307e 217 if(dist1 < 100 && dist1 >= 70){
Tomo1213 1:b2bd1511307e 218 break;
Tomo1213 1:b2bd1511307e 219 }
Tomo1213 1:b2bd1511307e 220 printf("dsit1 %d\r\n",dist1);
Tomo1213 1:b2bd1511307e 221 }
ngokystk 0:b4b94eb28093 222
Tomo1213 1:b2bd1511307e 223 //速度を指定
Tomo1213 1:b2bd1511307e 224 sendTgtVel(1,0);
Tomo1213 1:b2bd1511307e 225 sendTgtVel(2,0);
Tomo1213 1:b2bd1511307e 226 sendTgtVel(3,0);
Tomo1213 1:b2bd1511307e 227 sendTgtVel(4,0);
Tomo1213 1:b2bd1511307e 228 //指令値を送信
Tomo1213 1:b2bd1511307e 229 for(int i=1;i<= 4;i++){
Tomo1213 1:b2bd1511307e 230 sendCtrlEN(i);
Tomo1213 1:b2bd1511307e 231 }
Tomo1213 1:b2bd1511307e 232 //実行時間
Tomo1213 1:b2bd1511307e 233 printf("stop!!\r\n");
Tomo1213 1:b2bd1511307e 234 wait(5.0);
Tomo1213 1:b2bd1511307e 235
Tomo1213 1:b2bd1511307e 236 //printf("2\r\n");
Tomo1213 1:b2bd1511307e 237
Tomo1213 1:b2bd1511307e 238 //速度を指定
Tomo1213 1:b2bd1511307e 239
Tomo1213 1:b2bd1511307e 240 while(1){
Tomo1213 1:b2bd1511307e 241 readActPos(1);
Tomo1213 1:b2bd1511307e 242 ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
Tomo1213 1:b2bd1511307e 243 if(ActPos > 8388608){
Tomo1213 1:b2bd1511307e 244 ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
Tomo1213 1:b2bd1511307e 245 //printf("check\r\n");
Tomo1213 1:b2bd1511307e 246 }
ngokystk 0:b4b94eb28093 247
Tomo1213 1:b2bd1511307e 248 if(ActPos > -10000){
Tomo1213 1:b2bd1511307e 249 break;
Tomo1213 1:b2bd1511307e 250 }
Tomo1213 1:b2bd1511307e 251
Tomo1213 1:b2bd1511307e 252 sendTgtVel(1,rpm);
Tomo1213 1:b2bd1511307e 253 sendTgtVel(2,rpm);
Tomo1213 1:b2bd1511307e 254 sendTgtVel(3,rpm*(-1));
Tomo1213 1:b2bd1511307e 255 sendTgtVel(4,rpm*(-1));
Tomo1213 1:b2bd1511307e 256 //指令値を送信
Tomo1213 1:b2bd1511307e 257 for(int i=1;i<= 4;i++){
Tomo1213 1:b2bd1511307e 258 sendCtrlEN(i);
Tomo1213 1:b2bd1511307e 259 }
Tomo1213 1:b2bd1511307e 260 //printf("3\r\n");
Tomo1213 1:b2bd1511307e 261 }
Tomo1213 1:b2bd1511307e 262
Tomo1213 1:b2bd1511307e 263 //ブレーキ
Tomo1213 1:b2bd1511307e 264 sendTgtVel(1,0);
Tomo1213 1:b2bd1511307e 265 sendTgtVel(2,0);
Tomo1213 1:b2bd1511307e 266 sendTgtVel(3,0);
Tomo1213 1:b2bd1511307e 267 sendTgtVel(4,0);
Tomo1213 1:b2bd1511307e 268 for(int i=1;i<= 4;i++){
Tomo1213 1:b2bd1511307e 269 sendCtrlEN(i);
Tomo1213 1:b2bd1511307e 270 }
ngokystk 0:b4b94eb28093 271
Tomo1213 1:b2bd1511307e 272 wait(0.5);
Tomo1213 1:b2bd1511307e 273
Tomo1213 1:b2bd1511307e 274 set_MODE_T();
ngokystk 0:b4b94eb28093 275
Tomo1213 1:b2bd1511307e 276 dist1 = 0;
Tomo1213 1:b2bd1511307e 277
Tomo1213 1:b2bd1511307e 278 wait(3.0);
Tomo1213 1:b2bd1511307e 279 break;
Tomo1213 1:b2bd1511307e 280 }
ngokystk 0:b4b94eb28093 281
ngokystk 0:b4b94eb28093 282 /*--------------------------*/
ngokystk 0:b4b94eb28093 283
ngokystk 0:b4b94eb28093 284 /*--------------------------*/
ngokystk 0:b4b94eb28093 285 //右方向に入力があった時
Tomo1213 1:b2bd1511307e 286 if(ActPos > Init_Pos + 2000){
Tomo1213 1:b2bd1511307e 287 set_ACC(ACC_B);
Tomo1213 1:b2bd1511307e 288 set_DEC(ACC_C);
Tomo1213 1:b2bd1511307e 289
Tomo1213 1:b2bd1511307e 290 //速度制御モード送信
Tomo1213 1:b2bd1511307e 291 set_MODE_V();
ngokystk 0:b4b94eb28093 292
Tomo1213 1:b2bd1511307e 293 //速度を指定
Tomo1213 1:b2bd1511307e 294 sendTgtVel(1,rpm);
Tomo1213 1:b2bd1511307e 295 sendTgtVel(2,rpm*(-1));
Tomo1213 1:b2bd1511307e 296 sendTgtVel(3,rpm*(-1));
Tomo1213 1:b2bd1511307e 297 sendTgtVel(4,rpm);
Tomo1213 1:b2bd1511307e 298 //指令値を送信
Tomo1213 1:b2bd1511307e 299 for(int i=1;i<= 4;i++){
Tomo1213 1:b2bd1511307e 300 sendCtrlEN(i);
Tomo1213 1:b2bd1511307e 301 }
Tomo1213 1:b2bd1511307e 302 //実行時間
Tomo1213 1:b2bd1511307e 303 wait(2.0);
ngokystk 0:b4b94eb28093 304
Tomo1213 1:b2bd1511307e 305 //速度を指定
Tomo1213 1:b2bd1511307e 306 sendTgtVel(1,0);
Tomo1213 1:b2bd1511307e 307 sendTgtVel(2,0);
Tomo1213 1:b2bd1511307e 308 sendTgtVel(3,0);
Tomo1213 1:b2bd1511307e 309 sendTgtVel(4,0);
Tomo1213 1:b2bd1511307e 310 //指令値を送信
Tomo1213 1:b2bd1511307e 311 for(int i=1;i<= 4;i++){
Tomo1213 1:b2bd1511307e 312 sendCtrlEN(i);
Tomo1213 1:b2bd1511307e 313 }
Tomo1213 1:b2bd1511307e 314 //実行時間
Tomo1213 1:b2bd1511307e 315 wait(5.0);
Tomo1213 1:b2bd1511307e 316
ngokystk 0:b4b94eb28093 317
Tomo1213 1:b2bd1511307e 318 //速度を指定
Tomo1213 1:b2bd1511307e 319 sendTgtVel(1,rpm*(-1));
Tomo1213 1:b2bd1511307e 320 sendTgtVel(2,rpm);
Tomo1213 1:b2bd1511307e 321 sendTgtVel(3,rpm);
Tomo1213 1:b2bd1511307e 322 sendTgtVel(4,rpm*(-1));
Tomo1213 1:b2bd1511307e 323 //指令値を送信
Tomo1213 1:b2bd1511307e 324 for(int i=1;i<= 4;i++){
Tomo1213 1:b2bd1511307e 325 sendCtrlEN(i);
Tomo1213 1:b2bd1511307e 326 }
Tomo1213 1:b2bd1511307e 327 //実行時間
Tomo1213 1:b2bd1511307e 328 wait(2.0);
Tomo1213 1:b2bd1511307e 329
Tomo1213 1:b2bd1511307e 330 //ブレーキ
Tomo1213 1:b2bd1511307e 331 sendTgtVel(1,0);
Tomo1213 1:b2bd1511307e 332 sendTgtVel(2,0);
Tomo1213 1:b2bd1511307e 333 sendTgtVel(3,0);
Tomo1213 1:b2bd1511307e 334 sendTgtVel(4,0);
Tomo1213 1:b2bd1511307e 335 for(int i=1;i<= 4;i++){
Tomo1213 1:b2bd1511307e 336 sendCtrlEN(i);
Tomo1213 1:b2bd1511307e 337 }
Tomo1213 1:b2bd1511307e 338 wait(0.5);
ngokystk 0:b4b94eb28093 339
Tomo1213 1:b2bd1511307e 340 set_MODE_T();
ngokystk 0:b4b94eb28093 341
Tomo1213 1:b2bd1511307e 342 wait(4.0);
Tomo1213 1:b2bd1511307e 343 break;
Tomo1213 1:b2bd1511307e 344 }
Tomo1213 1:b2bd1511307e 345 //一定時間入力がない場合
Tomo1213 1:b2bd1511307e 346 if(Time > Standby_Time){
Tomo1213 1:b2bd1511307e 347 sendProAcc(1,5000);
Tomo1213 1:b2bd1511307e 348 sendProAcc(2,5000);
Tomo1213 1:b2bd1511307e 349 sendProAcc(3,5000);
Tomo1213 1:b2bd1511307e 350 sendProAcc(4,5000);
ngokystk 0:b4b94eb28093 351
Tomo1213 1:b2bd1511307e 352 sendProDec(1,5000);
Tomo1213 1:b2bd1511307e 353 sendProDec(2,5000);
Tomo1213 1:b2bd1511307e 354 sendProDec(3,5000);
Tomo1213 1:b2bd1511307e 355 sendProDec(4,5000);
Tomo1213 1:b2bd1511307e 356
Tomo1213 1:b2bd1511307e 357 //速度制御モード送信
Tomo1213 1:b2bd1511307e 358 set_MODE_V();
Tomo1213 1:b2bd1511307e 359
Tomo1213 1:b2bd1511307e 360 //速度を指定
Tomo1213 1:b2bd1511307e 361 sendTgtVel(1,rpm);
Tomo1213 1:b2bd1511307e 362 sendTgtVel(2,rpm);
Tomo1213 1:b2bd1511307e 363 sendTgtVel(3,rpm);
Tomo1213 1:b2bd1511307e 364 sendTgtVel(4,rpm);
Tomo1213 1:b2bd1511307e 365 //指令値を送信
Tomo1213 1:b2bd1511307e 366 for(int i=1;i<= 4;i++){
Tomo1213 1:b2bd1511307e 367 sendCtrlEN(i);
Tomo1213 1:b2bd1511307e 368 }
Tomo1213 1:b2bd1511307e 369 //実行時間
Tomo1213 1:b2bd1511307e 370 wait(1.0);
Tomo1213 1:b2bd1511307e 371
Tomo1213 1:b2bd1511307e 372 //速度を指定
Tomo1213 1:b2bd1511307e 373 sendTgtVel(1,rpm*(-1));
Tomo1213 1:b2bd1511307e 374 sendTgtVel(2,rpm*(-1));
Tomo1213 1:b2bd1511307e 375 sendTgtVel(3,rpm*(-1));
Tomo1213 1:b2bd1511307e 376 sendTgtVel(4,rpm*(-1));
Tomo1213 1:b2bd1511307e 377 //指令値を送信
Tomo1213 1:b2bd1511307e 378 for(int i=1;i<= 4;i++){
Tomo1213 1:b2bd1511307e 379 sendCtrlEN(i);
Tomo1213 1:b2bd1511307e 380 }
Tomo1213 1:b2bd1511307e 381 //実行時間
Tomo1213 1:b2bd1511307e 382 wait(1.3);
ngokystk 0:b4b94eb28093 383
Tomo1213 1:b2bd1511307e 384 //ブレーキ
Tomo1213 1:b2bd1511307e 385 sendTgtVel(1,0);
Tomo1213 1:b2bd1511307e 386 sendTgtVel(2,0);
Tomo1213 1:b2bd1511307e 387 sendTgtVel(3,0);
Tomo1213 1:b2bd1511307e 388 sendTgtVel(4,0);
Tomo1213 1:b2bd1511307e 389 for(int i=1;i<= 4;i++){
Tomo1213 1:b2bd1511307e 390 sendCtrlEN(i);
Tomo1213 1:b2bd1511307e 391 }
Tomo1213 1:b2bd1511307e 392
Tomo1213 1:b2bd1511307e 393 wait(0.5);
Tomo1213 1:b2bd1511307e 394
Tomo1213 1:b2bd1511307e 395 set_MODE_T();
Tomo1213 1:b2bd1511307e 396
Tomo1213 1:b2bd1511307e 397 wait(5.0);
Tomo1213 1:b2bd1511307e 398 break;
Tomo1213 1:b2bd1511307e 399 }
ngokystk 0:b4b94eb28093 400 }
ngokystk 0:b4b94eb28093 401 wait(0.1);
ngokystk 0:b4b94eb28093 402 }
Tomo1213 1:b2bd1511307e 403 }
Tomo1213 1:b2bd1511307e 404
Tomo1213 1:b2bd1511307e 405 void set_ACC(int setACC_val){
Tomo1213 1:b2bd1511307e 406 sendProAcc(1,setACC_val);
Tomo1213 1:b2bd1511307e 407 sendProAcc(2,setACC_val);
Tomo1213 1:b2bd1511307e 408 sendProAcc(3,setACC_val);
Tomo1213 1:b2bd1511307e 409 sendProAcc(4,setACC_val);
ngokystk 0:b4b94eb28093 410 }
ngokystk 0:b4b94eb28093 411
Tomo1213 1:b2bd1511307e 412 void set_DEC(int setDEC_val){
Tomo1213 1:b2bd1511307e 413 sendProDec(1,setDEC_val);
Tomo1213 1:b2bd1511307e 414 sendProDec(2,setDEC_val);
Tomo1213 1:b2bd1511307e 415 sendProDec(3,setDEC_val);
Tomo1213 1:b2bd1511307e 416 sendProDec(4,setDEC_val);
Tomo1213 1:b2bd1511307e 417 }
ngokystk 0:b4b94eb28093 418
Tomo1213 1:b2bd1511307e 419 void set_MODE_V(){
Tomo1213 1:b2bd1511307e 420 sendOPModeV(1);
Tomo1213 1:b2bd1511307e 421 sendOPModeV(2);
Tomo1213 1:b2bd1511307e 422 sendOPModeV(3);
Tomo1213 1:b2bd1511307e 423 sendOPModeV(4);
Tomo1213 1:b2bd1511307e 424 }
ngokystk 0:b4b94eb28093 425
Tomo1213 1:b2bd1511307e 426 void set_MODE_T(){
Tomo1213 1:b2bd1511307e 427 sendOPModeT(1);
Tomo1213 1:b2bd1511307e 428 sendOPModeT(2);
Tomo1213 1:b2bd1511307e 429 sendOPModeT(3);
Tomo1213 1:b2bd1511307e 430 sendOPModeT(4);
Tomo1213 1:b2bd1511307e 431 }
ngokystk 0:b4b94eb28093 432
ngokystk 0:b4b94eb28093 433 //0x2F-6060-00-fd-//-//-//
ngokystk 0:b4b94eb28093 434 void sendOPModeT(int nodeID){
ngokystk 0:b4b94eb28093 435 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 436 canmsgTx.len = 5; //Data Length
ngokystk 0:b4b94eb28093 437 canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 438 canmsgTx.data[1] = 0x60;//Index LowByte
ngokystk 0:b4b94eb28093 439 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 440 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 441 canmsgTx.data[4] = 0xFD;//data:fd = "current Mode"
ngokystk 0:b4b94eb28093 442 /*
ngokystk 0:b4b94eb28093 443 canmsgTx.data[5] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 444 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 445 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 446 */
ngokystk 0:b4b94eb28093 447 printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 448 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 449 wait_ms(1);
ngokystk 0:b4b94eb28093 450 }
Tomo1213 1:b2bd1511307e 451
Tomo1213 1:b2bd1511307e 452
ngokystk 0:b4b94eb28093 453 //0x2F-6060-00-03-//-//-//
ngokystk 0:b4b94eb28093 454 void sendOPModeV(int nodeID){
ngokystk 0:b4b94eb28093 455 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 456 canmsgTx.len = 5; //Data Length
ngokystk 0:b4b94eb28093 457 canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 458 canmsgTx.data[1] = 0x60;//Index LowByte
ngokystk 0:b4b94eb28093 459 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 460 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 461 canmsgTx.data[4] = 0x03;//data:0x03 = "Profile Velocity Mode"
ngokystk 0:b4b94eb28093 462 /*
ngokystk 0:b4b94eb28093 463 canmsgTx.data[5] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 464 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 465 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 466 */
ngokystk 0:b4b94eb28093 467 printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 468 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 469 wait_ms(1);
ngokystk 0:b4b94eb28093 470 }
Tomo1213 1:b2bd1511307e 471
ngokystk 0:b4b94eb28093 472 //0x2B-6040-00-0000-//-//
ngokystk 0:b4b94eb28093 473 void sendCtrlRS(int nodeID){
ngokystk 0:b4b94eb28093 474 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 475 canmsgTx.len = 6; //Data Length
ngokystk 0:b4b94eb28093 476 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 477 canmsgTx.data[1] = 0x40;//Index LowByte
ngokystk 0:b4b94eb28093 478 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 479 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 480 canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Shutdown)"
ngokystk 0:b4b94eb28093 481 canmsgTx.data[5] = 0x00;//data:0x"00"80
ngokystk 0:b4b94eb28093 482 /*
ngokystk 0:b4b94eb28093 483 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 484 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 485 */
ngokystk 0:b4b94eb28093 486 printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 487 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 488 wait_ms(1);
ngokystk 0:b4b94eb28093 489 }
Tomo1213 1:b2bd1511307e 490
Tomo1213 1:b2bd1511307e 491
ngokystk 0:b4b94eb28093 492 //0x2B-6040-00-0006-//-//
ngokystk 0:b4b94eb28093 493 void sendCtrlSD(int nodeID){
ngokystk 0:b4b94eb28093 494 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 495 canmsgTx.len = 6; //Data Length
ngokystk 0:b4b94eb28093 496 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 497 canmsgTx.data[1] = 0x40;//Index LowByte
ngokystk 0:b4b94eb28093 498 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 499 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 500 canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)"
ngokystk 0:b4b94eb28093 501 canmsgTx.data[5] = 0x00;//data:0x"00"06
ngokystk 0:b4b94eb28093 502 /*
ngokystk 0:b4b94eb28093 503 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 504 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 505 */
ngokystk 0:b4b94eb28093 506 printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 507 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 508 wait_ms(1);
ngokystk 0:b4b94eb28093 509 }
Tomo1213 1:b2bd1511307e 510
ngokystk 0:b4b94eb28093 511 //0x2B-6040-00-000F-//-//
ngokystk 0:b4b94eb28093 512 void sendCtrlEN(int nodeID){
ngokystk 0:b4b94eb28093 513 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 514 canmsgTx.len = 6; //Data Length
ngokystk 0:b4b94eb28093 515 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 516 canmsgTx.data[1] = 0x40;//Index LowByte
ngokystk 0:b4b94eb28093 517 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 518 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 519 canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)"
ngokystk 0:b4b94eb28093 520 canmsgTx.data[5] = 0x00;//data:0x"00"0F
ngokystk 0:b4b94eb28093 521 /*
ngokystk 0:b4b94eb28093 522 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 523 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 524 */
ngokystk 0:b4b94eb28093 525 //printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 526 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 527 wait_ms(1);
ngokystk 0:b4b94eb28093 528 }
Tomo1213 1:b2bd1511307e 529
ngokystk 0:b4b94eb28093 530 //0x2B-6040-00-000B-//-//
ngokystk 0:b4b94eb28093 531 void sendCtrlQS(int nodeID){
ngokystk 0:b4b94eb28093 532 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 533 canmsgTx.len = 6; //Data Length
ngokystk 0:b4b94eb28093 534 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 535 canmsgTx.data[1] = 0x40;//Index LowByte
ngokystk 0:b4b94eb28093 536 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 537 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 538 canmsgTx.data[4] = 0x0B;//data:0x00"0B" = "Quick Stop"
ngokystk 0:b4b94eb28093 539 canmsgTx.data[5] = 0x00;//data:0x"00"0B
ngokystk 0:b4b94eb28093 540 /*
ngokystk 0:b4b94eb28093 541 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 542 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 543 */
ngokystk 0:b4b94eb28093 544 printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 545 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 546 wait_ms(1);
ngokystk 0:b4b94eb28093 547 }
Tomo1213 1:b2bd1511307e 548
ngokystk 0:b4b94eb28093 549 //0x2B-6040-00-010F-//-//
ngokystk 0:b4b94eb28093 550 void sendCtrlHL(int nodeID){
ngokystk 0:b4b94eb28093 551 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 552 canmsgTx.len = 6; //Data Length
ngokystk 0:b4b94eb28093 553 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 554 canmsgTx.data[1] = 0x40;//Index LowByte
ngokystk 0:b4b94eb28093 555 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 556 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 557 canmsgTx.data[4] = 0x0F;//data:0x01"0F" = "Halt"
ngokystk 0:b4b94eb28093 558 canmsgTx.data[5] = 0x01;//data:0x"01"0F
ngokystk 0:b4b94eb28093 559 /*
ngokystk 0:b4b94eb28093 560 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 561 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 562 */
ngokystk 0:b4b94eb28093 563 printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 564 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 565 wait_ms(1);
ngokystk 0:b4b94eb28093 566 }
Tomo1213 1:b2bd1511307e 567
ngokystk 0:b4b94eb28093 568 //0x2B-60FF-00-[user data(4Byte)]
ngokystk 0:b4b94eb28093 569 void sendTgtTrq(int nodeID,int trq){
ngokystk 0:b4b94eb28093 570 //pc.printf("%dmA0x%08x\r\n",trq,trq); //回転数送信データの表示
ngokystk 0:b4b94eb28093 571 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 572 canmsgTx.len = 6; //Data Length
ngokystk 0:b4b94eb28093 573 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 574 canmsgTx.data[1] = 0x30;//Index LowByte 71
ngokystk 0:b4b94eb28093 575 canmsgTx.data[2] = 0x20;//Index HighByte 60
ngokystk 0:b4b94eb28093 576 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 577 //下位から1Byteずつdataに格納
ngokystk 0:b4b94eb28093 578 if(trq<0){
ngokystk 0:b4b94eb28093 579 trq=0xFFFF+trq+1;
ngokystk 0:b4b94eb28093 580 }
ngokystk 0:b4b94eb28093 581
ngokystk 0:b4b94eb28093 582 //pc.printf("iii%d\r\n",trq);
ngokystk 0:b4b94eb28093 583 //canmsgTx.data[7]=((trq>>24)&0xFF);
ngokystk 0:b4b94eb28093 584 //canmsgTx.data[6]=((trq>>16)&0xFF);
ngokystk 0:b4b94eb28093 585
ngokystk 0:b4b94eb28093 586 canmsgTx.data[5]=((trq>>8)&0xFF);
ngokystk 0:b4b94eb28093 587 canmsgTx.data[4]=((trq>>0)&0xFF);
ngokystk 0:b4b94eb28093 588 //printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 589 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 590 wait_ms(2);
ngokystk 0:b4b94eb28093 591 //send Enable
ngokystk 0:b4b94eb28093 592 //pc.printf("Send Enable Command\r\n");
ngokystk 0:b4b94eb28093 593 //sendCtrlEN(nodeID);
ngokystk 0:b4b94eb28093 594 //wait(0.5);
ngokystk 0:b4b94eb28093 595 }
Tomo1213 1:b2bd1511307e 596
Tomo1213 1:b2bd1511307e 597
Tomo1213 1:b2bd1511307e 598
Tomo1213 1:b2bd1511307e 599
ngokystk 0:b4b94eb28093 600 //0x2B-60FF-00-[user data(4Byte)]
ngokystk 0:b4b94eb28093 601 void sendTgtVel(int nodeID,int rpm){
ngokystk 0:b4b94eb28093 602 //pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示
ngokystk 0:b4b94eb28093 603 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 604 canmsgTx.len = 8; //Data Length
ngokystk 0:b4b94eb28093 605 canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 606 canmsgTx.data[1] = 0xFF;//Index LowByte
ngokystk 0:b4b94eb28093 607 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 608 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 609 //下位から1Byteずつdataに格納
ngokystk 0:b4b94eb28093 610
ngokystk 0:b4b94eb28093 611 //pc.printf("%d\r\n",rpm);
ngokystk 0:b4b94eb28093 612 if(rpm<0){
ngokystk 0:b4b94eb28093 613 rpm=0xFFFFFFFF+rpm+1;
ngokystk 0:b4b94eb28093 614 }
ngokystk 0:b4b94eb28093 615 canmsgTx.data[7]=((rpm>>24)&0xFF);
ngokystk 0:b4b94eb28093 616 canmsgTx.data[6]=((rpm>>16)&0xFF);
ngokystk 0:b4b94eb28093 617 canmsgTx.data[5]=((rpm>>8)&0xFF);
ngokystk 0:b4b94eb28093 618 canmsgTx.data[4]=((rpm>>0)&0xFF);
ngokystk 0:b4b94eb28093 619
ngokystk 0:b4b94eb28093 620 //printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 621 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 622 wait_ms(1);
ngokystk 0:b4b94eb28093 623
ngokystk 0:b4b94eb28093 624 }
Tomo1213 1:b2bd1511307e 625
ngokystk 0:b4b94eb28093 626 void readActVel(int nodeID){
ngokystk 0:b4b94eb28093 627 //値が欲しいobjectのアドレスを送る
ngokystk 0:b4b94eb28093 628 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 629 canmsgTx.len = 4; //Data Length
ngokystk 0:b4b94eb28093 630 canmsgTx.data[0] = 0x40;//|0Byte:40|
ngokystk 0:b4b94eb28093 631 canmsgTx.data[1] = 0x6C;//Index LowByte
ngokystk 0:b4b94eb28093 632 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 633 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 634 canPort.write(canmsgTx);
ngokystk 0:b4b94eb28093 635 wait_ms(1);
ngokystk 0:b4b94eb28093 636 }
Tomo1213 1:b2bd1511307e 637
ngokystk 0:b4b94eb28093 638 void readActPos(int nodeID){
ngokystk 0:b4b94eb28093 639 //値が欲しいobjectのアドレスを送る
ngokystk 0:b4b94eb28093 640 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 641 canmsgTx.len = 4; //Data Length
ngokystk 0:b4b94eb28093 642 canmsgTx.data[0] = 0x40;//|0Byte:40|
ngokystk 0:b4b94eb28093 643 canmsgTx.data[1] = 0x64;//Index LowByte
ngokystk 0:b4b94eb28093 644 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 645 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 646 canPort.write(canmsgTx);
ngokystk 0:b4b94eb28093 647 wait_ms(1);
ngokystk 0:b4b94eb28093 648 }
Tomo1213 1:b2bd1511307e 649
ngokystk 0:b4b94eb28093 650 //加速度指定
ngokystk 0:b4b94eb28093 651 void sendProAcc(int nodeID,int rpm){
ngokystk 0:b4b94eb28093 652 if(rpm < 0){
ngokystk 0:b4b94eb28093 653 rpm += 0xFFFFFFFF;
ngokystk 0:b4b94eb28093 654 }
ngokystk 0:b4b94eb28093 655 // pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示
ngokystk 0:b4b94eb28093 656 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 657 canmsgTx.len = 8; //Data Length
ngokystk 0:b4b94eb28093 658 canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 659 canmsgTx.data[1] = 0x83;//Index LowByte
ngokystk 0:b4b94eb28093 660 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 661 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 662 //下位から1Byteずつdataに格納
ngokystk 0:b4b94eb28093 663 canmsgTx.data[4] = (rpm >> 0) & 0xFF;
ngokystk 0:b4b94eb28093 664 canmsgTx.data[5] = (rpm >> 8) & 0xFF;
ngokystk 0:b4b94eb28093 665 canmsgTx.data[6] = (rpm >> 16) & 0xFF;
ngokystk 0:b4b94eb28093 666 canmsgTx.data[7] = (rpm >> 24) & 0xFF;
ngokystk 0:b4b94eb28093 667 // printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 668 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 669 wait(0.01);
ngokystk 0:b4b94eb28093 670 //send Enable
ngokystk 0:b4b94eb28093 671 // pc.printf("Send Enable Command\r\n");
ngokystk 0:b4b94eb28093 672 sendCtrlEN(nodeID);
ngokystk 0:b4b94eb28093 673 wait(0.01);
ngokystk 0:b4b94eb28093 674 }
Tomo1213 1:b2bd1511307e 675
ngokystk 0:b4b94eb28093 676 //減速度指定
ngokystk 0:b4b94eb28093 677 void sendProDec(int nodeID,int rpm){
ngokystk 0:b4b94eb28093 678 if(rpm < 0){
ngokystk 0:b4b94eb28093 679 rpm += 0xFFFFFFFF;
ngokystk 0:b4b94eb28093 680 }
ngokystk 0:b4b94eb28093 681 // pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示
ngokystk 0:b4b94eb28093 682 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 683 canmsgTx.len = 8; //Data Length
ngokystk 0:b4b94eb28093 684 canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 685 canmsgTx.data[1] = 0x84;//Index LowByte
ngokystk 0:b4b94eb28093 686 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 687 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 688 //下位から1Byteずつdataに格納
ngokystk 0:b4b94eb28093 689 canmsgTx.data[4] = (rpm >> 0) & 0xFF;
ngokystk 0:b4b94eb28093 690 canmsgTx.data[5] = (rpm >> 8) & 0xFF;
ngokystk 0:b4b94eb28093 691 canmsgTx.data[6] = (rpm >> 16) & 0xFF;
ngokystk 0:b4b94eb28093 692 canmsgTx.data[7] = (rpm >> 24) & 0xFF;
ngokystk 0:b4b94eb28093 693 // printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 694 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 695 wait(0.01);
ngokystk 0:b4b94eb28093 696 //send Enable
ngokystk 0:b4b94eb28093 697 // pc.printf("Send Enable Command\r\n");
ngokystk 0:b4b94eb28093 698 sendCtrlEN(nodeID);
ngokystk 0:b4b94eb28093 699 wait(0.01);
ngokystk 0:b4b94eb28093 700 }
Tomo1213 1:b2bd1511307e 701
Tomo1213 1:b2bd1511307e 702
ngokystk 0:b4b94eb28093 703 //送信データの表示
ngokystk 0:b4b94eb28093 704 void printCANTX(void){
ngokystk 0:b4b94eb28093 705 //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
ngokystk 0:b4b94eb28093 706 pc.printf("0x%3x|",canmsgTx.id);
ngokystk 0:b4b94eb28093 707 for(char i=0;i < canmsgTx.len;i++){
ngokystk 0:b4b94eb28093 708 pc.printf("%02x|",canmsgTx.data[i]);
ngokystk 0:b4b94eb28093 709 }
ngokystk 0:b4b94eb28093 710 //pc.printf("\r\n");
ngokystk 0:b4b94eb28093 711 }
Tomo1213 1:b2bd1511307e 712
ngokystk 0:b4b94eb28093 713 //受信データの表示
Tomo1213 1:b2bd1511307e 714
ngokystk 0:b4b94eb28093 715 void printCANRX(void){
ngokystk 0:b4b94eb28093 716 //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
ngokystk 0:b4b94eb28093 717 //pc.printf("0x%3x|",canmsgRx.id);
ngokystk 0:b4b94eb28093 718 for(char i=0;i < canmsgRx.len;i++){
ngokystk 0:b4b94eb28093 719 //pc.printf("%02x|",canmsgRx.data[i]);
ngokystk 0:b4b94eb28093 720 }
ngokystk 0:b4b94eb28093 721 pc.printf("\r\n");
ngokystk 0:b4b94eb28093 722 }
Tomo1213 1:b2bd1511307e 723
ngokystk 0:b4b94eb28093 724 void CANdataRX(void){
ngokystk 0:b4b94eb28093 725 canPort.read(canmsgRx);
ngokystk 0:b4b94eb28093 726 printCANRX();
ngokystk 0:b4b94eb28093 727 }
Tomo1213 1:b2bd1511307e 728
ngokystk 0:b4b94eb28093 729 void SerialRX(void){
ngokystk 0:b4b94eb28093 730 Serialdata = pc.getc();
ngokystk 0:b4b94eb28093 731 //pc.printf("%c\r\n",Serialdata);
ngokystk 0:b4b94eb28093 732 }
Tomo1213 1:b2bd1511307e 733
Tomo1213 1:b2bd1511307e 734