Diff: yuka.h

Revision:

6:dd35cc3b5ca0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/yuka.h	Mon Dec 20 15:49:55 2021 +0000
@@ -0,0 +1,536 @@
+#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);
+    }
+}
+ 
\ No newline at end of file