Jun Suto / Mbed OS STM32F446_MG400_DIRECT_V0R17

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:  

ROBO2_CONT.cpp@2:71b3736a1bd7, 2022-10-21 (annotated)

Committer:
js
Date:
Fri Oct 21 10:54:03 2022 +0000
Revision:
2:71b3736a1bd7
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
js 2:71b3736a1bd7 1 #include "mbed.h"
js 2:71b3736a1bd7 2 #include "main.h"
js 2:71b3736a1bd7 3 #include "nextion_lcd.h"
js 2:71b3736a1bd7 4 #include "APP.h"
js 2:71b3736a1bd7 5 #include "ROBO2_CONT.h"
js 2:71b3736a1bd7 6 #include "SAVE_CONT.h"
js 2:71b3736a1bd7 7 ///////////////////////
js 2:71b3736a1bd7 8 #include "WIZnetInterface.h"
js 2:71b3736a1bd7 9 #define USE_DHCP 0
js 2:71b3736a1bd7 10 #define LOOPBACKPORT 7
js 2:71b3736a1bd7 11 Ticker RB2_RX_TIME_OUT;
js 2:71b3736a1bd7 12
js 2:71b3736a1bd7 13 extern const char * IP_Subnet;
js 2:71b3736a1bd7 14 extern const char * IP_Gateway;
js 2:71b3736a1bd7 15 extern unsigned char MAC_Addr[6];
js 2:71b3736a1bd7 16
js 2:71b3736a1bd7 17 const char *RB2_SV_Addr = "192.168.2.8";
js 2:71b3736a1bd7 18 const int RB2_SV_port =30003;
js 2:71b3736a1bd7 19 //const int SV_port =30005;
js 2:71b3736a1bd7 20 extern SPI spi;
js 2:71b3736a1bd7 21 WIZnetInterface RB2_ethernet(&spi,PB_1, PB_2);//CONT BOARD MINIは、PA_15,PB_2
js 2:71b3736a1bd7 22 TCPSocketConnection RB2_server;
js 2:71b3736a1bd7 23
js 2:71b3736a1bd7 24 ///////////////////////
js 2:71b3736a1bd7 25 char RB2_rob_tx_buffer[128];
js 2:71b3736a1bd7 26 char RB2_rob_rx_buffer[128];
js 2:71b3736a1bd7 27
js 2:71b3736a1bd7 28
js 2:71b3736a1bd7 29 double RB2_Q1SET=0.0;
js 2:71b3736a1bd7 30 double RB2_Q2SET=0.0;
js 2:71b3736a1bd7 31 double RB2_Q3SET=0.0;
js 2:71b3736a1bd7 32 double RB2_Q4SET=0.0;
js 2:71b3736a1bd7 33 double RB2_Q5SET=0.0;
js 2:71b3736a1bd7 34 double RB2_Q6SET=0.0;
js 2:71b3736a1bd7 35
js 2:71b3736a1bd7 36
js 2:71b3736a1bd7 37 double RB2_X_SET=0.0;
js 2:71b3736a1bd7 38 double RB2_Y_SET=0.0;
js 2:71b3736a1bd7 39 double RB2_Z_SET=0.0;
js 2:71b3736a1bd7 40 double RB2_A_SET=0.0;
js 2:71b3736a1bd7 41 double RB2_B_SET=0.0;
js 2:71b3736a1bd7 42 double RB2_C_SET=0.0;
js 2:71b3736a1bd7 43
js 2:71b3736a1bd7 44 extern double RB2_POINT_RD[32][4];
js 2:71b3736a1bd7 45
js 2:71b3736a1bd7 46
js 2:71b3736a1bd7 47 unsigned char RB2_SPEED_ADJ=100;
js 2:71b3736a1bd7 48 unsigned char RB2_ACC_ADJ=80;
js 2:71b3736a1bd7 49
js 2:71b3736a1bd7 50 unsigned char RB2_PSEL_BAK=0xFF;
js 2:71b3736a1bd7 51
js 2:71b3736a1bd7 52
js 2:71b3736a1bd7 53 unsigned char RB2_RX_TIME_OUT_F=0;
js 2:71b3736a1bd7 54
js 2:71b3736a1bd7 55 unsigned char RB2_HAND_ST=RB2_HAND_OPEN;
js 2:71b3736a1bd7 56
js 2:71b3736a1bd7 57 char RB2_J1_DAT[32];
js 2:71b3736a1bd7 58 char RB2_J2_DAT[32];
js 2:71b3736a1bd7 59 char RB2_J3_DAT[32];
js 2:71b3736a1bd7 60 char RB2_J4_DAT[32];
js 2:71b3736a1bd7 61 char RB2_XDAT[32];
js 2:71b3736a1bd7 62 char RB2_YDAT[32];
js 2:71b3736a1bd7 63 char RB2_ZDAT[32];
js 2:71b3736a1bd7 64 char RB2_RDAT[32];
js 2:71b3736a1bd7 65
js 2:71b3736a1bd7 66 double RB2_J1_GET=0.0;
js 2:71b3736a1bd7 67 double RB2_J2_GET=0.0;
js 2:71b3736a1bd7 68 double RB2_J3_GET=0.0;
js 2:71b3736a1bd7 69 double RB2_J4_GET=0.0;
js 2:71b3736a1bd7 70
js 2:71b3736a1bd7 71 double RB2_X_GET=0.0;
js 2:71b3736a1bd7 72 double RB2_Y_GET=0.0;
js 2:71b3736a1bd7 73 double RB2_Z_GET=0.0;
js 2:71b3736a1bd7 74 double RB2_R_GET=0.0;
js 2:71b3736a1bd7 75
js 2:71b3736a1bd7 76
js 2:71b3736a1bd7 77 unsigned char R2_WAIT_MASK=0;//1でマスク　現在位置への移動指定時WAITをマスクする
js 2:71b3736a1bd7 78
js 2:71b3736a1bd7 79 void RB2_RX_TIME_OUT_IRQ(){
js 2:71b3736a1bd7 80 RB2_RX_TIME_OUT_F=1;
js 2:71b3736a1bd7 81 RB2_RX_TIME_OUT.detach();//シリアル受信タイムアウトカウンタ停止
js 2:71b3736a1bd7 82 RB2_eth_buf_clr();
js 2:71b3736a1bd7 83
js 2:71b3736a1bd7 84 }
js 2:71b3736a1bd7 85
js 2:71b3736a1bd7 86 void RB2_eth_buf_clr(){
js 2:71b3736a1bd7 87
js 2:71b3736a1bd7 88 for(unsigned long clcnt=0; clcnt<sizeof(RB2_rob_tx_buffer); clcnt++){
js 2:71b3736a1bd7 89 RB2_rob_tx_buffer[clcnt]=0;
js 2:71b3736a1bd7 90 }
js 2:71b3736a1bd7 91 for(unsigned long clcnt=0; clcnt<sizeof(RB2_rob_rx_buffer); clcnt++){
js 2:71b3736a1bd7 92 RB2_rob_rx_buffer[clcnt]=0;
js 2:71b3736a1bd7 93 }
js 2:71b3736a1bd7 94 }
js 2:71b3736a1bd7 95 void RB2_POS_CLR(){
js 2:71b3736a1bd7 96 RB2_Q1SET=0.0;
js 2:71b3736a1bd7 97 RB2_Q2SET=0.0;
js 2:71b3736a1bd7 98 RB2_Q3SET=0.0;
js 2:71b3736a1bd7 99 RB2_Q4SET=0.0;
js 2:71b3736a1bd7 100 RB2_Q5SET=0.0;
js 2:71b3736a1bd7 101 RB2_Q6SET=0.0;
js 2:71b3736a1bd7 102
js 2:71b3736a1bd7 103
js 2:71b3736a1bd7 104 RB2_X_SET=0.0;
js 2:71b3736a1bd7 105 RB2_Y_SET=0.0;
js 2:71b3736a1bd7 106 RB2_Z_SET=0.0;
js 2:71b3736a1bd7 107 RB2_A_SET=0.0;
js 2:71b3736a1bd7 108 RB2_B_SET=0.0;
js 2:71b3736a1bd7 109 RB2_C_SET=0.0;
js 2:71b3736a1bd7 110 }
js 2:71b3736a1bd7 111 char RB2_ETH_CONECT_CHK(){
js 2:71b3736a1bd7 112
js 2:71b3736a1bd7 113 if(RB2_server.is_connected()==true){
js 2:71b3736a1bd7 114 return 0;
js 2:71b3736a1bd7 115 }else{
js 2:71b3736a1bd7 116 lcd_print("t0","LAN切断中です。再接続してください。");
js 2:71b3736a1bd7 117 return -1;
js 2:71b3736a1bd7 118 }
js 2:71b3736a1bd7 119
js 2:71b3736a1bd7 120 }
js 2:71b3736a1bd7 121
js 2:71b3736a1bd7 122
js 2:71b3736a1bd7 123
js 2:71b3736a1bd7 124 char RB2_CONNECT(){
js 2:71b3736a1bd7 125 unsigned long timeout=0;
js 2:71b3736a1bd7 126 sprintf(lcd_print_buf,"ロボット２(%s) 接続開始...",RB2_SV_Addr);
js 2:71b3736a1bd7 127 lcd_print("t1",lcd_print_buf);
js 2:71b3736a1bd7 128
js 2:71b3736a1bd7 129 int ret =RB2_server.connect(RB2_SV_Addr,RB2_SV_port);
js 2:71b3736a1bd7 130
js 2:71b3736a1bd7 131 if(ret !=0){
js 2:71b3736a1bd7 132 sprintf(lcd_print_buf,"ロボット２(%s) 接続待機中...",RB2_SV_Addr);
js 2:71b3736a1bd7 133 lcd_print("t1",lcd_print_buf);
js 2:71b3736a1bd7 134 }
js 2:71b3736a1bd7 135 while(ret !=0 && timeout<100){
js 2:71b3736a1bd7 136 ret =RB2_server.connect(RB2_SV_Addr,RB2_SV_port);
js 2:71b3736a1bd7 137 wait_ms(100);
js 2:71b3736a1bd7 138 }
js 2:71b3736a1bd7 139 if(timeout<100){
js 2:71b3736a1bd7 140 printf("connect end\r\n");
js 2:71b3736a1bd7 141 sprintf(lcd_print_buf,"ロボット２(%s) 接続完了",RB2_SV_Addr);
js 2:71b3736a1bd7 142 lcd_print("t1",lcd_print_buf);
js 2:71b3736a1bd7 143 return 0;
js 2:71b3736a1bd7 144 }else{
js 2:71b3736a1bd7 145 sprintf(lcd_print_buf,"ロボット２(%s) 接続異常",RB2_SV_Addr);
js 2:71b3736a1bd7 146 lcd_print("t1",lcd_print_buf);
js 2:71b3736a1bd7 147 return -1;
js 2:71b3736a1bd7 148 }
js 2:71b3736a1bd7 149
js 2:71b3736a1bd7 150 }
js 2:71b3736a1bd7 151
js 2:71b3736a1bd7 152 char RB2_DISCONNECT(){
js 2:71b3736a1bd7 153 lcd_print("t0","R2 Disconnect");
js 2:71b3736a1bd7 154 int ret =RB2_ethernet.disconnect();
js 2:71b3736a1bd7 155 wait_ms(2000);
js 2:71b3736a1bd7 156 if(ret==0){
js 2:71b3736a1bd7 157 lcd_print("t0","R2 Disconnect End");
js 2:71b3736a1bd7 158 return 0;
js 2:71b3736a1bd7 159 }else{
js 2:71b3736a1bd7 160 lcd_print("t0","R2 Disconnect ERR");
js 2:71b3736a1bd7 161 return -1;
js 2:71b3736a1bd7 162 }
js 2:71b3736a1bd7 163
js 2:71b3736a1bd7 164
js 2:71b3736a1bd7 165 }
js 2:71b3736a1bd7 166 /*
js 2:71b3736a1bd7 167 ロボットのタイムアウト検出は割り込みを使用して行う。
js 2:71b3736a1bd7 168 通常のwaitをカウントする方法では、時間が大きくずれる為
js 2:71b3736a1bd7 169 ＊Etherの受信処理にもタイムアウト設定がある為
js 2:71b3736a1bd7 170 WIZnet_Socket.hの以下の設定　下記だと一度ETHER受信しタイムアウトまで300msかかる
js 2:71b3736a1bd7 171 set_blocking(bool blocking, unsigned int timeout=300);//1500
js 2:71b3736a1bd7 172
js 2:71b3736a1bd7 173 */
js 2:71b3736a1bd7 174 void RB2_GETANG(unsigned long tout_ms){
js 2:71b3736a1bd7 175 char *cmp1_txt="GetAngle();";
js 2:71b3736a1bd7 176 unsigned char DAT_GET_F=0;
js 2:71b3736a1bd7 177
js 2:71b3736a1bd7 178 RB2_SEND_CMD("GetAngle()");
js 2:71b3736a1bd7 179 RB2_RX_TIME_OUT_F=0;
js 2:71b3736a1bd7 180 RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
js 2:71b3736a1bd7 181 while(RB2_RX_TIME_OUT_F==0 && DAT_GET_F==0){
js 2:71b3736a1bd7 182 if(RB2_RX_DATA()>0){
js 2:71b3736a1bd7 183 //移動開始確認///
js 2:71b3736a1bd7 184 if(strstr(RB2_rob_rx_buffer,cmp1_txt) !=NULL){
js 2:71b3736a1bd7 185 DAT_GET_F=1;
js 2:71b3736a1bd7 186 }else{
js 2:71b3736a1bd7 187 RB2_SEND_CMD("GetAngle()");
js 2:71b3736a1bd7 188 }
js 2:71b3736a1bd7 189 }
js 2:71b3736a1bd7 190 wait_us(10*1000);
js 2:71b3736a1bd7 191 }
js 2:71b3736a1bd7 192 unsigned char DAT_CNT=0;
js 2:71b3736a1bd7 193 unsigned char DAT_CNT_T=3;
js 2:71b3736a1bd7 194 if(RB2_RX_TIME_OUT_F==1){
js 2:71b3736a1bd7 195 lcd_print("t0","RB2 GetAngle TIMEOUT");
js 2:71b3736a1bd7 196
js 2:71b3736a1bd7 197 }else{
js 2:71b3736a1bd7 198 RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
js 2:71b3736a1bd7 199 while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_J1_DAT)){
js 2:71b3736a1bd7 200 RB2_J1_DAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
js 2:71b3736a1bd7 201 }
js 2:71b3736a1bd7 202 DAT_CNT_T++;//カンマを飛ばす
js 2:71b3736a1bd7 203 DAT_CNT=0;
js 2:71b3736a1bd7 204 while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_J2_DAT)){
js 2:71b3736a1bd7 205 RB2_J2_DAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
js 2:71b3736a1bd7 206 }
js 2:71b3736a1bd7 207 DAT_CNT_T++;//カンマを飛ばす
js 2:71b3736a1bd7 208 DAT_CNT=0;
js 2:71b3736a1bd7 209 while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_J3_DAT)){
js 2:71b3736a1bd7 210 RB2_J3_DAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
js 2:71b3736a1bd7 211 }
js 2:71b3736a1bd7 212 DAT_CNT_T++;//カンマを飛ばす
js 2:71b3736a1bd7 213 DAT_CNT=0;
js 2:71b3736a1bd7 214 while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_J4_DAT)){
js 2:71b3736a1bd7 215 RB2_J4_DAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
js 2:71b3736a1bd7 216 }
js 2:71b3736a1bd7 217 RB2_J1_GET=atof(RB2_J1_DAT);
js 2:71b3736a1bd7 218 RB2_J2_GET=atof(RB2_J2_DAT);
js 2:71b3736a1bd7 219 RB2_J3_GET=atof(RB2_J3_DAT);
js 2:71b3736a1bd7 220 RB2_J4_GET=atof(RB2_J4_DAT);
js 2:71b3736a1bd7 221
js 2:71b3736a1bd7 222 }
js 2:71b3736a1bd7 223
js 2:71b3736a1bd7 224
js 2:71b3736a1bd7 225 RB2_eth_buf_clr();
js 2:71b3736a1bd7 226 }
js 2:71b3736a1bd7 227 void RB2_GETPOS(unsigned long tout_ms){
js 2:71b3736a1bd7 228 char *cmp1_txt="GetPose();";
js 2:71b3736a1bd7 229 unsigned char DAT_GET_F=0;
js 2:71b3736a1bd7 230 RB2_SEND_CMD("GetPose()");
js 2:71b3736a1bd7 231 RB2_RX_TIME_OUT_F=0;
js 2:71b3736a1bd7 232 RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
js 2:71b3736a1bd7 233 while(RB2_RX_TIME_OUT_F==0 && DAT_GET_F==0){
js 2:71b3736a1bd7 234 if(RB2_RX_DATA()>0){
js 2:71b3736a1bd7 235 //移動開始確認///
js 2:71b3736a1bd7 236 if(strstr(RB2_rob_rx_buffer,cmp1_txt) !=NULL){
js 2:71b3736a1bd7 237 DAT_GET_F=1;
js 2:71b3736a1bd7 238 }else{
js 2:71b3736a1bd7 239 RB2_SEND_CMD("GetPose()");
js 2:71b3736a1bd7 240 }
js 2:71b3736a1bd7 241 }
js 2:71b3736a1bd7 242 wait_us(10*1000);
js 2:71b3736a1bd7 243 }
js 2:71b3736a1bd7 244 unsigned char DAT_CNT=0;
js 2:71b3736a1bd7 245 unsigned char DAT_CNT_T=3;
js 2:71b3736a1bd7 246 if(RB2_RX_TIME_OUT_F==1){
js 2:71b3736a1bd7 247 lcd_print("t0","RB2 GetPose TIMEOUT");
js 2:71b3736a1bd7 248 }else{
js 2:71b3736a1bd7 249 RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
js 2:71b3736a1bd7 250 while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_XDAT)){
js 2:71b3736a1bd7 251 RB2_XDAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
js 2:71b3736a1bd7 252 }
js 2:71b3736a1bd7 253 DAT_CNT_T++;//カンマを飛ばす
js 2:71b3736a1bd7 254 DAT_CNT=0;
js 2:71b3736a1bd7 255 while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_YDAT)){
js 2:71b3736a1bd7 256 RB2_YDAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
js 2:71b3736a1bd7 257 }
js 2:71b3736a1bd7 258 DAT_CNT_T++;//カンマを飛ばす
js 2:71b3736a1bd7 259 DAT_CNT=0;
js 2:71b3736a1bd7 260 while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_ZDAT)){
js 2:71b3736a1bd7 261 RB2_ZDAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
js 2:71b3736a1bd7 262 }
js 2:71b3736a1bd7 263 DAT_CNT_T++;//カンマを飛ばす
js 2:71b3736a1bd7 264 DAT_CNT=0;
js 2:71b3736a1bd7 265 while(RB2_rob_rx_buffer[DAT_CNT_T] !=0x2c && DAT_CNT<sizeof(RB2_RDAT)){
js 2:71b3736a1bd7 266 RB2_RDAT[DAT_CNT++]=RB2_rob_rx_buffer[DAT_CNT_T++];
js 2:71b3736a1bd7 267 }
js 2:71b3736a1bd7 268 RB2_X_GET=atof(RB2_XDAT);
js 2:71b3736a1bd7 269 RB2_Y_GET=atof(RB2_YDAT);
js 2:71b3736a1bd7 270 RB2_Z_GET=atof(RB2_ZDAT);
js 2:71b3736a1bd7 271 RB2_R_GET=atof(RB2_RDAT);
js 2:71b3736a1bd7 272
js 2:71b3736a1bd7 273 }
js 2:71b3736a1bd7 274
js 2:71b3736a1bd7 275
js 2:71b3736a1bd7 276 RB2_eth_buf_clr();
js 2:71b3736a1bd7 277 }
js 2:71b3736a1bd7 278
js 2:71b3736a1bd7 279
js 2:71b3736a1bd7 280 /*
js 2:71b3736a1bd7 281 RobotMode()コマンドを使用してロボットの移動完了(Mode 5 idle)を確認する
js 2:71b3736a1bd7 282 Mode | Description | NOTE |
js 2:71b3736a1bd7 283 | ---- | --------------------- | -------------------------------- |
js 2:71b3736a1bd7 284 | 1 | ROBOT_MODE_INIT | initialization |
js 2:71b3736a1bd7 285 | 2 | ROBOT_MODE_BRAKE_OPEN | The brake release |
js 2:71b3736a1bd7 286 | 3 | | Reserved |
js 2:71b3736a1bd7 287 | 4 | ROBOT_MODE_DISABLED | Disabled (brake is not released) |
js 2:71b3736a1bd7 288 | 5 | ROBOT_MODE_ENABLE | Enable (idle) |
js 2:71b3736a1bd7 289 | 6 | ROBOT_MODE_BACKDRIVE | Dragging state |
js 2:71b3736a1bd7 290 | 7 | ROBOT_MODE_RUNNING | Running state |
js 2:71b3736a1bd7 291 | 8 | ROBOT_MODE_RECORDING | Dragging recording |
js 2:71b3736a1bd7 292 | 9 | ROBOT_MODE_ERROR | Alarm state |
js 2:71b3736a1bd7 293 | 10 | ROBOT_MODE_PAUSE | pause state |
js 2:71b3736a1bd7 294 | 11 | ROBOT_MODE_JOG | jogging state |
js 2:71b3736a1bd7 295
js 2:71b3736a1bd7 296
js 2:71b3736a1bd7 297 */
js 2:71b3736a1bd7 298 void RB2_MOVE_WAIT(unsigned long tout_ms){//移動開始待ち
js 2:71b3736a1bd7 299 char *cmp1_txt="0,{7},RobotMode()";
js 2:71b3736a1bd7 300 char *cmp2_txt="0,{5},RobotMode()";
js 2:71b3736a1bd7 301 char *cmp3_txt="0,{9},RobotMode()";
js 2:71b3736a1bd7 302
js 2:71b3736a1bd7 303 unsigned char MOVE_START_F=0;
js 2:71b3736a1bd7 304 unsigned char MOVE_END_F=0;
js 2:71b3736a1bd7 305 unsigned char MOVE_ERR_F=0;
js 2:71b3736a1bd7 306 if(R2_WAIT_MASK==0){//1でマスク　現在位置への移動指定時WAITをマスクする
js 2:71b3736a1bd7 307 RB2_SEND_CMD("RobotMode()");
js 2:71b3736a1bd7 308 RB2_RX_TIME_OUT_F=0;
js 2:71b3736a1bd7 309 RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
js 2:71b3736a1bd7 310 while(RB2_RX_TIME_OUT_F==0 && MOVE_START_F==0 && MOVE_ERR_F==0){
js 2:71b3736a1bd7 311 if(RB2_RX_DATA()>0){
js 2:71b3736a1bd7 312 if(strstr(RB2_rob_rx_buffer,cmp3_txt) ==NULL){//err状態でないことを確認
js 2:71b3736a1bd7 313 //移動開始確認///
js 2:71b3736a1bd7 314 if(strstr(RB2_rob_rx_buffer,cmp1_txt) !=NULL){
js 2:71b3736a1bd7 315 MOVE_START_F=1;
js 2:71b3736a1bd7 316 }else{
js 2:71b3736a1bd7 317 RB2_SEND_CMD("RobotMode()");
js 2:71b3736a1bd7 318 }
js 2:71b3736a1bd7 319 }else{
js 2:71b3736a1bd7 320 MOVE_ERR_F=1;
js 2:71b3736a1bd7 321 ALM_ST=ALM_DOBOT2;
js 2:71b3736a1bd7 322 ALM_HOLD_SET();
js 2:71b3736a1bd7 323 }
js 2:71b3736a1bd7 324 }
js 2:71b3736a1bd7 325 wait_us(10*1000);
js 2:71b3736a1bd7 326 }
js 2:71b3736a1bd7 327 if(RB2_RX_TIME_OUT_F==1){
js 2:71b3736a1bd7 328 //printf("MOVE START TIMEOUT!! TX=%s Rx=%s\r\n",RB2_rob_tx_buffer,RB2_rob_rx_buffer);
js 2:71b3736a1bd7 329 ALM_ST=ALM_DOBOT2_ST;
js 2:71b3736a1bd7 330 ALM_HOLD_SET();
js 2:71b3736a1bd7 331 }else{
js 2:71b3736a1bd7 332 RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
js 2:71b3736a1bd7 333 }
js 2:71b3736a1bd7 334 if(ALM_ST==ALM_NONE){
js 2:71b3736a1bd7 335 ///移動完了チェック///
js 2:71b3736a1bd7 336 RB2_RX_TIME_OUT_F=0;
js 2:71b3736a1bd7 337 RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
js 2:71b3736a1bd7 338 RB2_SEND_CMD("RobotMode()");
js 2:71b3736a1bd7 339 while(RB2_RX_TIME_OUT_F==0 && MOVE_END_F==0 && MOVE_ERR_F==0){
js 2:71b3736a1bd7 340 if(RB2_RX_DATA()>0){
js 2:71b3736a1bd7 341 if(strstr(RB2_rob_rx_buffer,cmp3_txt) ==NULL){//err状態でないことを確認
js 2:71b3736a1bd7 342 //移動完了確認///
js 2:71b3736a1bd7 343 if(strstr(RB2_rob_rx_buffer,cmp2_txt) !=NULL){
js 2:71b3736a1bd7 344 MOVE_END_F=1;
js 2:71b3736a1bd7 345 }else{
js 2:71b3736a1bd7 346 RB2_SEND_CMD("RobotMode()");
js 2:71b3736a1bd7 347 }
js 2:71b3736a1bd7 348 }else{
js 2:71b3736a1bd7 349 MOVE_ERR_F=1;
js 2:71b3736a1bd7 350 ALM_ST=ALM_DOBOT2;
js 2:71b3736a1bd7 351 ALM_HOLD_SET();
js 2:71b3736a1bd7 352 }
js 2:71b3736a1bd7 353 }
js 2:71b3736a1bd7 354 wait_us(10*1000); //10ms
js 2:71b3736a1bd7 355 }
js 2:71b3736a1bd7 356 if(RB2_RX_TIME_OUT_F==1){
js 2:71b3736a1bd7 357 ALM_ST=ALM_DOBOT2_ED;
js 2:71b3736a1bd7 358 ALM_HOLD_SET();
js 2:71b3736a1bd7 359 }else{
js 2:71b3736a1bd7 360 RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
js 2:71b3736a1bd7 361 }
js 2:71b3736a1bd7 362 }
js 2:71b3736a1bd7 363
js 2:71b3736a1bd7 364 RB2_eth_buf_clr();
js 2:71b3736a1bd7 365 }
js 2:71b3736a1bd7 366
js 2:71b3736a1bd7 367
js 2:71b3736a1bd7 368 }
js 2:71b3736a1bd7 369 void RB2_MOVE_SWAIT(unsigned long tout_ms){//移動開始待ち
js 2:71b3736a1bd7 370 char *cmp1_txt="0,{7},RobotMode()";
js 2:71b3736a1bd7 371 char *cmp2_txt="0,{5},RobotMode()";
js 2:71b3736a1bd7 372 char *cmp3_txt="0,{9},RobotMode()";
js 2:71b3736a1bd7 373 unsigned char MOVE_START_F=0;
js 2:71b3736a1bd7 374 unsigned char MOVE_ERR_F=0;
js 2:71b3736a1bd7 375
js 2:71b3736a1bd7 376 if(R2_WAIT_MASK==0){//1でマスク　現在位置への移動指定時WAITをマスクする
js 2:71b3736a1bd7 377 RB2_SEND_CMD("RobotMode()");
js 2:71b3736a1bd7 378 RB2_RX_TIME_OUT_F=0;
js 2:71b3736a1bd7 379 RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
js 2:71b3736a1bd7 380 while(RB2_RX_TIME_OUT_F==0 && MOVE_START_F==0 && MOVE_ERR_F==0){
js 2:71b3736a1bd7 381 if(RB2_RX_DATA()>0){
js 2:71b3736a1bd7 382 if(strstr(RB2_rob_rx_buffer,cmp3_txt) ==NULL){//err状態でないことを確認
js 2:71b3736a1bd7 383 //移動開始確認///
js 2:71b3736a1bd7 384 if(strstr(RB2_rob_rx_buffer,cmp1_txt) !=NULL){
js 2:71b3736a1bd7 385 MOVE_START_F=1;
js 2:71b3736a1bd7 386 }else{
js 2:71b3736a1bd7 387 RB2_SEND_CMD("RobotMode()");
js 2:71b3736a1bd7 388 }
js 2:71b3736a1bd7 389 }else{
js 2:71b3736a1bd7 390 MOVE_ERR_F=1;
js 2:71b3736a1bd7 391 ALM_ST=ALM_DOBOT2;
js 2:71b3736a1bd7 392 ALM_HOLD_SET();
js 2:71b3736a1bd7 393 }
js 2:71b3736a1bd7 394 }
js 2:71b3736a1bd7 395 wait_us(10*1000); //10ms
js 2:71b3736a1bd7 396 }
js 2:71b3736a1bd7 397 if(RB2_RX_TIME_OUT_F==1){
js 2:71b3736a1bd7 398 //printf("MOVE START TIMEOUT!! TX=%s Rx=%s\r\n",RB2_rob_tx_buffer,RB2_rob_rx_buffer);
js 2:71b3736a1bd7 399 ALM_ST=ALM_DOBOT2_ST;
js 2:71b3736a1bd7 400 ALM_HOLD_SET();
js 2:71b3736a1bd7 401
js 2:71b3736a1bd7 402 }else{
js 2:71b3736a1bd7 403 RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
js 2:71b3736a1bd7 404 }
js 2:71b3736a1bd7 405 }
js 2:71b3736a1bd7 406 }
js 2:71b3736a1bd7 407 void RB2_MOVE_EWAIT(unsigned long tout_ms){
js 2:71b3736a1bd7 408
js 2:71b3736a1bd7 409 char *cmp1_txt="0,{7},RobotMode()";
js 2:71b3736a1bd7 410 char *cmp2_txt="0,{5},RobotMode()";
js 2:71b3736a1bd7 411 char *cmp3_txt="0,{9},RobotMode()";
js 2:71b3736a1bd7 412 unsigned char MOVE_END_F=0;
js 2:71b3736a1bd7 413 unsigned char MOVE_ERR_F=0;
js 2:71b3736a1bd7 414 if(R2_WAIT_MASK==0){//1でマスク　現在位置への移動指定時WAITをマスクする
js 2:71b3736a1bd7 415 ///移動完了チェック///
js 2:71b3736a1bd7 416 RB2_SEND_CMD("RobotMode()");
js 2:71b3736a1bd7 417 RB2_RX_TIME_OUT_F=0;
js 2:71b3736a1bd7 418 RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
js 2:71b3736a1bd7 419 while(RB2_RX_TIME_OUT_F==0 && MOVE_END_F==0 && MOVE_ERR_F==0){
js 2:71b3736a1bd7 420 if(RB2_RX_DATA()>0){
js 2:71b3736a1bd7 421 if(strstr(RB2_rob_rx_buffer,cmp3_txt) ==NULL){//err状態でないことを確認
js 2:71b3736a1bd7 422 //移動完了確認///
js 2:71b3736a1bd7 423 if(strstr(RB2_rob_rx_buffer,cmp2_txt) !=NULL){
js 2:71b3736a1bd7 424 MOVE_END_F=1;
js 2:71b3736a1bd7 425 }else{
js 2:71b3736a1bd7 426 RB2_SEND_CMD("RobotMode()");
js 2:71b3736a1bd7 427 }
js 2:71b3736a1bd7 428 }else{
js 2:71b3736a1bd7 429 MOVE_ERR_F=1;
js 2:71b3736a1bd7 430 ALM_ST=ALM_DOBOT2;
js 2:71b3736a1bd7 431 ALM_HOLD_SET();
js 2:71b3736a1bd7 432 }
js 2:71b3736a1bd7 433 }
js 2:71b3736a1bd7 434 wait_us(10*1000); //10ms
js 2:71b3736a1bd7 435
js 2:71b3736a1bd7 436 }
js 2:71b3736a1bd7 437 if(RB2_RX_TIME_OUT_F==1){
js 2:71b3736a1bd7 438 // printf("MOVE END TIMEOUT!! TX=%s Rx=%s\r\n",RB2_rob_tx_buffer,RB2_rob_rx_buffer);
js 2:71b3736a1bd7 439 ALM_ST=ALM_DOBOT2_ED;
js 2:71b3736a1bd7 440 ALM_HOLD_SET();
js 2:71b3736a1bd7 441 }else{
js 2:71b3736a1bd7 442 RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
js 2:71b3736a1bd7 443 }
js 2:71b3736a1bd7 444 RB2_eth_buf_clr();
js 2:71b3736a1bd7 445 }
js 2:71b3736a1bd7 446 }
js 2:71b3736a1bd7 447 void RB2_JOG_WAIT(unsigned long tout_ms){
js 2:71b3736a1bd7 448 char *cmp1_txt="0,{7},RobotMode()";
js 2:71b3736a1bd7 449 char *cmp2_txt="0,{5},RobotMode()";
js 2:71b3736a1bd7 450 char *cmp3_txt="0,{9},RobotMode()";
js 2:71b3736a1bd7 451 unsigned char MOVE_END_F=0;
js 2:71b3736a1bd7 452 unsigned char MOVE_ERR_F=0;
js 2:71b3736a1bd7 453 ///移動完了チェック///
js 2:71b3736a1bd7 454 RB2_SEND_CMD("RobotMode()");
js 2:71b3736a1bd7 455 RB2_RX_TIME_OUT_F=0;
js 2:71b3736a1bd7 456 RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
js 2:71b3736a1bd7 457 while(RB2_RX_TIME_OUT_F==0 && MOVE_END_F==0 && MOVE_ERR_F==0){
js 2:71b3736a1bd7 458 if(RB2_RX_DATA()>0){
js 2:71b3736a1bd7 459 if(strstr(RB2_rob_rx_buffer,cmp3_txt) ==NULL){//err状態でないことを確認
js 2:71b3736a1bd7 460 //移動完了確認///
js 2:71b3736a1bd7 461 if(strstr(RB2_rob_rx_buffer,cmp2_txt) !=NULL){
js 2:71b3736a1bd7 462 MOVE_END_F=1;
js 2:71b3736a1bd7 463 }else{
js 2:71b3736a1bd7 464 RB2_SEND_CMD("RobotMode()");
js 2:71b3736a1bd7 465 }
js 2:71b3736a1bd7 466 }else{
js 2:71b3736a1bd7 467 MOVE_ERR_F=1;
js 2:71b3736a1bd7 468 ALM_ST=ALM_DOBOT2;
js 2:71b3736a1bd7 469 ALM_HOLD_SET();
js 2:71b3736a1bd7 470 }
js 2:71b3736a1bd7 471 }
js 2:71b3736a1bd7 472 wait_us(10*1000); //10ms
js 2:71b3736a1bd7 473 }
js 2:71b3736a1bd7 474 if(RB2_RX_TIME_OUT_F==1){
js 2:71b3736a1bd7 475 //printf("JOG MOVE END TIMEOUT!! TX=%s Rx=%s\r\n",RB2_rob_tx_buffer,RB2_rob_rx_buffer);
js 2:71b3736a1bd7 476 ALM_ST=ALM_DOBOT2_ED;
js 2:71b3736a1bd7 477 ALM_HOLD_SET();
js 2:71b3736a1bd7 478 }else{
js 2:71b3736a1bd7 479 RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
js 2:71b3736a1bd7 480 }
js 2:71b3736a1bd7 481 RB2_eth_buf_clr();
js 2:71b3736a1bd7 482 }
js 2:71b3736a1bd7 483
js 2:71b3736a1bd7 484
js 2:71b3736a1bd7 485 void RB2_STATUS_READ(unsigned long tout_ms){
js 2:71b3736a1bd7 486 RB2_SEND_CMD("RobotMode()");
js 2:71b3736a1bd7 487 RB2_RX_TIME_OUT_F=0;
js 2:71b3736a1bd7 488 RB2_RX_DATA();
js 2:71b3736a1bd7 489 RB2_RX_TIME_OUT_F=0;
js 2:71b3736a1bd7 490 RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
js 2:71b3736a1bd7 491
js 2:71b3736a1bd7 492 while(RB2_RX_TIME_OUT_F==0 && strstr(RB2_rob_rx_buffer,RB2_rob_tx_buffer) ==NULL){
js 2:71b3736a1bd7 493 RB2_RX_DATA();
js 2:71b3736a1bd7 494 wait_us(10*1000); //10ms
js 2:71b3736a1bd7 495 }
js 2:71b3736a1bd7 496
js 2:71b3736a1bd7 497 if(RB2_RX_TIME_OUT_F==1){
js 2:71b3736a1bd7 498 //printf("CMD ACK TIMEOUT!!\r\n");
js 2:71b3736a1bd7 499 ALM_ST=ALM_DOBOT2;
js 2:71b3736a1bd7 500 ALM_HOLD_SET();
js 2:71b3736a1bd7 501 }else{
js 2:71b3736a1bd7 502 RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
js 2:71b3736a1bd7 503 }
js 2:71b3736a1bd7 504
js 2:71b3736a1bd7 505 sprintf(lcd_print_buf,"STATUS=%s",RB2_rob_rx_buffer);
js 2:71b3736a1bd7 506 lcd_print("t0",lcd_print_buf);
js 2:71b3736a1bd7 507 RB2_eth_buf_clr();
js 2:71b3736a1bd7 508 }
js 2:71b3736a1bd7 509 ////データ受信処理/////////
js 2:71b3736a1bd7 510 unsigned long RB2_RX_DATA(){//WIZnet_Socket.hのset_blocking(bool blocking, unsigned int timeout=1500);の設定で空読みしたときのタイムアウト時間がかわる
js 2:71b3736a1bd7 511 int n= RB2_server.receive(RB2_rob_rx_buffer, sizeof(RB2_rob_rx_buffer));
js 2:71b3736a1bd7 512 if(n > 0){//def 0
js 2:71b3736a1bd7 513 return n;
js 2:71b3736a1bd7 514 }else{
js 2:71b3736a1bd7 515 return 0;
js 2:71b3736a1bd7 516 }
js 2:71b3736a1bd7 517 }
js 2:71b3736a1bd7 518 ////データ受信処理テスト/////////
js 2:71b3736a1bd7 519 unsigned long RB2_RX_DATA_TEST(){
js 2:71b3736a1bd7 520 int n= RB2_server.receive(RB2_rob_rx_buffer, sizeof(RB2_rob_rx_buffer));
js 2:71b3736a1bd7 521 if(n > 0){//def 0
js 2:71b3736a1bd7 522 //printf("ROBO1_DATA=%s\r\n",RB2_rob_rx_buffer);
js 2:71b3736a1bd7 523 RB2_eth_buf_clr();
js 2:71b3736a1bd7 524 return n;
js 2:71b3736a1bd7 525 }else{
js 2:71b3736a1bd7 526 return 0;
js 2:71b3736a1bd7 527 }
js 2:71b3736a1bd7 528 }
js 2:71b3736a1bd7 529 void RB2_CMD_WAIT(unsigned long tout_ms){
js 2:71b3736a1bd7 530
js 2:71b3736a1bd7 531 RB2_RX_DATA();
js 2:71b3736a1bd7 532 RB2_RX_TIME_OUT_F=0;
js 2:71b3736a1bd7 533 RB2_RX_TIME_OUT.attach_us(&RB2_RX_TIME_OUT_IRQ,tout_ms*1000);
js 2:71b3736a1bd7 534
js 2:71b3736a1bd7 535 while(RB2_RX_TIME_OUT_F==0 && strstr(RB2_rob_rx_buffer,RB2_rob_tx_buffer) ==NULL){
js 2:71b3736a1bd7 536 RB2_RX_DATA();
js 2:71b3736a1bd7 537 wait_us(10*1000); //10ms
js 2:71b3736a1bd7 538 }
js 2:71b3736a1bd7 539
js 2:71b3736a1bd7 540 if(RB2_RX_TIME_OUT_F==1){
js 2:71b3736a1bd7 541 //printf("CMD ACK TIMEOUT!!\r\n");
js 2:71b3736a1bd7 542 ALM_ST=ALM_DOBOT2;
js 2:71b3736a1bd7 543 ALM_HOLD_SET();
js 2:71b3736a1bd7 544 }else{
js 2:71b3736a1bd7 545 RB2_RX_TIME_OUT.detach();//タイムアウトカウンタ停止
js 2:71b3736a1bd7 546 }
js 2:71b3736a1bd7 547 RB2_eth_buf_clr();
js 2:71b3736a1bd7 548 }
js 2:71b3736a1bd7 549
js 2:71b3736a1bd7 550 char RB2_SEND_CMD(char *txt){
js 2:71b3736a1bd7 551 unsigned char len = strlen(txt);//文字列の長さを確認
js 2:71b3736a1bd7 552 unsigned char SEND_RET=0;
js 2:71b3736a1bd7 553 sprintf(RB2_rob_tx_buffer,"%s\0",txt);
js 2:71b3736a1bd7 554 SEND_RET=RB2_server.send_all(RB2_rob_tx_buffer,len+1);
js 2:71b3736a1bd7 555 if(SEND_RET !=len+1){
js 2:71b3736a1bd7 556 sprintf(lcd_print_buf,"コマンド送信失敗!! 送信指定数 : %02d 送信完了数 : %02d",len+1,SEND_RET);
js 2:71b3736a1bd7 557 lcd_print("t0",RB2_rob_tx_buffer);
js 2:71b3736a1bd7 558 return -1;
js 2:71b3736a1bd7 559 }else{
js 2:71b3736a1bd7 560
js 2:71b3736a1bd7 561 return 0;
js 2:71b3736a1bd7 562 }
js 2:71b3736a1bd7 563
js 2:71b3736a1bd7 564 }
js 2:71b3736a1bd7 565
js 2:71b3736a1bd7 566
js 2:71b3736a1bd7 567
js 2:71b3736a1bd7 568
js 2:71b3736a1bd7 569
js 2:71b3736a1bd7 570 void RB2_MovJ(double x,double y,double z,double a,double b,double c,unsigned int speed,unsigned int acc){
js 2:71b3736a1bd7 571 char RB2_BUF[128];
js 2:71b3736a1bd7 572 // printf("MovJ(%f,%f,%f,%f,%f,%f,SpeedJ=%d)",x,y,z,a,b,c,speed);
js 2:71b3736a1bd7 573 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){
js 2:71b3736a1bd7 574 lcd_print("t0","現在位置です");
js 2:71b3736a1bd7 575 R2_WAIT_MASK=1;//1でマスク　現在位置への移動指定時WAITをマスクする
js 2:71b3736a1bd7 576 }else{
js 2:71b3736a1bd7 577 sprintf(RB2_BUF,"MovJ(%f,%f,%f,%f,%f,%f,SpeedJ=%d,AccJ=%d)",x,y,z,a,b,c,speed,acc);
js 2:71b3736a1bd7 578 R2_WAIT_MASK=0;//1でマスク　現在位置への移動指定時WAITをマスクする
js 2:71b3736a1bd7 579 if(RB2_SEND_CMD(RB2_BUF)==0){
js 2:71b3736a1bd7 580 RB2_X_SET=x;
js 2:71b3736a1bd7 581 RB2_Y_SET=y;
js 2:71b3736a1bd7 582 RB2_Z_SET=z;
js 2:71b3736a1bd7 583 RB2_A_SET=a;
js 2:71b3736a1bd7 584 RB2_B_SET=b;
js 2:71b3736a1bd7 585 RB2_C_SET=c;
js 2:71b3736a1bd7 586 }else{//コマンド送信失敗
js 2:71b3736a1bd7 587 ALM_ST=ALM_DOBOT2_CM;
js 2:71b3736a1bd7 588 ALM_HOLD_SET();
js 2:71b3736a1bd7 589 }
js 2:71b3736a1bd7 590 }
js 2:71b3736a1bd7 591
js 2:71b3736a1bd7 592 }
js 2:71b3736a1bd7 593
js 2:71b3736a1bd7 594 void RB2_JointMovJ(double Q1,double Q2,double Q3,double Q4,double Q5,double Q6,unsigned int speed,unsigned int acc){
js 2:71b3736a1bd7 595 char RB2_BUF[128];
js 2:71b3736a1bd7 596 if(Q1==RB2_Q1SET && Q2==RB2_Q2SET && Q3==RB2_Q3SET && Q4==RB2_Q4SET &&Q5==RB2_Q5SET && Q6==RB2_Q6SET){
js 2:71b3736a1bd7 597 lcd_print("t0","現在位置です");
js 2:71b3736a1bd7 598 R2_WAIT_MASK=1;//1でマスク　現在位置への移動指定時WAITをマスクする
js 2:71b3736a1bd7 599 }else{
js 2:71b3736a1bd7 600 sprintf(RB2_rob_tx_buffer,"JointMovJ(%f,%f,%f,%f,%f,%f,SpeedJ=%d,AccJ=%d)",Q1,Q2,Q3,Q4,Q5,Q6,speed,acc);
js 2:71b3736a1bd7 601 R2_WAIT_MASK=0;//1でマスク　現在位置への移動指定時WAITをマスクする
js 2:71b3736a1bd7 602 if(RB2_SEND_CMD(RB2_rob_tx_buffer)==0){
js 2:71b3736a1bd7 603 RB2_Q1SET=Q1;
js 2:71b3736a1bd7 604 RB2_Q2SET=Q2;
js 2:71b3736a1bd7 605 RB2_Q3SET=Q3;
js 2:71b3736a1bd7 606 RB2_Q4SET=Q4;
js 2:71b3736a1bd7 607 RB2_Q5SET=Q5;
js 2:71b3736a1bd7 608 RB2_Q6SET=Q6;
js 2:71b3736a1bd7 609 }else{//コマンド送信失敗
js 2:71b3736a1bd7 610 ALM_ST=ALM_DOBOT2_CM;
js 2:71b3736a1bd7 611 ALM_HOLD_SET();
js 2:71b3736a1bd7 612 }
js 2:71b3736a1bd7 613 }
js 2:71b3736a1bd7 614
js 2:71b3736a1bd7 615 }
js 2:71b3736a1bd7 616 void RB2_MovL(double x,double y,double z,double a,double b,double c,unsigned int speed,unsigned int acc){
js 2:71b3736a1bd7 617 char RB2_BUF[128];
js 2:71b3736a1bd7 618 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){
js 2:71b3736a1bd7 619 lcd_print("t0","現在位置です");
js 2:71b3736a1bd7 620 R2_WAIT_MASK=1;//1でマスク　現在位置への移動指定時WAITをマスクする
js 2:71b3736a1bd7 621 }else{
js 2:71b3736a1bd7 622 sprintf(RB2_BUF,"MovL(%f,%f,%f,%f,%f,%f,SpeedL=%d,AccL=%d)",x,y,z,a,b,c,speed,acc);
js 2:71b3736a1bd7 623 R2_WAIT_MASK=0;//1でマスク　現在位置への移動指定時WAITをマスクする
js 2:71b3736a1bd7 624 if(RB2_SEND_CMD(RB2_BUF)==0){
js 2:71b3736a1bd7 625 RB2_X_SET=x;
js 2:71b3736a1bd7 626 RB2_Y_SET=y;
js 2:71b3736a1bd7 627 RB2_Z_SET=z;
js 2:71b3736a1bd7 628 RB2_A_SET=a;
js 2:71b3736a1bd7 629 RB2_B_SET=b;
js 2:71b3736a1bd7 630 RB2_C_SET=c;
js 2:71b3736a1bd7 631 }else{//コマンド送信失敗
js 2:71b3736a1bd7 632 ALM_ST=ALM_DOBOT2_CM;
js 2:71b3736a1bd7 633 ALM_HOLD_SET();
js 2:71b3736a1bd7 634 }
js 2:71b3736a1bd7 635 }
js 2:71b3736a1bd7 636 }
js 2:71b3736a1bd7 637
js 2:71b3736a1bd7 638
js 2:71b3736a1bd7 639 void RB2_HOME_MOVE(){
js 2:71b3736a1bd7 640 ///現在位置を確認///
js 2:71b3736a1bd7 641 RB2_GETPOS(RB_EWAIT);
js 2:71b3736a1bd7 642 ///Zが-40以下の場合は、Zを上げてから原点復帰///
js 2:71b3736a1bd7 643
js 2:71b3736a1bd7 644 if(RB2_Z_GET<100){
js 2:71b3736a1bd7 645 RB2_MovJ(RB2_X_GET,RB2_Y_GET,100.0,RB2_R_GET,0,0,RB2_SPEED_ADJ,RB2_ACC_ADJ);
js 2:71b3736a1bd7 646 RB2_MOVE_WAIT(5000);
js 2:71b3736a1bd7 647 }
js 2:71b3736a1bd7 648
js 2:71b3736a1bd7 649 RB2_MovJ(178.0,124.0,100.0,0,0,0,RB2_SPEED_ADJ,RB2_ACC_ADJ);
js 2:71b3736a1bd7 650 RB2_MOVE_WAIT(10000);
js 2:71b3736a1bd7 651 RB2_MovJ(220.0,0.0,80.0,0,0,0,RB2_SPEED_ADJ,RB2_ACC_ADJ);
js 2:71b3736a1bd7 652 RB2_MOVE_WAIT(10000);
js 2:71b3736a1bd7 653
js 2:71b3736a1bd7 654 RB2_PSEL_BAK=0xFE;
js 2:71b3736a1bd7 655
js 2:71b3736a1bd7 656 ///ハンドの制御ポートを開側にする///
js 2:71b3736a1bd7 657 RB2_DO_CONT(1,0);
js 2:71b3736a1bd7 658 RB2_HAND_ST=RB2_HAND_OPEN;
js 2:71b3736a1bd7 659
js 2:71b3736a1bd7 660 }
js 2:71b3736a1bd7 661
js 2:71b3736a1bd7 662 void RB2_SEL_MOVE(unsigned char SEL,unsigned int speed,unsigned int acc){
js 2:71b3736a1bd7 663 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);
js 2:71b3736a1bd7 664 }
js 2:71b3736a1bd7 665
js 2:71b3736a1bd7 666 void RB2_SEL_MOVE_OLG(unsigned char SEL,unsigned int speed,unsigned int acc){//現在値と差分があるときだけ移動
js 2:71b3736a1bd7 667 unsigned char POS_CHK_F=0;
js 2:71b3736a1bd7 668
js 2:71b3736a1bd7 669 RB2_GETPOS(RB_EWAIT);
js 2:71b3736a1bd7 670 ////現在位置と指定座標に差分があるか確認///
js 2:71b3736a1bd7 671 if(RB2_X_GET > RB2_POINT_RD[SEL][PX]+0.1 || RB2_X_GET < RB2_POINT_RD[SEL][PX]-0.1){
js 2:71b3736a1bd7 672 POS_CHK_F=1;
js 2:71b3736a1bd7 673 }
js 2:71b3736a1bd7 674 if(RB2_Y_GET > RB2_POINT_RD[SEL][PY]+0.1 || RB2_Y_GET < RB2_POINT_RD[SEL][PY]-0.1){
js 2:71b3736a1bd7 675 POS_CHK_F=1;
js 2:71b3736a1bd7 676 }
js 2:71b3736a1bd7 677 if(RB2_Z_GET > RB2_POINT_RD[SEL][PZ]+0.1 || RB2_Z_GET < RB2_POINT_RD[SEL][PZ]-0.1){
js 2:71b3736a1bd7 678 POS_CHK_F=1;
js 2:71b3736a1bd7 679 }
js 2:71b3736a1bd7 680 if(RB2_R_GET > RB2_POINT_RD[SEL][PR]+0.1 || RB2_R_GET < RB2_POINT_RD[SEL][PR]-0.1){
js 2:71b3736a1bd7 681 POS_CHK_F=1;
js 2:71b3736a1bd7 682 }
js 2:71b3736a1bd7 683
js 2:71b3736a1bd7 684 if(POS_CHK_F==1){
js 2:71b3736a1bd7 685 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);
js 2:71b3736a1bd7 686 }else{
js 2:71b3736a1bd7 687 lcd_print("t0","現在位置です。");
js 2:71b3736a1bd7 688 }
js 2:71b3736a1bd7 689
js 2:71b3736a1bd7 690
js 2:71b3736a1bd7 691 }
js 2:71b3736a1bd7 692 void RB2_SEL_MOVE_SQ(unsigned char SEL,unsigned char MOV_MOD,unsigned int speed,unsigned int acc){
js 2:71b3736a1bd7 693
js 2:71b3736a1bd7 694 if(SEL !=RB2_PSEL_BAK){
js 2:71b3736a1bd7 695 if(MOV_MOD==0){
js 2:71b3736a1bd7 696 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);
js 2:71b3736a1bd7 697 }else{
js 2:71b3736a1bd7 698 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);
js 2:71b3736a1bd7 699 }
js 2:71b3736a1bd7 700 R2_WAIT_MASK=0;//1でマスク　現在位置への移動指定時WAITをマスクする
js 2:71b3736a1bd7 701 RB2_PSEL_BAK=SEL;
js 2:71b3736a1bd7 702 }else{
js 2:71b3736a1bd7 703 lcd_print("t0","現在位置です。");
js 2:71b3736a1bd7 704 R2_WAIT_MASK=1;//1でマスク　現在位置への移動指定時WAITをマスクする
js 2:71b3736a1bd7 705 }
js 2:71b3736a1bd7 706
js 2:71b3736a1bd7 707
js 2:71b3736a1bd7 708 }
js 2:71b3736a1bd7 709 void RB2_DO_CONT(unsigned char INDEX,unsigned char STATUS){//例:DO(11,1) でDO11がON DO(11,0) でDO11がOFF
js 2:71b3736a1bd7 710 char RB2_BUF[64];
js 2:71b3736a1bd7 711 sprintf(RB2_BUF,"DO(%d,%d)",INDEX, STATUS);
js 2:71b3736a1bd7 712 RB2_SEND_CMD(RB2_BUF);
js 2:71b3736a1bd7 713 RB2_CMD_WAIT(RB_SWAIT);
js 2:71b3736a1bd7 714 }
js 2:71b3736a1bd7 715
js 2:71b3736a1bd7 716
js 2:71b3736a1bd7 717 void RB2_HAND_CONT(unsigned char cont){
js 2:71b3736a1bd7 718 if(cont != RB2_HAND_ST){
js 2:71b3736a1bd7 719 if(cont==RB2_HAND_CLOSE){
js 2:71b3736a1bd7 720 RB2_DO_CONT(1,1);
js 2:71b3736a1bd7 721 }else{
js 2:71b3736a1bd7 722 RB2_DO_CONT(1,0);
js 2:71b3736a1bd7 723 }
js 2:71b3736a1bd7 724 RB2_HAND_ST=cont;
js 2:71b3736a1bd7 725 }
js 2:71b3736a1bd7 726 }
js 2:71b3736a1bd7 727 ////現在位置からx1,y1,z1を経由してx2,y2,z2まで移動（円運動）////
js 2:71b3736a1bd7 728 void RB2_ARC(double x1,double y1,double z1,double x2,double y2,double z2,unsigned int speed,unsigned int acc){
js 2:71b3736a1bd7 729 char RB2_BUF[128];
js 2:71b3736a1bd7 730
js 2:71b3736a1bd7 731 sprintf(RB2_BUF,"Arc(%f,%f,%f,%f,%f,%f,SpeedL=%d,AccL=%d)",x1,y1,z1,x2,y2,z2,speed,acc);
js 2:71b3736a1bd7 732 if(RB2_SEND_CMD(RB2_BUF)==0){
js 2:71b3736a1bd7 733 RB2_X_SET=x2;
js 2:71b3736a1bd7 734 RB2_Y_SET=y2;
js 2:71b3736a1bd7 735 RB2_Z_SET=z2;
js 2:71b3736a1bd7 736 }else{//コマンド送信失敗
js 2:71b3736a1bd7 737 ALM_ST=ALM_DOBOT2_CM;
js 2:71b3736a1bd7 738 ALM_HOLD_SET();
js 2:71b3736a1bd7 739 }
js 2:71b3736a1bd7 740
js 2:71b3736a1bd7 741 }
js 2:71b3736a1bd7 742 void RB2_EMG_STOP(){
js 2:71b3736a1bd7 743 RB2_SEND_CMD("EmergencyStop()");
js 2:71b3736a1bd7 744 }
js 2:71b3736a1bd7 745
js 2:71b3736a1bd7 746 void RB2_SPEED_SET(unsigned char SET){
js 2:71b3736a1bd7 747 if(SET==RB_HIGH){
js 2:71b3736a1bd7 748 RB2_SPEED_ADJ = RB_SPEED_HIGH;
js 2:71b3736a1bd7 749 RB2_ACC_ADJ = RB_ACC_HIGH;
js 2:71b3736a1bd7 750 }else{
js 2:71b3736a1bd7 751 RB2_SPEED_ADJ = RB_SPEED_LOW;
js 2:71b3736a1bd7 752 RB2_ACC_ADJ = RB_ACC_LOW;
js 2:71b3736a1bd7 753 }
js 2:71b3736a1bd7 754 }