Kanta Takasu
/
YUKA_tks
yuka_tks
yuka.h
- Committer:
- tks1
- Date:
- 2021-12-20
- Revision:
- 6:dd35cc3b5ca0
File content as of revision 6:dd35cc3b5ca0:
#include "mbed.h" #include "hcsr04.h" #define RPM_RIDE 400 #define RPM_CLEAN 400 #define ACC_RIDE 1000 #define DEC_RIDE 700 #define ACC_CLEAN 1000 #define DEC_CLEAN 1000 #define KICK 2000 #define CLEAN_OFFSET 6000 #define WALL 45 #define WALL_MIN 25 #define Standby_Time 10 #define GETOFF_TIME 3 //プロトタイプ宣言 //------------------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); Serial pc(USBTX, USBRX); Serial LED(PB_6,PB_7); 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 int nodeall=4; DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); //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); } 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); } 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); } }