Kanta Takasu / Mbed 2 deprecated YUKA_ak_home_pcon

add pcon f and b

Dependencies:   mbed HCSR04

main.cpp@6:07534de7cdb4, 2021-12-19 (annotated)

Committer:
Tomo1213
Date:
Sun Dec 19 17:23:45 2021 +0000
Revision:
6:07534de7cdb4
Parent:
5:a7c3f446a1f1
Child:
7:df29c4de6522
kurikaesidekita

Who changed what in which revision?

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