File content as of revision 2:71b3736a1bd7:

#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;     
    }   
}