File content as of revision 7:459f81b0b6c6:

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

#define RPM_RIDE 400
#define RPM_CLEAN 200
#define ACC_RIDE 1000
#define DEC_RIDE 700
#define ACC_CLEAN 1000
#define DEC_CLEAN 1000

#define KICK 2000
#define CLEAN_OFFSET 2000

#define WALL 45
#define WALL_MIN 25
#define Standby_Time 10
#define GETOFF_TIME 1
 
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
 
Timer t;
double Time = 0;

Serial pc(USBTX, USBRX);
char Serialdata;
BusOut LED(PA_4,PB_6,PB_7,PA_9);

 
//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受信処理
//---------------------------------------------
void set_ACC(int);
void set_DEC(int);
void set_MODE_V(void);
void set_MODE_T(void);

void vel_stop(void);

void vel_forward(int);
void vel_forward_con(int);
void vel_backward(int);
void vel_backward_con(int);
void vel_right(int);
void vel_right_con(int);
void vel_left(int);

//unsigned int get_cm_n(HCSR04, unsigned int);
//USE -> unsigned int dist_UnitA = get_cm_n(u2, 5);
unsigned int get_cm_n(HCSR04 &echo_unit,int echo_n){
    unsigned int sampled_dist=0;
    for (int iter_n = 0; iter_n <echo_n; iter_n++){
        echo_unit.start();
        sampled_dist += echo_unit.get_dist_cm();
    }
    return  (sampled_dist / echo_n);
} 

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 ActPos = 0;
    int Init_Pos = 0;
    
    //超音波センサ関係パラメータ
    int dist1,dist2,dist3,dist4;
    
    //PID制御関係
    //角度調整パラメータ


    #define DELTA_T1  0.1
    #define target_val1  0
    #define Kp1  3
    #define Ki1  0
    #define Kd1  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);
             
    int state_1 = 0;
    int state_2 = 0;    
    int ride_count = 0;

    int X_POS_TMP = 0;
    int dist1_ori = 0;
    int dist2_ori = 0;

    dist1 = 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();

    //set_MODE_T();

    printf("\nstart\r\n");

    while(1){
                
        Time = t.read();
        //pc.printf("state_1:%d  state_2:%d\r\n",state_1,state_2);
        pc.printf("state_1:%d state_2:%d ACT:%d TMP:%d \r\n",state_1,state_2,ActPos,X_POS_TMP);
        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");
        }
        dist1 = get_cm_n(u1, 5);
        dist2 = get_cm_n(u2, 5);
        dist3 = get_cm_n(u3, 5);
         
        /*--------------------------*/
        //
        if(state_1 == 0){//入力判断フェーズ
            state_2 = 0;
            if(ride_count >= 2 && Time > Standby_Time){
                state_1 = 20;    
            }else{   
                if(ActPos < (Init_Pos - KICK)){ //前入力検出
                    ride_count++; 
                    state_1 = 1;
                }else if(ActPos > (Init_Pos + KICK)){ //右入力検出
                    ride_count++;
                    state_1 = 11;
                }else{
                    set_MODE_T();
                }
            }
            
        }else if(state_1 == 1){//前進→壁検出フェーズ
            if(dist1 < WALL && dist1 >= WALL_MIN){
                vel_stop();
                wait(GETOFF_TIME);
                state_1 = 2;
            }else{
                set_ACC(ACC_RIDE);//加速度設定
                set_DEC(DEC_CLEAN);//減速度設定
                set_MODE_V();//速度制御モード送信
                vel_forward_con(RPM_RIDE);//前進速度指令
            }
        }else if(state_1 == 2){//前進からの帰還フェーズ
            if(ActPos > -3000){
                vel_stop();
                t.reset();
                t.start();
                state_1 = 0;
                wait(1.0);
            }else{
                vel_backward_con(RPM_RIDE);
            }
        }else if(state_1 == 11){//右進→壁検出フェーズ
            if(dist3 < WALL && dist3 >= WALL_MIN){
                vel_stop();
                wait(GETOFF_TIME);
                state_1 = 12;
            }else{
                set_ACC(ACC_RIDE);//加速度設定
                set_DEC(DEC_RIDE);//減速度設定
                set_MODE_V();//速度制御モード送信
                vel_right(RPM_RIDE);//R進速度指令
            }
        }else if(state_1 == 12){//右進からの帰還フェーズ
            if(ActPos < 3000){
                vel_stop();
                t.reset();
                t.start();
                state_1 = 0;
                wait(1.0);
            }else{
                vel_left(RPM_RIDE);
            } 
        }else if(state_1 == 20){//消毒モード
            if(state_2 == 0){
                if(dist1 < WALL && dist1 >= WALL_MIN){
                    X_POS_TMP = ActPos;
                    state_2 = 1;
                }else{
                    set_ACC(ACC_CLEAN);//加速度設定
                    set_DEC(DEC_CLEAN);//減速度設定
                    set_MODE_V();//速度制御モード送信
                    vel_forward_con(RPM_CLEAN);//前進速度指令
                }   
            }else if(state_2 == 1){
                if(abs(ActPos - X_POS_TMP) > CLEAN_OFFSET){
                    state_2 == 2;
                }else{
                    if(dist3 < WALL && dist3 >= WALL_MIN){
                        state_2 = 4;
                    }else{
                        set_ACC(ACC_CLEAN);//加速度設定
                        set_DEC(DEC_CLEAN);//減速度設定
                        set_MODE_V();//速度制御モード送信
                        vel_right(RPM_CLEAN);//右進速度指令
                    }
                }
            }else if(state_2 == 2){
                if(ActPos > -3000){
                    state_2 = 3;
                }else{
                    set_ACC(ACC_CLEAN);//加速度設定
                    set_DEC(DEC_CLEAN);//減速度設定
                    set_MODE_V();//速度制御モード送信
                    vel_backward_con(RPM_CLEAN);//後進速度指令
                }
            }else if(state_2 == 3){
                if(abs(ActPos - X_POS_TMP ) > CLEAN_OFFSET){
                    state_2 == 0;
                }else{
                    if(dist3 < WALL && dist3 >= WALL_MIN){
                        state_2 = 4;
                    }else{
                    set_ACC(ACC_CLEAN);//加速度設定
                    set_DEC(DEC_CLEAN);//減速度設定
                    set_MODE_V();//速度制御モード送信
                    vel_right(RPM_CLEAN);//右進速度指令
                    }
                } 
            }else if(state_2 == 4){
                if(ActPos < 3000){
                    state_2 = 5;
                }else{
                    vel_left(RPM_CLEAN);
                }        
            }else if(state_2 == 5){
                if(ActPos > -3000){
                    t.reset();
                    t.start();
                    state_1 = 0;
                    state_2 = 0;
                }else{
                    vel_backward_con(RPM_CLEAN);
                }
            }
        }        
    }
}      

void vel_right(int rpm){
    sendTgtVel(1,rpm);
    sendTgtVel(2,rpm*(-1));
    sendTgtVel(3,rpm*(-1));
    sendTgtVel(4,rpm);
    for(int i=1;i<= 4;i++){
        sendCtrlEN(i);
    }
}
void vel_right_con(int rpmA){
    int dis1 = get_cm_n(u1,5);
    int dis2 = get_cm_n(u2,5);
    
    //速度を指定
    int robot_angle = ((dis1 - dis2)*5);
    sendTgtVel(1,rpmA+robot_angle);
    sendTgtVel(2,rpmA*(-1)+robot_angle);
    sendTgtVel(3,rpmA*(-1)+robot_angle);
    sendTgtVel(4,rpmA+robot_angle);
    //指令値を送信
    for(int i=1;i<= 4;i++){
        sendCtrlEN(i);
    }
}
 

void vel_left(int rpm){
    sendTgtVel(1,rpm*(-1));
    sendTgtVel(2,rpm);
    sendTgtVel(3,rpm);
    sendTgtVel(4,rpm*(-1));
    for(int i=1;i<= 4;i++){
        sendCtrlEN(i);
    }
}


void vel_left_con(int rpmA){
    int dis1 = get_cm_n(u1,5);
    int dis2 = get_cm_n(u2,5);
    
    //速度を指定
    int robot_angle = ((dis1 - dis2)*5);
    sendTgtVel(1,rpmA*(-1)+robot_angle);
    sendTgtVel(2,rpmA+robot_angle);
    sendTgtVel(3,rpmA+robot_angle);
    sendTgtVel(4,rpmA*(-1)+robot_angle);
    //指令値を送信
    for(int i=1;i<= 4;i++){
        sendCtrlEN(i);
    }
}

void vel_stop(){
    //速度を指定
    sendTgtVel(1,0);
    sendTgtVel(2,0);
    sendTgtVel(3,0);
    sendTgtVel(4,0);
    //指令値を送信
    for(int i=1;i<= 4;i++){
        sendCtrlEN(i);
    }
}

void vel_backward(int rpm){
    //速度を指定
    sendTgtVel(1,rpm);
    sendTgtVel(2,rpm);
    sendTgtVel(3,rpm*(-1));
    sendTgtVel(4,rpm*(-1));
    //指令値を送信
    for(int i=1;i<= 4;i++){
        sendCtrlEN(i);
    }
}

void vel_backward_con(int rpmA){
    int dis1 = get_cm_n(u1,5);
    int dis2 = get_cm_n(u2,5);
    
    //速度を指定
    int robot_angle = ((dis1 - dis2)*10);
    sendTgtVel(1,rpmA+robot_angle);
    sendTgtVel(2,rpmA+robot_angle);
    sendTgtVel(3,rpmA*(-1)+robot_angle);
    sendTgtVel(4,rpmA*(-1)+robot_angle);
    //指令値を送信
    for(int i=1;i<= 4;i++){
        sendCtrlEN(i);
    }
}


void vel_forward_con(int rpmA){
    int dis1 = get_cm_n(u1,5);
    int dis2 = get_cm_n(u2,5);
    
    //速度を指定
    int robot_angle = ((dis1 - dis2)*10);
    sendTgtVel(1,rpmA*(-1)+robot_angle);
    sendTgtVel(2,rpmA*(-1)+robot_angle);
    sendTgtVel(3,rpmA+robot_angle);
    sendTgtVel(4,rpmA+robot_angle);
    //指令値を送信
    for(int i=1;i<= 4;i++){
        sendCtrlEN(i);
    }
}
 
void set_ACC(int setACC_val){
    sendProAcc(1,setACC_val);
    sendProAcc(2,setACC_val);
    sendProAcc(3,setACC_val);
    sendProAcc(4,setACC_val);
}

void set_DEC(int setDEC_val){
    sendProDec(1,setDEC_val);
    sendProDec(2,setDEC_val);
    sendProDec(3,setDEC_val);
    sendProDec(4,setDEC_val); 
}

void set_MODE_V(){
    sendOPModeV(1);
    sendOPModeV(2);
    sendOPModeV(3);
    sendOPModeV(4);
}

void set_MODE_T(){
    sendOPModeT(1);
    sendOPModeT(2);
    sendOPModeT(3);
    sendOPModeT(4);
}

//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);
}