main.cpp

00001 //2021/12/20更新
00002 //YUKA本番機用プログラム
00003 //入力切替確認済み
00004 //一定時間の入力なしで動作切替
00005 //ジグザグ動作実装前
00006 //エンコーダ読み取りによる入力
00007 //加減速度調整パラメータ実装
00008 //PID制御による機体角度補正_
00009 //aoki 
00010  
00011 #include "mbed.h"
00012 #include "hcsr04.h"
00013 
00014 #define ACC_B 1000
00015 #define DEC_B 700
00016 #define ACC_C 1000
00017 #define DEC_C 1000
00018 
00019 #define KICK 2000
00020 
00021 #define WALL 45
00022 
00023  
00024 DigitalOut led1(LED1);
00025 DigitalOut led2(LED2);
00026 DigitalOut led3(LED3);
00027 DigitalOut led4(LED4);
00028  
00029 Timer t;
00030 double Time = 0;
00031 #define Standby_Time 10
00032 Serial pc(USBTX, USBRX);
00033 char Serialdata;
00034 BusOut myled(LED1, LED2, LED3, LED4);
00035  
00036 //HCSR04 u1(p13, p14), u2(p11, p12), u3(p23, p24), u4(p25, p26); // Trigger(DO), Echo(PWMIN); LPC1768
00037 HCSR04 u1(PA_0, PA_1),u2(PA_5, PA_6),u3(PB_0, PA_10); // Trigger(DO), Echo(PWMIN); f303k8
00038  
00039 CANMessage canmsgTx;
00040 CANMessage canmsgRx;
00041 //CAN canPort(p30, p29);  //CAN name(PinName rd, PinName td) LPC1768
00042 CAN canPort(PA_11, PA_12);  //CAN name(PinName rd, PinName td) F303k8
00043  
00044 //プロトタイプ宣言
00045 //------------------send関数-------------------
00046 //mode Setting
00047 void sendOPModeT(int);       //Operating Mode
00048 void sendOPModeV(int);       //Operating Mode
00049 //Control Word
00050 void sendCtrlRS(int);       //Reset
00051 void sendCtrlSD(int);       //Shutdown
00052 void sendCtrlEN(int);       //Switch on & Enable
00053 void sendCtrlQS(int);       //Quick Stop
00054 void sendCtrlHL(int);       //Halt
00055 //Velocity Setting
00056 void sendTgtVel(int,int);   //Target Velocity
00057 //Torque Setting
00058 void sendTgtTrq(int,int);   //Target Torque
00059 //Acceleration Setting
00060 void sendProAcc(int,int);   //Plof Acceleration
00061 void sendProDec(int,int);   //Plof Deceleration
00062 //------------------read関数-------------------
00063 void readActVel(int);       //Actual Velocity
00064 void readActPos(int);       //Actual Position
00065 //-------------------その他--------------------
00066 void printCANTX(void);      //CAN送信データをPCに表示
00067 void printCANRX(void);      //CAN受信データをPCに表示
00068 void CANdataRX(void);       //CAN受信処理
00069 void SerialRX(void);        //Serial受信処理
00070 //---------------------------------------------
00071 void set_ACC(int);
00072 void set_DEC(int);
00073 void set_MODE_V(void);
00074 void set_MODE_T(void);
00075 
00076 void vel_stop(void);
00077 
00078 void vel_forward(int);
00079 void vel_forward_con(int);
00080 void vel_backward(int);
00081 void vel_backward_con(int);
00082 void vel_right(int);
00083 void vel_right_con(int);
00084 void vel_left(int);
00085 
00086 //unsigned int get_cm_n(HCSR04, unsigned int);
00087 //USE -> unsigned int dist_UnitA = get_cm_n(u2, 5);
00088 unsigned int get_cm_n(HCSR04 &echo_unit,int echo_n){
00089     unsigned int sampled_dist=0;
00090     for (int iter_n = 0; iter_n <echo_n; iter_n++){
00091         echo_unit.start();
00092         sampled_dist += echo_unit.get_dist_cm();
00093     }
00094     return  (sampled_dist / echo_n);
00095 } 
00096 
00097 int nodeall=4;
00098  
00099 int main(){
00100     //Serial
00101     pc.attach(SerialRX);
00102     //pc.baud(115200);
00103     
00104     //CAN
00105     canPort.frequency(1000000); //Bit Rate:1MHz
00106     canPort.attach(CANdataRX,CAN::RxIrq);
00107     int node1 = 1;  //CAN node Setting
00108     int node2 = 2;
00109     int node3 = 3;
00110     int node4 = 4;
00111     
00112     //モータ回転数
00113     int rpm = 400;
00114     
00115     //エンコーダ関係
00116     int ActPos = 0;
00117     int Init_Pos = 0;
00118     
00119     //超音波センサ関係パラメータ
00120     int dist1,dist2,dist3,dist4;
00121     
00122     //PID制御関係
00123     //角度調整パラメータ
00124 
00125 
00126     #define DELTA_T1  0.1
00127     #define target_val1  0
00128     #define Kp1  3
00129     #define Ki1  0
00130     #define Kd1  0
00131        
00132     pc.printf("YUKA PROGRAM START\r\n");
00133     wait(0.1);
00134     //-------------起動時に必ず送信---------------
00135     //オペレーティングモードを送信
00136     sendOPModeT(node1);
00137     sendOPModeT(node2);
00138     sendOPModeT(node3);
00139     sendOPModeT(node4);
00140         
00141     //Shutdown,Enableコマンド送信｜リセット
00142     sendCtrlSD(node1);
00143     sendCtrlSD(node2);
00144     sendCtrlSD(node3);
00145     sendCtrlSD(node4);
00146  
00147     sendCtrlEN(node1);
00148     sendCtrlEN(node2);
00149     sendCtrlEN(node3);
00150     sendCtrlEN(node4);
00151     
00152     //初期加減速度
00153     int Acc = 2000;
00154     int Dec = 2000;
00155     
00156     sendProAcc(1,Acc);
00157     sendProAcc(2,Acc);
00158     sendProAcc(3,Acc);
00159     sendProAcc(4,Acc);
00160            
00161     sendProDec(1,Dec);
00162     sendProDec(2,Dec);
00163     sendProDec(3,Dec);
00164     sendProDec(4,Dec);  
00165  
00166     //トルク設定
00167     int trq = 100;   //torque Setting[mA]
00168   
00169     sendTgtTrq(1,trq);
00170     sendTgtTrq(2,trq);
00171     sendTgtTrq(3,trq);
00172     sendTgtTrq(4,trq);
00173              
00174     int state_1 = 0;
00175 
00176     ActPos = 0;
00177     Init_Pos = 0;
00178     dist1 = 0;
00179     readActPos(1);
00180     ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
00181     if(ActPos > 8388608){
00182         ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
00183     }
00184     Init_Pos = ActPos;//起動時の角度を保存
00185     t.reset();
00186     t.start();
00187 
00188     //set_MODE_T();
00189 
00190     printf("\nstart\r\n");
00191 
00192     while(1){
00193                 
00194         Time = t.read();
00195                 
00196         readActPos(1);
00197         ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
00198         if(ActPos > 8388608){
00199             ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
00200             //printf("check\r\n");
00201         }
00202         dist1 = get_cm_n(u1, 5);
00203         dist2 = get_cm_n(u2, 5);
00204         dist3 = get_cm_n(u3, 5);
00205          
00206         printf("%d\r\n",state_1);
00207         /*--------------------------*/
00208         //
00209         if(state_1 == 0){//入力判断フェーズ
00210             if(ActPos < (Init_Pos - KICK)){
00211                 state_1 = 1;
00212             }else if(ActPos > (Init_Pos + KICK)){
00213                 state_1 = 11;
00214             }else{
00215                 set_MODE_T();
00216             }
00217             
00218         }else if(state_1 == 1){//前進→壁検出フェーズ
00219             if(dist1 < WALL && dist1 >= 25){
00220                 state_1 = 2;
00221             }else{
00222                 set_ACC(ACC_B);//加速度設定
00223                 set_DEC(ACC_C);//減速度設定
00224                 set_MODE_V();//速度制御モード送信
00225                 vel_forward_con(rpm);//前進速度指令
00226             }
00227         }else if(state_1 == 2){//前進からの帰還フェーズ
00228             if(ActPos > -3000){
00229                 vel_stop();
00230                 state_1 = 0;
00231                 wait(1.0);
00232                 //break;
00233             }else{
00234                 vel_backward_con(rpm);
00235             }
00236         }else if(state_1 == 11){//右進→壁検出フェーズ
00237             if(dist3 < WALL && dist3 >= 25){
00238                 state_1 = 12;
00239             }else{
00240                 set_ACC(ACC_B);//加速度設定
00241                 set_DEC(ACC_C);//減速度設定
00242                 set_MODE_V();//速度制御モード送信
00243                 vel_right_con(rpm);//R進速度指令
00244             }
00245         }else if(state_1 == 12){//右進からの帰還フェーズ
00246             if(ActPos < 3000){
00247                 vel_stop();
00248                 state_1 = 0;
00249                 wait(1.0);
00250                 //break;
00251             }else{
00252                 vel_left(rpm);
00253             } 
00254         }else if(state_1 == 41){
00255             //Zig Zag
00256             if(dist1 < WALL && dist1 >= 25){
00257                 state_1 = 2;
00258             }else{
00259                 set_ACC(ACC_B);//加速度設定
00260                 set_DEC(ACC_C);//減速度設定
00261                 set_MODE_V();//速度制御モード送信
00262                 vel_forward_con(rpm);//前進速度指令
00263         }
00264     }      
00265         
00266 }
00267 }
00268 
00269 void vel_right(int rpm){
00270     sendTgtVel(1,rpm);
00271     sendTgtVel(2,rpm*(-1));
00272     sendTgtVel(3,rpm*(-1));
00273     sendTgtVel(4,rpm);
00274     for(int i=1;i<= 4;i++){
00275         sendCtrlEN(i);
00276     }
00277 }
00278 void vel_right_con(int rpmA){
00279     int dis1 = get_cm_n(u1,15);
00280     int dis2 = get_cm_n(u2,15);
00281     
00282     //速度を指定
00283     int robot_angle = ((dis1 - dis2)*5);
00284     sendTgtVel(1,rpmA+robot_angle);
00285     sendTgtVel(2,rpmA*(-1)+robot_angle);
00286     sendTgtVel(3,rpmA*(-1)+robot_angle);
00287     sendTgtVel(4,rpmA+robot_angle);
00288     //指令値を送信
00289     for(int i=1;i<= 4;i++){
00290         sendCtrlEN(i);
00291     }
00292 }
00293  
00294 
00295 void vel_left(int rpm){
00296     sendTgtVel(1,rpm*(-1));
00297     sendTgtVel(2,rpm);
00298     sendTgtVel(3,rpm);
00299     sendTgtVel(4,rpm*(-1));
00300     for(int i=1;i<= 4;i++){
00301         sendCtrlEN(i);
00302     }
00303 }
00304 
00305 
00306 void vel_left_con(int rpmA){
00307     int dis1 = get_cm_n(u1,15);
00308     int dis2 = get_cm_n(u2,15);
00309     
00310     //速度を指定
00311     int robot_angle = ((dis1 - dis2)*5);
00312     sendTgtVel(1,rpmA*(-1)+robot_angle);
00313     sendTgtVel(2,rpmA+robot_angle);
00314     sendTgtVel(3,rpmA+robot_angle);
00315     sendTgtVel(4,rpmA*(-1)+robot_angle);
00316     //指令値を送信
00317     for(int i=1;i<= 4;i++){
00318         sendCtrlEN(i);
00319     }
00320 }
00321 
00322 void vel_stop(){
00323     //速度を指定
00324     sendTgtVel(1,0);
00325     sendTgtVel(2,0);
00326     sendTgtVel(3,0);
00327     sendTgtVel(4,0);
00328     //指令値を送信
00329     for(int i=1;i<= 4;i++){
00330         sendCtrlEN(i);
00331     }
00332 }
00333 
00334 void vel_backward(int rpm){
00335     //速度を指定
00336     sendTgtVel(1,rpm);
00337     sendTgtVel(2,rpm);
00338     sendTgtVel(3,rpm*(-1));
00339     sendTgtVel(4,rpm*(-1));
00340     //指令値を送信
00341     for(int i=1;i<= 4;i++){
00342         sendCtrlEN(i);
00343     }
00344 }
00345 
00346 void vel_backward_con(int rpmA){
00347     int dis1 = get_cm_n(u1,15);
00348     int dis2 = get_cm_n(u2,15);
00349     
00350     //速度を指定
00351     int robot_angle = ((dis1 - dis2)*10);
00352     sendTgtVel(1,rpmA+robot_angle);
00353     sendTgtVel(2,rpmA+robot_angle);
00354     sendTgtVel(3,rpmA*(-1)+robot_angle);
00355     sendTgtVel(4,rpmA*(-1)+robot_angle);
00356     //指令値を送信
00357     for(int i=1;i<= 4;i++){
00358         sendCtrlEN(i);
00359     }
00360 }
00361 
00362 
00363 void vel_forward_con(int rpmA){
00364     int dis1 = get_cm_n(u1,15);
00365     int dis2 = get_cm_n(u2,15);
00366     
00367     //速度を指定
00368     int robot_angle = ((dis1 - dis2)*10);
00369     sendTgtVel(1,rpmA*(-1)+robot_angle);
00370     sendTgtVel(2,rpmA*(-1)+robot_angle);
00371     sendTgtVel(3,rpmA+robot_angle);
00372     sendTgtVel(4,rpmA+robot_angle);
00373     //指令値を送信
00374     for(int i=1;i<= 4;i++){
00375         sendCtrlEN(i);
00376     }
00377 }
00378  
00379 void set_ACC(int setACC_val){
00380     sendProAcc(1,setACC_val);
00381     sendProAcc(2,setACC_val);
00382     sendProAcc(3,setACC_val);
00383     sendProAcc(4,setACC_val);
00384 }
00385 
00386 void set_DEC(int setDEC_val){
00387     sendProDec(1,setDEC_val);
00388     sendProDec(2,setDEC_val);
00389     sendProDec(3,setDEC_val);
00390     sendProDec(4,setDEC_val); 
00391 }
00392 
00393 void set_MODE_V(){
00394     sendOPModeV(1);
00395     sendOPModeV(2);
00396     sendOPModeV(3);
00397     sendOPModeV(4);
00398 }
00399 
00400 void set_MODE_T(){
00401     sendOPModeT(1);
00402     sendOPModeT(2);
00403     sendOPModeT(3);
00404     sendOPModeT(4);
00405 }
00406 
00407 //0x2F-6060-00-fd-//-//-//
00408 void sendOPModeT(int nodeID){
00409     canmsgTx.id = 0x600+nodeID;
00410     canmsgTx.len = 5;       //Data Length
00411     canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
00412     canmsgTx.data[1] = 0x60;//Index LowByte
00413     canmsgTx.data[2] = 0x60;//Index HighByte
00414     canmsgTx.data[3] = 0x00;//sub-Index
00415     canmsgTx.data[4] = 0xFD;//data:fd = "current Mode"
00416     /*
00417     canmsgTx.data[5] = 0x00;//data:(user value)
00418     canmsgTx.data[6] = 0x00;//data:(user value)
00419     canmsgTx.data[7] = 0x00;//data:(user value)
00420     */
00421     printCANTX();          //CAN送信データをPCに表示
00422     canPort.write(canmsgTx);//CANでデータ送信
00423     wait_ms(1);
00424 }
00425  
00426  
00427 //0x2F-6060-00-03-//-//-//
00428 void sendOPModeV(int nodeID){
00429     canmsgTx.id = 0x600+nodeID;
00430     canmsgTx.len = 5;       //Data Length
00431     canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
00432     canmsgTx.data[1] = 0x60;//Index LowByte
00433     canmsgTx.data[2] = 0x60;//Index HighByte
00434     canmsgTx.data[3] = 0x00;//sub-Index
00435     canmsgTx.data[4] = 0x03;//data:0x03 = "Profile Velocity Mode"
00436     /*
00437     canmsgTx.data[5] = 0x00;//data:(user value)
00438     canmsgTx.data[6] = 0x00;//data:(user value)
00439     canmsgTx.data[7] = 0x00;//data:(user value)
00440     */
00441     printCANTX();          //CAN送信データをPCに表示
00442     canPort.write(canmsgTx);//CANでデータ送信
00443     wait_ms(1);
00444 }
00445  
00446 //0x2B-6040-00-0000-//-//
00447 void sendCtrlRS(int nodeID){
00448     canmsgTx.id = 0x600+nodeID;
00449     canmsgTx.len = 6;       //Data Length
00450     canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
00451     canmsgTx.data[1] = 0x40;//Index LowByte
00452     canmsgTx.data[2] = 0x60;//Index HighByte
00453     canmsgTx.data[3] = 0x00;//sub-Index
00454     canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Shutdown)"
00455     canmsgTx.data[5] = 0x00;//data:0x"00"80
00456     /*
00457     canmsgTx.data[6] = 0x00;//data:(user value)
00458     canmsgTx.data[7] = 0x00;//data:(user value)
00459     */
00460     printCANTX();          //CAN送信データをPCに表示
00461     canPort.write(canmsgTx);//CANでデータ送信
00462     wait_ms(1);
00463 }
00464  
00465  
00466 //0x2B-6040-00-0006-//-//
00467 void sendCtrlSD(int nodeID){
00468     canmsgTx.id = 0x600+nodeID;
00469     canmsgTx.len = 6;       //Data Length
00470     canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
00471     canmsgTx.data[1] = 0x40;//Index LowByte
00472     canmsgTx.data[2] = 0x60;//Index HighByte
00473     canmsgTx.data[3] = 0x00;//sub-Index
00474     canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)"
00475     canmsgTx.data[5] = 0x00;//data:0x"00"06
00476     /*
00477     canmsgTx.data[6] = 0x00;//data:(user value)
00478     canmsgTx.data[7] = 0x00;//data:(user value)
00479     */
00480     printCANTX();          //CAN送信データをPCに表示
00481     canPort.write(canmsgTx);//CANでデータ送信
00482     wait_ms(1);
00483 }
00484  
00485 //0x2B-6040-00-000F-//-//
00486 void sendCtrlEN(int nodeID){
00487     canmsgTx.id = 0x600+nodeID;
00488     canmsgTx.len = 6;       //Data Length
00489     canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
00490     canmsgTx.data[1] = 0x40;//Index LowByte
00491     canmsgTx.data[2] = 0x60;//Index HighByte
00492     canmsgTx.data[3] = 0x00;//sub-Index
00493     canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)"
00494     canmsgTx.data[5] = 0x00;//data:0x"00"0F
00495     /*
00496     canmsgTx.data[6] = 0x00;//data:(user value)
00497     canmsgTx.data[7] = 0x00;//data:(user value)
00498     */
00499     //printCANTX();          //CAN送信データをPCに表示
00500     canPort.write(canmsgTx);//CANでデータ送信
00501     wait_ms(1);
00502 }
00503  
00504 //0x2B-6040-00-000B-//-//
00505 void sendCtrlQS(int nodeID){
00506     canmsgTx.id = 0x600+nodeID;
00507     canmsgTx.len = 6;       //Data Length
00508     canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
00509     canmsgTx.data[1] = 0x40;//Index LowByte
00510     canmsgTx.data[2] = 0x60;//Index HighByte
00511     canmsgTx.data[3] = 0x00;//sub-Index
00512     canmsgTx.data[4] = 0x0B;//data:0x00"0B" = "Quick Stop"
00513     canmsgTx.data[5] = 0x00;//data:0x"00"0B
00514     /*
00515     canmsgTx.data[6] = 0x00;//data:(user value)
00516     canmsgTx.data[7] = 0x00;//data:(user value)
00517     */
00518     printCANTX();          //CAN送信データをPCに表示
00519     canPort.write(canmsgTx);//CANでデータ送信
00520     wait_ms(1);
00521 }
00522  
00523 //0x2B-6040-00-010F-//-//
00524 void sendCtrlHL(int nodeID){
00525     canmsgTx.id = 0x600+nodeID;
00526     canmsgTx.len = 6;       //Data Length
00527     canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
00528     canmsgTx.data[1] = 0x40;//Index LowByte
00529     canmsgTx.data[2] = 0x60;//Index HighByte
00530     canmsgTx.data[3] = 0x00;//sub-Index
00531     canmsgTx.data[4] = 0x0F;//data:0x01"0F" = "Halt"
00532     canmsgTx.data[5] = 0x01;//data:0x"01"0F
00533     /*
00534     canmsgTx.data[6] = 0x00;//data:(user value)
00535     canmsgTx.data[7] = 0x00;//data:(user value)
00536     */
00537     printCANTX();          //CAN送信データをPCに表示
00538     canPort.write(canmsgTx);//CANでデータ送信
00539     wait_ms(1);
00540 }
00541  
00542 //0x2B-60FF-00-[user data(4Byte)]
00543 void sendTgtTrq(int nodeID,int trq){
00544     //pc.printf("%dmA0x%08x\r\n",trq,trq);  //回転数送信データの表示
00545     canmsgTx.id = 0x600+nodeID;
00546     canmsgTx.len = 6;       //Data Length
00547     canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
00548     canmsgTx.data[1] = 0x30;//Index LowByte 71
00549     canmsgTx.data[2] = 0x20;//Index HighByte 60
00550     canmsgTx.data[3] = 0x00;//sub-Index
00551     //下位から1Byteずつdataに格納
00552     if(trq<0){
00553     trq=0xFFFF+trq+1;
00554     }
00555     
00556     //pc.printf("iii%d\r\n",trq);
00557     //canmsgTx.data[7]=((trq>>24)&0xFF);
00558     //canmsgTx.data[6]=((trq>>16)&0xFF);
00559     
00560     canmsgTx.data[5]=((trq>>8)&0xFF);
00561     canmsgTx.data[4]=((trq>>0)&0xFF);
00562     //printCANTX();          //CAN送信データをPCに表示
00563     canPort.write(canmsgTx);//CANでデータ送信
00564     wait_ms(2);
00565     //send Enable
00566     //pc.printf("Send Enable Command\r\n");
00567     //sendCtrlEN(nodeID);
00568     //wait(0.5);
00569 }
00570  
00571  
00572  
00573  
00574 //0x2B-60FF-00-[user data(4Byte)]
00575 void sendTgtVel(int nodeID,int rpm){
00576     //pc.printf("%drpm|0x%08x\r\n",rpm,rpm);  //回転数送信データの表示
00577     canmsgTx.id = 0x600+nodeID;
00578     canmsgTx.len = 8;       //Data Length
00579     canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
00580     canmsgTx.data[1] = 0xFF;//Index LowByte
00581     canmsgTx.data[2] = 0x60;//Index HighByte
00582     canmsgTx.data[3] = 0x00;//sub-Index
00583     //下位から1Byteずつdataに格納
00584     
00585     //pc.printf("%d\r\n",rpm);
00586     if(rpm<0){
00587     rpm=0xFFFFFFFF+rpm+1;
00588     } 
00589     canmsgTx.data[7]=((rpm>>24)&0xFF);
00590     canmsgTx.data[6]=((rpm>>16)&0xFF);
00591     canmsgTx.data[5]=((rpm>>8)&0xFF);
00592     canmsgTx.data[4]=((rpm>>0)&0xFF);
00593     
00594     //printCANTX();          //CAN送信データをPCに表示
00595     canPort.write(canmsgTx);//CANでデータ送信
00596     wait_ms(1);
00597     
00598 }
00599  
00600 void readActVel(int nodeID){
00601     //値が欲しいobjectのアドレスを送る
00602     canmsgTx.id = 0x600+nodeID;
00603     canmsgTx.len = 4;       //Data Length
00604     canmsgTx.data[0] = 0x40;//|0Byte:40|
00605     canmsgTx.data[1] = 0x6C;//Index LowByte
00606     canmsgTx.data[2] = 0x60;//Index HighByte
00607     canmsgTx.data[3] = 0x00;//sub-Index
00608     canPort.write(canmsgTx);
00609     wait_ms(1);
00610 }
00611  
00612 void readActPos(int nodeID){
00613     //値が欲しいobjectのアドレスを送る
00614     canmsgTx.id = 0x600+nodeID;
00615     canmsgTx.len = 4;       //Data Length
00616     canmsgTx.data[0] = 0x40;//|0Byte:40|
00617     canmsgTx.data[1] = 0x64;//Index LowByte
00618     canmsgTx.data[2] = 0x60;//Index HighByte
00619     canmsgTx.data[3] = 0x00;//sub-Index
00620     canPort.write(canmsgTx);
00621     wait_ms(1);
00622 }
00623  
00624 //加速度指定
00625 void sendProAcc(int nodeID,int rpm){
00626     if(rpm < 0){
00627         rpm += 0xFFFFFFFF;
00628         }
00629 //    pc.printf("%drpm|0x%08x\r\n",rpm,rpm);  //回転数送信データの表示
00630     canmsgTx.id = 0x600+nodeID;
00631     canmsgTx.len = 8;       //Data Length
00632     canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
00633     canmsgTx.data[1] = 0x83;//Index LowByte
00634     canmsgTx.data[2] = 0x60;//Index HighByte
00635     canmsgTx.data[3] = 0x00;//sub-Index
00636     //下位から1Byteずつdataに格納
00637     canmsgTx.data[4] = (rpm >> 0) & 0xFF;
00638     canmsgTx.data[5] = (rpm >> 8) & 0xFF;
00639     canmsgTx.data[6] = (rpm >> 16) & 0xFF;
00640     canmsgTx.data[7] = (rpm >> 24) & 0xFF;
00641 //    printCANTX();          //CAN送信データをPCに表示
00642     canPort.write(canmsgTx);//CANでデータ送信
00643     wait(0.01);
00644     //send Enable
00645 //    pc.printf("Send Enable Command\r\n");
00646     sendCtrlEN(nodeID);
00647     wait(0.01);
00648 }
00649  
00650 //減速度指定
00651 void sendProDec(int nodeID,int rpm){
00652     if(rpm < 0){
00653         rpm += 0xFFFFFFFF;
00654         }
00655 //    pc.printf("%drpm|0x%08x\r\n",rpm,rpm);  //回転数送信データの表示
00656     canmsgTx.id = 0x600+nodeID;
00657     canmsgTx.len = 8;       //Data Length
00658     canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
00659     canmsgTx.data[1] = 0x84;//Index LowByte
00660     canmsgTx.data[2] = 0x60;//Index HighByte
00661     canmsgTx.data[3] = 0x00;//sub-Index
00662     //下位から1Byteずつdataに格納
00663     canmsgTx.data[4] = (rpm >> 0) & 0xFF;
00664     canmsgTx.data[5] = (rpm >> 8) & 0xFF;
00665     canmsgTx.data[6] = (rpm >> 16) & 0xFF;
00666     canmsgTx.data[7] = (rpm >> 24) & 0xFF;
00667 //    printCANTX();          //CAN送信データをPCに表示
00668     canPort.write(canmsgTx);//CANでデータ送信
00669     wait(0.01);
00670     //send Enable
00671 //    pc.printf("Send Enable Command\r\n");
00672     sendCtrlEN(nodeID);
00673     wait(0.01);
00674 }
00675  
00676  
00677 //送信データの表示
00678 void printCANTX(void){
00679   //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
00680     pc.printf("0x%3x|",canmsgTx.id);
00681     for(char i=0;i < canmsgTx.len;i++){
00682         pc.printf("%02x|",canmsgTx.data[i]);
00683     }
00684     //pc.printf("\r\n");
00685 }
00686  
00687 //受信データの表示
00688  
00689 void printCANRX(void){
00690   //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
00691     //pc.printf("0x%3x|",canmsgRx.id);
00692     for(char i=0;i < canmsgRx.len;i++){
00693       //pc.printf("%02x|",canmsgRx.data[i]);
00694     }
00695     pc.printf("\r\n");
00696 }
00697  
00698 void CANdataRX(void){
00699     canPort.read(canmsgRx);
00700     printCANRX();
00701 }
00702  
00703 void SerialRX(void){
00704     Serialdata = pc.getc();
00705     //pc.printf("%c\r\n",Serialdata);
00706 }