Diff: mainwmx.cpp

Revision:

13:a6e12b4c7192

Parent:

12:51be919e2c78

Child:

14:6a6c6def773a

--- a/mainwmx.cpp	Wed Aug 01 03:26:19 2018 +0000
+++ b/mainwmx.cpp	Thu Aug 02 06:45:17 2018 +0000
@@ -1,8 +1,8 @@
 /************************************************************************************************
 File name:          main.cpp
-Description:        项目激光部分客户端代码
-Author:             秦智
-Date:               June 4, 2018
+Description:        项目部分客户端代码
+Author:             
+Date:               
 Others:             本程序编写在mbed平台(https://os.mbed.com/)上
 
 *************************************************************************************************/
@@ -13,17 +13,14 @@
 #include <stdlib.h>
 #include "SDFileSystem.h"
 #include "networking.h"
-
+#include <cmath>
 
 
-typedef enum {
-    PointerS,PointerW,PointerX,PointerY,PointerZ,PointerA,PointerL,PointerWW,PointerP
-} STATE;                     //定义结构体枚举类型STATE
-
-
-#define run 32000
-#define rad 360
-#define turn 1
+#define run 10000
+#define M_PI 3.141526535
+#define ky 4.0//   52/13
+#define kx 5.84615//   76/13
+#define lianggang 30//   量纲
 
 /////调试
 Serial pc(PA_9, PA_10);
@@ -41,74 +38,85 @@
 //电子开关,激光开关
 DigitalOut switch_GS(PC_15);
 //行程开关
- DigitalIn switch_pos1(PC_8);  //P25
+ DigitalIn switch_pos1(PA_3);  //P25
  DigitalIn switch_pos2(PA_6);  //P26
  DigitalIn switch_pos3(PC_6);  //P27
  
- InterruptIn switch_posx(PC_8);  //P25
- InterruptIn switch_posy(PA_6);  //P26
- InterruptIn switch_posz(PC_6);  //P27
-
-//运行中的全局变量
-volatile bool Working, Getawork, Isend, Dataused, Getdata;
-//FILE *fp_drawing;// 存储图案的文件
-int status = 0; //0: 初始化;  1:建立通信;  2:等候任务;  3:等待数据    4:正在执行一个任务
-int now_x, now_y,now_z;
-int Endoffile = 0;
-
-// float thedata[50][3];
-// char sdata[1024];
-int Nofdata;
-bool ifreceive;
-
 //与实际有关参数
-float Lenoflattice = 1 ;//mm  取1mm为xy单元  unit_xy/Lenoflattice=100
-int unit_xy = 100;  //单位长度(xy移动一格)对应unit_xy转   大约10cm/3圈/9600step 约96mm/9600step=0.1mm/10step
-int unit_x = 103;  //单位长度(xy移动一格)对应unit_x转   大约10cm/3圈/9600step 约96mm/9600step=0.1mm/10step
-int unit_y = 103;  //单位长度(xy移动一格)对应unit_y转   大约10cm/3圈/9600step 约96mm/9600step=0.1mm/10step
+double now_x = 0,
+    now_y = 0,
+    now_z = 0,
+    now_theta=0,
+    now_alpha=1.0;
+double first[]={150,150,0};
 ///Ticker ticker_step;
 float step_halfperiod = 0.0001;//0.0002;
-int max_x, max_y;
-int dir_x = 1;///调试时调整
-int dir_y = 1;///调试时调整
+int dir_x = 0;///调试时调整
+int dir_y = 0;///调试时调整
 int dir_z = 1;///调试时调整
 
-bool ready = 0;
-bool start = 0;
-bool done = 0;
+FILE *fp_drawing;// 存储图案的文件
+
+double theta=0,alpha=0;
 
 char buf[256];
 int cur;
+bool ready;
 bool received;
-bool flagx, flagy , flagz , flaga , flagw , flagp;
-float X1 , X2 , Y1 , Y2 , Z , A;              // 定义浮点数值用以储存返回给机械臂的执行步数
+bool start;
+bool done;
 
 void rotate(int id ,int pix);
 void init_zero();
+void moveto(double x, double y,double z);
 
 
 void beginning()
 {
-                                 //                 x    
-                                //                ##  
-    pc.printf("y\n");           //                ||    
-    while(switch_pos2.read())  //    |====||=====|||#y
-    {                           //                ||
-    rotate(1,-5);               //                ##
-    }                           //                 z
-                                //  
-    pc.printf("z\n");           // 
-    while(switch_pos3.read())  // 
-    {                           // 
-    rotate(2,-5);               //   
-    }                           //
-                                //  
-    pc.printf("x\n");           //  
+                                 //                   
+                               //                 
+    pc.printf("y\r\n");           //                   
+    while(switch_pos2.read())  //    
+    {    
+        //pc.printf("y\r\n");     //               
+        rotate(1,-5);           //          --      
+    }                           //         ||        
+    rotate(1,200);             //        ||
+    pc.printf("z\r\n");         //         ||         y      
+    while(switch_pos3.read())  //       z |==|======[]======[]
+    {                          //          ||
+        pc.printf("z\r\n");     //         ||
+        rotate(2,-5);           //         ||x
+    }                           //   ---------------------------------
+    rotate(2,50000);             //  
+    pc.printf("x\r\n");         //  
     while(switch_pos1.read())  // 
-    {                           //
-    rotate(0,-5);                //
-    }          
+    {         
+        //pc.printf("x\r\n");                   //
+        rotate(0,-5);                //
+    }      
+    rotate(0,400); 
+    //moveto(first[0],first[1],first[2]);
     init_zero();
+    pc.printf("x %f | y %f || th %f | al %f \r\n",now_x,now_y,theta,alpha);
+    //wait(3);
+    //moveto(200,0,0);
+    //wait(3);
+    //moveto(200,200,0);    
+
+}
+/****************
+ * x, y单位为mm
+ * theta, alpha单位为rad
+******************/
+void scaraArmCal(double x, double y, double &theta, double &alpha)
+{
+    const double a(200.0), b(200.0);
+
+    double rouSquare = x * x + y * y;
+    double rou = sqrt(rouSquare);
+    alpha = acos((a * a + b * b - rouSquare) / (2 * a * b));
+    theta = acos((a * a + rouSquare - b * b) / (2 * a * rou)) + (y >= 0 ? atan2(y, x) : (atan2(y, x) + M_PI * 2));
 }
 
 void init_zero()
@@ -116,6 +124,8 @@
     now_x = 0;
     now_y = 0;
     now_z = 0;
+    now_theta=0;
+    now_alpha=1.0;
     strcpy(buf,"");
 //  step[0] = 0;
 //  step[1] = 0;
@@ -146,25 +156,41 @@
     }
 }
 
-void moveto(float x, float y,float z)
+void moveto(double x, double y,double z)
 {
-    if((x>(z-40))&&(x<(z+70))&&x>=0&&z>=0)//机械干涉限定条件
+    scaraArmCal(x, y , theta, alpha);
+    if(theta>=0&&alpha>=1.0&&((x*x+y*y)<160000))
     {
-        rotate(0, (x - now_x)*run/rad);
-        rotate(1, (y - now_y)*run/rad);
-        rotate(2, (z - now_z)*run/rad);
+        if(z-now_z<=0)
+        {
+            rotate(0, (theta-now_theta)*run*kx/(2*M_PI));
+            rotate(1, (alpha-now_alpha)*run*ky/(2*M_PI));
+            rotate(2, (z - now_z)*run/10);
+        }
+        else
+        {
+            rotate(2, (z - now_z)*run/10);
+            rotate(0, (theta-now_theta)*run*kx/(2*M_PI));
+            rotate(1, (alpha-now_alpha)*run*ky/(2*M_PI));
+        }    
+        pc.printf("x %f | y %f | th| %f | al %f \r\n",x,y,theta,alpha);
         now_x = x;
         now_y = y;
         now_z = z;
+        now_theta=theta;
+        now_alpha=alpha;
+
     }
     else
     {
-        pc.printf("limited\r\n");
+        pc.printf("x %f | y %f | th| %f | al %f limited\r\n",x,y,theta,alpha);
     }
+    theta=0;
+    alpha=0;
 }
 
 
-
+/*
 void draw()
 {
     //unit_xy=unit_xy*k;
@@ -185,176 +211,7 @@
     pc.printf("Here4!\r\n");
 }
 
-void btInterrupt()    //中断接收函数
-{
-    if(!bt.readable())
-        return;
-    char strRe1;                          //定义字符串
-    static STATE State = PointerS;        // 初始状态待检查
-    strRe1 = bt.getc();                   //读取一个byte数据
 
-    switch(State) 
-    {
-        case PointerS:
-            if (strRe1 == (char)'S') 
-            {
-                State = PointerW;        //准备跳转至数据选择
-                cur = 0;
-            } 
-            else
-                State = PointerS;
-            break;
-        case PointerW:
-            switch (strRe1) 
-            {
-                case (char)'X':
-                    State = PointerX;
-                    break;
-                case (char)'Y':
-                    State = PointerY;
-                    break;
-                case (char)'Z':
-                    State = PointerZ;
-                    break;
-                case (char)'S':
-                    State = PointerS;
-                    break;
-                case (char)'L':
-                    State = PointerL;
-                    break;
-                case (char)'A':
-                    State = PointerA;
-                    break;
-                case (char)'W':
-                    State = PointerWW;
-                    break;
-                case (char)'P':
-                    State = PointerP;
-                    break;
-                case (char)'K':
-                    State = PointerS;
-                    pc.printf("beginning\r\n");
-                    beginning();
-                    break;
-                
-            }
-        //case PointerX:
-//            if (strRe1 == (char)'\n') {
-//                buf[cur]='\0';
-//                pc.printf("%s\n",buf);
-//                X = atof(buf+1);
-//                State = PointerS;
-//                pc.printf("%f\n",X);
-//            } else {
-//                buf[cur++]=strRe1;
-//            }
-//            break;
-//        case PointerY:
-//            if (strRe1 == (char)'\n') {
-//                buf[cur]='\0';
-//                pc.printf("%s\n",buf);
-//                Y = atof(buf+1);
-//                State = PointerS;
-//                pc.printf("%f\n",Y);
-//            } else {
-//                buf[cur++]=strRe1;
-//            }
-//            break;
-//        case PointerZ:
-//            if (strRe1 == (char)'\n') {
-//                buf[cur]='\0';
-//                pc.printf("%s\n",buf);
-//                Z = atof(buf+1);
-//                State = PointerS;
-//                pc.printf("%f\n",Z);
-//            } else {
-//                buf[cur++]=strRe1;
-//            }
-//            break;
-        case PointerX:
-            if (strRe1 == (char)'\n') 
-            {
-                buf[cur]='\0';
-                pc.printf("%s\r\n",buf);
-                X1 = atof(buf+1);
-                State = PointerS;
-                pc.printf("%f\r\n",X1);      //32000bu = 360du
-                flagx = true;
-            } 
-            else 
-            {
-                buf[cur++]=strRe1;
-            }
-            break;
-        case PointerY:
-            if (strRe1 == (char)'\n') 
-            {
-                buf[cur]='\0';
-                pc.printf("%s\r\n",buf);
-                Y1 = atof(buf+1);
-                //Y2 = Y1;
-//                Y1 = Y2 - Y1;
-                State = PointerS;
-                pc.printf("%f\r\n",Y1);
-                flagy = true;
-            } 
-            else 
-            {
-                buf[cur++]=strRe1;
-            }
-            break;
-        case PointerZ:
-            if (strRe1 == (char)'\n') 
-            {
-                buf[cur]='\0';
-                pc.printf("%s\r\n",buf);
-                Z = atof(buf+1);
-                State = PointerS;
-                pc.printf("%f\r\n",Z);
-                flagz = true;
-            } 
-            else 
-            {
-                buf[cur++]=strRe1;
-            }
-            break;
-        case PointerA:
-            if (strRe1 == (char)'\n') 
-            {
-                buf[cur]='\0';
-                pc.printf("%s\n",buf);
-                A = atof(buf+1);
-                State = PointerS;
-                pc.printf("%f\n",A);
-            } 
-            else 
-            {
-                buf[cur++]=strRe1;
-            }
-            break;
-        case PointerL:
-            if (strRe1 == (char)'\n') 
-            {
-                buf[cur]='\0';
-                State = PointerS;
-                received = true;
-                pc.printf("received\r\n");
-            } 
-            else 
-            {
-                buf[cur++]=strRe1;
-            }
-            break;
-        case PointerWW:
-            flagw = true; 
-            State = PointerS;         
-            break; 
-        case PointerP:
-            flagp = true;
-            State = PointerS;
-            break;  
-    }
-}
 
 void on_control_cmd(const char* actuator_name, const char* control_value)
 {
@@ -382,17 +239,56 @@
         
     }
 }
+*/
 
+void on_control_cmd(const char* actuator_name, const char* control_value)
+{
+    //pc.printf("Received [%s] [%s]\r\n", actuator_name, control_value);
+    if (strcmp(actuator_name, "move") == 0) 
+    {
+        //接受到的坐标数据写入存储卡
+        if(strcmp(control_value, "(255,255,255)") == 0)
+        {
+            pc.printf("received (255,255,255)\r\n");
+            received=1;
+            fclose(fp_drawing);
+        }
+        else
+        {
+            fprintf(fp_drawing, "%s", control_value);
+            pc.printf("received %s\r\n",control_value);
+        }
+    } 
+    if (strcmp(actuator_name, "command") == 0) 
+    {
+        int the_command = atoi(control_value);
+        //
+        if(the_command==1)
+        {
+            ready=1;
+            pc.printf("get ready\r\n");
+        }
+        else if(the_command==2)
+        {
+            start=1;
+            pc.printf("get start\r\n");
+        }
+        else if(the_command==3)
+        {
+            done=1;
+            pc.printf("get done\r\n");
+        }
+        //
+        
+    }
+
+      
+        
+    
+}
 
 int main()
 {
-    init_zero();     //初始化
-    
-    bt.attach(btInterrupt);
-    pc.printf("beginning\r\n");
-    beginning();
-    pc.printf("finish\r\n");
-    
     MQTTSocket sock;
     MClient client(sock);
     //声明所有的传感器，每行一个，每个由名字、单位两部分组成，最后一行必须为空指针作为结尾
@@ -412,119 +308,65 @@
 
     pc.printf("Initialization done.\r\n");
     pc.printf("laser printer send ready.haha\r\n");
+    
+    
+    init_zero();     //初始化
+    pc.printf("beginning\r\n");
+    beginning();
+    pc.printf("finish\r\n");
+
+
     ready=0;
+    received=0;
     start=0;
     done=0;
-
+    
+    
     while(1) 
     {   
         if(!ready)
         {
             publish_value(client, "report", "ready.");
-            //pc.printf("send ready.%d\r\n",flagw);
-            pc.printf("x%d|y%d|z%d\r\n",now_x,now_y,now_z);
-            
-        }
-        if(!start&&ready)
-        {
-            pc.printf("send start.%d\r\n",flagw);
-            publish_value(client, "report", "wait to start.");
             
         }
-//        btInterrupt();
-        //wait(0.1);
-        //if (sw.read() == 0)
-        //{
-        // pc.printf("%s.\r\n", strX);
-        // pc.printf("%s.\r\n", strY);
-        // pc.printf("%s.\r\n", strZ);
-        // pc.printf("%s.\r\n", strA);
-        //}
-        
-        if (flagx)
+        else if(ready&&!received)
         {
-            moveto(X1,now_y,now_z);
-            flagx = false;
+            pc.printf("send wait.\r\n");
+            fp_drawing = fopen("/sd/write.txt", "w");
+            pc.printf("opened\r\n");
+            publish_value(client, "report", "wait data.");
+            ready=0;
         }
-        if (flagy)
-        {
-            moveto(now_x,Y1,now_z);
-            flagy = false;
-        }
-        if (flagz)
-        {
-            moveto(now_x,now_y,Z);
-            flagz = false;
-        }
-        if(flagw||start)
-        {
-            pc.printf("openning %d\r\n",flagw);
-         
-            FILE *fp = fopen("/sd/write.txt", "r"); //打开文件，路径以“/sd/”开头
         
-            if (fp == NULL) //打开失败，原因可能是文件不存在，或卡没有连接好
-            {
-                printf("open error!!\r\n");
-                return 1;
-            }
-            fgets(buf, sizeof(buf), fp); //读入一行的C函数
-            pc.printf("read\r\n");
-            fclose(fp);  //关闭文件，释放资源
-    
-            pc.printf("%s\r\n",buf);
-            char *p = buf;
-            float a,x,y,z;
-            while(true)
+        else if(received&&!start)
+        {
+            publish_value(client, "report", "received.");
+            pc.printf("send received.\r\n");
+            received=0;
+        }
+        
+        else if(start)
+        {
+            float x, y, z;
+            fp_drawing = fopen("/sd/write.txt", "r");
+            pc.printf("reading\r\n");
+            for (int i = 0; fscanf(fp_drawing, "(%f,%f,%f)", &x, &y, &z) == 3; i++) 
             {
-                p = strchr(p, '(');
-                if(!p) break;
-                p++;
-                sscanf(p, "%f%f%f%f",&x,&y,&z,&a);
-                pc.printf("%f %f %f %f\r\n",x,y,z,a);
-                moveto(x,y,z);
-                wait(0.5);
+                pc.printf("(%f,%f,%f)|%d\r\n", x, y, z,i);
+                moveto(x*lianggang+first[0], y*lianggang+first[1],z*lianggang+first[2]);
+                pc.printf("(%f,%f,%f)|%d\r\n", x*lianggang+first[0], y*lianggang+first[1],z*lianggang+first[2],i);
             }
-            flagw = false;
-            pc.printf("%d\r\n",flagw);
-            if(!done)
-            {
-                publish_value(client, "report", "done.");
-                pc.printf("send done.%d\r\n",flagw);
-                
-            }
+            fclose(fp_drawing);
             ready=0;
+            received=0;
             start=0;
             done=0;
+            publish_value(client, "report", "done.");
+            pc.printf("done.\r\n");
         }
-        /*if (flagw)
-        {
-            //if (Arr[][] != 
-            //{
-            pc.printf("Here1!\r\n");  
+        
+        
 
-            pc.printf("Here2!\r\n");    
-            init_zero();
-            beginning();
-            draw(); 
-            pc.printf("Here3!\r\n");   
-            //} 
-        }    */      
-        if(received) 
-        {
-            pc.printf("%s\r\n",buf);
-            FILE *fp_drawing = fopen("/sd/write.txt", "w");
-            pc.printf("opening\r\n");
-            if (fp_drawing == NULL)
-            {
-                printf("open error2!!\r\n");
-                return 1;
-            }
-            fprintf(fp_drawing, "%s",buf);
-            pc.printf("close\r\n");
-            fclose(fp_drawing);
-            pc.printf("write end");
-            received = false;
-        }
         client.yield(1000);
-    } 
+    }
 }