Kanta Takasu / Mbed 2 deprecated YUKA_tks

yuka_tks

Dependencies:   mbed HCSR04

main.cpp@0:b4b94eb28093, 2021-12-19 (annotated)

Committer:
ngokystk
Date:
Sun Dec 19 05:53:03 2021 +0000
Revision:
0:b4b94eb28093
Child:
1:b2bd1511307e
YUKA

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制御による機体角度補正_
ngokystk 0:b4b94eb28093 9
ngokystk 0:b4b94eb28093 10 #include "mbed.h"
ngokystk 0:b4b94eb28093 11 #include "hcsr04.h"
ngokystk 0:b4b94eb28093 12
ngokystk 0:b4b94eb28093 13
ngokystk 0:b4b94eb28093 14 DigitalOut led1(LED1);
ngokystk 0:b4b94eb28093 15 DigitalOut led2(LED2);
ngokystk 0:b4b94eb28093 16 DigitalOut led3(LED3);
ngokystk 0:b4b94eb28093 17 DigitalOut led4(LED4);
ngokystk 0:b4b94eb28093 18
ngokystk 0:b4b94eb28093 19 Timer t;
ngokystk 0:b4b94eb28093 20 double Time = 0;
ngokystk 0:b4b94eb28093 21 #define Standby_Time 10
ngokystk 0:b4b94eb28093 22 Serial pc(USBTX, USBRX);
ngokystk 0:b4b94eb28093 23 char Serialdata;
ngokystk 0:b4b94eb28093 24 BusOut myled(LED1, LED2, LED3, LED4);
ngokystk 0:b4b94eb28093 25
ngokystk 0:b4b94eb28093 26 //HCSR04 u1(p13, p14), u2(p11, p12), u3(p23, p24), u4(p25, p26); // Trigger(DO), Echo(PWMIN); LPC1768
ngokystk 0:b4b94eb28093 27 HCSR04 u1(PA_0, PA_1),u2(PA_5, PA_6),u3(PB_0, PA_10); // Trigger(DO), Echo(PWMIN); f303k8
ngokystk 0:b4b94eb28093 28
ngokystk 0:b4b94eb28093 29 CANMessage canmsgTx;
ngokystk 0:b4b94eb28093 30 CANMessage canmsgRx;
ngokystk 0:b4b94eb28093 31 //CAN canPort(p30, p29); //CAN name(PinName rd, PinName td) LPC1768
ngokystk 0:b4b94eb28093 32 CAN canPort(PA_11, PA_12); //CAN name(PinName rd, PinName td) F303k8
ngokystk 0:b4b94eb28093 33
ngokystk 0:b4b94eb28093 34 //プロトタイプ宣言
ngokystk 0:b4b94eb28093 35 //------------------send関数-------------------
ngokystk 0:b4b94eb28093 36 //mode Setting
ngokystk 0:b4b94eb28093 37 void sendOPModeT(int); //Operating Mode
ngokystk 0:b4b94eb28093 38 void sendOPModeV(int); //Operating Mode
ngokystk 0:b4b94eb28093 39 //Control Word
ngokystk 0:b4b94eb28093 40 void sendCtrlRS(int); //Reset
ngokystk 0:b4b94eb28093 41 void sendCtrlSD(int); //Shutdown
ngokystk 0:b4b94eb28093 42 void sendCtrlEN(int); //Switch on & Enable
ngokystk 0:b4b94eb28093 43 void sendCtrlQS(int); //Quick Stop
ngokystk 0:b4b94eb28093 44 void sendCtrlHL(int); //Halt
ngokystk 0:b4b94eb28093 45 //Velocity Setting
ngokystk 0:b4b94eb28093 46 void sendTgtVel(int,int); //Target Velocity
ngokystk 0:b4b94eb28093 47 //Torque Setting
ngokystk 0:b4b94eb28093 48 void sendTgtTrq(int,int); //Target Torque
ngokystk 0:b4b94eb28093 49 //Acceleration Setting
ngokystk 0:b4b94eb28093 50 void sendProAcc(int,int); //Plof Acceleration
ngokystk 0:b4b94eb28093 51 void sendProDec(int,int); //Plof Deceleration
ngokystk 0:b4b94eb28093 52 //------------------read関数-------------------
ngokystk 0:b4b94eb28093 53 void readActVel(int); //Actual Velocity
ngokystk 0:b4b94eb28093 54 void readActPos(int); //Actual Position
ngokystk 0:b4b94eb28093 55 //-------------------その他--------------------
ngokystk 0:b4b94eb28093 56 void printCANTX(void); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 57 void printCANRX(void); //CAN受信データをPCに表示
ngokystk 0:b4b94eb28093 58 void CANdataRX(void); //CAN受信処理
ngokystk 0:b4b94eb28093 59 void SerialRX(void); //Serial受信処理
ngokystk 0:b4b94eb28093 60
ngokystk 0:b4b94eb28093 61 int nodeall=4;
ngokystk 0:b4b94eb28093 62
ngokystk 0:b4b94eb28093 63 int main(){
ngokystk 0:b4b94eb28093 64 //Serial
ngokystk 0:b4b94eb28093 65 pc.attach(SerialRX);
ngokystk 0:b4b94eb28093 66 //pc.baud(115200);
ngokystk 0:b4b94eb28093 67
ngokystk 0:b4b94eb28093 68 //CAN
ngokystk 0:b4b94eb28093 69 canPort.frequency(1000000); //Bit Rate:1MHz
ngokystk 0:b4b94eb28093 70 canPort.attach(CANdataRX,CAN::RxIrq);
ngokystk 0:b4b94eb28093 71 int node1 = 1; //CAN node Setting
ngokystk 0:b4b94eb28093 72 int node2 = 2;
ngokystk 0:b4b94eb28093 73 int node3 = 3;
ngokystk 0:b4b94eb28093 74 int node4 = 4;
ngokystk 0:b4b94eb28093 75
ngokystk 0:b4b94eb28093 76 //モータ回転数
ngokystk 0:b4b94eb28093 77 int rpm = 600;
ngokystk 0:b4b94eb28093 78
ngokystk 0:b4b94eb28093 79 //各モータ回転数
ngokystk 0:b4b94eb28093 80 int rpm1 = rpm; //Velocity Setting[rpm]
ngokystk 0:b4b94eb28093 81 int rpm2 = rpm; //Velocity Setting[rpm]
ngokystk 0:b4b94eb28093 82 int rpm3 = rpm; //Velocity Setting[rpm]
ngokystk 0:b4b94eb28093 83 int rpm4 = rpm; //Velocity Setting[rpm]
ngokystk 0:b4b94eb28093 84
ngokystk 0:b4b94eb28093 85 //エンコーダ関係
ngokystk 0:b4b94eb28093 86 int ActPos = 0;
ngokystk 0:b4b94eb28093 87 int Init_Pos = 0;
ngokystk 0:b4b94eb28093 88
ngokystk 0:b4b94eb28093 89 //超音波センサ関係パラメータ
ngokystk 0:b4b94eb28093 90 int max_rpm = rpm + 200;
ngokystk 0:b4b94eb28093 91 int min_rpm = rpm -200;
ngokystk 0:b4b94eb28093 92 int wall_distance = 0;
ngokystk 0:b4b94eb28093 93 int dist1, dist2,dist3, dist4;
ngokystk 0:b4b94eb28093 94
ngokystk 0:b4b94eb28093 95
ngokystk 0:b4b94eb28093 96 //PID制御関係
ngokystk 0:b4b94eb28093 97 //角度調整パラメータ
ngokystk 0:b4b94eb28093 98 int p1, i1, d1;
ngokystk 0:b4b94eb28093 99 int error_before1 = 0;
ngokystk 0:b4b94eb28093 100 int error_now1 = 0;
ngokystk 0:b4b94eb28093 101 int feedback_val1;
ngokystk 0:b4b94eb28093 102 int integral1;
ngokystk 0:b4b94eb28093 103 int pid_sum1;
ngokystk 0:b4b94eb28093 104 #define DELTA_T1 0.1
ngokystk 0:b4b94eb28093 105 #define target_val1 0
ngokystk 0:b4b94eb28093 106 #define Kp1 3
ngokystk 0:b4b94eb28093 107 #define Ki1 0
ngokystk 0:b4b94eb28093 108 #define Kd1 0
ngokystk 0:b4b94eb28093 109
ngokystk 0:b4b94eb28093 110 /*
ngokystk 0:b4b94eb28093 111 //距離調整パラメータ
ngokystk 0:b4b94eb28093 112 int p2, i2, d2;
ngokystk 0:b4b94eb28093 113 int error_before2 = 0;
ngokystk 0:b4b94eb28093 114 int error_now2 = 0;
ngokystk 0:b4b94eb28093 115 int feedback_val2;
ngokystk 0:b4b94eb28093 116 int integral2;
ngokystk 0:b4b94eb28093 117 int pid_sum2;
ngokystk 0:b4b94eb28093 118 #define DELTA_T2 0.01
ngokystk 0:b4b94eb28093 119 #define target_val2 50
ngokystk 0:b4b94eb28093 120 #define Kp2 2.0
ngokystk 0:b4b94eb28093 121 #define Ki2 3.0
ngokystk 0:b4b94eb28093 122 #define Kd2 3.0
ngokystk 0:b4b94eb28093 123 */
ngokystk 0:b4b94eb28093 124
ngokystk 0:b4b94eb28093 125 pc.printf("YUKA PROGRAM START\r\n");
ngokystk 0:b4b94eb28093 126 wait(0.1);
ngokystk 0:b4b94eb28093 127 //-------------起動時に必ず送信---------------
ngokystk 0:b4b94eb28093 128 //オペレーティングモードを送信
ngokystk 0:b4b94eb28093 129 sendOPModeT(node1);
ngokystk 0:b4b94eb28093 130 sendOPModeT(node2);
ngokystk 0:b4b94eb28093 131 sendOPModeT(node3);
ngokystk 0:b4b94eb28093 132 sendOPModeT(node4);
ngokystk 0:b4b94eb28093 133
ngokystk 0:b4b94eb28093 134 //Shutdown,Enableコマンド送信｜リセット
ngokystk 0:b4b94eb28093 135 sendCtrlSD(node1);
ngokystk 0:b4b94eb28093 136 sendCtrlSD(node2);
ngokystk 0:b4b94eb28093 137 sendCtrlSD(node3);
ngokystk 0:b4b94eb28093 138 sendCtrlSD(node4);
ngokystk 0:b4b94eb28093 139
ngokystk 0:b4b94eb28093 140 sendCtrlEN(node1);
ngokystk 0:b4b94eb28093 141 sendCtrlEN(node2);
ngokystk 0:b4b94eb28093 142 sendCtrlEN(node3);
ngokystk 0:b4b94eb28093 143 sendCtrlEN(node4);
ngokystk 0:b4b94eb28093 144
ngokystk 0:b4b94eb28093 145 //初期加減速度
ngokystk 0:b4b94eb28093 146 int Acc = 2000;
ngokystk 0:b4b94eb28093 147 int Dec = 2000;
ngokystk 0:b4b94eb28093 148
ngokystk 0:b4b94eb28093 149 sendProAcc(1,Acc);
ngokystk 0:b4b94eb28093 150 sendProAcc(2,Acc);
ngokystk 0:b4b94eb28093 151 sendProAcc(3,Acc);
ngokystk 0:b4b94eb28093 152 sendProAcc(4,Acc);
ngokystk 0:b4b94eb28093 153
ngokystk 0:b4b94eb28093 154 sendProDec(1,Dec);
ngokystk 0:b4b94eb28093 155 sendProDec(2,Dec);
ngokystk 0:b4b94eb28093 156 sendProDec(3,Dec);
ngokystk 0:b4b94eb28093 157 sendProDec(4,Dec);
ngokystk 0:b4b94eb28093 158
ngokystk 0:b4b94eb28093 159 //トルク設定
ngokystk 0:b4b94eb28093 160 int trq = 100; //torque Setting[mA]
ngokystk 0:b4b94eb28093 161
ngokystk 0:b4b94eb28093 162 sendTgtTrq(1,trq);
ngokystk 0:b4b94eb28093 163 sendTgtTrq(2,trq);
ngokystk 0:b4b94eb28093 164 sendTgtTrq(3,trq);
ngokystk 0:b4b94eb28093 165 sendTgtTrq(4,trq);
ngokystk 0:b4b94eb28093 166
ngokystk 0:b4b94eb28093 167
ngokystk 0:b4b94eb28093 168 //-------------------------------------------
ngokystk 0:b4b94eb28093 169 /*
ngokystk 0:b4b94eb28093 170 sendCtrlHL(node1);
ngokystk 0:b4b94eb28093 171 sendCtrlHL(node2);
ngokystk 0:b4b94eb28093 172 sendCtrlHL(node3);
ngokystk 0:b4b94eb28093 173 sendCtrlHL(node4);
ngokystk 0:b4b94eb28093 174
ngokystk 0:b4b94eb28093 175 //-------------送信コマンドを選択--------------
ngokystk 0:b4b94eb28093 176 if(Serialdata == 'a'){
ngokystk 0:b4b94eb28093 177 //左移動
ngokystk 0:b4b94eb28093 178 sendTgtVel(node1,rpm*(-1));
ngokystk 0:b4b94eb28093 179 sendTgtVel(node2,rpm);
ngokystk 0:b4b94eb28093 180 sendTgtVel(node3,rpm);
ngokystk 0:b4b94eb28093 181 sendTgtVel(node4,rpm*(-1));
ngokystk 0:b4b94eb28093 182 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 183 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 184 }
ngokystk 0:b4b94eb28093 185
ngokystk 0:b4b94eb28093 186 Serialdata = 0;
ngokystk 0:b4b94eb28093 187 fl=1;
ngokystk 0:b4b94eb28093 188 wait(0.5);
ngokystk 0:b4b94eb28093 189 }
ngokystk 0:b4b94eb28093 190
ngokystk 0:b4b94eb28093 191 else if(Serialdata == 'd'){
ngokystk 0:b4b94eb28093 192 //右移動
ngokystk 0:b4b94eb28093 193 sendTgtVel(node1,rpm);
ngokystk 0:b4b94eb28093 194 sendTgtVel(node2,rpm*(-1));
ngokystk 0:b4b94eb28093 195 sendTgtVel(node3,rpm*(-1));
ngokystk 0:b4b94eb28093 196 sendTgtVel(node4,rpm);
ngokystk 0:b4b94eb28093 197 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 198 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 199 }
ngokystk 0:b4b94eb28093 200 Serialdata = 0;
ngokystk 0:b4b94eb28093 201 fl=1;
ngokystk 0:b4b94eb28093 202 wait(0.5);
ngokystk 0:b4b94eb28093 203 }
ngokystk 0:b4b94eb28093 204
ngokystk 0:b4b94eb28093 205 else if(Serialdata == 'w'){
ngokystk 0:b4b94eb28093 206 //前移動
ngokystk 0:b4b94eb28093 207 sendTgtVel(node1,rpm*(-1));
ngokystk 0:b4b94eb28093 208 sendTgtVel(node2,rpm*(-1));
ngokystk 0:b4b94eb28093 209 sendTgtVel(node3,rpm);
ngokystk 0:b4b94eb28093 210 sendTgtVel(node4,rpm);
ngokystk 0:b4b94eb28093 211 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 212 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 213 }
ngokystk 0:b4b94eb28093 214 Serialdata = 0;
ngokystk 0:b4b94eb28093 215 fl=1;
ngokystk 0:b4b94eb28093 216 wait(0.5);
ngokystk 0:b4b94eb28093 217 }
ngokystk 0:b4b94eb28093 218
ngokystk 0:b4b94eb28093 219 else if(Serialdata == 's'){
ngokystk 0:b4b94eb28093 220 //後移動
ngokystk 0:b4b94eb28093 221 sendTgtVel(node1,rpm);
ngokystk 0:b4b94eb28093 222 sendTgtVel(node2,rpm);
ngokystk 0:b4b94eb28093 223 sendTgtVel(node3,rpm*(-1));
ngokystk 0:b4b94eb28093 224 sendTgtVel(node4,rpm*(-1));
ngokystk 0:b4b94eb28093 225 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 226 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 227 }
ngokystk 0:b4b94eb28093 228 Serialdata = 0;
ngokystk 0:b4b94eb28093 229 fl=1;
ngokystk 0:b4b94eb28093 230 wait(0.5);
ngokystk 0:b4b94eb28093 231 }
ngokystk 0:b4b94eb28093 232
ngokystk 0:b4b94eb28093 233 else if(Serialdata == 'e'){
ngokystk 0:b4b94eb28093 234 //右旋回
ngokystk 0:b4b94eb28093 235 sendTgtVel(node1,rpm);
ngokystk 0:b4b94eb28093 236 sendTgtVel(node2,rpm);
ngokystk 0:b4b94eb28093 237 sendTgtVel(node3,rpm);
ngokystk 0:b4b94eb28093 238 sendTgtVel(node4,rpm);
ngokystk 0:b4b94eb28093 239 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 240 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 241 }
ngokystk 0:b4b94eb28093 242 Serialdata = 0;
ngokystk 0:b4b94eb28093 243 fl=1;
ngokystk 0:b4b94eb28093 244 wait(0.1);
ngokystk 0:b4b94eb28093 245 }
ngokystk 0:b4b94eb28093 246 else if(Serialdata == 'q'){
ngokystk 0:b4b94eb28093 247 //左旋回
ngokystk 0:b4b94eb28093 248 sendTgtVel(node1,rpm*(-1));
ngokystk 0:b4b94eb28093 249 sendTgtVel(node2,rpm*(-1));
ngokystk 0:b4b94eb28093 250 sendTgtVel(node3,rpm*(-1));
ngokystk 0:b4b94eb28093 251 sendTgtVel(node4,rpm*(-1));
ngokystk 0:b4b94eb28093 252 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 253 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 254 }
ngokystk 0:b4b94eb28093 255 Serialdata = 0;
ngokystk 0:b4b94eb28093 256 fl=1;
ngokystk 0:b4b94eb28093 257 wait(0.1);
ngokystk 0:b4b94eb28093 258 }
ngokystk 0:b4b94eb28093 259 */
ngokystk 0:b4b94eb28093 260 //------------------------------------------
ngokystk 0:b4b94eb28093 261
ngokystk 0:b4b94eb28093 262 //人の入力待ち
ngokystk 0:b4b94eb28093 263 /*
ngokystk 0:b4b94eb28093 264 while(1){
ngokystk 0:b4b94eb28093 265 readActPos(1);
ngokystk 0:b4b94eb28093 266 ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
ngokystk 0:b4b94eb28093 267 if(ActPos > 8388608){
ngokystk 0:b4b94eb28093 268 ActPos -= 0x1000000;
ngokystk 0:b4b94eb28093 269 printf("check\r\n");
ngokystk 0:b4b94eb28093 270 }
ngokystk 0:b4b94eb28093 271 printf("ActPos = %d\r\n",ActPos);
ngokystk 0:b4b94eb28093 272 wait(0.1);
ngokystk 0:b4b94eb28093 273 }
ngokystk 0:b4b94eb28093 274 */
ngokystk 0:b4b94eb28093 275
ngokystk 0:b4b94eb28093 276 while(1){
ngokystk 0:b4b94eb28093 277
ngokystk 0:b4b94eb28093 278 ActPos = 0;
ngokystk 0:b4b94eb28093 279 Init_Pos = 0;
ngokystk 0:b4b94eb28093 280 readActPos(1);
ngokystk 0:b4b94eb28093 281 ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
ngokystk 0:b4b94eb28093 282 if(ActPos > 8388608){
ngokystk 0:b4b94eb28093 283 ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
ngokystk 0:b4b94eb28093 284 }
ngokystk 0:b4b94eb28093 285 Init_Pos = ActPos;//起動時の角度を保存
ngokystk 0:b4b94eb28093 286 t.reset();
ngokystk 0:b4b94eb28093 287 t.start();
ngokystk 0:b4b94eb28093 288
ngokystk 0:b4b94eb28093 289 while(1){
ngokystk 0:b4b94eb28093 290
ngokystk 0:b4b94eb28093 291 Time = t.read();
ngokystk 0:b4b94eb28093 292
ngokystk 0:b4b94eb28093 293 readActPos(1);
ngokystk 0:b4b94eb28093 294 ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
ngokystk 0:b4b94eb28093 295 if(ActPos > 8388608){
ngokystk 0:b4b94eb28093 296 ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
ngokystk 0:b4b94eb28093 297 //printf("check\r\n");
ngokystk 0:b4b94eb28093 298 }
ngokystk 0:b4b94eb28093 299 printf("dsit1 %d\r\n",dist1);
ngokystk 0:b4b94eb28093 300 /*--------------------------*/
ngokystk 0:b4b94eb28093 301 //前方向に入力があった時
ngokystk 0:b4b94eb28093 302 if(ActPos < Init_Pos - 2000){
ngokystk 0:b4b94eb28093 303
ngokystk 0:b4b94eb28093 304 sendProAcc(1,1000);
ngokystk 0:b4b94eb28093 305 sendProAcc(2,1000);
ngokystk 0:b4b94eb28093 306 sendProAcc(3,1000);
ngokystk 0:b4b94eb28093 307 sendProAcc(4,1000);
ngokystk 0:b4b94eb28093 308
ngokystk 0:b4b94eb28093 309 sendProDec(1,700);
ngokystk 0:b4b94eb28093 310 sendProDec(2,700);
ngokystk 0:b4b94eb28093 311 sendProDec(3,700);
ngokystk 0:b4b94eb28093 312 sendProDec(4,700);
ngokystk 0:b4b94eb28093 313
ngokystk 0:b4b94eb28093 314 //速度制御モード送信
ngokystk 0:b4b94eb28093 315 sendOPModeV(1);
ngokystk 0:b4b94eb28093 316 sendOPModeV(2);
ngokystk 0:b4b94eb28093 317 sendOPModeV(3);
ngokystk 0:b4b94eb28093 318 sendOPModeV(4);
ngokystk 0:b4b94eb28093 319
ngokystk 0:b4b94eb28093 320 while(1){
ngokystk 0:b4b94eb28093 321
ngokystk 0:b4b94eb28093 322 u1.start();
ngokystk 0:b4b94eb28093 323 u2.start();
ngokystk 0:b4b94eb28093 324 dist1 = u1.get_dist_cm();
ngokystk 0:b4b94eb28093 325 dist2 = u2.get_dist_cm();
ngokystk 0:b4b94eb28093 326
ngokystk 0:b4b94eb28093 327 //速度を指定
ngokystk 0:b4b94eb28093 328 sendTgtVel(1,rpm*(-1));
ngokystk 0:b4b94eb28093 329 sendTgtVel(2,rpm*(-1));
ngokystk 0:b4b94eb28093 330 sendTgtVel(3,rpm);
ngokystk 0:b4b94eb28093 331 sendTgtVel(4,rpm);
ngokystk 0:b4b94eb28093 332 //指令値を送信
ngokystk 0:b4b94eb28093 333 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 334 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 335 }
ngokystk 0:b4b94eb28093 336
ngokystk 0:b4b94eb28093 337 if(dist1 < 100 && dist1 >= 70){
ngokystk 0:b4b94eb28093 338 break;
ngokystk 0:b4b94eb28093 339 }
ngokystk 0:b4b94eb28093 340 printf("dsit1 %d\r\n",dist1);
ngokystk 0:b4b94eb28093 341 }
ngokystk 0:b4b94eb28093 342
ngokystk 0:b4b94eb28093 343 //速度を指定
ngokystk 0:b4b94eb28093 344 sendTgtVel(1,0);
ngokystk 0:b4b94eb28093 345 sendTgtVel(2,0);
ngokystk 0:b4b94eb28093 346 sendTgtVel(3,0);
ngokystk 0:b4b94eb28093 347 sendTgtVel(4,0);
ngokystk 0:b4b94eb28093 348 //指令値を送信
ngokystk 0:b4b94eb28093 349 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 350 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 351 }
ngokystk 0:b4b94eb28093 352 //実行時間
ngokystk 0:b4b94eb28093 353 printf("stop!!\r\n");
ngokystk 0:b4b94eb28093 354 wait(5.0);
ngokystk 0:b4b94eb28093 355
ngokystk 0:b4b94eb28093 356 //printf("2\r\n");
ngokystk 0:b4b94eb28093 357
ngokystk 0:b4b94eb28093 358 //速度を指定
ngokystk 0:b4b94eb28093 359
ngokystk 0:b4b94eb28093 360 while(1){
ngokystk 0:b4b94eb28093 361 readActPos(1);
ngokystk 0:b4b94eb28093 362 ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
ngokystk 0:b4b94eb28093 363 if(ActPos > 8388608){
ngokystk 0:b4b94eb28093 364 ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
ngokystk 0:b4b94eb28093 365 //printf("check\r\n");
ngokystk 0:b4b94eb28093 366 }
ngokystk 0:b4b94eb28093 367
ngokystk 0:b4b94eb28093 368 if(ActPos > -10000){
ngokystk 0:b4b94eb28093 369 break;
ngokystk 0:b4b94eb28093 370 }
ngokystk 0:b4b94eb28093 371
ngokystk 0:b4b94eb28093 372 sendTgtVel(1,rpm);
ngokystk 0:b4b94eb28093 373 sendTgtVel(2,rpm);
ngokystk 0:b4b94eb28093 374 sendTgtVel(3,rpm*(-1));
ngokystk 0:b4b94eb28093 375 sendTgtVel(4,rpm*(-1));
ngokystk 0:b4b94eb28093 376 //指令値を送信
ngokystk 0:b4b94eb28093 377 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 378 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 379 }
ngokystk 0:b4b94eb28093 380 //printf("3\r\n");
ngokystk 0:b4b94eb28093 381 }
ngokystk 0:b4b94eb28093 382
ngokystk 0:b4b94eb28093 383 //ブレーキ
ngokystk 0:b4b94eb28093 384 sendTgtVel(1,0);
ngokystk 0:b4b94eb28093 385 sendTgtVel(2,0);
ngokystk 0:b4b94eb28093 386 sendTgtVel(3,0);
ngokystk 0:b4b94eb28093 387 sendTgtVel(4,0);
ngokystk 0:b4b94eb28093 388 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 389 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 390 }
ngokystk 0:b4b94eb28093 391
ngokystk 0:b4b94eb28093 392 wait(0.5);
ngokystk 0:b4b94eb28093 393
ngokystk 0:b4b94eb28093 394 sendOPModeT(1);
ngokystk 0:b4b94eb28093 395 sendOPModeT(2);
ngokystk 0:b4b94eb28093 396 sendOPModeT(3);
ngokystk 0:b4b94eb28093 397 sendOPModeT(4);
ngokystk 0:b4b94eb28093 398
ngokystk 0:b4b94eb28093 399 dist1 = 0;
ngokystk 0:b4b94eb28093 400
ngokystk 0:b4b94eb28093 401 wait(3.0);
ngokystk 0:b4b94eb28093 402 break;
ngokystk 0:b4b94eb28093 403 }
ngokystk 0:b4b94eb28093 404 /*--------------------------*/
ngokystk 0:b4b94eb28093 405
ngokystk 0:b4b94eb28093 406 /*--------------------------*/
ngokystk 0:b4b94eb28093 407 //右方向に入力があった時
ngokystk 0:b4b94eb28093 408 if(ActPos > Init_Pos + 2000){
ngokystk 0:b4b94eb28093 409 sendProAcc(1,1000);
ngokystk 0:b4b94eb28093 410 sendProAcc(2,1000);
ngokystk 0:b4b94eb28093 411 sendProAcc(3,1000);
ngokystk 0:b4b94eb28093 412 sendProAcc(4,1000);
ngokystk 0:b4b94eb28093 413
ngokystk 0:b4b94eb28093 414 sendProDec(1,1000);
ngokystk 0:b4b94eb28093 415 sendProDec(2,1000);
ngokystk 0:b4b94eb28093 416 sendProDec(3,1000);
ngokystk 0:b4b94eb28093 417 sendProDec(4,1000);
ngokystk 0:b4b94eb28093 418
ngokystk 0:b4b94eb28093 419 //速度制御モード送信
ngokystk 0:b4b94eb28093 420 sendOPModeV(1);
ngokystk 0:b4b94eb28093 421 sendOPModeV(2);
ngokystk 0:b4b94eb28093 422 sendOPModeV(3);
ngokystk 0:b4b94eb28093 423 sendOPModeV(4);
ngokystk 0:b4b94eb28093 424 //速度を指定
ngokystk 0:b4b94eb28093 425 sendTgtVel(1,rpm);
ngokystk 0:b4b94eb28093 426 sendTgtVel(2,rpm*(-1));
ngokystk 0:b4b94eb28093 427 sendTgtVel(3,rpm*(-1));
ngokystk 0:b4b94eb28093 428 sendTgtVel(4,rpm);
ngokystk 0:b4b94eb28093 429 //指令値を送信
ngokystk 0:b4b94eb28093 430 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 431 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 432 }
ngokystk 0:b4b94eb28093 433 //実行時間
ngokystk 0:b4b94eb28093 434 wait(2.0);
ngokystk 0:b4b94eb28093 435
ngokystk 0:b4b94eb28093 436 //速度を指定
ngokystk 0:b4b94eb28093 437 sendTgtVel(1,0);
ngokystk 0:b4b94eb28093 438 sendTgtVel(2,0);
ngokystk 0:b4b94eb28093 439 sendTgtVel(3,0);
ngokystk 0:b4b94eb28093 440 sendTgtVel(4,0);
ngokystk 0:b4b94eb28093 441 //指令値を送信
ngokystk 0:b4b94eb28093 442 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 443 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 444 }
ngokystk 0:b4b94eb28093 445 //実行時間
ngokystk 0:b4b94eb28093 446 wait(5.0);
ngokystk 0:b4b94eb28093 447
ngokystk 0:b4b94eb28093 448
ngokystk 0:b4b94eb28093 449 //速度を指定
ngokystk 0:b4b94eb28093 450 sendTgtVel(1,rpm*(-1));
ngokystk 0:b4b94eb28093 451 sendTgtVel(2,rpm);
ngokystk 0:b4b94eb28093 452 sendTgtVel(3,rpm);
ngokystk 0:b4b94eb28093 453 sendTgtVel(4,rpm*(-1));
ngokystk 0:b4b94eb28093 454 //指令値を送信
ngokystk 0:b4b94eb28093 455 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 456 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 457 }
ngokystk 0:b4b94eb28093 458 //実行時間
ngokystk 0:b4b94eb28093 459 wait(2.0);
ngokystk 0:b4b94eb28093 460
ngokystk 0:b4b94eb28093 461 //ブレーキ
ngokystk 0:b4b94eb28093 462 sendTgtVel(1,0);
ngokystk 0:b4b94eb28093 463 sendTgtVel(2,0);
ngokystk 0:b4b94eb28093 464 sendTgtVel(3,0);
ngokystk 0:b4b94eb28093 465 sendTgtVel(4,0);
ngokystk 0:b4b94eb28093 466 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 467 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 468 }
ngokystk 0:b4b94eb28093 469 wait(0.5);
ngokystk 0:b4b94eb28093 470
ngokystk 0:b4b94eb28093 471 sendOPModeT(1);
ngokystk 0:b4b94eb28093 472 sendOPModeT(2);
ngokystk 0:b4b94eb28093 473 sendOPModeT(3);
ngokystk 0:b4b94eb28093 474 sendOPModeT(4);
ngokystk 0:b4b94eb28093 475
ngokystk 0:b4b94eb28093 476 wait(4.0);
ngokystk 0:b4b94eb28093 477 break;
ngokystk 0:b4b94eb28093 478 }
ngokystk 0:b4b94eb28093 479 /*--------------------------*/
ngokystk 0:b4b94eb28093 480
ngokystk 0:b4b94eb28093 481 /*--------------------------*/
ngokystk 0:b4b94eb28093 482 //一定時間入力がない場合
ngokystk 0:b4b94eb28093 483 if(Time > Standby_Time){
ngokystk 0:b4b94eb28093 484 sendProAcc(1,5000);
ngokystk 0:b4b94eb28093 485 sendProAcc(2,5000);
ngokystk 0:b4b94eb28093 486 sendProAcc(3,5000);
ngokystk 0:b4b94eb28093 487 sendProAcc(4,5000);
ngokystk 0:b4b94eb28093 488
ngokystk 0:b4b94eb28093 489 sendProDec(1,5000);
ngokystk 0:b4b94eb28093 490 sendProDec(2,5000);
ngokystk 0:b4b94eb28093 491 sendProDec(3,5000);
ngokystk 0:b4b94eb28093 492 sendProDec(4,5000);
ngokystk 0:b4b94eb28093 493
ngokystk 0:b4b94eb28093 494 //速度制御モード送信
ngokystk 0:b4b94eb28093 495 sendOPModeV(1);
ngokystk 0:b4b94eb28093 496 sendOPModeV(2);
ngokystk 0:b4b94eb28093 497 sendOPModeV(3);
ngokystk 0:b4b94eb28093 498 sendOPModeV(4);
ngokystk 0:b4b94eb28093 499
ngokystk 0:b4b94eb28093 500 //速度を指定
ngokystk 0:b4b94eb28093 501 sendTgtVel(1,rpm);
ngokystk 0:b4b94eb28093 502 sendTgtVel(2,rpm);
ngokystk 0:b4b94eb28093 503 sendTgtVel(3,rpm);
ngokystk 0:b4b94eb28093 504 sendTgtVel(4,rpm);
ngokystk 0:b4b94eb28093 505 //指令値を送信
ngokystk 0:b4b94eb28093 506 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 507 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 508 }
ngokystk 0:b4b94eb28093 509 //実行時間
ngokystk 0:b4b94eb28093 510 wait(1.0);
ngokystk 0:b4b94eb28093 511
ngokystk 0:b4b94eb28093 512 //速度を指定
ngokystk 0:b4b94eb28093 513 sendTgtVel(1,rpm*(-1));
ngokystk 0:b4b94eb28093 514 sendTgtVel(2,rpm*(-1));
ngokystk 0:b4b94eb28093 515 sendTgtVel(3,rpm*(-1));
ngokystk 0:b4b94eb28093 516 sendTgtVel(4,rpm*(-1));
ngokystk 0:b4b94eb28093 517 //指令値を送信
ngokystk 0:b4b94eb28093 518 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 519 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 520 }
ngokystk 0:b4b94eb28093 521 //実行時間
ngokystk 0:b4b94eb28093 522 wait(1.3);
ngokystk 0:b4b94eb28093 523
ngokystk 0:b4b94eb28093 524 //ブレーキ
ngokystk 0:b4b94eb28093 525 sendTgtVel(1,0);
ngokystk 0:b4b94eb28093 526 sendTgtVel(2,0);
ngokystk 0:b4b94eb28093 527 sendTgtVel(3,0);
ngokystk 0:b4b94eb28093 528 sendTgtVel(4,0);
ngokystk 0:b4b94eb28093 529 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 530 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 531 }
ngokystk 0:b4b94eb28093 532
ngokystk 0:b4b94eb28093 533 wait(0.5);
ngokystk 0:b4b94eb28093 534
ngokystk 0:b4b94eb28093 535 sendOPModeT(1);
ngokystk 0:b4b94eb28093 536 sendOPModeT(2);
ngokystk 0:b4b94eb28093 537 sendOPModeT(3);
ngokystk 0:b4b94eb28093 538 sendOPModeT(4);
ngokystk 0:b4b94eb28093 539
ngokystk 0:b4b94eb28093 540 wait(5.0);
ngokystk 0:b4b94eb28093 541 break;
ngokystk 0:b4b94eb28093 542 }
ngokystk 0:b4b94eb28093 543 /*--------------------------*/
ngokystk 0:b4b94eb28093 544
ngokystk 0:b4b94eb28093 545 }
ngokystk 0:b4b94eb28093 546 wait(0.1);
ngokystk 0:b4b94eb28093 547 }
ngokystk 0:b4b94eb28093 548
ngokystk 0:b4b94eb28093 549 /*
ngokystk 0:b4b94eb28093 550 //ジグザグ
ngokystk 0:b4b94eb28093 551 else if(Serialdata == 'j'){
ngokystk 0:b4b94eb28093 552 int turn_count = 0;
ngokystk 0:b4b94eb28093 553 p1 = 0;
ngokystk 0:b4b94eb28093 554 i1 = 0;
ngokystk 0:b4b94eb28093 555 d1 = 0;
ngokystk 0:b4b94eb28093 556 error_before1 = 0;
ngokystk 0:b4b94eb28093 557 error_now1 = 0;
ngokystk 0:b4b94eb28093 558 feedback_val1 = 0;
ngokystk 0:b4b94eb28093 559 integral1 = 0;
ngokystk 0:b4b94eb28093 560 pid_sum1 = 0;
ngokystk 0:b4b94eb28093 561
ngokystk 0:b4b94eb28093 562 rpm1 = rpm;
ngokystk 0:b4b94eb28093 563 rpm2 = rpm;
ngokystk 0:b4b94eb28093 564 rpm3 = rpm;
ngokystk 0:b4b94eb28093 565 rpm4 = rpm;
ngokystk 0:b4b94eb28093 566
ngokystk 0:b4b94eb28093 567 dist1 = 30;
ngokystk 0:b4b94eb28093 568 dist2 = 30;
ngokystk 0:b4b94eb28093 569 dist3 = 30;
ngokystk 0:b4b94eb28093 570
ngokystk 0:b4b94eb28093 571 sendProAcc(1,5000);
ngokystk 0:b4b94eb28093 572 sendProAcc(2,5000);
ngokystk 0:b4b94eb28093 573 sendProAcc(3,5000);
ngokystk 0:b4b94eb28093 574 sendProAcc(4,5000);
ngokystk 0:b4b94eb28093 575
ngokystk 0:b4b94eb28093 576 sendProDec(1,5000);
ngokystk 0:b4b94eb28093 577 sendProDec(2,5000);
ngokystk 0:b4b94eb28093 578 sendProDec(3,5000);
ngokystk 0:b4b94eb28093 579 sendProDec(4,5000);
ngokystk 0:b4b94eb28093 580
ngokystk 0:b4b94eb28093 581 //正面壁に近づく
ngokystk 0:b4b94eb28093 582 while(!(dist1 <30 && dist1 >5)){
ngokystk 0:b4b94eb28093 583 printf("phase1\r\n");
ngokystk 0:b4b94eb28093 584 u1.start();
ngokystk 0:b4b94eb28093 585 u2.start();
ngokystk 0:b4b94eb28093 586 dist1 = u1.get_dist_cm();
ngokystk 0:b4b94eb28093 587 dist2 = u2.get_dist_cm();
ngokystk 0:b4b94eb28093 588 pc.printf("%ld %ld %ld ID1_%ld ID2_%ld ID3_%ld ID_4%ld\r\n",dist1,dist2,dist3,rpm1,rpm2,rpm3,rpm4);
ngokystk 0:b4b94eb28093 589
ngokystk 0:b4b94eb28093 590 //壁との角度をPID制御
ngokystk 0:b4b94eb28093 591 feedback_val1 = (dist1 - dist2);
ngokystk 0:b4b94eb28093 592
ngokystk 0:b4b94eb28093 593 error_before1 = error_now1;
ngokystk 0:b4b94eb28093 594 error_now1 = feedback_val1 - target_val1;
ngokystk 0:b4b94eb28093 595 integral1 += (error_now1 + error_before1) / 2.0 * DELTA_T1;
ngokystk 0:b4b94eb28093 596
ngokystk 0:b4b94eb28093 597 p1 = Kp1 * error_now1;
ngokystk 0:b4b94eb28093 598 i1 = Ki1 * integral1;
ngokystk 0:b4b94eb28093 599 d1 = Kd1 * (error_now1 - error_before1) / DELTA_T1;
ngokystk 0:b4b94eb28093 600
ngokystk 0:b4b94eb28093 601 pid_sum1 = (p1 + i1 + d1);
ngokystk 0:b4b94eb28093 602
ngokystk 0:b4b94eb28093 603 //モータの回転数を更新
ngokystk 0:b4b94eb28093 604 rpm1 -= pid_sum1;
ngokystk 0:b4b94eb28093 605 rpm2 -= pid_sum1;
ngokystk 0:b4b94eb28093 606 rpm3 += pid_sum1;
ngokystk 0:b4b94eb28093 607 rpm4 += pid_sum1;
ngokystk 0:b4b94eb28093 608
ngokystk 0:b4b94eb28093 609 sendTgtVel(node1,rpm1);
ngokystk 0:b4b94eb28093 610 sendTgtVel(node2,rpm2);
ngokystk 0:b4b94eb28093 611 sendTgtVel(node3,rpm3*(-1));
ngokystk 0:b4b94eb28093 612 sendTgtVel(node4,rpm4*(-1));
ngokystk 0:b4b94eb28093 613 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 614 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 615 }
ngokystk 0:b4b94eb28093 616
ngokystk 0:b4b94eb28093 617 //回転数フィルタ
ngokystk 0:b4b94eb28093 618 if(rpm1 >= max_rpm){
ngokystk 0:b4b94eb28093 619 rpm1 = max_rpm;
ngokystk 0:b4b94eb28093 620 sendTgtVel(node1,rpm1*(-1));
ngokystk 0:b4b94eb28093 621 sendCtrlEN(1);
ngokystk 0:b4b94eb28093 622 }
ngokystk 0:b4b94eb28093 623
ngokystk 0:b4b94eb28093 624 if(rpm2 >= max_rpm){
ngokystk 0:b4b94eb28093 625 rpm2 = max_rpm;
ngokystk 0:b4b94eb28093 626 sendTgtVel(node2,rpm2);
ngokystk 0:b4b94eb28093 627 sendCtrlEN(2);
ngokystk 0:b4b94eb28093 628 }
ngokystk 0:b4b94eb28093 629
ngokystk 0:b4b94eb28093 630 if(rpm3 >= max_rpm){
ngokystk 0:b4b94eb28093 631 rpm3 = max_rpm;
ngokystk 0:b4b94eb28093 632 sendCtrlEN(3);
ngokystk 0:b4b94eb28093 633 }
ngokystk 0:b4b94eb28093 634
ngokystk 0:b4b94eb28093 635 if(rpm4 >= max_rpm){
ngokystk 0:b4b94eb28093 636 rpm4 = max_rpm;
ngokystk 0:b4b94eb28093 637 sendTgtVel(node4,rpm4*(-1));
ngokystk 0:b4b94eb28093 638 sendCtrlEN(4);
ngokystk 0:b4b94eb28093 639 }
ngokystk 0:b4b94eb28093 640
ngokystk 0:b4b94eb28093 641
ngokystk 0:b4b94eb28093 642 if(rpm1 <= min_rpm){
ngokystk 0:b4b94eb28093 643 rpm1 = min_rpm;
ngokystk 0:b4b94eb28093 644 sendTgtVel(node1,rpm1*(-1));
ngokystk 0:b4b94eb28093 645 sendCtrlEN(1);
ngokystk 0:b4b94eb28093 646 }
ngokystk 0:b4b94eb28093 647
ngokystk 0:b4b94eb28093 648 if(rpm2 <= min_rpm){
ngokystk 0:b4b94eb28093 649 rpm2 = min_rpm;
ngokystk 0:b4b94eb28093 650 sendTgtVel(node2,rpm2);
ngokystk 0:b4b94eb28093 651 sendCtrlEN(2);
ngokystk 0:b4b94eb28093 652 }
ngokystk 0:b4b94eb28093 653
ngokystk 0:b4b94eb28093 654 if(rpm3 <= min_rpm){
ngokystk 0:b4b94eb28093 655 rpm3 = min_rpm;
ngokystk 0:b4b94eb28093 656 sendTgtVel(node3,rpm3);
ngokystk 0:b4b94eb28093 657 sendCtrlEN(3);
ngokystk 0:b4b94eb28093 658 }
ngokystk 0:b4b94eb28093 659
ngokystk 0:b4b94eb28093 660 if(rpm4 <= min_rpm){
ngokystk 0:b4b94eb28093 661 rpm4 = min_rpm;
ngokystk 0:b4b94eb28093 662 sendTgtVel(node4,rpm4*(-1));
ngokystk 0:b4b94eb28093 663 sendCtrlEN(4);
ngokystk 0:b4b94eb28093 664 }
ngokystk 0:b4b94eb28093 665
ngokystk 0:b4b94eb28093 666 if(Serialdata == 'h'){
ngokystk 0:b4b94eb28093 667 sendCtrlHL(node1);
ngokystk 0:b4b94eb28093 668 sendCtrlHL(node2);
ngokystk 0:b4b94eb28093 669 sendCtrlHL(node3);
ngokystk 0:b4b94eb28093 670 sendCtrlHL(node4);
ngokystk 0:b4b94eb28093 671 }
ngokystk 0:b4b94eb28093 672
ngokystk 0:b4b94eb28093 673
ngokystk 0:b4b94eb28093 674 wait(0.01);
ngokystk 0:b4b94eb28093 675 }
ngokystk 0:b4b94eb28093 676
ngokystk 0:b4b94eb28093 677 //u1,u2が一定以上壁から離れると終了
ngokystk 0:b4b94eb28093 678 while(!(turn_count == 4)){
ngokystk 0:b4b94eb28093 679
ngokystk 0:b4b94eb28093 680 printf("phase2\r\n");
ngokystk 0:b4b94eb28093 681
ngokystk 0:b4b94eb28093 682 dist3 = 30;
ngokystk 0:b4b94eb28093 683
ngokystk 0:b4b94eb28093 684 //右端まで移動
ngokystk 0:b4b94eb28093 685 //while(dist3 > 20){
ngokystk 0:b4b94eb28093 686 while(!(dist3 < 30& dist3 >10)){
ngokystk 0:b4b94eb28093 687 printf("phase3\r\n");
ngokystk 0:b4b94eb28093 688 u1.start();
ngokystk 0:b4b94eb28093 689 u2.start();
ngokystk 0:b4b94eb28093 690 u3.start();
ngokystk 0:b4b94eb28093 691
ngokystk 0:b4b94eb28093 692 dist1 = u3.get_dist_cm();
ngokystk 0:b4b94eb28093 693 dist2 = u3.get_dist_cm();
ngokystk 0:b4b94eb28093 694 dist3 = u3.get_dist_cm();
ngokystk 0:b4b94eb28093 695 pc.printf("%ld %ld %ld ID1_%ld ID2_%ld ID3_%ld ID_4%ld\r\n",dist1,dist2,dist3,rpm1,rpm2,rpm3,rpm4);
ngokystk 0:b4b94eb28093 696
ngokystk 0:b4b94eb28093 697 //壁との角度をPID制御
ngokystk 0:b4b94eb28093 698 feedback_val1 = (dist1 - dist2);
ngokystk 0:b4b94eb28093 699 error_before1 = error_now1;
ngokystk 0:b4b94eb28093 700 error_now1 = feedback_val1 - target_val1;
ngokystk 0:b4b94eb28093 701 integral1 += (error_now1 + error_before1) / 2.0 * DELTA_T1;
ngokystk 0:b4b94eb28093 702
ngokystk 0:b4b94eb28093 703 p1 = Kp1 * error_now1;
ngokystk 0:b4b94eb28093 704 i1 = Ki1 * integral1;
ngokystk 0:b4b94eb28093 705 d1 = Kd1 * (error_now1 - error_before1) / DELTA_T1;
ngokystk 0:b4b94eb28093 706
ngokystk 0:b4b94eb28093 707 pid_sum1 = (p1 + i1 + d1);
ngokystk 0:b4b94eb28093 708
ngokystk 0:b4b94eb28093 709 //モータの回転数を更新
ngokystk 0:b4b94eb28093 710 rpm1 += pid_sum1;
ngokystk 0:b4b94eb28093 711 rpm2 -= pid_sum1;
ngokystk 0:b4b94eb28093 712 rpm3 -= pid_sum1;
ngokystk 0:b4b94eb28093 713 rpm4 += pid_sum1;
ngokystk 0:b4b94eb28093 714
ngokystk 0:b4b94eb28093 715 //右移動
ngokystk 0:b4b94eb28093 716 sendTgtVel(node1,rpm1*(-1));
ngokystk 0:b4b94eb28093 717 sendTgtVel(node2,rpm2);
ngokystk 0:b4b94eb28093 718 sendTgtVel(node3,rpm3);
ngokystk 0:b4b94eb28093 719 sendTgtVel(node4,rpm4*(-1));
ngokystk 0:b4b94eb28093 720 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 721 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 722 }
ngokystk 0:b4b94eb28093 723 //回転数フィルタ
ngokystk 0:b4b94eb28093 724 if(rpm1 >= max_rpm){
ngokystk 0:b4b94eb28093 725 rpm1 = max_rpm;
ngokystk 0:b4b94eb28093 726 sendTgtVel(node1,rpm1*(-1));
ngokystk 0:b4b94eb28093 727 sendCtrlEN(1);
ngokystk 0:b4b94eb28093 728 }
ngokystk 0:b4b94eb28093 729
ngokystk 0:b4b94eb28093 730 if(rpm2 >= max_rpm){
ngokystk 0:b4b94eb28093 731 rpm2 = max_rpm;
ngokystk 0:b4b94eb28093 732 sendTgtVel(node2,rpm2);
ngokystk 0:b4b94eb28093 733 sendCtrlEN(2);
ngokystk 0:b4b94eb28093 734 }
ngokystk 0:b4b94eb28093 735
ngokystk 0:b4b94eb28093 736 if(rpm3 >= max_rpm){
ngokystk 0:b4b94eb28093 737 rpm3 = max_rpm;
ngokystk 0:b4b94eb28093 738 sendCtrlEN(3);
ngokystk 0:b4b94eb28093 739 }
ngokystk 0:b4b94eb28093 740
ngokystk 0:b4b94eb28093 741 if(rpm4 >= max_rpm){
ngokystk 0:b4b94eb28093 742 rpm4 = max_rpm;
ngokystk 0:b4b94eb28093 743 sendTgtVel(node4,rpm4*(-1));
ngokystk 0:b4b94eb28093 744 sendCtrlEN(4);
ngokystk 0:b4b94eb28093 745 }
ngokystk 0:b4b94eb28093 746
ngokystk 0:b4b94eb28093 747
ngokystk 0:b4b94eb28093 748 if(rpm1 <= min_rpm){
ngokystk 0:b4b94eb28093 749 rpm1 = min_rpm;
ngokystk 0:b4b94eb28093 750 sendTgtVel(node1,rpm1*(-1));
ngokystk 0:b4b94eb28093 751 sendCtrlEN(1);
ngokystk 0:b4b94eb28093 752 }
ngokystk 0:b4b94eb28093 753
ngokystk 0:b4b94eb28093 754 if(rpm2 <= min_rpm){
ngokystk 0:b4b94eb28093 755 rpm2 = min_rpm;
ngokystk 0:b4b94eb28093 756 sendTgtVel(node2,rpm2);
ngokystk 0:b4b94eb28093 757 sendCtrlEN(2);
ngokystk 0:b4b94eb28093 758 }
ngokystk 0:b4b94eb28093 759
ngokystk 0:b4b94eb28093 760 if(rpm3 <= min_rpm){
ngokystk 0:b4b94eb28093 761 rpm3 = min_rpm;
ngokystk 0:b4b94eb28093 762 sendTgtVel(node3,rpm3);
ngokystk 0:b4b94eb28093 763 sendCtrlEN(3);
ngokystk 0:b4b94eb28093 764 }
ngokystk 0:b4b94eb28093 765
ngokystk 0:b4b94eb28093 766 if(rpm4 <= min_rpm){
ngokystk 0:b4b94eb28093 767 rpm4 = min_rpm;
ngokystk 0:b4b94eb28093 768 sendTgtVel(node4,rpm4*(-1));
ngokystk 0:b4b94eb28093 769 sendCtrlEN(4);
ngokystk 0:b4b94eb28093 770 }
ngokystk 0:b4b94eb28093 771
ngokystk 0:b4b94eb28093 772 if(Serialdata == 'h'){
ngokystk 0:b4b94eb28093 773 sendCtrlHL(node1);
ngokystk 0:b4b94eb28093 774 sendCtrlHL(node2);
ngokystk 0:b4b94eb28093 775 sendCtrlHL(node3);
ngokystk 0:b4b94eb28093 776 sendCtrlHL(node4);
ngokystk 0:b4b94eb28093 777 }
ngokystk 0:b4b94eb28093 778
ngokystk 0:b4b94eb28093 779
ngokystk 0:b4b94eb28093 780 wait(0.01);
ngokystk 0:b4b94eb28093 781 }
ngokystk 0:b4b94eb28093 782
ngokystk 0:b4b94eb28093 783 //下に少し移動
ngokystk 0:b4b94eb28093 784
ngokystk 0:b4b94eb28093 785 printf("phase4\r\n");
ngokystk 0:b4b94eb28093 786
ngokystk 0:b4b94eb28093 787 sendTgtVel(node1,rpm*(-1));
ngokystk 0:b4b94eb28093 788 sendTgtVel(node2,rpm*(-1));
ngokystk 0:b4b94eb28093 789 sendTgtVel(node3,rpm);
ngokystk 0:b4b94eb28093 790 sendTgtVel(node4,rpm);
ngokystk 0:b4b94eb28093 791
ngokystk 0:b4b94eb28093 792 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 793 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 794 }
ngokystk 0:b4b94eb28093 795
ngokystk 0:b4b94eb28093 796 turn_count += 1;
ngokystk 0:b4b94eb28093 797
ngokystk 0:b4b94eb28093 798 wait(0.5);
ngokystk 0:b4b94eb28093 799
ngokystk 0:b4b94eb28093 800
ngokystk 0:b4b94eb28093 801 //左に移動
ngokystk 0:b4b94eb28093 802 while(!(dist3 < 80 && dist3 >60)){
ngokystk 0:b4b94eb28093 803 printf("phase5\r\n");
ngokystk 0:b4b94eb28093 804 u1.start();
ngokystk 0:b4b94eb28093 805 u2.start();
ngokystk 0:b4b94eb28093 806 u3.start();
ngokystk 0:b4b94eb28093 807 dist1 = u1.get_dist_cm();
ngokystk 0:b4b94eb28093 808 dist2 = u2.get_dist_cm();
ngokystk 0:b4b94eb28093 809 dist3 = u3.get_dist_cm();
ngokystk 0:b4b94eb28093 810 pc.printf("%ld %ld %ld ID1_%ld ID2_%ld ID3_%ld ID_4%ld\r\n",dist1,dist2,dist3,rpm1,rpm2,rpm3,rpm4);
ngokystk 0:b4b94eb28093 811
ngokystk 0:b4b94eb28093 812 //壁との角度をPID制御
ngokystk 0:b4b94eb28093 813 feedback_val1 = (dist1 - dist2);
ngokystk 0:b4b94eb28093 814 error_before1 = error_now1;
ngokystk 0:b4b94eb28093 815 error_now1 = feedback_val1 - target_val1;
ngokystk 0:b4b94eb28093 816 integral1 += (error_now1 + error_before1) / 2.0 * DELTA_T1;
ngokystk 0:b4b94eb28093 817
ngokystk 0:b4b94eb28093 818 p1 = Kp1 * error_now1;
ngokystk 0:b4b94eb28093 819 i1 = Ki1 * integral1;
ngokystk 0:b4b94eb28093 820 d1 = Kd1 * (error_now1 - error_before1) / DELTA_T1;
ngokystk 0:b4b94eb28093 821
ngokystk 0:b4b94eb28093 822 pid_sum1 = (p1 + i1 + d1);
ngokystk 0:b4b94eb28093 823
ngokystk 0:b4b94eb28093 824 //モータの回転数を更新
ngokystk 0:b4b94eb28093 825 rpm1 -= pid_sum1;
ngokystk 0:b4b94eb28093 826 rpm2 += pid_sum1;
ngokystk 0:b4b94eb28093 827 rpm3 += pid_sum1;
ngokystk 0:b4b94eb28093 828 rpm4 -= pid_sum1;
ngokystk 0:b4b94eb28093 829
ngokystk 0:b4b94eb28093 830 //左移動
ngokystk 0:b4b94eb28093 831 sendTgtVel(node1,rpm1);
ngokystk 0:b4b94eb28093 832 sendTgtVel(node2,rpm2*(-1));
ngokystk 0:b4b94eb28093 833 sendTgtVel(node3,rpm3*(-1));
ngokystk 0:b4b94eb28093 834 sendTgtVel(node4,rpm4);
ngokystk 0:b4b94eb28093 835 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 836 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 837 }
ngokystk 0:b4b94eb28093 838 //回転数フィルタ
ngokystk 0:b4b94eb28093 839 if(rpm1 >= max_rpm){
ngokystk 0:b4b94eb28093 840 rpm1 = max_rpm;
ngokystk 0:b4b94eb28093 841 sendTgtVel(node1,rpm1*(-1));
ngokystk 0:b4b94eb28093 842 sendCtrlEN(1);
ngokystk 0:b4b94eb28093 843 }
ngokystk 0:b4b94eb28093 844
ngokystk 0:b4b94eb28093 845 if(rpm2 >= max_rpm){
ngokystk 0:b4b94eb28093 846 rpm2 = max_rpm;
ngokystk 0:b4b94eb28093 847 sendTgtVel(node2,rpm2);
ngokystk 0:b4b94eb28093 848 sendCtrlEN(2);
ngokystk 0:b4b94eb28093 849 }
ngokystk 0:b4b94eb28093 850
ngokystk 0:b4b94eb28093 851 if(rpm3 >= max_rpm){
ngokystk 0:b4b94eb28093 852 rpm3 = max_rpm;
ngokystk 0:b4b94eb28093 853 sendCtrlEN(3);
ngokystk 0:b4b94eb28093 854 }
ngokystk 0:b4b94eb28093 855
ngokystk 0:b4b94eb28093 856 if(rpm4 >= max_rpm){
ngokystk 0:b4b94eb28093 857 rpm4 = max_rpm;
ngokystk 0:b4b94eb28093 858 sendTgtVel(node4,rpm4*(-1));
ngokystk 0:b4b94eb28093 859 sendCtrlEN(4);
ngokystk 0:b4b94eb28093 860 }
ngokystk 0:b4b94eb28093 861
ngokystk 0:b4b94eb28093 862
ngokystk 0:b4b94eb28093 863 if(rpm1 <= min_rpm){
ngokystk 0:b4b94eb28093 864 rpm1 = min_rpm;
ngokystk 0:b4b94eb28093 865 sendTgtVel(node1,rpm1*(-1));
ngokystk 0:b4b94eb28093 866 sendCtrlEN(1);
ngokystk 0:b4b94eb28093 867 }
ngokystk 0:b4b94eb28093 868
ngokystk 0:b4b94eb28093 869 if(rpm2 <= min_rpm){
ngokystk 0:b4b94eb28093 870 rpm2 = min_rpm;
ngokystk 0:b4b94eb28093 871 sendTgtVel(node2,rpm2);
ngokystk 0:b4b94eb28093 872 sendCtrlEN(2);
ngokystk 0:b4b94eb28093 873 }
ngokystk 0:b4b94eb28093 874
ngokystk 0:b4b94eb28093 875 if(rpm3 <= min_rpm){
ngokystk 0:b4b94eb28093 876 rpm3 = min_rpm;
ngokystk 0:b4b94eb28093 877 sendTgtVel(node3,rpm3);
ngokystk 0:b4b94eb28093 878 sendCtrlEN(3);
ngokystk 0:b4b94eb28093 879 }
ngokystk 0:b4b94eb28093 880
ngokystk 0:b4b94eb28093 881 if(rpm4 <= min_rpm){
ngokystk 0:b4b94eb28093 882 rpm4 = min_rpm;
ngokystk 0:b4b94eb28093 883 sendTgtVel(node4,rpm4*(-1));
ngokystk 0:b4b94eb28093 884 sendCtrlEN(4);
ngokystk 0:b4b94eb28093 885 }
ngokystk 0:b4b94eb28093 886
ngokystk 0:b4b94eb28093 887 if(Serialdata == 'h'){
ngokystk 0:b4b94eb28093 888 sendCtrlHL(node1);
ngokystk 0:b4b94eb28093 889 sendCtrlHL(node2);
ngokystk 0:b4b94eb28093 890 sendCtrlHL(node3);
ngokystk 0:b4b94eb28093 891 sendCtrlHL(node4);
ngokystk 0:b4b94eb28093 892 }
ngokystk 0:b4b94eb28093 893
ngokystk 0:b4b94eb28093 894 wait(0.01);
ngokystk 0:b4b94eb28093 895 }
ngokystk 0:b4b94eb28093 896
ngokystk 0:b4b94eb28093 897 //下に少し移動
ngokystk 0:b4b94eb28093 898
ngokystk 0:b4b94eb28093 899 printf("phase6\r\n");
ngokystk 0:b4b94eb28093 900
ngokystk 0:b4b94eb28093 901
ngokystk 0:b4b94eb28093 902 sendTgtVel(node1,rpm*(-1));
ngokystk 0:b4b94eb28093 903 sendTgtVel(node2,rpm*(-1));
ngokystk 0:b4b94eb28093 904 sendTgtVel(node3,rpm);
ngokystk 0:b4b94eb28093 905 sendTgtVel(node4,rpm);
ngokystk 0:b4b94eb28093 906 for(int i=1;i<= 4;i++){
ngokystk 0:b4b94eb28093 907 sendCtrlEN(i);
ngokystk 0:b4b94eb28093 908 }
ngokystk 0:b4b94eb28093 909
ngokystk 0:b4b94eb28093 910 turn_count += 1;
ngokystk 0:b4b94eb28093 911
ngokystk 0:b4b94eb28093 912 wait(0.5);
ngokystk 0:b4b94eb28093 913
ngokystk 0:b4b94eb28093 914 }
ngokystk 0:b4b94eb28093 915
ngokystk 0:b4b94eb28093 916
ngokystk 0:b4b94eb28093 917 if(Serialdata == 'h'){
ngokystk 0:b4b94eb28093 918 sendCtrlHL(node1);
ngokystk 0:b4b94eb28093 919 sendCtrlHL(node2);
ngokystk 0:b4b94eb28093 920 sendCtrlHL(node3);
ngokystk 0:b4b94eb28093 921 sendCtrlHL(node4);
ngokystk 0:b4b94eb28093 922 break;
ngokystk 0:b4b94eb28093 923 }
ngokystk 0:b4b94eb28093 924
ngokystk 0:b4b94eb28093 925 wait(0.1);
ngokystk 0:b4b94eb28093 926 }
ngokystk 0:b4b94eb28093 927
ngokystk 0:b4b94eb28093 928 }
ngokystk 0:b4b94eb28093 929 */
ngokystk 0:b4b94eb28093 930 //-------------------------------------------
ngokystk 0:b4b94eb28093 931
ngokystk 0:b4b94eb28093 932 }
ngokystk 0:b4b94eb28093 933
ngokystk 0:b4b94eb28093 934
ngokystk 0:b4b94eb28093 935
ngokystk 0:b4b94eb28093 936
ngokystk 0:b4b94eb28093 937 //0x2F-6060-00-fd-//-//-//
ngokystk 0:b4b94eb28093 938 void sendOPModeT(int nodeID){
ngokystk 0:b4b94eb28093 939 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 940 canmsgTx.len = 5; //Data Length
ngokystk 0:b4b94eb28093 941 canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 942 canmsgTx.data[1] = 0x60;//Index LowByte
ngokystk 0:b4b94eb28093 943 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 944 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 945 canmsgTx.data[4] = 0xFD;//data:fd = "current Mode"
ngokystk 0:b4b94eb28093 946 /*
ngokystk 0:b4b94eb28093 947 canmsgTx.data[5] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 948 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 949 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 950 */
ngokystk 0:b4b94eb28093 951 printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 952 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 953 wait_ms(1);
ngokystk 0:b4b94eb28093 954 }
ngokystk 0:b4b94eb28093 955
ngokystk 0:b4b94eb28093 956
ngokystk 0:b4b94eb28093 957 //0x2F-6060-00-03-//-//-//
ngokystk 0:b4b94eb28093 958 void sendOPModeV(int nodeID){
ngokystk 0:b4b94eb28093 959 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 960 canmsgTx.len = 5; //Data Length
ngokystk 0:b4b94eb28093 961 canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 962 canmsgTx.data[1] = 0x60;//Index LowByte
ngokystk 0:b4b94eb28093 963 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 964 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 965 canmsgTx.data[4] = 0x03;//data:0x03 = "Profile Velocity Mode"
ngokystk 0:b4b94eb28093 966 /*
ngokystk 0:b4b94eb28093 967 canmsgTx.data[5] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 968 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 969 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 970 */
ngokystk 0:b4b94eb28093 971 printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 972 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 973 wait_ms(1);
ngokystk 0:b4b94eb28093 974 }
ngokystk 0:b4b94eb28093 975
ngokystk 0:b4b94eb28093 976 //0x2B-6040-00-0000-//-//
ngokystk 0:b4b94eb28093 977 void sendCtrlRS(int nodeID){
ngokystk 0:b4b94eb28093 978 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 979 canmsgTx.len = 6; //Data Length
ngokystk 0:b4b94eb28093 980 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 981 canmsgTx.data[1] = 0x40;//Index LowByte
ngokystk 0:b4b94eb28093 982 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 983 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 984 canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Shutdown)"
ngokystk 0:b4b94eb28093 985 canmsgTx.data[5] = 0x00;//data:0x"00"80
ngokystk 0:b4b94eb28093 986 /*
ngokystk 0:b4b94eb28093 987 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 988 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 989 */
ngokystk 0:b4b94eb28093 990 printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 991 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 992 wait_ms(1);
ngokystk 0:b4b94eb28093 993 }
ngokystk 0:b4b94eb28093 994
ngokystk 0:b4b94eb28093 995
ngokystk 0:b4b94eb28093 996 //0x2B-6040-00-0006-//-//
ngokystk 0:b4b94eb28093 997 void sendCtrlSD(int nodeID){
ngokystk 0:b4b94eb28093 998 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 999 canmsgTx.len = 6; //Data Length
ngokystk 0:b4b94eb28093 1000 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 1001 canmsgTx.data[1] = 0x40;//Index LowByte
ngokystk 0:b4b94eb28093 1002 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 1003 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 1004 canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)"
ngokystk 0:b4b94eb28093 1005 canmsgTx.data[5] = 0x00;//data:0x"00"06
ngokystk 0:b4b94eb28093 1006 /*
ngokystk 0:b4b94eb28093 1007 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 1008 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 1009 */
ngokystk 0:b4b94eb28093 1010 printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 1011 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 1012 wait_ms(1);
ngokystk 0:b4b94eb28093 1013 }
ngokystk 0:b4b94eb28093 1014
ngokystk 0:b4b94eb28093 1015 //0x2B-6040-00-000F-//-//
ngokystk 0:b4b94eb28093 1016 void sendCtrlEN(int nodeID){
ngokystk 0:b4b94eb28093 1017 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 1018 canmsgTx.len = 6; //Data Length
ngokystk 0:b4b94eb28093 1019 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 1020 canmsgTx.data[1] = 0x40;//Index LowByte
ngokystk 0:b4b94eb28093 1021 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 1022 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 1023 canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)"
ngokystk 0:b4b94eb28093 1024 canmsgTx.data[5] = 0x00;//data:0x"00"0F
ngokystk 0:b4b94eb28093 1025 /*
ngokystk 0:b4b94eb28093 1026 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 1027 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 1028 */
ngokystk 0:b4b94eb28093 1029 //printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 1030 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 1031 wait_ms(1);
ngokystk 0:b4b94eb28093 1032 }
ngokystk 0:b4b94eb28093 1033
ngokystk 0:b4b94eb28093 1034 //0x2B-6040-00-000B-//-//
ngokystk 0:b4b94eb28093 1035 void sendCtrlQS(int nodeID){
ngokystk 0:b4b94eb28093 1036 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 1037 canmsgTx.len = 6; //Data Length
ngokystk 0:b4b94eb28093 1038 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 1039 canmsgTx.data[1] = 0x40;//Index LowByte
ngokystk 0:b4b94eb28093 1040 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 1041 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 1042 canmsgTx.data[4] = 0x0B;//data:0x00"0B" = "Quick Stop"
ngokystk 0:b4b94eb28093 1043 canmsgTx.data[5] = 0x00;//data:0x"00"0B
ngokystk 0:b4b94eb28093 1044 /*
ngokystk 0:b4b94eb28093 1045 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 1046 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 1047 */
ngokystk 0:b4b94eb28093 1048 printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 1049 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 1050 wait_ms(1);
ngokystk 0:b4b94eb28093 1051 }
ngokystk 0:b4b94eb28093 1052
ngokystk 0:b4b94eb28093 1053 //0x2B-6040-00-010F-//-//
ngokystk 0:b4b94eb28093 1054 void sendCtrlHL(int nodeID){
ngokystk 0:b4b94eb28093 1055 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 1056 canmsgTx.len = 6; //Data Length
ngokystk 0:b4b94eb28093 1057 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 1058 canmsgTx.data[1] = 0x40;//Index LowByte
ngokystk 0:b4b94eb28093 1059 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 1060 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 1061 canmsgTx.data[4] = 0x0F;//data:0x01"0F" = "Halt"
ngokystk 0:b4b94eb28093 1062 canmsgTx.data[5] = 0x01;//data:0x"01"0F
ngokystk 0:b4b94eb28093 1063 /*
ngokystk 0:b4b94eb28093 1064 canmsgTx.data[6] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 1065 canmsgTx.data[7] = 0x00;//data:(user value)
ngokystk 0:b4b94eb28093 1066 */
ngokystk 0:b4b94eb28093 1067 printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 1068 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 1069 wait_ms(1);
ngokystk 0:b4b94eb28093 1070 }
ngokystk 0:b4b94eb28093 1071
ngokystk 0:b4b94eb28093 1072 //0x2B-60FF-00-[user data(4Byte)]
ngokystk 0:b4b94eb28093 1073 void sendTgtTrq(int nodeID,int trq){
ngokystk 0:b4b94eb28093 1074 //pc.printf("%dmA0x%08x\r\n",trq,trq); //回転数送信データの表示
ngokystk 0:b4b94eb28093 1075 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 1076 canmsgTx.len = 6; //Data Length
ngokystk 0:b4b94eb28093 1077 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 1078 canmsgTx.data[1] = 0x30;//Index LowByte 71
ngokystk 0:b4b94eb28093 1079 canmsgTx.data[2] = 0x20;//Index HighByte 60
ngokystk 0:b4b94eb28093 1080 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 1081 //下位から1Byteずつdataに格納
ngokystk 0:b4b94eb28093 1082 if(trq<0){
ngokystk 0:b4b94eb28093 1083 trq=0xFFFF+trq+1;
ngokystk 0:b4b94eb28093 1084 }
ngokystk 0:b4b94eb28093 1085
ngokystk 0:b4b94eb28093 1086 //pc.printf("iii%d\r\n",trq);
ngokystk 0:b4b94eb28093 1087 //canmsgTx.data[7]=((trq>>24)&0xFF);
ngokystk 0:b4b94eb28093 1088 //canmsgTx.data[6]=((trq>>16)&0xFF);
ngokystk 0:b4b94eb28093 1089
ngokystk 0:b4b94eb28093 1090 canmsgTx.data[5]=((trq>>8)&0xFF);
ngokystk 0:b4b94eb28093 1091 canmsgTx.data[4]=((trq>>0)&0xFF);
ngokystk 0:b4b94eb28093 1092 //printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 1093 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 1094 wait_ms(2);
ngokystk 0:b4b94eb28093 1095 //send Enable
ngokystk 0:b4b94eb28093 1096 //pc.printf("Send Enable Command\r\n");
ngokystk 0:b4b94eb28093 1097 //sendCtrlEN(nodeID);
ngokystk 0:b4b94eb28093 1098 //wait(0.5);
ngokystk 0:b4b94eb28093 1099 }
ngokystk 0:b4b94eb28093 1100
ngokystk 0:b4b94eb28093 1101
ngokystk 0:b4b94eb28093 1102
ngokystk 0:b4b94eb28093 1103
ngokystk 0:b4b94eb28093 1104 //0x2B-60FF-00-[user data(4Byte)]
ngokystk 0:b4b94eb28093 1105 void sendTgtVel(int nodeID,int rpm){
ngokystk 0:b4b94eb28093 1106 //pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示
ngokystk 0:b4b94eb28093 1107 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 1108 canmsgTx.len = 8; //Data Length
ngokystk 0:b4b94eb28093 1109 canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 1110 canmsgTx.data[1] = 0xFF;//Index LowByte
ngokystk 0:b4b94eb28093 1111 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 1112 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 1113 //下位から1Byteずつdataに格納
ngokystk 0:b4b94eb28093 1114
ngokystk 0:b4b94eb28093 1115 //pc.printf("%d\r\n",rpm);
ngokystk 0:b4b94eb28093 1116 if(rpm<0){
ngokystk 0:b4b94eb28093 1117 rpm=0xFFFFFFFF+rpm+1;
ngokystk 0:b4b94eb28093 1118 }
ngokystk 0:b4b94eb28093 1119 canmsgTx.data[7]=((rpm>>24)&0xFF);
ngokystk 0:b4b94eb28093 1120 canmsgTx.data[6]=((rpm>>16)&0xFF);
ngokystk 0:b4b94eb28093 1121 canmsgTx.data[5]=((rpm>>8)&0xFF);
ngokystk 0:b4b94eb28093 1122 canmsgTx.data[4]=((rpm>>0)&0xFF);
ngokystk 0:b4b94eb28093 1123
ngokystk 0:b4b94eb28093 1124 //printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 1125 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 1126 wait_ms(1);
ngokystk 0:b4b94eb28093 1127
ngokystk 0:b4b94eb28093 1128 }
ngokystk 0:b4b94eb28093 1129
ngokystk 0:b4b94eb28093 1130 void readActVel(int nodeID){
ngokystk 0:b4b94eb28093 1131 //値が欲しいobjectのアドレスを送る
ngokystk 0:b4b94eb28093 1132 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 1133 canmsgTx.len = 4; //Data Length
ngokystk 0:b4b94eb28093 1134 canmsgTx.data[0] = 0x40;//|0Byte:40|
ngokystk 0:b4b94eb28093 1135 canmsgTx.data[1] = 0x6C;//Index LowByte
ngokystk 0:b4b94eb28093 1136 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 1137 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 1138 canPort.write(canmsgTx);
ngokystk 0:b4b94eb28093 1139 wait_ms(1);
ngokystk 0:b4b94eb28093 1140 }
ngokystk 0:b4b94eb28093 1141
ngokystk 0:b4b94eb28093 1142 void readActPos(int nodeID){
ngokystk 0:b4b94eb28093 1143 //値が欲しいobjectのアドレスを送る
ngokystk 0:b4b94eb28093 1144 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 1145 canmsgTx.len = 4; //Data Length
ngokystk 0:b4b94eb28093 1146 canmsgTx.data[0] = 0x40;//|0Byte:40|
ngokystk 0:b4b94eb28093 1147 canmsgTx.data[1] = 0x64;//Index LowByte
ngokystk 0:b4b94eb28093 1148 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 1149 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 1150 canPort.write(canmsgTx);
ngokystk 0:b4b94eb28093 1151 wait_ms(1);
ngokystk 0:b4b94eb28093 1152 }
ngokystk 0:b4b94eb28093 1153
ngokystk 0:b4b94eb28093 1154 //加速度指定
ngokystk 0:b4b94eb28093 1155 void sendProAcc(int nodeID,int rpm){
ngokystk 0:b4b94eb28093 1156 if(rpm < 0){
ngokystk 0:b4b94eb28093 1157 rpm += 0xFFFFFFFF;
ngokystk 0:b4b94eb28093 1158 }
ngokystk 0:b4b94eb28093 1159 // pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示
ngokystk 0:b4b94eb28093 1160 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 1161 canmsgTx.len = 8; //Data Length
ngokystk 0:b4b94eb28093 1162 canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 1163 canmsgTx.data[1] = 0x83;//Index LowByte
ngokystk 0:b4b94eb28093 1164 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 1165 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 1166 //下位から1Byteずつdataに格納
ngokystk 0:b4b94eb28093 1167 canmsgTx.data[4] = (rpm >> 0) & 0xFF;
ngokystk 0:b4b94eb28093 1168 canmsgTx.data[5] = (rpm >> 8) & 0xFF;
ngokystk 0:b4b94eb28093 1169 canmsgTx.data[6] = (rpm >> 16) & 0xFF;
ngokystk 0:b4b94eb28093 1170 canmsgTx.data[7] = (rpm >> 24) & 0xFF;
ngokystk 0:b4b94eb28093 1171 // printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 1172 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 1173 wait(0.01);
ngokystk 0:b4b94eb28093 1174 //send Enable
ngokystk 0:b4b94eb28093 1175 // pc.printf("Send Enable Command\r\n");
ngokystk 0:b4b94eb28093 1176 sendCtrlEN(nodeID);
ngokystk 0:b4b94eb28093 1177 wait(0.01);
ngokystk 0:b4b94eb28093 1178 }
ngokystk 0:b4b94eb28093 1179
ngokystk 0:b4b94eb28093 1180 //減速度指定
ngokystk 0:b4b94eb28093 1181 void sendProDec(int nodeID,int rpm){
ngokystk 0:b4b94eb28093 1182 if(rpm < 0){
ngokystk 0:b4b94eb28093 1183 rpm += 0xFFFFFFFF;
ngokystk 0:b4b94eb28093 1184 }
ngokystk 0:b4b94eb28093 1185 // pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示
ngokystk 0:b4b94eb28093 1186 canmsgTx.id = 0x600+nodeID;
ngokystk 0:b4b94eb28093 1187 canmsgTx.len = 8; //Data Length
ngokystk 0:b4b94eb28093 1188 canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
ngokystk 0:b4b94eb28093 1189 canmsgTx.data[1] = 0x84;//Index LowByte
ngokystk 0:b4b94eb28093 1190 canmsgTx.data[2] = 0x60;//Index HighByte
ngokystk 0:b4b94eb28093 1191 canmsgTx.data[3] = 0x00;//sub-Index
ngokystk 0:b4b94eb28093 1192 //下位から1Byteずつdataに格納
ngokystk 0:b4b94eb28093 1193 canmsgTx.data[4] = (rpm >> 0) & 0xFF;
ngokystk 0:b4b94eb28093 1194 canmsgTx.data[5] = (rpm >> 8) & 0xFF;
ngokystk 0:b4b94eb28093 1195 canmsgTx.data[6] = (rpm >> 16) & 0xFF;
ngokystk 0:b4b94eb28093 1196 canmsgTx.data[7] = (rpm >> 24) & 0xFF;
ngokystk 0:b4b94eb28093 1197 // printCANTX(); //CAN送信データをPCに表示
ngokystk 0:b4b94eb28093 1198 canPort.write(canmsgTx);//CANでデータ送信
ngokystk 0:b4b94eb28093 1199 wait(0.01);
ngokystk 0:b4b94eb28093 1200 //send Enable
ngokystk 0:b4b94eb28093 1201 // pc.printf("Send Enable Command\r\n");
ngokystk 0:b4b94eb28093 1202 sendCtrlEN(nodeID);
ngokystk 0:b4b94eb28093 1203 wait(0.01);
ngokystk 0:b4b94eb28093 1204 }
ngokystk 0:b4b94eb28093 1205
ngokystk 0:b4b94eb28093 1206
ngokystk 0:b4b94eb28093 1207 //送信データの表示
ngokystk 0:b4b94eb28093 1208 void printCANTX(void){
ngokystk 0:b4b94eb28093 1209 //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
ngokystk 0:b4b94eb28093 1210 pc.printf("0x%3x|",canmsgTx.id);
ngokystk 0:b4b94eb28093 1211 for(char i=0;i < canmsgTx.len;i++){
ngokystk 0:b4b94eb28093 1212 pc.printf("%02x|",canmsgTx.data[i]);
ngokystk 0:b4b94eb28093 1213 }
ngokystk 0:b4b94eb28093 1214 //pc.printf("\r\n");
ngokystk 0:b4b94eb28093 1215 }
ngokystk 0:b4b94eb28093 1216
ngokystk 0:b4b94eb28093 1217 //受信データの表示
ngokystk 0:b4b94eb28093 1218
ngokystk 0:b4b94eb28093 1219 void printCANRX(void){
ngokystk 0:b4b94eb28093 1220 //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
ngokystk 0:b4b94eb28093 1221 //pc.printf("0x%3x|",canmsgRx.id);
ngokystk 0:b4b94eb28093 1222 for(char i=0;i < canmsgRx.len;i++){
ngokystk 0:b4b94eb28093 1223 //pc.printf("%02x|",canmsgRx.data[i]);
ngokystk 0:b4b94eb28093 1224 }
ngokystk 0:b4b94eb28093 1225 pc.printf("\r\n");
ngokystk 0:b4b94eb28093 1226 }
ngokystk 0:b4b94eb28093 1227
ngokystk 0:b4b94eb28093 1228 void CANdataRX(void){
ngokystk 0:b4b94eb28093 1229 canPort.read(canmsgRx);
ngokystk 0:b4b94eb28093 1230 printCANRX();
ngokystk 0:b4b94eb28093 1231 }
ngokystk 0:b4b94eb28093 1232
ngokystk 0:b4b94eb28093 1233 void SerialRX(void){
ngokystk 0:b4b94eb28093 1234 Serialdata = pc.getc();
ngokystk 0:b4b94eb28093 1235 //pc.printf("%c\r\n",Serialdata);
ngokystk 0:b4b94eb28093 1236 }