Kanta Takasu / Mbed 2 deprecated YUKA_FIN

gohome ok

Dependencies:   mbed HCSR04

main.cpp@2:93d72af1b94c, 2021-12-19 (annotated)

Committer:
Tomo1213
Date:
Sun Dec 19 09:36:45 2021 +0000
Revision:
2:93d72af1b94c
Parent:
1:b2bd1511307e
Child:
3:c39c14cfc811
pos

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