File content as of revision 0:b4b94eb28093:

//2021/12/18更新
//YUKA本番機用プログラム
//入力切替確認済み
//一定時間の入力なしで動作切替
//ジグザグ動作実装前
//エンコーダ読み取りによる入力
//加減速度調整パラメータ実装
//PID制御による機体角度補正_

#include "mbed.h"
#include "hcsr04.h"


DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

Timer t;
double Time = 0;
#define Standby_Time 10
Serial pc(USBTX, USBRX);
char Serialdata;
BusOut myled(LED1, LED2, LED3, LED4);

//HCSR04 u1(p13, p14), u2(p11, p12), u3(p23, p24), u4(p25, p26); // Trigger(DO), Echo(PWMIN); LPC1768
HCSR04 u1(PA_0, PA_1),u2(PA_5, PA_6),u3(PB_0, PA_10); // Trigger(DO), Echo(PWMIN); f303k8

CANMessage canmsgTx;
CANMessage canmsgRx;
//CAN canPort(p30, p29);  //CAN name(PinName rd, PinName td) LPC1768
CAN canPort(PA_11, PA_12);  //CAN name(PinName rd, PinName td) F303k8

//プロトタイプ宣言
//------------------send関数-------------------
//mode Setting
void sendOPModeT(int);       //Operating Mode
void sendOPModeV(int);       //Operating Mode
//Control Word
void sendCtrlRS(int);       //Reset
void sendCtrlSD(int);       //Shutdown
void sendCtrlEN(int);       //Switch on & Enable
void sendCtrlQS(int);       //Quick Stop
void sendCtrlHL(int);       //Halt
//Velocity Setting
void sendTgtVel(int,int);   //Target Velocity
//Torque Setting
void sendTgtTrq(int,int);   //Target Torque
//Acceleration Setting
void sendProAcc(int,int);   //Plof Acceleration
void sendProDec(int,int);   //Plof Deceleration
//------------------read関数-------------------
void readActVel(int);       //Actual Velocity
void readActPos(int);       //Actual Position
//-------------------その他--------------------
void printCANTX(void);      //CAN送信データをPCに表示
void printCANRX(void);      //CAN受信データをPCに表示
void CANdataRX(void);       //CAN受信処理
void SerialRX(void);        //Serial受信処理

int nodeall=4;

int main(){
    //Serial
    pc.attach(SerialRX);
    //pc.baud(115200);
    
    //CAN
    canPort.frequency(1000000); //Bit Rate:1MHz
    canPort.attach(CANdataRX,CAN::RxIrq);
    int node1 = 1;  //CAN node Setting
    int node2 = 2;
    int node3 = 3;
    int node4 = 4;
    
    //モータ回転数
    int rpm = 600;
    
    //各モータ回転数
    int rpm1 = rpm; //Velocity Setting[rpm]
    int rpm2 = rpm; //Velocity Setting[rpm]
    int rpm3 = rpm; //Velocity Setting[rpm]
    int rpm4 = rpm; //Velocity Setting[rpm]
    
    //エンコーダ関係
    int ActPos = 0;
    int Init_Pos = 0;
    
    //超音波センサ関係パラメータ
    int max_rpm = rpm + 200;
    int min_rpm = rpm -200;
    int wall_distance = 0;
    int dist1, dist2,dist3, dist4;
    
    
    //PID制御関係
    //角度調整パラメータ
    int p1, i1, d1;
    int error_before1 = 0;
    int error_now1 = 0;
    int feedback_val1;
    int integral1;
    int pid_sum1;
    #define DELTA_T1  0.1
    #define target_val1  0
    #define Kp1  3
    #define Ki1  0
    #define Kd1  0
    
    /*
    //距離調整パラメータ
    int p2, i2, d2;
    int error_before2 = 0;
    int error_now2 = 0;
    int feedback_val2;
    int integral2;
    int pid_sum2;
    #define DELTA_T2  0.01
    #define target_val2  50
    #define Kp2  2.0
    #define Ki2  3.0
    #define Kd2  3.0
    */
      
    pc.printf("YUKA PROGRAM START\r\n");
    wait(0.1);
    //-------------起動時に必ず送信---------------
    //オペレーティングモードを送信
    sendOPModeT(node1);
    sendOPModeT(node2);
    sendOPModeT(node3);
    sendOPModeT(node4);
        
    //Shutdown,Enableコマンド送信｜リセット
    sendCtrlSD(node1);
    sendCtrlSD(node2);
    sendCtrlSD(node3);
    sendCtrlSD(node4);

    sendCtrlEN(node1);
    sendCtrlEN(node2);
    sendCtrlEN(node3);
    sendCtrlEN(node4);
    
    //初期加減速度
    int Acc = 2000;
    int Dec = 2000;
    
    sendProAcc(1,Acc);
    sendProAcc(2,Acc);
    sendProAcc(3,Acc);
    sendProAcc(4,Acc);
           
    sendProDec(1,Dec);
    sendProDec(2,Dec);
    sendProDec(3,Dec);
    sendProDec(4,Dec);  

    //トルク設定
    int trq = 100;   //torque Setting[mA]
  
    sendTgtTrq(1,trq);
    sendTgtTrq(2,trq);
    sendTgtTrq(3,trq);
    sendTgtTrq(4,trq);
    
    
    //-------------------------------------------   
        /*
        sendCtrlHL(node1);
        sendCtrlHL(node2);
        sendCtrlHL(node3);
        sendCtrlHL(node4);

        //-------------送信コマンドを選択--------------
        if(Serialdata == 'a'){
            //左移動
                sendTgtVel(node1,rpm*(-1));
                sendTgtVel(node2,rpm);
                sendTgtVel(node3,rpm);
                sendTgtVel(node4,rpm*(-1));
                for(int i=1;i<= 4;i++){
                        sendCtrlEN(i);
                }
                
            Serialdata = 0;
            fl=1;
            wait(0.5);
        }
        
        else if(Serialdata == 'd'){
            //右移動
                sendTgtVel(node1,rpm);
                sendTgtVel(node2,rpm*(-1));
                sendTgtVel(node3,rpm*(-1));
                sendTgtVel(node4,rpm);
                for(int i=1;i<= 4;i++){
                        sendCtrlEN(i);
                }
            Serialdata = 0;
            fl=1;
            wait(0.5);
        }
        
        else if(Serialdata == 'w'){
            //前移動
                sendTgtVel(node1,rpm*(-1));
                sendTgtVel(node2,rpm*(-1));
                sendTgtVel(node3,rpm);
                sendTgtVel(node4,rpm);
                for(int i=1;i<= 4;i++){
                        sendCtrlEN(i);
                }
            Serialdata = 0;
            fl=1;
            wait(0.5);
        }
        
        else if(Serialdata == 's'){
            //後移動
                sendTgtVel(node1,rpm);
                sendTgtVel(node2,rpm);
                sendTgtVel(node3,rpm*(-1));
                sendTgtVel(node4,rpm*(-1));
                for(int i=1;i<= 4;i++){
                        sendCtrlEN(i);
                }
            Serialdata = 0;
            fl=1;
            wait(0.5);
        }
        
        else if(Serialdata == 'e'){
            //右旋回
                sendTgtVel(node1,rpm);
                sendTgtVel(node2,rpm);
                sendTgtVel(node3,rpm);
                sendTgtVel(node4,rpm);
                for(int i=1;i<= 4;i++){
                        sendCtrlEN(i);
                }
            Serialdata = 0;
            fl=1;
            wait(0.1);
        }
        else if(Serialdata == 'q'){
            //左旋回
                sendTgtVel(node1,rpm*(-1));
                sendTgtVel(node2,rpm*(-1));
                sendTgtVel(node3,rpm*(-1));
                sendTgtVel(node4,rpm*(-1));
                for(int i=1;i<= 4;i++){
                        sendCtrlEN(i);
                }
            Serialdata = 0;
            fl=1;
            wait(0.1);
        }
        */
        //------------------------------------------
        
          //人の入力待ち
          /*
          while(1){
          readActPos(1);
          ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
          if(ActPos > 8388608){
              ActPos -= 0x1000000;
              printf("check\r\n");
          } 
          printf("ActPos = %d\r\n",ActPos);
          wait(0.1);
          }               
          */
             
          while(1){  
                      
            ActPos = 0;
            Init_Pos = 0;
            readActPos(1);
            ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
            if(ActPos > 8388608){
              ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
            }
            Init_Pos = ActPos;//起動時の角度を保存
            t.reset();
            t.start();

            while(1){
                 
                 Time = t.read();
                 
                 readActPos(1);
                 ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
                 if(ActPos > 8388608){
                   ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
                   //printf("check\r\n");
                 }
                 printf("dsit1 %d\r\n",dist1);
                 /*--------------------------*/
                 //前方向に入力があった時
                 if(ActPos < Init_Pos - 2000){
                                  
                 sendProAcc(1,1000);
                 sendProAcc(2,1000);
                 sendProAcc(3,1000);
                 sendProAcc(4,1000);
           
                 sendProDec(1,700);
                 sendProDec(2,700);
                 sendProDec(3,700);
                 sendProDec(4,700);  
    
                 //速度制御モード送信    
                 sendOPModeV(1);
                 sendOPModeV(2);
                 sendOPModeV(3);
                 sendOPModeV(4);
                 
                 while(1){
                     
                 u1.start();
                 u2.start();           
                 dist1 = u1.get_dist_cm();
                 dist2 = u2.get_dist_cm();    
   
                 //速度を指定
                 sendTgtVel(1,rpm*(-1));
                 sendTgtVel(2,rpm*(-1));
                 sendTgtVel(3,rpm);
                 sendTgtVel(4,rpm);
                 //指令値を送信
                 for(int i=1;i<= 4;i++){
                     sendCtrlEN(i);
                 }
                 
                 if(dist1 < 100 && dist1 >= 70){
                     break;
                 }
                 printf("dsit1 %d\r\n",dist1);
                 }
                 
                 //速度を指定
                 sendTgtVel(1,0);
                 sendTgtVel(2,0);
                 sendTgtVel(3,0);
                 sendTgtVel(4,0);
                 //指令値を送信
                 for(int i=1;i<= 4;i++){
                     sendCtrlEN(i);    
                 }
                 //実行時間
                 printf("stop!!\r\n");
                 wait(5.0);
                 
                 //printf("2\r\n");
                 
                 //速度を指定
                 
                 while(1){
                 readActPos(1);
                 ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
                 if(ActPos > 8388608){
                   ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
                   //printf("check\r\n");
                 }
                 
                 if(ActPos > -10000){
                     break;
                 }    

                 sendTgtVel(1,rpm);
                 sendTgtVel(2,rpm);
                 sendTgtVel(3,rpm*(-1));
                 sendTgtVel(4,rpm*(-1));
                 //指令値を送信
                 for(int i=1;i<= 4;i++){
                     sendCtrlEN(i);    
                 }
                 //printf("3\r\n");
                 }

                 //ブレーキ
                 sendTgtVel(1,0);
                 sendTgtVel(2,0);
                 sendTgtVel(3,0);
                 sendTgtVel(4,0);
                 for(int i=1;i<= 4;i++){
                     sendCtrlEN(i);    
                 }
                 
                 wait(0.5);
                 
                 sendOPModeT(1);
                 sendOPModeT(2);
                 sendOPModeT(3);
                 sendOPModeT(4);
                 
                 dist1 = 0;
                 
                 wait(3.0);
                 break;
                 }
                 /*--------------------------*/
                 
                 /*--------------------------*/
                 //右方向に入力があった時
                 if(ActPos > Init_Pos + 2000){
                 sendProAcc(1,1000);
                 sendProAcc(2,1000);
                 sendProAcc(3,1000);
                 sendProAcc(4,1000);
           
                 sendProDec(1,1000);
                 sendProDec(2,1000);
                 sendProDec(3,1000);
                 sendProDec(4,1000);  

                 //速度制御モード送信    
                 sendOPModeV(1);
                 sendOPModeV(2);
                 sendOPModeV(3);
                 sendOPModeV(4);
                 //速度を指定
                 sendTgtVel(1,rpm);
                 sendTgtVel(2,rpm*(-1));
                 sendTgtVel(3,rpm*(-1));
                 sendTgtVel(4,rpm);
                 //指令値を送信
                 for(int i=1;i<= 4;i++){
                     sendCtrlEN(i);
                 }
                 //実行時間
                 wait(2.0);
                 
                 //速度を指定
                 sendTgtVel(1,0);
                 sendTgtVel(2,0);
                 sendTgtVel(3,0);
                 sendTgtVel(4,0);
                 //指令値を送信
                 for(int i=1;i<= 4;i++){
                     sendCtrlEN(i);    
                 }
                 //実行時間
                 wait(5.0);

                 
                 //速度を指定
                 sendTgtVel(1,rpm*(-1));
                 sendTgtVel(2,rpm);
                 sendTgtVel(3,rpm);
                 sendTgtVel(4,rpm*(-1));
                 //指令値を送信
                 for(int i=1;i<= 4;i++){
                     sendCtrlEN(i);    
                 }
                 //実行時間
                 wait(2.0);

                 //ブレーキ
                 sendTgtVel(1,0);
                 sendTgtVel(2,0);
                 sendTgtVel(3,0);
                 sendTgtVel(4,0);
                 for(int i=1;i<= 4;i++){
                     sendCtrlEN(i);    
                 }
                 wait(0.5);
                 
                 sendOPModeT(1);
                 sendOPModeT(2);
                 sendOPModeT(3);
                 sendOPModeT(4);
                 
                 wait(4.0);
                 break;
                 }
                 /*--------------------------*/
                 
                 /*--------------------------*/
                 //一定時間入力がない場合
                 if(Time > Standby_Time){
                 sendProAcc(1,5000);
                 sendProAcc(2,5000);
                 sendProAcc(3,5000);
                 sendProAcc(4,5000);
           
                 sendProDec(1,5000);
                 sendProDec(2,5000);
                 sendProDec(3,5000);
                 sendProDec(4,5000);  
    
                 //速度制御モード送信    
                 sendOPModeV(1);
                 sendOPModeV(2);
                 sendOPModeV(3);
                 sendOPModeV(4);
                     
                 //速度を指定
                 sendTgtVel(1,rpm);
                 sendTgtVel(2,rpm);
                 sendTgtVel(3,rpm);
                 sendTgtVel(4,rpm);
                 //指令値を送信
                 for(int i=1;i<= 4;i++){
                     sendCtrlEN(i);
                 }
                 //実行時間
                 wait(1.0);
                 
                 //速度を指定
                 sendTgtVel(1,rpm*(-1));
                 sendTgtVel(2,rpm*(-1));
                 sendTgtVel(3,rpm*(-1));
                 sendTgtVel(4,rpm*(-1));
                 //指令値を送信
                 for(int i=1;i<= 4;i++){
                     sendCtrlEN(i);    
                 }
                 //実行時間
                 wait(1.3);

                 //ブレーキ
                 sendTgtVel(1,0);
                 sendTgtVel(2,0);
                 sendTgtVel(3,0);
                 sendTgtVel(4,0);
                 for(int i=1;i<= 4;i++){
                     sendCtrlEN(i);    
                 }

                 wait(0.5);
                 
                 sendOPModeT(1);
                 sendOPModeT(2);
                 sendOPModeT(3);
                 sendOPModeT(4);
                 
                 wait(5.0);
                 break;
                 }
                 /*--------------------------*/
                 
           }     
                 wait(0.1);   
        }
             
        /*
        //ジグザグ        
        else if(Serialdata == 'j'){
          int turn_count = 0;
          p1 = 0;
          i1 = 0;
          d1 = 0;
          error_before1 = 0;
          error_now1 = 0;
          feedback_val1 = 0;
          integral1 = 0;
          pid_sum1 = 0;
          
          rpm1 = rpm;
          rpm2 = rpm;
          rpm3 = rpm;
          rpm4 = rpm;
          
          dist1 = 30;
          dist2 = 30;
          dist3 = 30;
          
          sendProAcc(1,5000);
          sendProAcc(2,5000);
          sendProAcc(3,5000);
          sendProAcc(4,5000);
            
          sendProDec(1,5000);
          sendProDec(2,5000);
          sendProDec(3,5000);
          sendProDec(4,5000);  

          //正面壁に近づく
          while(!(dist1 <30 && dist1 >5)){
                printf("phase1\r\n");
                u1.start();
                u2.start();           
                dist1 = u1.get_dist_cm();
                dist2 = u2.get_dist_cm();
                pc.printf("%ld %ld %ld ID1_%ld ID2_%ld ID3_%ld ID_4%ld\r\n",dist1,dist2,dist3,rpm1,rpm2,rpm3,rpm4);
                
                //壁との角度をPID制御
                feedback_val1 = (dist1 - dist2);
                
                error_before1 = error_now1;
                error_now1 = feedback_val1 - target_val1;
                integral1 += (error_now1 + error_before1) / 2.0 * DELTA_T1;
                
                p1 = Kp1 * error_now1;
                i1 = Ki1 * integral1;
                d1 = Kd1 * (error_now1 - error_before1) / DELTA_T1;
                
                pid_sum1 = (p1 + i1 + d1);
                
                //モータの回転数を更新
                rpm1 -= pid_sum1;
                rpm2 -= pid_sum1;
                rpm3 += pid_sum1;
                rpm4 += pid_sum1;   
                                 
                sendTgtVel(node1,rpm1);
                sendTgtVel(node2,rpm2);
                sendTgtVel(node3,rpm3*(-1));
                sendTgtVel(node4,rpm4*(-1));
                for(int i=1;i<= 4;i++){
                sendCtrlEN(i);
                }
                
                //回転数フィルタ 
            if(rpm1 >= max_rpm){
                rpm1 = max_rpm;
                sendTgtVel(node1,rpm1*(-1));
                sendCtrlEN(1);
            }
            
            if(rpm2 >= max_rpm){
                rpm2 = max_rpm;
                sendTgtVel(node2,rpm2);
                sendCtrlEN(2);
            }
            
            if(rpm3 >= max_rpm){
                rpm3 = max_rpm;
                sendCtrlEN(3);
            }
            
            if(rpm4 >= max_rpm){
                rpm4 = max_rpm;
                sendTgtVel(node4,rpm4*(-1));
                sendCtrlEN(4);
            }
            
            
            if(rpm1 <= min_rpm){
                rpm1 = min_rpm;
                sendTgtVel(node1,rpm1*(-1));
                sendCtrlEN(1);
            }
            
            if(rpm2 <= min_rpm){
                rpm2 = min_rpm;
                sendTgtVel(node2,rpm2);
                sendCtrlEN(2);
            }
            
            if(rpm3 <= min_rpm){
                rpm3 = min_rpm;
                sendTgtVel(node3,rpm3);
                sendCtrlEN(3);
            }
            
            if(rpm4 <= min_rpm){
                rpm4 = min_rpm;
                sendTgtVel(node4,rpm4*(-1));
                sendCtrlEN(4);
            }
            
            if(Serialdata == 'h'){     
                sendCtrlHL(node1);
                sendCtrlHL(node2);
                sendCtrlHL(node3);
                sendCtrlHL(node4);    
             }


                wait(0.01);
           }
           
          //u1,u2が一定以上壁から離れると終了
          while(!(turn_count == 4)){
                
                printf("phase2\r\n");
                
                dist3 = 30;
                
               //右端まで移動
               //while(dist3 > 20){
                 while(!(dist3 < 30& dist3 >10)){
                    printf("phase3\r\n");
                    u1.start();
                    u2.start();
                    u3.start();
                    
                    dist1 = u3.get_dist_cm();
                    dist2 = u3.get_dist_cm();            
                    dist3 = u3.get_dist_cm();
                    pc.printf("%ld %ld %ld ID1_%ld ID2_%ld ID3_%ld ID_4%ld\r\n",dist1,dist2,dist3,rpm1,rpm2,rpm3,rpm4); 
                    
                    //壁との角度をPID制御
                    feedback_val1 = (dist1 - dist2);
                    error_before1 = error_now1;
                    error_now1 = feedback_val1 - target_val1;
                    integral1 += (error_now1 + error_before1) / 2.0 * DELTA_T1;
                
                    p1 = Kp1 * error_now1;
                    i1 = Ki1 * integral1;
                    d1 = Kd1 * (error_now1 - error_before1) / DELTA_T1;
                
                    pid_sum1 = (p1 + i1 + d1);     
                    
                    //モータの回転数を更新
                    rpm1 += pid_sum1;
                    rpm2 -= pid_sum1;
                    rpm3 -= pid_sum1;
                    rpm4 += pid_sum1;
                                                       
                    //右移動
                    sendTgtVel(node1,rpm1*(-1));
                    sendTgtVel(node2,rpm2);
                    sendTgtVel(node3,rpm3);
                    sendTgtVel(node4,rpm4*(-1));
                    for(int i=1;i<= 4;i++){
                    sendCtrlEN(i);
                    }
                    //回転数フィルタ 
            if(rpm1 >= max_rpm){
                rpm1 = max_rpm;
                sendTgtVel(node1,rpm1*(-1));
                sendCtrlEN(1);
            }
            
            if(rpm2 >= max_rpm){
                rpm2 = max_rpm;
                sendTgtVel(node2,rpm2);
                sendCtrlEN(2);
            }
            
            if(rpm3 >= max_rpm){
                rpm3 = max_rpm;
                sendCtrlEN(3);
            }
            
            if(rpm4 >= max_rpm){
                rpm4 = max_rpm;
                sendTgtVel(node4,rpm4*(-1));
                sendCtrlEN(4);
            }
            
            
            if(rpm1 <= min_rpm){
                rpm1 = min_rpm;
                sendTgtVel(node1,rpm1*(-1));
                sendCtrlEN(1);
            }
            
            if(rpm2 <= min_rpm){
                rpm2 = min_rpm;
                sendTgtVel(node2,rpm2);
                sendCtrlEN(2);
            }
            
            if(rpm3 <= min_rpm){
                rpm3 = min_rpm;
                sendTgtVel(node3,rpm3);
                sendCtrlEN(3);
            }
            
            if(rpm4 <= min_rpm){
                rpm4 = min_rpm;
                sendTgtVel(node4,rpm4*(-1));
                sendCtrlEN(4);
            }
            
            if(Serialdata == 'h'){     
                sendCtrlHL(node1);
                sendCtrlHL(node2);
                sendCtrlHL(node3);
                sendCtrlHL(node4);       
             }


                    wait(0.01);
                }
                
               //下に少し移動

                    printf("phase4\r\n");

                    sendTgtVel(node1,rpm*(-1));
                    sendTgtVel(node2,rpm*(-1));
                    sendTgtVel(node3,rpm);
                    sendTgtVel(node4,rpm);
                    
                    for(int i=1;i<= 4;i++){
                    sendCtrlEN(i);
                    }
                    
                    turn_count += 1;
                                  
                    wait(0.5);
                
                
               //左に移動 
               while(!(dist3 < 80 && dist3 >60)){
                    printf("phase5\r\n");
                    u1.start();
                    u2.start();
                    u3.start();             
                    dist1 = u1.get_dist_cm();
                    dist2 = u2.get_dist_cm();
                    dist3 = u3.get_dist_cm();
                    pc.printf("%ld %ld %ld ID1_%ld ID2_%ld ID3_%ld ID_4%ld\r\n",dist1,dist2,dist3,rpm1,rpm2,rpm3,rpm4); 
                    
                    //壁との角度をPID制御
                    feedback_val1 = (dist1 - dist2);                
                    error_before1 = error_now1;
                    error_now1 = feedback_val1 - target_val1;
                    integral1 += (error_now1 + error_before1) / 2.0 * DELTA_T1;
                
                    p1 = Kp1 * error_now1;
                    i1 = Ki1 * integral1;
                    d1 = Kd1 * (error_now1 - error_before1) / DELTA_T1;
                
                    pid_sum1 = (p1 + i1 + d1);
                                    
                    //モータの回転数を更新
                    rpm1 -= pid_sum1;
                    rpm2 += pid_sum1;
                    rpm3 += pid_sum1;
                    rpm4 -= pid_sum1;  
                                    
                    //左移動
                    sendTgtVel(node1,rpm1);
                    sendTgtVel(node2,rpm2*(-1));
                    sendTgtVel(node3,rpm3*(-1));
                    sendTgtVel(node4,rpm4);
                    for(int i=1;i<= 4;i++){                 
                    sendCtrlEN(i);
                    }
                    //回転数フィルタ 
            if(rpm1 >= max_rpm){
                rpm1 = max_rpm;
                sendTgtVel(node1,rpm1*(-1));
                sendCtrlEN(1);
            }
            
            if(rpm2 >= max_rpm){
                rpm2 = max_rpm;
                sendTgtVel(node2,rpm2);
                sendCtrlEN(2);
            }
            
            if(rpm3 >= max_rpm){
                rpm3 = max_rpm;
                sendCtrlEN(3);
            }
            
            if(rpm4 >= max_rpm){
                rpm4 = max_rpm;
                sendTgtVel(node4,rpm4*(-1));
                sendCtrlEN(4);
            }
            
            
            if(rpm1 <= min_rpm){
                rpm1 = min_rpm;
                sendTgtVel(node1,rpm1*(-1));
                sendCtrlEN(1);
            }
            
            if(rpm2 <= min_rpm){
                rpm2 = min_rpm;
                sendTgtVel(node2,rpm2);
                sendCtrlEN(2);
            }
            
            if(rpm3 <= min_rpm){
                rpm3 = min_rpm;
                sendTgtVel(node3,rpm3);
                sendCtrlEN(3);
            }
            
            if(rpm4 <= min_rpm){
                rpm4 = min_rpm;
                sendTgtVel(node4,rpm4*(-1));
                sendCtrlEN(4);
            }

            if(Serialdata == 'h'){     
                sendCtrlHL(node1);
                sendCtrlHL(node2);
                sendCtrlHL(node3);
                sendCtrlHL(node4);       
             }

                    wait(0.01);
                }
                
               //下に少し移動 
                
                    printf("phase6\r\n");
                    
                    
                    sendTgtVel(node1,rpm*(-1));
                    sendTgtVel(node2,rpm*(-1));
                    sendTgtVel(node3,rpm);
                    sendTgtVel(node4,rpm);
                    for(int i=1;i<= 4;i++){
                    sendCtrlEN(i);
                    }
                    
                    turn_count += 1;
                    
                    wait(0.5);
               
          }
               
                
            if(Serialdata == 'h'){     
                sendCtrlHL(node1);
                sendCtrlHL(node2);
                sendCtrlHL(node3);
                sendCtrlHL(node4);  
                break;     
             }  
             
            wait(0.1);
        }      
        
        }
        */
        //-------------------------------------------
        
}




//0x2F-6060-00-fd-//-//-//
void sendOPModeT(int nodeID){
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 5;       //Data Length
    canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x60;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    canmsgTx.data[4] = 0xFD;//data:fd = "current Mode"
    /*
    canmsgTx.data[5] = 0x00;//data:(user value)
    canmsgTx.data[6] = 0x00;//data:(user value)
    canmsgTx.data[7] = 0x00;//data:(user value)
    */
    printCANTX();          //CAN送信データをPCに表示
    canPort.write(canmsgTx);//CANでデータ送信
    wait_ms(1);
}


//0x2F-6060-00-03-//-//-//
void sendOPModeV(int nodeID){
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 5;       //Data Length
    canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x60;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    canmsgTx.data[4] = 0x03;//data:0x03 = "Profile Velocity Mode"
    /*
    canmsgTx.data[5] = 0x00;//data:(user value)
    canmsgTx.data[6] = 0x00;//data:(user value)
    canmsgTx.data[7] = 0x00;//data:(user value)
    */
    printCANTX();          //CAN送信データをPCに表示
    canPort.write(canmsgTx);//CANでデータ送信
    wait_ms(1);
}

//0x2B-6040-00-0000-//-//
void sendCtrlRS(int nodeID){
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 6;       //Data Length
    canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x40;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Shutdown)"
    canmsgTx.data[5] = 0x00;//data:0x"00"80
    /*
    canmsgTx.data[6] = 0x00;//data:(user value)
    canmsgTx.data[7] = 0x00;//data:(user value)
    */
    printCANTX();          //CAN送信データをPCに表示
    canPort.write(canmsgTx);//CANでデータ送信
    wait_ms(1);
}


//0x2B-6040-00-0006-//-//
void sendCtrlSD(int nodeID){
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 6;       //Data Length
    canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x40;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)"
    canmsgTx.data[5] = 0x00;//data:0x"00"06
    /*
    canmsgTx.data[6] = 0x00;//data:(user value)
    canmsgTx.data[7] = 0x00;//data:(user value)
    */
    printCANTX();          //CAN送信データをPCに表示
    canPort.write(canmsgTx);//CANでデータ送信
    wait_ms(1);
}

//0x2B-6040-00-000F-//-//
void sendCtrlEN(int nodeID){
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 6;       //Data Length
    canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x40;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)"
    canmsgTx.data[5] = 0x00;//data:0x"00"0F
    /*
    canmsgTx.data[6] = 0x00;//data:(user value)
    canmsgTx.data[7] = 0x00;//data:(user value)
    */
    //printCANTX();          //CAN送信データをPCに表示
    canPort.write(canmsgTx);//CANでデータ送信
    wait_ms(1);
}

//0x2B-6040-00-000B-//-//
void sendCtrlQS(int nodeID){
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 6;       //Data Length
    canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x40;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    canmsgTx.data[4] = 0x0B;//data:0x00"0B" = "Quick Stop"
    canmsgTx.data[5] = 0x00;//data:0x"00"0B
    /*
    canmsgTx.data[6] = 0x00;//data:(user value)
    canmsgTx.data[7] = 0x00;//data:(user value)
    */
    printCANTX();          //CAN送信データをPCに表示
    canPort.write(canmsgTx);//CANでデータ送信
    wait_ms(1);
}

//0x2B-6040-00-010F-//-//
void sendCtrlHL(int nodeID){
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 6;       //Data Length
    canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x40;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    canmsgTx.data[4] = 0x0F;//data:0x01"0F" = "Halt"
    canmsgTx.data[5] = 0x01;//data:0x"01"0F
    /*
    canmsgTx.data[6] = 0x00;//data:(user value)
    canmsgTx.data[7] = 0x00;//data:(user value)
    */
    printCANTX();          //CAN送信データをPCに表示
    canPort.write(canmsgTx);//CANでデータ送信
    wait_ms(1);
}

//0x2B-60FF-00-[user data(4Byte)]
void sendTgtTrq(int nodeID,int trq){
    //pc.printf("%dmA0x%08x\r\n",trq,trq);  //回転数送信データの表示
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 6;       //Data Length
    canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x30;//Index LowByte 71
    canmsgTx.data[2] = 0x20;//Index HighByte 60
    canmsgTx.data[3] = 0x00;//sub-Index
    //下位から1Byteずつdataに格納
    if(trq<0){
    trq=0xFFFF+trq+1;
    }
    
    //pc.printf("iii%d\r\n",trq);
    //canmsgTx.data[7]=((trq>>24)&0xFF);
    //canmsgTx.data[6]=((trq>>16)&0xFF);
    
    canmsgTx.data[5]=((trq>>8)&0xFF);
    canmsgTx.data[4]=((trq>>0)&0xFF);
    //printCANTX();          //CAN送信データをPCに表示
    canPort.write(canmsgTx);//CANでデータ送信
    wait_ms(2);
    //send Enable
    //pc.printf("Send Enable Command\r\n");
    //sendCtrlEN(nodeID);
    //wait(0.5);
}




//0x2B-60FF-00-[user data(4Byte)]
void sendTgtVel(int nodeID,int rpm){
    //pc.printf("%drpm|0x%08x\r\n",rpm,rpm);  //回転数送信データの表示
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 8;       //Data Length
    canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0xFF;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    //下位から1Byteずつdataに格納
    
    //pc.printf("%d\r\n",rpm);
    if(rpm<0){
    rpm=0xFFFFFFFF+rpm+1;
    } 
    canmsgTx.data[7]=((rpm>>24)&0xFF);
    canmsgTx.data[6]=((rpm>>16)&0xFF);
    canmsgTx.data[5]=((rpm>>8)&0xFF);
    canmsgTx.data[4]=((rpm>>0)&0xFF);
    
    //printCANTX();          //CAN送信データをPCに表示
    canPort.write(canmsgTx);//CANでデータ送信
    wait_ms(1);
    
}

void readActVel(int nodeID){
    //値が欲しいobjectのアドレスを送る
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 4;       //Data Length
    canmsgTx.data[0] = 0x40;//|0Byte:40|
    canmsgTx.data[1] = 0x6C;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    canPort.write(canmsgTx);
    wait_ms(1);
}

void readActPos(int nodeID){
    //値が欲しいobjectのアドレスを送る
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 4;       //Data Length
    canmsgTx.data[0] = 0x40;//|0Byte:40|
    canmsgTx.data[1] = 0x64;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    canPort.write(canmsgTx);
    wait_ms(1);
}

//加速度指定
void sendProAcc(int nodeID,int rpm){
    if(rpm < 0){
        rpm += 0xFFFFFFFF;
        }
//    pc.printf("%drpm|0x%08x\r\n",rpm,rpm);  //回転数送信データの表示
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 8;       //Data Length
    canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x83;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    //下位から1Byteずつdataに格納
    canmsgTx.data[4] = (rpm >> 0) & 0xFF;
    canmsgTx.data[5] = (rpm >> 8) & 0xFF;
    canmsgTx.data[6] = (rpm >> 16) & 0xFF;
    canmsgTx.data[7] = (rpm >> 24) & 0xFF;
//    printCANTX();          //CAN送信データをPCに表示
    canPort.write(canmsgTx);//CANでデータ送信
    wait(0.01);
    //send Enable
//    pc.printf("Send Enable Command\r\n");
    sendCtrlEN(nodeID);
    wait(0.01);
}

//減速度指定
void sendProDec(int nodeID,int rpm){
    if(rpm < 0){
        rpm += 0xFFFFFFFF;
        }
//    pc.printf("%drpm|0x%08x\r\n",rpm,rpm);  //回転数送信データの表示
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 8;       //Data Length
    canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x84;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    //下位から1Byteずつdataに格納
    canmsgTx.data[4] = (rpm >> 0) & 0xFF;
    canmsgTx.data[5] = (rpm >> 8) & 0xFF;
    canmsgTx.data[6] = (rpm >> 16) & 0xFF;
    canmsgTx.data[7] = (rpm >> 24) & 0xFF;
//    printCANTX();          //CAN送信データをPCに表示
    canPort.write(canmsgTx);//CANでデータ送信
    wait(0.01);
    //send Enable
//    pc.printf("Send Enable Command\r\n");
    sendCtrlEN(nodeID);
    wait(0.01);
}


//送信データの表示
void printCANTX(void){
  //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
    pc.printf("0x%3x|",canmsgTx.id);
    for(char i=0;i < canmsgTx.len;i++){
        pc.printf("%02x|",canmsgTx.data[i]);
    }
    //pc.printf("\r\n");
}

//受信データの表示

void printCANRX(void){
  //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
    //pc.printf("0x%3x|",canmsgRx.id);
    for(char i=0;i < canmsgRx.len;i++){
      //pc.printf("%02x|",canmsgRx.data[i]);
    }
    pc.printf("\r\n");
}

void CANdataRX(void){
    canPort.read(canmsgRx);
    printCANRX();
}

void SerialRX(void){
    Serialdata = pc.getc();
    //pc.printf("%c\r\n",Serialdata);
}