w mx / Mbed 2 deprecated wmx_arm_scara

scara机械臂

Dependencies:   MQTT SDFileSystem WIZnet_Library mbed

Fork of wmx_arm_communication by w mx

mainwmx.cpp@14:6a6c6def773a, 2018-08-03 (annotated)

Committer:
xmwmx
Date:
Fri Aug 03 10:51:17 2018 +0000
Revision:
14:6a6c6def773a
Parent:
13:a6e12b4c7192 
xxx

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xmwmx 8:e76bebe9c45f 1 /************************************************************************************************
xmwmx 8:e76bebe9c45f 2 File name: main.cpp
xmwmx 13:a6e12b4c7192 3 Description: 项目部分客户端代码
xmwmx 13:a6e12b4c7192 4 Author:
xmwmx 13:a6e12b4c7192 5 Date:
xmwmx 8:e76bebe9c45f 6 Others: 本程序编写在mbed平台(https://os.mbed.com/)上
xmwmx 8:e76bebe9c45f 7
xmwmx 8:e76bebe9c45f 8 *************************************************************************************************/
xmwmx 8:e76bebe9c45f 9
xmwmx 8:e76bebe9c45f 10 #include "mbed.h"
xmwmx 8:e76bebe9c45f 11 #include <math.h>
xmwmx 8:e76bebe9c45f 12 #include <cstring>
xmwmx 8:e76bebe9c45f 13 #include <stdlib.h>
xmwmx 8:e76bebe9c45f 14 #include "SDFileSystem.h"
xmwmx 8:e76bebe9c45f 15 #include "networking.h"
xmwmx 13:a6e12b4c7192 16 #include <cmath>
xmwmx 8:e76bebe9c45f 17
xmwmx 8:e76bebe9c45f 18
xmwmx 13:a6e12b4c7192 19 #define run 10000
xmwmx 13:a6e12b4c7192 20 #define M_PI 3.141526535
xmwmx 13:a6e12b4c7192 21 #define ky 4.0// 52/13
xmwmx 13:a6e12b4c7192 22 #define kx 5.84615// 76/13
xmwmx 13:a6e12b4c7192 23 #define lianggang 30// 量纲
xmwmx 10:acc53f50c9df 24
xmwmx 8:e76bebe9c45f 25 /////调试
xmwmx 8:e76bebe9c45f 26 Serial pc(PA_9, PA_10);
xmwmx 10:acc53f50c9df 27 Serial bt(PB_10, PB_11);
xmwmx 8:e76bebe9c45f 28 DigitalOut LED(PB_8);
xmwmx 8:e76bebe9c45f 29 /////
xmwmx 8:e76bebe9c45f 30
xmwmx 8:e76bebe9c45f 31 //硬件接口
xmwmx 8:e76bebe9c45f 32 // mosi, miso, sclk, cs, name
xmwmx 8:e76bebe9c45f 33 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
xmwmx 8:e76bebe9c45f 34 //步进电机
xmwmx 10:acc53f50c9df 35 DigitalOut step[3] = {DigitalOut(PC_5), DigitalOut(PA_5),DigitalOut(PC_3)}; //0--x P9,1--y P17
xmwmx 10:acc53f50c9df 36 DigitalOut dir[3] = {DigitalOut(PC_4), DigitalOut(PA_4),DigitalOut(PC_2)}; //0--x,1--y
xmwmx 10:acc53f50c9df 37 DigitalOut en[3] = {DigitalOut(PD_2), DigitalOut(PA_2),DigitalOut(PC_13)}; //0--x,1--y
xmwmx 8:e76bebe9c45f 38 //电子开关,激光开关
xmwmx 8:e76bebe9c45f 39 DigitalOut switch_GS(PC_15);
xmwmx 8:e76bebe9c45f 40 //行程开关
xmwmx 13:a6e12b4c7192 41 DigitalIn switch_pos1(PA_3); //P25
xmwmx 8:e76bebe9c45f 42 DigitalIn switch_pos2(PA_6); //P26
xmwmx 10:acc53f50c9df 43 DigitalIn switch_pos3(PC_6); //P27
xmwmx 12:51be919e2c78 44
xmwmx 8:e76bebe9c45f 45 //与实际有关参数
xmwmx 13:a6e12b4c7192 46 double now_x = 0,
xmwmx 13:a6e12b4c7192 47 now_y = 0,
xmwmx 13:a6e12b4c7192 48 now_z = 0,
xmwmx 13:a6e12b4c7192 49 now_theta=0,
xmwmx 13:a6e12b4c7192 50 now_alpha=1.0;
xmwmx 13:a6e12b4c7192 51 double first[]={150,150,0};
xmwmx 8:e76bebe9c45f 52 ///Ticker ticker_step;
xmwmx 8:e76bebe9c45f 53 float step_halfperiod = 0.0001;//0.0002;
xmwmx 13:a6e12b4c7192 54 int dir_x = 0;///调试时调整
xmwmx 13:a6e12b4c7192 55 int dir_y = 0;///调试时调整
xmwmx 10:acc53f50c9df 56 int dir_z = 1;///调试时调整
xmwmx 10:acc53f50c9df 57
xmwmx 13:a6e12b4c7192 58 FILE *fp_drawing;// 存储图案的文件
xmwmx 13:a6e12b4c7192 59
xmwmx 13:a6e12b4c7192 60 double theta=0,alpha=0;
xmwmx 11:568789ce1397 61
xmwmx 10:acc53f50c9df 62 char buf[256];
xmwmx 10:acc53f50c9df 63 int cur;
xmwmx 13:a6e12b4c7192 64 bool ready;
xmwmx 10:acc53f50c9df 65 bool received;
xmwmx 13:a6e12b4c7192 66 bool start;
xmwmx 13:a6e12b4c7192 67 bool done;
xmwmx 14:6a6c6def773a 68 bool panduan;
xmwmx 8:e76bebe9c45f 69
xmwmx 8:e76bebe9c45f 70 void rotate(int id ,int pix);
xmwmx 12:51be919e2c78 71 void init_zero();
xmwmx 13:a6e12b4c7192 72 void moveto(double x, double y,double z);
xmwmx 8:e76bebe9c45f 73
xmwmx 8:e76bebe9c45f 74
xmwmx 10:acc53f50c9df 75 void beginning()
xmwmx 8:e76bebe9c45f 76 {
xmwmx 13:a6e12b4c7192 77 //
xmwmx 13:a6e12b4c7192 78 //
xmwmx 13:a6e12b4c7192 79 pc.printf("y\r\n"); //
xmwmx 13:a6e12b4c7192 80 while(switch_pos2.read()) //
xmwmx 13:a6e12b4c7192 81 {
xmwmx 13:a6e12b4c7192 82 //pc.printf("y\r\n"); //
xmwmx 13:a6e12b4c7192 83 rotate(1,-5); // --
xmwmx 13:a6e12b4c7192 84 } // ||
xmwmx 13:a6e12b4c7192 85 rotate(1,200); // ||
xmwmx 13:a6e12b4c7192 86 pc.printf("z\r\n"); // || y
xmwmx 13:a6e12b4c7192 87 while(switch_pos3.read()) // z |==|======[]======[]
xmwmx 13:a6e12b4c7192 88 { // ||
xmwmx 13:a6e12b4c7192 89 pc.printf("z\r\n"); // ||
xmwmx 13:a6e12b4c7192 90 rotate(2,-5); // ||x
xmwmx 13:a6e12b4c7192 91 } // ---------------------------------
xmwmx 13:a6e12b4c7192 92 rotate(2,50000); //
xmwmx 13:a6e12b4c7192 93 pc.printf("x\r\n"); //
xmwmx 10:acc53f50c9df 94 while(switch_pos1.read()) //
xmwmx 13:a6e12b4c7192 95 {
xmwmx 13:a6e12b4c7192 96 //pc.printf("x\r\n"); //
xmwmx 13:a6e12b4c7192 97 rotate(0,-5); //
xmwmx 13:a6e12b4c7192 98 }
xmwmx 13:a6e12b4c7192 99 rotate(0,400);
xmwmx 13:a6e12b4c7192 100 //moveto(first[0],first[1],first[2]);
xmwmx 12:51be919e2c78 101 init_zero();
xmwmx 13:a6e12b4c7192 102 pc.printf("x %f | y %f || th %f | al %f \r\n",now_x,now_y,theta,alpha);
xmwmx 13:a6e12b4c7192 103 //wait(3);
xmwmx 13:a6e12b4c7192 104 //moveto(200,0,0);
xmwmx 13:a6e12b4c7192 105 //wait(3);
xmwmx 13:a6e12b4c7192 106 //moveto(200,200,0);
xmwmx 13:a6e12b4c7192 107
xmwmx 13:a6e12b4c7192 108 }
xmwmx 13:a6e12b4c7192 109 /****************
xmwmx 13:a6e12b4c7192 110 * x, y单位为mm
xmwmx 13:a6e12b4c7192 111 * theta, alpha单位为rad
xmwmx 13:a6e12b4c7192 112 ******************/
xmwmx 13:a6e12b4c7192 113 void scaraArmCal(double x, double y, double &theta, double &alpha)
xmwmx 13:a6e12b4c7192 114 {
xmwmx 13:a6e12b4c7192 115 const double a(200.0), b(200.0);
xmwmx 13:a6e12b4c7192 116
xmwmx 13:a6e12b4c7192 117 double rouSquare = x * x + y * y;
xmwmx 13:a6e12b4c7192 118 double rou = sqrt(rouSquare);
xmwmx 13:a6e12b4c7192 119 alpha = acos((a * a + b * b - rouSquare) / (2 * a * b));
xmwmx 13:a6e12b4c7192 120 theta = acos((a * a + rouSquare - b * b) / (2 * a * rou)) + (y >= 0 ? atan2(y, x) : (atan2(y, x) + M_PI * 2));
xmwmx 8:e76bebe9c45f 121 }
xmwmx 8:e76bebe9c45f 122
xmwmx 10:acc53f50c9df 123 void init_zero()
xmwmx 8:e76bebe9c45f 124 {
xmwmx 8:e76bebe9c45f 125 now_x = 0;
xmwmx 8:e76bebe9c45f 126 now_y = 0;
xmwmx 10:acc53f50c9df 127 now_z = 0;
xmwmx 13:a6e12b4c7192 128 now_theta=0;
xmwmx 13:a6e12b4c7192 129 now_alpha=1.0;
xmwmx 10:acc53f50c9df 130 strcpy(buf,"");
xmwmx 10:acc53f50c9df 131 // step[0] = 0;
xmwmx 10:acc53f50c9df 132 // step[1] = 0;
xmwmx 10:acc53f50c9df 133 // step[2] = 0;
xmwmx 12:51be919e2c78 134 }
xmwmx 8:e76bebe9c45f 135
xmwmx 12:51be919e2c78 136 void rotate(int id, int pix) //id= 0--x,1--y ,2--z pix=32000为一圈
xmwmx 8:e76bebe9c45f 137 {
xmwmx 12:51be919e2c78 138 if (pix >= 0)
xmwmx 12:51be919e2c78 139 {
xmwmx 8:e76bebe9c45f 140 dir[0] = dir_x;
xmwmx 8:e76bebe9c45f 141 dir[1] = dir_y;
xmwmx 10:acc53f50c9df 142 dir[2] = dir_z;
xmwmx 12:51be919e2c78 143 }
xmwmx 12:51be919e2c78 144 else
xmwmx 12:51be919e2c78 145 {
xmwmx 8:e76bebe9c45f 146 pix = -pix;
xmwmx 8:e76bebe9c45f 147 dir[0] = 1 - dir_x;
xmwmx 8:e76bebe9c45f 148 dir[1] = 1 - dir_y;
xmwmx 10:acc53f50c9df 149 dir[2] = 1 - dir_z;
xmwmx 8:e76bebe9c45f 150 }
xmwmx 12:51be919e2c78 151 for (int i = 0; i < pix; i++)
xmwmx 12:51be919e2c78 152 {
xmwmx 8:e76bebe9c45f 153 step[id] = 1;
xmwmx 8:e76bebe9c45f 154 wait(step_halfperiod);
xmwmx 8:e76bebe9c45f 155 step[id] = 0;
xmwmx 8:e76bebe9c45f 156 wait(step_halfperiod);
xmwmx 8:e76bebe9c45f 157 }
xmwmx 8:e76bebe9c45f 158 }
xmwmx 8:e76bebe9c45f 159
xmwmx 13:a6e12b4c7192 160 void moveto(double x, double y,double z)
xmwmx 8:e76bebe9c45f 161 {
xmwmx 13:a6e12b4c7192 162 scaraArmCal(x, y , theta, alpha);
xmwmx 13:a6e12b4c7192 163 if(theta>=0&&alpha>=1.0&&((x*x+y*y)<160000))
xmwmx 12:51be919e2c78 164 {
xmwmx 13:a6e12b4c7192 165 if(z-now_z<=0)
xmwmx 13:a6e12b4c7192 166 {
xmwmx 13:a6e12b4c7192 167 rotate(0, (theta-now_theta)*run*kx/(2*M_PI));
xmwmx 13:a6e12b4c7192 168 rotate(1, (alpha-now_alpha)*run*ky/(2*M_PI));
xmwmx 13:a6e12b4c7192 169 rotate(2, (z - now_z)*run/10);
xmwmx 13:a6e12b4c7192 170 }
xmwmx 13:a6e12b4c7192 171 else
xmwmx 13:a6e12b4c7192 172 {
xmwmx 13:a6e12b4c7192 173 rotate(2, (z - now_z)*run/10);
xmwmx 13:a6e12b4c7192 174 rotate(0, (theta-now_theta)*run*kx/(2*M_PI));
xmwmx 13:a6e12b4c7192 175 rotate(1, (alpha-now_alpha)*run*ky/(2*M_PI));
xmwmx 13:a6e12b4c7192 176 }
xmwmx 13:a6e12b4c7192 177 pc.printf("x %f | y %f | th| %f | al %f \r\n",x,y,theta,alpha);
xmwmx 12:51be919e2c78 178 now_x = x;
xmwmx 12:51be919e2c78 179 now_y = y;
xmwmx 12:51be919e2c78 180 now_z = z;
xmwmx 13:a6e12b4c7192 181 now_theta=theta;
xmwmx 13:a6e12b4c7192 182 now_alpha=alpha;
xmwmx 13:a6e12b4c7192 183
xmwmx 12:51be919e2c78 184 }
xmwmx 12:51be919e2c78 185 else
xmwmx 12:51be919e2c78 186 {
xmwmx 13:a6e12b4c7192 187 pc.printf("x %f | y %f | th| %f | al %f limited\r\n",x,y,theta,alpha);
xmwmx 12:51be919e2c78 188 }
xmwmx 13:a6e12b4c7192 189 theta=0;
xmwmx 13:a6e12b4c7192 190 alpha=0;
xmwmx 8:e76bebe9c45f 191 }
xmwmx 10:acc53f50c9df 192
xmwmx 10:acc53f50c9df 193
xmwmx 13:a6e12b4c7192 194 /*
xmwmx 8:e76bebe9c45f 195 void draw()
xmwmx 8:e76bebe9c45f 196 {
xmwmx 10:acc53f50c9df 197 //unit_xy=unit_xy*k;
xmwmx 10:acc53f50c9df 198 float x, y, z,a;
xmwmx 10:acc53f50c9df 199 FILE *fp_drawing = fopen("/sd/write.txt", "r");
xmwmx 8:e76bebe9c45f 200 pc.printf("reading\r\n");
xmwmx 10:acc53f50c9df 201 flagp=0;
xmwmx 10:acc53f50c9df 202 for (int i = 0; fscanf(fp_drawing, "(%f %f %f %f)", &x, &y, &z,&a) == 4&&flagp==0; i++)
xmwmx 8:e76bebe9c45f 203 {
xmwmx 10:acc53f50c9df 204 pc.printf("(%f,%f,%f)|%d\r\n", x, y, z,i);
xmwmx 10:acc53f50c9df 205 moveto(x, y,z);
xmwmx 8:e76bebe9c45f 206 // moveto(thedata[i][0],thedata[i][1]);
xmwmx 8:e76bebe9c45f 207 // markd??>ot(thedata[i][2]);
xmwmx 8:e76bebe9c45f 208 }
xmwmx 8:e76bebe9c45f 209 fclose(fp_drawing);
xmwmx 10:acc53f50c9df 210 flagw = false;
xmwmx 10:acc53f50c9df 211 flagp = false;
xmwmx 10:acc53f50c9df 212 pc.printf("Here4!\r\n");
xmwmx 8:e76bebe9c45f 213 }
xmwmx 8:e76bebe9c45f 214
xmwmx 10:acc53f50c9df 215
xmwmx 8:e76bebe9c45f 216
xmwmx 11:568789ce1397 217 void on_control_cmd(const char* actuator_name, const char* control_value)
xmwmx 11:568789ce1397 218 {
xmwmx 11:568789ce1397 219
xmwmx 11:568789ce1397 220 if (strcmp(actuator_name, "create") == 0)
xmwmx 11:568789ce1397 221 {
xmwmx 11:568789ce1397 222 int the_command = atoi(control_value);
xmwmx 11:568789ce1397 223 //
xmwmx 11:568789ce1397 224 if(the_command==0)
xmwmx 11:568789ce1397 225 {
xmwmx 11:568789ce1397 226 ready=1;
xmwmx 11:568789ce1397 227 pc.printf("get ready\r\n");
xmwmx 11:568789ce1397 228 }
xmwmx 11:568789ce1397 229 else if(the_command==1)
xmwmx 11:568789ce1397 230 {
xmwmx 11:568789ce1397 231 start=1;
xmwmx 11:568789ce1397 232 pc.printf("get start\r\n");
xmwmx 11:568789ce1397 233 }
xmwmx 11:568789ce1397 234 else if(the_command==2)
xmwmx 11:568789ce1397 235 {
xmwmx 11:568789ce1397 236 done=1;
xmwmx 11:568789ce1397 237 pc.printf("get done\r\n");
xmwmx 11:568789ce1397 238 }
xmwmx 11:568789ce1397 239 //
xmwmx 11:568789ce1397 240
xmwmx 11:568789ce1397 241 }
xmwmx 11:568789ce1397 242 }
xmwmx 13:a6e12b4c7192 243 */
xmwmx 11:568789ce1397 244
xmwmx 13:a6e12b4c7192 245 void on_control_cmd(const char* actuator_name, const char* control_value)
xmwmx 13:a6e12b4c7192 246 {
xmwmx 13:a6e12b4c7192 247 //pc.printf("Received [%s] [%s]\r\n", actuator_name, control_value);
xmwmx 14:6a6c6def773a 248 if (strcmp(actuator_name, "dat") == 0)
xmwmx 13:a6e12b4c7192 249 {
xmwmx 13:a6e12b4c7192 250 //接受到的坐标数据写入存储卡
xmwmx 14:6a6c6def773a 251
xmwmx 13:a6e12b4c7192 252 received=1;
xmwmx 13:a6e12b4c7192 253 fprintf(fp_drawing, "%s", control_value);
xmwmx 13:a6e12b4c7192 254 pc.printf("received %s\r\n",control_value);
xmwmx 14:6a6c6def773a 255
xmwmx 13:a6e12b4c7192 256 }
xmwmx 13:a6e12b4c7192 257 if (strcmp(actuator_name, "command") == 0)
xmwmx 13:a6e12b4c7192 258 {
xmwmx 13:a6e12b4c7192 259 int the_command = atoi(control_value);
xmwmx 13:a6e12b4c7192 260 //
xmwmx 13:a6e12b4c7192 261 if(the_command==1)
xmwmx 13:a6e12b4c7192 262 {
xmwmx 13:a6e12b4c7192 263 ready=1;
xmwmx 13:a6e12b4c7192 264 pc.printf("get ready\r\n");
xmwmx 13:a6e12b4c7192 265 }
xmwmx 13:a6e12b4c7192 266 else if(the_command==2)
xmwmx 13:a6e12b4c7192 267 {
xmwmx 13:a6e12b4c7192 268 start=1;
xmwmx 13:a6e12b4c7192 269 pc.printf("get start\r\n");
xmwmx 13:a6e12b4c7192 270 }
xmwmx 13:a6e12b4c7192 271 else if(the_command==3)
xmwmx 13:a6e12b4c7192 272 {
xmwmx 13:a6e12b4c7192 273 done=1;
xmwmx 13:a6e12b4c7192 274 pc.printf("get done\r\n");
xmwmx 13:a6e12b4c7192 275 }
xmwmx 13:a6e12b4c7192 276 //
xmwmx 13:a6e12b4c7192 277
xmwmx 13:a6e12b4c7192 278 }
xmwmx 13:a6e12b4c7192 279
xmwmx 13:a6e12b4c7192 280
xmwmx 13:a6e12b4c7192 281
xmwmx 13:a6e12b4c7192 282
xmwmx 13:a6e12b4c7192 283 }
xmwmx 11:568789ce1397 284
xmwmx 8:e76bebe9c45f 285 int main()
xmwmx 8:e76bebe9c45f 286 {
xmwmx 11:568789ce1397 287 MQTTSocket sock;
xmwmx 11:568789ce1397 288 MClient client(sock);
xmwmx 11:568789ce1397 289 //声明所有的传感器，每行一个，每个由名字、单位两部分组成，最后一行必须为空指针作为结尾
xmwmx 11:568789ce1397 290 const char* sensors[][2] = {
xmwmx 11:568789ce1397 291 "report", "",
xmwmx 11:568789ce1397 292 NULL, NULL //最后一行以空指针作为结束标记
xmwmx 11:568789ce1397 293 };
xmwmx 11:568789ce1397 294
xmwmx 11:568789ce1397 295 //声明所有的执行器，每行一个，每个由名字、参数类型两部分组成，最后一行必须为空指针作为结尾
xmwmx 11:568789ce1397 296 const char* actuators[][2] = {
xmwmx 14:6a6c6def773a 297 "dat", "",
xmwmx 14:6a6c6def773a 298 "command", "",
xmwmx 11:568789ce1397 299 NULL, NULL //最后一行以空指针作为结束标记
xmwmx 11:568789ce1397 300 };
xmwmx 11:568789ce1397 301 pc.printf("connecting...\r\n");
xmwmx 11:568789ce1397 302
xmwmx 11:568789ce1397 303 networking_init(sock, client, "192.168.1.100", sensors, actuators, on_control_cmd);
xmwmx 11:568789ce1397 304
xmwmx 11:568789ce1397 305 pc.printf("Initialization done.\r\n");
xmwmx 11:568789ce1397 306 pc.printf("laser printer send ready.haha\r\n");
xmwmx 13:a6e12b4c7192 307
xmwmx 13:a6e12b4c7192 308
xmwmx 13:a6e12b4c7192 309 init_zero(); //初始化
xmwmx 13:a6e12b4c7192 310 pc.printf("beginning\r\n");
xmwmx 13:a6e12b4c7192 311 beginning();
xmwmx 13:a6e12b4c7192 312 pc.printf("finish\r\n");
xmwmx 13:a6e12b4c7192 313
xmwmx 13:a6e12b4c7192 314
xmwmx 11:568789ce1397 315 ready=0;
xmwmx 13:a6e12b4c7192 316 received=0;
xmwmx 11:568789ce1397 317 start=0;
xmwmx 11:568789ce1397 318 done=0;
xmwmx 14:6a6c6def773a 319 panduan=0;
xmwmx 13:a6e12b4c7192 320
xmwmx 13:a6e12b4c7192 321
xmwmx 11:568789ce1397 322 while(1)
xmwmx 11:568789ce1397 323 {
xmwmx 11:568789ce1397 324 if(!ready)
xmwmx 11:568789ce1397 325 {
xmwmx 11:568789ce1397 326 publish_value(client, "report", "ready.");
xmwmx 11:568789ce1397 327
xmwmx 11:568789ce1397 328 }
xmwmx 14:6a6c6def773a 329 else if(ready&&!received&&panduan==0)
xmwmx 9:a3f9faa9ca1f 330 {
xmwmx 13:a6e12b4c7192 331 pc.printf("send wait.\r\n");
xmwmx 13:a6e12b4c7192 332 fp_drawing = fopen("/sd/write.txt", "w");
xmwmx 13:a6e12b4c7192 333 pc.printf("opened\r\n");
xmwmx 13:a6e12b4c7192 334 publish_value(client, "report", "wait data.");
xmwmx 14:6a6c6def773a 335 panduan=1;
xmwmx 9:a3f9faa9ca1f 336 }
xmwmx 10:acc53f50c9df 337
xmwmx 13:a6e12b4c7192 338 else if(received&&!start)
xmwmx 13:a6e12b4c7192 339 {
xmwmx 13:a6e12b4c7192 340 publish_value(client, "report", "received.");
xmwmx 13:a6e12b4c7192 341 pc.printf("send received.\r\n");
xmwmx 13:a6e12b4c7192 342 received=0;
xmwmx 13:a6e12b4c7192 343 }
xmwmx 13:a6e12b4c7192 344
xmwmx 13:a6e12b4c7192 345 else if(start)
xmwmx 13:a6e12b4c7192 346 {
xmwmx 14:6a6c6def773a 347 fclose(fp_drawing);
xmwmx 13:a6e12b4c7192 348 float x, y, z;
xmwmx 13:a6e12b4c7192 349 fp_drawing = fopen("/sd/write.txt", "r");
xmwmx 13:a6e12b4c7192 350 pc.printf("reading\r\n");
xmwmx 13:a6e12b4c7192 351 for (int i = 0; fscanf(fp_drawing, "(%f,%f,%f)", &x, &y, &z) == 3; i++)
xmwmx 9:a3f9faa9ca1f 352 {
xmwmx 13:a6e12b4c7192 353 pc.printf("(%f,%f,%f)|%d\r\n", x, y, z,i);
xmwmx 13:a6e12b4c7192 354 moveto(x*lianggang+first[0], y*lianggang+first[1],z*lianggang+first[2]);
xmwmx 13:a6e12b4c7192 355 pc.printf("(%f,%f,%f)|%d\r\n", x*lianggang+first[0], y*lianggang+first[1],z*lianggang+first[2],i);
xmwmx 9:a3f9faa9ca1f 356 }
xmwmx 13:a6e12b4c7192 357 fclose(fp_drawing);
xmwmx 11:568789ce1397 358 ready=0;
xmwmx 13:a6e12b4c7192 359 received=0;
xmwmx 11:568789ce1397 360 start=0;
xmwmx 11:568789ce1397 361 done=0;
xmwmx 14:6a6c6def773a 362 panduan=0;
xmwmx 13:a6e12b4c7192 363 publish_value(client, "report", "done.");
xmwmx 13:a6e12b4c7192 364 pc.printf("done.\r\n");
xmwmx 10:acc53f50c9df 365 }
xmwmx 13:a6e12b4c7192 366
xmwmx 13:a6e12b4c7192 367
xmwmx 10:acc53f50c9df 368
xmwmx 11:568789ce1397 369 client.yield(1000);
xmwmx 13:a6e12b4c7192 370 }
xmwmx 8:e76bebe9c45f 371 }