File content as of revision 10:91d4b6117470:

//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 3

#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;
Timer cr_t;
double Time = 0;
double cr_time = 0;
int crflag=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);
int encX = 0;
int encY = 0;
int posX =0;
int posY =0;

//--------//
void  update_pos()
{
    readActPos(1);
    encX=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
    if(encX > 8388608) {
        encX -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
    }
    wait_ms(10);
    readActPos(2);
    encY=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
    if(encY > 8388608) {
        encY -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
    }
    wait_ms(10);
    posX = (encX*0.707)-(encY*0.707);
    posY = (-encX*0.707)-(encY*0.707);
    //printf("encX=[%4d],encY=[%4d],posX=[%4d],posY=[%d]\r\n",encX,encY,posX,posY);
}
void gohome()
{
    update_pos();
    while(posX>0) {
        update_pos();
        set_ACC(1000);//加速度設定
        set_DEC(1000);//減速度設定
        set_MODE_V();//速度制御モード送信
        vel_right(-300);//前進速度指令
    }
    vel_right(0);
    while(posY>0) {
        update_pos();
        set_ACC(1000);//加速度設定
        set_DEC(1000);//減速度設定
        set_MODE_V();//速度制御モード送信
        vel_forward_con(-300);//前進速度指令
    }
    
}

//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_DIST_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();
            cr_t.reset();


            //set_MODE_T();

            printf("\nstart\r\n");
            LED = 0b1111;

            while(1) {

                Time = t.read();
                cr_time = cr_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,dist3,X_DIST_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");
                }
                dist3 = get_cm_n(u3, 5);
                dist1 = get_cm_n(u1, 5);
                dist2 = get_cm_n(u2, 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++;
                            LED = 0b0111;
                            state_1 = 1;
                        } else if(ActPos > (Init_Pos + KICK)) { //右入力検出
                            ride_count++;
                            LED = LED = 0b1101;;
                            state_1 = 11;
                        } else {
                            set_MODE_T();
                            LED = 0b1111;
                        }
                    }

                } else if(state_1 == 1) { //前進→壁検出フェーズ
                    if(dist1 < WALL && dist1 >= WALL_MIN) {
                        vel_stop();
                        wait(GETOFF_TIME);
                        LED = 0b1011;
                        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();
                        LED = 0b1111;
                        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);
                        LED = 0b1110;
                        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();
                        LED = 1111;
                        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_DIST_TMP = dist3;
                            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) {
                        int dist3_tmp = get_cm_n(u1,5);
                        wait_ms(10);
                        // if(dist3 < WALL && dist3 >= WALL_MIN){
                        // if(dist3_tmp<10){
                        update_pos();
                        if(posX<12000) {
                            gohome();
                            state_2 = 4;
                        } else {
                            set_ACC(ACC_CLEAN);//加速度設定
                            set_DEC(DEC_CLEAN);//減速度設定
                            set_MODE_V();//速度制御モード送信
                            vel_right(RPM_CLEAN);//右進速度指令
                            wait(CLEAN_OFFSET);
                            state_2 = 2;
                        }

                    } 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(dist3 < WALL && dist3 >= WALL_MIN) {
                            state_2 = 4;
                        } else {
                            set_ACC(ACC_CLEAN);//加速度設定
                            set_DEC(DEC_CLEAN);//減速度設定
                            set_MODE_V();//速度制御モード送信
                            vel_right(RPM_CLEAN);//右進速度指令
                            wait(CLEAN_OFFSET);
                            state_2 = 0;
                        }
                    } 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(int rpmA) {

            //速度を指定
            sendTgtVel(1,rpmA*(-1));
            sendTgtVel(2,rpmA*(-1));
            sendTgtVel(3,rpmA);
            sendTgtVel(4,rpmA);
            //指令値を送信
            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);
        }