Kanta Takasu
/
YUKA_FIN
gohome ok
main.cpp
- Committer:
- tks1
- Date:
- 2021-12-21
- Revision:
- 11:d87a09eb77b9
- Parent:
- 10:91d4b6117470
File content as of revision 11:d87a09eb77b9:
//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); }