Kanta Takasu / Mbed 2 deprecated YUKA_ak_yn

left con

Dependencies:   mbed HCSR04

main.cpp@9:51325cc6496a, 2021-12-20 (annotated)

Committer:
tks1
Date:
Mon Dec 20 05:53:58 2021 +0000
Revision:
9:51325cc6496a
Parent:
8:0497ad6e03a4 
un debuged

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