Diff: mainwmx.cpp

Revision:

12:51be919e2c78

Parent:

11:568789ce1397

Child:

13:56063ceac462

--- a/mainwmx.cpp	Tue Jul 31 14:18:04 2018 +0000
+++ b/mainwmx.cpp	Wed Aug 01 03:26:19 2018 +0000
@@ -44,6 +44,10 @@
  DigitalIn switch_pos1(PC_8);  //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;
@@ -71,7 +75,7 @@
 
 bool ready = 0;
 bool start = 0;
-bool done =0;
+bool done = 0;
 
 char buf[256];
 int cur;
@@ -80,7 +84,7 @@
 float X1 , X2 , Y1 , Y2 , Z , A;              // 定义浮点数值用以储存返回给机械臂的执行步数
 
 void rotate(int id ,int pix);
-
+void init_zero();
 
 
 void beginning()
@@ -104,7 +108,7 @@
     {                           //
     rotate(0,-5);                //
     }          
-
+    init_zero();
 }
 
 void init_zero()
@@ -116,21 +120,25 @@
 //  step[0] = 0;
 //  step[1] = 0;
 //  step[2] = 0;
+}
 
-}
-void rotate(int id, int pix) //id= 0--x,1--y  pix=3200为一圈
+void rotate(int id, int pix) //id= 0--x,1--y ,2--z pix=32000为一圈
 {
-    if (pix >= 0) {
+    if (pix >= 0) 
+    {
         dir[0] = dir_x;
         dir[1] = dir_y;
         dir[2] = dir_z;
-    } else {
+    } 
+    else 
+    {
         pix = -pix;
         dir[0] = 1 - dir_x;
         dir[1] = 1 - dir_y;
         dir[2] = 1 - dir_z;
     }
-    for (int i = 0; i < pix; i++) {
+    for (int i = 0; i < pix; i++) 
+    {
         step[id] = 1;
         wait(step_halfperiod);
         step[id] = 0;
@@ -140,12 +148,19 @@
 
 void moveto(float x, float y,float z)
 {
-    rotate(0, (x - now_x)*run/rad);
-    rotate(1, (y - now_y)*run/rad);
-    rotate(2, (z - now_z)*run/rad);
-    now_x = x;
-    now_y = y;
-    now_z = z;
+    if((x>(z-40))&&(x<(z+70))&&x>=0&&z>=0)//机械干涉限定条件
+    {
+        rotate(0, (x - now_x)*run/rad);
+        rotate(1, (y - now_y)*run/rad);
+        rotate(2, (z - now_z)*run/rad);
+        now_x = x;
+        now_y = y;
+        now_z = z;
+    }
+    else
+    {
+        pc.printf("limited\r\n");
+    }
 }
 
 
@@ -372,6 +387,7 @@
 int main()
 {
     init_zero();     //初始化
+    
     bt.attach(btInterrupt);
     pc.printf("beginning\r\n");
     beginning();
@@ -405,7 +421,8 @@
         if(!ready)
         {
             publish_value(client, "report", "ready.");
-            pc.printf("send ready.%d\r\n",flagw);
+            //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)