Diff: ROBO2_CONT.cpp

Revision:

2:71b3736a1bd7

diff -r 4f6bcf445501 -r 71b3736a1bd7 ROBO2_CONT.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBO2_CONT.cpp	Fri Oct 21 10:54:03 2022 +0000
@@ -0,0 +1,754 @@
+#include "mbed.h"
+#include "main.h"
+#include "nextion_lcd.h"
+#include "APP.h"
+#include "ROBO2_CONT.h"
+#include "SAVE_CONT.h"
+///////////////////////
+#include "WIZnetInterface.h"
+#define USE_DHCP    0
+#define LOOPBACKPORT    7
+Ticker RB2_RX_TIME_OUT;
+
+extern const char * IP_Subnet;
+extern const char * IP_Gateway;
+extern unsigned char MAC_Addr[6];
+
+const char *RB2_SV_Addr     = "192.168.2.8";
+const int   RB2_SV_port     =30003;
+//const int   SV_port     =30005;
+extern SPI spi;
+WIZnetInterface RB2_ethernet(&spi,PB_1, PB_2);//CONT BOARD MINIは、PA_15,PB_2
+TCPSocketConnection RB2_server;
+
+///////////////////////
+char RB2_rob_tx_buffer[128];
+char RB2_rob_rx_buffer[128];
+
+
+double RB2_Q1SET=0.0;
+double RB2_Q2SET=0.0;
+double RB2_Q3SET=0.0;
+double RB2_Q4SET=0.0;
+double RB2_Q5SET=0.0;
+double RB2_Q6SET=0.0;
+
+
+double RB2_X_SET=0.0;
+double RB2_Y_SET=0.0;
+double RB2_Z_SET=0.0;
+double RB2_A_SET=0.0;
+double RB2_B_SET=0.0;
+double RB2_C_SET=0.0;
+
+extern double RB2_POINT_RD[32][4];
+
+
+unsigned char RB2_SPEED_ADJ=100;
+unsigned char RB2_ACC_ADJ=80;
+
+unsigned char RB2_PSEL_BAK=0xFF;
+
+
+unsigned char RB2_RX_TIME_OUT_F=0;
+
+unsigned char RB2_HAND_ST=RB2_HAND_OPEN;
+
+char RB2_J1_DAT[32];
+char RB2_J2_DAT[32];
+char RB2_J3_DAT[32];
+char RB2_J4_DAT[32];
+char RB2_XDAT[32];
+char RB2_YDAT[32];
+char RB2_ZDAT[32];
+char RB2_RDAT[32];
+
+double RB2_J1_GET=0.0;
+double RB2_J2_GET=0.0;
+double RB2_J3_GET=0.0;
+double RB2_J4_GET=0.0;
+
+double RB2_X_GET=0.0;
+double RB2_Y_GET=0.0;
+double RB2_Z_GET=0.0;
+double RB2_R_GET=0.0;
+
+
+unsigned char R2_WAIT_MASK=0;//1でマスク　現在位置への移動指定時WAITをマスクする
+
+void  RB2_RX_TIME_OUT_IRQ(){
+    RB2_RX_TIME_OUT_F=1;
+    RB2_RX_TIME_OUT.detach();//シリアル受信タイムアウトカウンタ停止
+    RB2_eth_buf_clr();
+
+}
+
+void RB2_eth_buf_clr(){
+    
+    for(unsigned long clcnt=0; clcnt<sizeof(RB2_rob_tx_buffer); clcnt++){  
+        RB2_rob_tx_buffer[clcnt]=0;
+    }
+    for(unsigned long clcnt=0; clcnt<sizeof(RB2_rob_rx_buffer); clcnt++){  
+        RB2_rob_rx_buffer[clcnt]=0;
+    }
+}
+void RB2_POS_CLR(){
+    RB2_Q1SET=0.0;
+    RB2_Q2SET=0.0;
+    RB2_Q3SET=0.0;
+    RB2_Q4SET=0.0;
+    RB2_Q5SET=0.0;
+    RB2_Q6SET=0.0;
+    
+    
+    RB2_X_SET=0.0;
+    RB2_Y_SET=0.0;
+    RB2_Z_SET=0.0;
+    RB2_A_SET=0.0;
+    RB2_B_SET=0.0;
+    RB2_C_SET=0.0;
+}
+char RB2_ETH_CONECT_CHK(){
+    
+    if(RB2_server.is_connected()==true){
+        return 0;   
+    }else{
+        lcd_print("t0","LAN切断中です。再接続してください。"); 
+        return -1; 
+    }   
+    
+}
+
+
+
+char RB2_CONNECT(){
+    unsigned long timeout=0;
+    sprintf(lcd_print_buf,"ロボット２(%s) 接続開始...",RB2_SV_Addr);
+    lcd_print("t1",lcd_print_buf); 
+  
+    int ret =RB2_server.connect(RB2_SV_Addr,RB2_SV_port);  
+
+    if(ret !=0){
+        sprintf(lcd_print_buf,"ロボット２(%s) 接続待機中...",RB2_SV_Addr);
+        lcd_print("t1",lcd_print_buf); 
+    } 
+    while(ret !=0 && timeout<100){
+        ret =RB2_server.connect(RB2_SV_Addr,RB2_SV_port); 
+        wait_ms(100); 
+    }
+    if(timeout<100){
+        printf("connect end\r\n");
+        sprintf(lcd_print_buf,"ロボット２(%s) 接続完了",RB2_SV_Addr);
+        lcd_print("t1",lcd_print_buf); 
+        return 0;
+    }else{
+        sprintf(lcd_print_buf,"ロボット２(%s) 接続異常",RB2_SV_Addr);
+        lcd_print("t1",lcd_print_buf); 
+        return -1;
+    }
+    
+}
+
+char RB2_DISCONNECT(){
+    lcd_print("t0","R2 Disconnect"); 
+    int ret =RB2_ethernet.disconnect();
+    wait_ms(2000);
+    if(ret==0){
+        lcd_print("t0","R2 Disconnect End"); 
+        return 0;
+    }else{
+        lcd_print("t0","R2 Disconnect ERR"); 
+        return -1;
+    }
+ 
+     
+}
+/*
+ロボットのタイムアウト検出は割り込みを使用して行う。
+通常のwaitをカウントする方法では、時間が大きくずれる為
+＊Etherの受信処理にもタイムアウト設定がある為
+WIZnet_Socket.hの以下の設定　下記だと一度ETHER受信しタイムアウトまで300msかかる
+set_blocking(bool blocking, unsigned int timeout=300);//1500
+
+*/
+void RB2_GETANG(unsigned long tout_ms){
+    char *cmp1_txt="GetAngle();";
+    unsigned char DAT_GET_F=0;
+    
+    RB2_SEND_CMD("GetAngle()");
+    RB2_RX_TIME_OUT_F=0;
+    RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000); 
+    while(RB2_RX_TIME_OUT_F==0 && DAT_GET_F==0){
+        if(RB2_RX_DATA()>0){
+            //移動開始確認///
+            if(strstr(RB2_rob_rx_buffer,cmp1_txt) !=NULL){
+                DAT_GET_F=1;
+            }else{
+                RB2_SEND_CMD("GetAngle()");
+            }
+        }
+    wait_us(10*1000); 
+    }
+    unsigned char DAT_CNT=0;
+    unsigned char DAT_CNT_T=3;
+    if(RB2_RX_TIME_OUT_F==1){
+        lcd_print("t0","RB2 GetAngle TIMEOUT");
+        
+    }else{
+        RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
+        while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_J1_DAT)){
+               RB2_J1_DAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
+        }
+        DAT_CNT_T++;//カンマを飛ばす
+        DAT_CNT=0;
+        while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_J2_DAT)){
+               RB2_J2_DAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
+        }   
+        DAT_CNT_T++;//カンマを飛ばす
+        DAT_CNT=0;
+        while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_J3_DAT)){
+               RB2_J3_DAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
+        }   
+        DAT_CNT_T++;//カンマを飛ばす
+        DAT_CNT=0;
+        while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_J4_DAT)){
+               RB2_J4_DAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
+        }  
+        RB2_J1_GET=atof(RB2_J1_DAT);
+        RB2_J2_GET=atof(RB2_J2_DAT);
+        RB2_J3_GET=atof(RB2_J3_DAT);
+        RB2_J4_GET=atof(RB2_J4_DAT);
+        
+    }
+
+
+    RB2_eth_buf_clr();
+}  
+void RB2_GETPOS(unsigned long tout_ms){
+    char *cmp1_txt="GetPose();";
+    unsigned char DAT_GET_F=0;
+    RB2_SEND_CMD("GetPose()");
+    RB2_RX_TIME_OUT_F=0;
+    RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000); 
+    while(RB2_RX_TIME_OUT_F==0 && DAT_GET_F==0){
+        if(RB2_RX_DATA()>0){
+            //移動開始確認///
+            if(strstr(RB2_rob_rx_buffer,cmp1_txt) !=NULL){
+                DAT_GET_F=1;
+            }else{
+               RB2_SEND_CMD("GetPose()");
+            }
+        } 
+        wait_us(10*1000); 
+    }
+    unsigned char DAT_CNT=0;
+    unsigned char DAT_CNT_T=3;
+    if(RB2_RX_TIME_OUT_F==1){
+        lcd_print("t0","RB2 GetPose TIMEOUT");
+    }else{
+        RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
+        while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_XDAT)){
+               RB2_XDAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
+        }
+        DAT_CNT_T++;//カンマを飛ばす
+        DAT_CNT=0;
+        while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_YDAT)){
+               RB2_YDAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
+        }   
+        DAT_CNT_T++;//カンマを飛ばす
+        DAT_CNT=0;
+        while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_ZDAT)){
+               RB2_ZDAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
+        }   
+        DAT_CNT_T++;//カンマを飛ばす
+        DAT_CNT=0;
+        while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_RDAT)){
+               RB2_RDAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
+        }  
+        RB2_X_GET=atof(RB2_XDAT);
+        RB2_Y_GET=atof(RB2_YDAT);
+        RB2_Z_GET=atof(RB2_ZDAT);
+        RB2_R_GET=atof(RB2_RDAT);
+        
+    }
+
+
+    RB2_eth_buf_clr();
+}  
+
+
+/*
+RobotMode()コマンドを使用してロボットの移動完了(Mode 5 idle)を確認する
+ Mode | Description           | NOTE                             |
+| ---- | --------------------- | -------------------------------- |
+| 1    | ROBOT_MODE_INIT       | initialization                   |
+| 2    | ROBOT_MODE_BRAKE_OPEN | The brake release                |
+| 3    |                       | Reserved                         |
+| 4    | ROBOT_MODE_DISABLED   | Disabled (brake is not released) |
+| 5    | ROBOT_MODE_ENABLE     | Enable (idle)                    |
+| 6    | ROBOT_MODE_BACKDRIVE  | Dragging state                   |
+| 7    | ROBOT_MODE_RUNNING    | Running state                    |
+| 8    | ROBOT_MODE_RECORDING  | Dragging recording               |
+| 9    | ROBOT_MODE_ERROR      | Alarm state                      |
+| 10   | ROBOT_MODE_PAUSE      | pause state                      |
+| 11   | ROBOT_MODE_JOG        | jogging state                    |
+
+
+*/
+void RB2_MOVE_WAIT(unsigned long tout_ms){//移動開始待ち
+    char *cmp1_txt="0,{7},RobotMode()";
+    char *cmp2_txt="0,{5},RobotMode()";
+    char *cmp3_txt="0,{9},RobotMode()";
+    
+    unsigned char MOVE_START_F=0;
+    unsigned char MOVE_END_F=0;
+    unsigned char MOVE_ERR_F=0;
+    if(R2_WAIT_MASK==0){//1でマスク　現在位置への移動指定時WAITをマスクする
+        RB2_SEND_CMD("RobotMode()");
+        RB2_RX_TIME_OUT_F=0;
+        RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
+        while(RB2_RX_TIME_OUT_F==0 && MOVE_START_F==0  && MOVE_ERR_F==0){
+            if(RB2_RX_DATA()>0){
+                if(strstr(RB2_rob_rx_buffer,cmp3_txt) ==NULL){//err状態でないことを確認
+                    //移動開始確認///
+                    if(strstr(RB2_rob_rx_buffer,cmp1_txt) !=NULL){
+                        MOVE_START_F=1;
+                    }else{
+                        RB2_SEND_CMD("RobotMode()");   
+                    }
+                }else{
+                    MOVE_ERR_F=1;
+                    ALM_ST=ALM_DOBOT2;
+                    ALM_HOLD_SET();
+                }
+            }
+            wait_us(10*1000); 
+        }
+         if(RB2_RX_TIME_OUT_F==1){
+            //printf("MOVE START TIMEOUT!! TX=%s  Rx=%s\r\n",RB2_rob_tx_buffer,RB2_rob_rx_buffer);
+            ALM_ST=ALM_DOBOT2_ST;
+            ALM_HOLD_SET();
+        }else{
+            RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
+        }
+        if(ALM_ST==ALM_NONE){
+             ///移動完了チェック///
+            RB2_RX_TIME_OUT_F=0;
+            RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000); 
+            RB2_SEND_CMD("RobotMode()");        
+            while(RB2_RX_TIME_OUT_F==0 && MOVE_END_F==0  && MOVE_ERR_F==0){
+                if(RB2_RX_DATA()>0){
+                    if(strstr(RB2_rob_rx_buffer,cmp3_txt) ==NULL){//err状態でないことを確認
+                        //移動完了確認///
+                        if(strstr(RB2_rob_rx_buffer,cmp2_txt) !=NULL){
+                            MOVE_END_F=1;
+                        }else{
+                             RB2_SEND_CMD("RobotMode()");   
+                        }
+                    }else{
+                        MOVE_ERR_F=1;
+                        ALM_ST=ALM_DOBOT2;
+                        ALM_HOLD_SET();
+                    }
+                }
+                wait_us(10*1000); //10ms
+            }
+             if(RB2_RX_TIME_OUT_F==1){
+                ALM_ST=ALM_DOBOT2_ED;
+                ALM_HOLD_SET();
+            }else{
+                RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
+            }
+        }
+    
+        RB2_eth_buf_clr();  
+    }
+        
+    
+}
+void RB2_MOVE_SWAIT(unsigned long tout_ms){//移動開始待ち
+    char *cmp1_txt="0,{7},RobotMode()";
+    char *cmp2_txt="0,{5},RobotMode()";
+    char *cmp3_txt="0,{9},RobotMode()";
+    unsigned char MOVE_START_F=0;
+    unsigned char MOVE_ERR_F=0;
+    
+    if(R2_WAIT_MASK==0){//1でマスク　現在位置への移動指定時WAITをマスクする
+        RB2_SEND_CMD("RobotMode()");
+        RB2_RX_TIME_OUT_F=0;
+        RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
+        while(RB2_RX_TIME_OUT_F==0 && MOVE_START_F==0  && MOVE_ERR_F==0){
+            if(RB2_RX_DATA()>0){
+                if(strstr(RB2_rob_rx_buffer,cmp3_txt) ==NULL){//err状態でないことを確認
+                    //移動開始確認///
+                    if(strstr(RB2_rob_rx_buffer,cmp1_txt) !=NULL){
+                        MOVE_START_F=1;
+                    }else{
+                        RB2_SEND_CMD("RobotMode()");   
+                    }
+                }else{
+                    MOVE_ERR_F=1;
+                    ALM_ST=ALM_DOBOT2;
+                    ALM_HOLD_SET();
+                }
+            }
+            wait_us(10*1000); //10ms
+        }
+        if(RB2_RX_TIME_OUT_F==1){
+            //printf("MOVE START TIMEOUT!! TX=%s  Rx=%s\r\n",RB2_rob_tx_buffer,RB2_rob_rx_buffer);
+            ALM_ST=ALM_DOBOT2_ST;
+            ALM_HOLD_SET();
+    
+        }else{
+            RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
+        }
+    }
+}
+void RB2_MOVE_EWAIT(unsigned long tout_ms){
+
+    char *cmp1_txt="0,{7},RobotMode()";
+    char *cmp2_txt="0,{5},RobotMode()";
+    char *cmp3_txt="0,{9},RobotMode()";
+    unsigned char MOVE_END_F=0;
+    unsigned char MOVE_ERR_F=0;
+     if(R2_WAIT_MASK==0){//1でマスク　現在位置への移動指定時WAITをマスクする
+        ///移動完了チェック///
+        RB2_SEND_CMD("RobotMode()");     
+        RB2_RX_TIME_OUT_F=0;
+        RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
+        while(RB2_RX_TIME_OUT_F==0 && MOVE_END_F==0  && MOVE_ERR_F==0){
+            if(RB2_RX_DATA()>0){
+                if(strstr(RB2_rob_rx_buffer,cmp3_txt) ==NULL){//err状態でないことを確認
+                    //移動完了確認///
+                    if(strstr(RB2_rob_rx_buffer,cmp2_txt) !=NULL){
+                        MOVE_END_F=1;
+                    }else{
+                         RB2_SEND_CMD("RobotMode()");   
+                    }
+                }else{
+                    MOVE_ERR_F=1;
+                    ALM_ST=ALM_DOBOT2;
+                    ALM_HOLD_SET();
+                }
+            }
+            wait_us(10*1000); //10ms
+
+        }
+        if(RB2_RX_TIME_OUT_F==1){
+           // printf("MOVE END TIMEOUT!! TX=%s  Rx=%s\r\n",RB2_rob_tx_buffer,RB2_rob_rx_buffer);
+            ALM_ST=ALM_DOBOT2_ED;
+            ALM_HOLD_SET();
+        }else{
+            RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
+        }
+        RB2_eth_buf_clr();
+    }  
+}
+void RB2_JOG_WAIT(unsigned long tout_ms){
+    char *cmp1_txt="0,{7},RobotMode()";
+    char *cmp2_txt="0,{5},RobotMode()";
+    char *cmp3_txt="0,{9},RobotMode()";
+    unsigned char MOVE_END_F=0;
+    unsigned char MOVE_ERR_F=0;
+    ///移動完了チェック///
+    RB2_SEND_CMD("RobotMode()");    
+    RB2_RX_TIME_OUT_F=0;
+    RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
+    while(RB2_RX_TIME_OUT_F==0 && MOVE_END_F==0 && MOVE_ERR_F==0){
+        if(RB2_RX_DATA()>0){
+            if(strstr(RB2_rob_rx_buffer,cmp3_txt) ==NULL){//err状態でないことを確認
+                //移動完了確認///
+                if(strstr(RB2_rob_rx_buffer,cmp2_txt) !=NULL){
+                    MOVE_END_F=1;
+                }else{
+                     RB2_SEND_CMD("RobotMode()");   
+                }
+            }else{
+                MOVE_ERR_F=1;
+                ALM_ST=ALM_DOBOT2;
+                ALM_HOLD_SET();
+            }
+        }
+        wait_us(10*1000); //10ms
+    }
+    if(RB2_RX_TIME_OUT_F==1){
+        //printf("JOG MOVE END TIMEOUT!! TX=%s  Rx=%s\r\n",RB2_rob_tx_buffer,RB2_rob_rx_buffer);
+        ALM_ST=ALM_DOBOT2_ED;
+        ALM_HOLD_SET();
+    }else{
+        RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
+    }
+    RB2_eth_buf_clr();  
+}
+
+
+void RB2_STATUS_READ(unsigned long tout_ms){
+    RB2_SEND_CMD("RobotMode()");
+    RB2_RX_TIME_OUT_F=0;       
+    RB2_RX_DATA();
+    RB2_RX_TIME_OUT_F=0;
+    RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
+
+    while(RB2_RX_TIME_OUT_F==0 && strstr(RB2_rob_rx_buffer,RB2_rob_tx_buffer) ==NULL){
+        RB2_RX_DATA();
+        wait_us(10*1000); //10ms
+    }
+
+    if(RB2_RX_TIME_OUT_F==1){
+        //printf("CMD ACK TIMEOUT!!\r\n");
+        ALM_ST=ALM_DOBOT2;
+        ALM_HOLD_SET();
+    }else{
+        RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
+    }
+
+    sprintf(lcd_print_buf,"STATUS=%s",RB2_rob_rx_buffer);
+    lcd_print("t0",lcd_print_buf);
+    RB2_eth_buf_clr();  
+}
+////データ受信処理/////////
+unsigned long RB2_RX_DATA(){//WIZnet_Socket.hのset_blocking(bool blocking, unsigned int timeout=1500);の設定で空読みしたときのタイムアウト時間がかわる
+    int n= RB2_server.receive(RB2_rob_rx_buffer, sizeof(RB2_rob_rx_buffer));
+    if(n > 0){//def 0
+        return n; 
+    }else{
+        return 0;
+    }
+}
+////データ受信処理テスト/////////
+unsigned long RB2_RX_DATA_TEST(){
+    int n= RB2_server.receive(RB2_rob_rx_buffer, sizeof(RB2_rob_rx_buffer));
+    if(n > 0){//def 0
+        //printf("ROBO1_DATA=%s\r\n",RB2_rob_rx_buffer);
+        RB2_eth_buf_clr();  
+        return n; 
+    }else{
+        return 0;
+    }
+}
+void RB2_CMD_WAIT(unsigned long tout_ms){
+
+    RB2_RX_DATA();
+    RB2_RX_TIME_OUT_F=0;
+    RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
+
+    while(RB2_RX_TIME_OUT_F==0 && strstr(RB2_rob_rx_buffer,RB2_rob_tx_buffer) ==NULL){
+        RB2_RX_DATA();
+        wait_us(10*1000); //10ms
+    }
+
+    if(RB2_RX_TIME_OUT_F==1){
+        //printf("CMD ACK TIMEOUT!!\r\n");
+        ALM_ST=ALM_DOBOT2;
+        ALM_HOLD_SET();
+    }else{
+        RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
+    }
+    RB2_eth_buf_clr();  
+}
+
+char RB2_SEND_CMD(char *txt){
+    unsigned char len = strlen(txt);//文字列の長さを確認
+    unsigned char SEND_RET=0;
+    sprintf(RB2_rob_tx_buffer,"%s\0",txt);
+    SEND_RET=RB2_server.send_all(RB2_rob_tx_buffer,len+1);
+    if(SEND_RET !=len+1){
+        sprintf(lcd_print_buf,"コマンド送信失敗!! 送信指定数 : %02d  送信完了数 : %02d",len+1,SEND_RET);
+        lcd_print("t0",RB2_rob_tx_buffer);   
+        return -1;
+    }else{
+        
+        return 0;
+    }
+    
+}
+
+
+
+
+
+void RB2_MovJ(double x,double y,double z,double a,double b,double c,unsigned int speed,unsigned int acc){
+   char RB2_BUF[128];
+   // printf("MovJ(%f,%f,%f,%f,%f,%f,SpeedJ=%d)",x,y,z,a,b,c,speed);
+    if(x==RB2_X_SET && y==RB2_Y_SET && z==RB2_Z_SET && a==RB2_A_SET && b==RB2_B_SET && c==RB2_C_SET){
+        lcd_print("t0","現在位置です");
+        R2_WAIT_MASK=1;//1でマスク　現在位置への移動指定時WAITをマスクする
+    }else{
+        sprintf(RB2_BUF,"MovJ(%f,%f,%f,%f,%f,%f,SpeedJ=%d,AccJ=%d)",x,y,z,a,b,c,speed,acc);
+        R2_WAIT_MASK=0;//1でマスク　現在位置への移動指定時WAITをマスクする
+        if(RB2_SEND_CMD(RB2_BUF)==0){
+            RB2_X_SET=x;
+            RB2_Y_SET=y;
+            RB2_Z_SET=z;
+            RB2_A_SET=a;
+            RB2_B_SET=b;
+            RB2_C_SET=c;
+        }else{//コマンド送信失敗
+            ALM_ST=ALM_DOBOT2_CM;
+            ALM_HOLD_SET();
+        }
+    }
+  
+}
+
+void RB2_JointMovJ(double Q1,double Q2,double Q3,double Q4,double Q5,double Q6,unsigned int speed,unsigned int acc){
+    char RB2_BUF[128];
+    if(Q1==RB2_Q1SET && Q2==RB2_Q2SET && Q3==RB2_Q3SET && Q4==RB2_Q4SET &&Q5==RB2_Q5SET && Q6==RB2_Q6SET){
+         lcd_print("t0","現在位置です");
+         R2_WAIT_MASK=1;//1でマスク　現在位置への移動指定時WAITをマスクする
+    }else{
+        sprintf(RB2_rob_tx_buffer,"JointMovJ(%f,%f,%f,%f,%f,%f,SpeedJ=%d,AccJ=%d)",Q1,Q2,Q3,Q4,Q5,Q6,speed,acc);
+        R2_WAIT_MASK=0;//1でマスク　現在位置への移動指定時WAITをマスクする
+        if(RB2_SEND_CMD(RB2_rob_tx_buffer)==0){
+            RB2_Q1SET=Q1;
+            RB2_Q2SET=Q2;
+            RB2_Q3SET=Q3;
+            RB2_Q4SET=Q4;
+            RB2_Q5SET=Q5;
+            RB2_Q6SET=Q6;
+        }else{//コマンド送信失敗
+            ALM_ST=ALM_DOBOT2_CM;
+            ALM_HOLD_SET();
+        }
+    }
+    
+}
+void RB2_MovL(double x,double y,double z,double a,double b,double c,unsigned int speed,unsigned int acc){
+    char RB2_BUF[128];
+    if(x==RB2_X_SET && y==RB2_Y_SET && z==RB2_Z_SET && a==RB2_A_SET && b==RB2_B_SET && c==RB2_C_SET){
+        lcd_print("t0","現在位置です");
+        R2_WAIT_MASK=1;//1でマスク　現在位置への移動指定時WAITをマスクする
+    }else{
+        sprintf(RB2_BUF,"MovL(%f,%f,%f,%f,%f,%f,SpeedL=%d,AccL=%d)",x,y,z,a,b,c,speed,acc);
+        R2_WAIT_MASK=0;//1でマスク　現在位置への移動指定時WAITをマスクする
+        if(RB2_SEND_CMD(RB2_BUF)==0){
+            RB2_X_SET=x;
+            RB2_Y_SET=y;
+            RB2_Z_SET=z;
+            RB2_A_SET=a;
+            RB2_B_SET=b;
+            RB2_C_SET=c;
+        }else{//コマンド送信失敗
+            ALM_ST=ALM_DOBOT2_CM;
+            ALM_HOLD_SET();
+        }
+    }
+}
+
+
+void RB2_HOME_MOVE(){
+    ///現在位置を確認///
+    RB2_GETPOS(RB_EWAIT); 
+    ///Zが-40以下の場合は、Zを上げてから原点復帰///
+    
+    if(RB2_Z_GET<100){
+        RB2_MovJ(RB2_X_GET,RB2_Y_GET,100.0,RB2_R_GET,0,0,RB2_SPEED_ADJ,RB2_ACC_ADJ);
+        RB2_MOVE_WAIT(5000);
+    }
+    
+    RB2_MovJ(178.0,124.0,100.0,0,0,0,RB2_SPEED_ADJ,RB2_ACC_ADJ);
+    RB2_MOVE_WAIT(10000);
+    RB2_MovJ(220.0,0.0,80.0,0,0,0,RB2_SPEED_ADJ,RB2_ACC_ADJ);
+    RB2_MOVE_WAIT(10000);
+    
+    RB2_PSEL_BAK=0xFE;
+
+    ///ハンドの制御ポートを開側にする///  
+    RB2_DO_CONT(1,0);
+    RB2_HAND_ST=RB2_HAND_OPEN;
+    
+}
+
+void RB2_SEL_MOVE(unsigned char SEL,unsigned int speed,unsigned int acc){
+    RB2_MovJ(RB2_POINT_RD[SEL][PX],RB2_POINT_RD[SEL][PY],RB2_POINT_RD[SEL][PZ],RB2_POINT_RD[SEL][PR],0,0,speed,acc);
+}
+
+void RB2_SEL_MOVE_OLG(unsigned char SEL,unsigned int speed,unsigned int acc){//現在値と差分があるときだけ移動
+    unsigned char POS_CHK_F=0;
+    
+    RB2_GETPOS(RB_EWAIT);
+    ////現在位置と指定座標に差分があるか確認///
+   if(RB2_X_GET > RB2_POINT_RD[SEL][PX]+0.1 || RB2_X_GET < RB2_POINT_RD[SEL][PX]-0.1){
+        POS_CHK_F=1;
+    }
+    if(RB2_Y_GET > RB2_POINT_RD[SEL][PY]+0.1 || RB2_Y_GET < RB2_POINT_RD[SEL][PY]-0.1){
+        POS_CHK_F=1;
+    }
+    if(RB2_Z_GET > RB2_POINT_RD[SEL][PZ]+0.1 || RB2_Z_GET < RB2_POINT_RD[SEL][PZ]-0.1){
+        POS_CHK_F=1;
+    }
+    if(RB2_R_GET > RB2_POINT_RD[SEL][PR]+0.1 || RB2_R_GET < RB2_POINT_RD[SEL][PR]-0.1){
+        POS_CHK_F=1;
+    }
+    
+    if(POS_CHK_F==1){
+        RB2_MovJ(RB2_POINT_RD[SEL][PX],RB2_POINT_RD[SEL][PY],RB2_POINT_RD[SEL][PZ],RB2_POINT_RD[SEL][PR],0,0,speed,acc);
+    }else{
+        lcd_print("t0","現在位置です。");
+    }
+    
+    
+}
+void RB2_SEL_MOVE_SQ(unsigned char SEL,unsigned char MOV_MOD,unsigned int speed,unsigned int acc){
+
+    if(SEL !=RB2_PSEL_BAK){
+        if(MOV_MOD==0){
+            RB2_MovL(RB2_POINT_RD[SEL][PX],RB2_POINT_RD[SEL][PY],RB2_POINT_RD[SEL][PZ],RB2_POINT_RD[SEL][PR],0,0,speed,acc);
+        }else{
+            RB2_MovJ(RB2_POINT_RD[SEL][PX],RB2_POINT_RD[SEL][PY],RB2_POINT_RD[SEL][PZ],RB2_POINT_RD[SEL][PR],0,0,speed,acc);
+        }
+        R2_WAIT_MASK=0;//1でマスク　現在位置への移動指定時WAITをマスクする
+        RB2_PSEL_BAK=SEL;
+    }else{
+        lcd_print("t0","現在位置です。");
+        R2_WAIT_MASK=1;//1でマスク　現在位置への移動指定時WAITをマスクする
+    }
+    
+    
+}
+void RB2_DO_CONT(unsigned char INDEX,unsigned char STATUS){//例:DO(11,1) でDO11がON DO(11,0) でDO11がOFF
+        char RB2_BUF[64];
+        sprintf(RB2_BUF,"DO(%d,%d)",INDEX, STATUS);       
+        RB2_SEND_CMD(RB2_BUF);    
+        RB2_CMD_WAIT(RB_SWAIT);
+ }
+ 
+
+void RB2_HAND_CONT(unsigned char cont){
+    if(cont !=  RB2_HAND_ST){
+        if(cont==RB2_HAND_CLOSE){
+            RB2_DO_CONT(1,1);
+        }else{
+            RB2_DO_CONT(1,0);
+        }
+     RB2_HAND_ST=cont;
+    }     
+}
+////現在位置からx1,y1,z1を経由してx2,y2,z2まで移動（円運動）////
+void RB2_ARC(double x1,double y1,double z1,double x2,double y2,double z2,unsigned int speed,unsigned int acc){
+    char RB2_BUF[128];
+
+        sprintf(RB2_BUF,"Arc(%f,%f,%f,%f,%f,%f,SpeedL=%d,AccL=%d)",x1,y1,z1,x2,y2,z2,speed,acc);
+        if(RB2_SEND_CMD(RB2_BUF)==0){
+            RB2_X_SET=x2;
+            RB2_Y_SET=y2;
+            RB2_Z_SET=z2;
+        }else{//コマンド送信失敗
+            ALM_ST=ALM_DOBOT2_CM;
+            ALM_HOLD_SET();
+        }
+
+}
+void RB2_EMG_STOP(){ 
+        RB2_SEND_CMD("EmergencyStop()");    
+ }
+ 
+void RB2_SPEED_SET(unsigned char SET){
+    if(SET==RB_HIGH){
+        RB2_SPEED_ADJ = RB_SPEED_HIGH;
+        RB2_ACC_ADJ = RB_ACC_HIGH; 
+    }else{
+        RB2_SPEED_ADJ = RB_SPEED_LOW;
+        RB2_ACC_ADJ = RB_ACC_LOW;     
+    }   
+}
\ No newline at end of file