w mx
示教机械臂
Dependencies: MQTT SDFileSystem WIZnet_Library mbed
Fork of
wmx_laser_copy
by 
w mx
Diff: mainwmx.cpp
- Revision:
- 10:acc53f50c9df
- Parent:
- 9:a3f9faa9ca1f
- Child:
- 11:568789ce1397
--- a/mainwmx.cpp Tue Jul 31 03:56:05 2018 +0000
+++ b/mainwmx.cpp Tue Jul 31 12:51:28 2018 +0000
@@ -13,26 +13,21 @@
#include <stdlib.h>
#include "SDFileSystem.h"
#include "networking.h"
-//#include <string>
-#define edgex1 125 // 上x(正朝向开关)下y(负朝向开关)
-#define edgex2 155 // __
-#define edgey1 70 // ||
-#define edgey2 100 // 125mm y2
-#define widthx 30 // x2 || x1
-#define widthy 30 // //=====210mm====//35mm/|开关
-#define mm 100//100步1mm // ||
- // ||
- // ||
- // __ y1
- //
- // -- 开关 15mm
+typedef enum {
+ PointerS,PointerW,PointerX,PointerY,PointerZ,PointerA,PointerL,PointerWW,PointerP
+} STATE; //定义结构体枚举类型STATE
+#define run 32000
+#define rad 360
+#define turn 1
+
/////调试
Serial pc(PA_9, PA_10);
+Serial bt(PB_10, PB_11);
DigitalOut LED(PB_8);
/////
@@ -40,21 +35,21 @@
// mosi, miso, sclk, cs, name
SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
//步进电机
-DigitalOut step[2] = {DigitalOut(PC_5), DigitalOut(PA_5)}; //0--x P9,1--y P17
-DigitalOut dir[2] = {DigitalOut(PC_4), DigitalOut(PA_4)}; //0--x,1--y
-DigitalOut en[2] = {DigitalOut(PD_2), DigitalOut(PA_2)}; //0--x,1--y
+DigitalOut step[3] = {DigitalOut(PC_5), DigitalOut(PA_5),DigitalOut(PC_3)}; //0--x P9,1--y P17
+DigitalOut dir[3] = {DigitalOut(PC_4), DigitalOut(PA_4),DigitalOut(PC_2)}; //0--x,1--y
+DigitalOut en[3] = {DigitalOut(PD_2), DigitalOut(PA_2),DigitalOut(PC_13)}; //0--x,1--y
//电子开关,激光开关
DigitalOut switch_GS(PC_15);
//行程开关
- DigitalIn switch_pos1(PC_8); //P25
+ DigitalIn switch_pos1(PC_8); //P25
DigitalIn switch_pos2(PA_6); //P26
- DigitalIn switch_pos3(PA_11); //P27
+ DigitalIn switch_pos3(PC_6); //P27
//运行中的全局变量
volatile bool Working, Getawork, Isend, Dataused, Getdata;
-FILE *fp_drawing; // 存储图案的文件
+//FILE *fp_drawing;// 存储图案的文件
int status = 0; //0: 初始化; 1:建立通信; 2:等候任务; 3:等待数据 4:正在执行一个任务
-int now_x, now_y;
+int now_x, now_y,now_z;
int Endoffile = 0;
// float thedata[50][3];
@@ -72,69 +67,51 @@
int max_x, max_y;
int dir_x = 1;///调试时调整
int dir_y = 1;///调试时调整
-int dot_max = 30; //灰度最大值对应多少次激光点击
-float dot_last = 0.005; //每次激光点击持续多久
-int H_max = 255; //灰度值最大值
-double k = 1.0; //缩放比例
-double y=0;//图片y边长
-double x=0;//图片x边长
-int key=0;//判断变量
-int dot=0;//点计数
-int flag=0;
-//char w[4294967295];
+int dir_z = 1;///调试时调整
+
+char buf[256];
+int cur;
+bool received;
+bool flagx, flagy , flagz , flaga , flagw , flagp;
+float X1 , X2 , Y1 , Y2 , Z , A; // 定义浮点数值用以储存返回给机械臂的执行步数
void rotate(int id ,int pix);
-void biginning()
+void beginning()
{
- pc.printf("x\n"); // 上x(正朝向开关)下y(负朝向开关)
- while(!switch_pos1.read()) // __ y2
- { // ||
- rotate(0,5); // 115mm
- } // x2 || x1
- rotate(0,-mm*edgex2); // //=====200mm====//42mm/|开关
- pc.printf("y\n"); // ||
- while(!switch_pos2.read()) // ||
- { // ||
- rotate(1,-5); // __ y1
+ // 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); //
} //
- rotate(1,mm*edgey1); // __ 开关 15mm
- //定位
- for (int i = 0; i < 500; i++)
- {
- switch_GS = 1;
- wait(0.005);
- switch_GS = 0;
- wait(0.005);
- }
- //走轮廓
- rotate(0,mm*widthx);
- rotate(1,mm*widthy);
- rotate(0,mm*-widthx);
- rotate(1,mm*-widthy);
+ //
+ pc.printf("x\n"); //
+ while(switch_pos1.read()) //
+ { //
+ rotate(0,-5); //
+ }
+
}
-void report()
+void init_zero()
{
- // ser2usb.printf("-1- status: %d \r\n", status);
- // ser2usb.printf("-2- now_x: %d now_y: %d \r\n", now_x,now_y);
- //ser2usb.printf("-3- switch_GS: %i \r\n", switch_GS.read());
- //ser2usb.printf("-4- switch_pos1: %i switch_pos2: %i switch_pos3: %i \r\n", switch_pos1.read(),switch_pos2.read(),switch_pos3.read());
- //ser2usb.printf("-5- en[0]: %i en[1]: %i \r\n", en[0].read(),en[1].read());
- //ser2usb.printf("-6- dir[0]: %i dir[1]: %i \r\n", dir[0].read(),dir[1].read());
- //ser2usb.printf("-7- step[0]: %i step[1]: %i \r\n", step[0].read(),step[1].read());
-}
-
-void init()
-{
- status = 0;
now_x = 0;
now_y = 0;
- step[0] = 0;
- step[1] = 0;
- //上x下y
+ now_z = 0;
+ strcpy(buf,"");
+// step[0] = 0;
+// step[1] = 0;
+// step[2] = 0;
}
void rotate(int id, int pix) //id= 0--x,1--y pix=3200为一圈
@@ -142,10 +119,12 @@
if (pix >= 0) {
dir[0] = dir_x;
dir[1] = dir_y;
+ dir[2] = dir_z;
} 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++) {
step[id] = 1;
@@ -155,260 +134,302 @@
}
}
-void moveto(float x, float y)
+void moveto(float x, float y,float z)
{
- rotate(0, (x - now_x)*unit_xy);
- rotate(1, (y - now_y)*unit_xy);
+ 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;
-}
-void fuwei()
-{
- moveto(0, 0);
- status = 1;
-}
-void markdot(float value)
-{
- //switch_GS=1;
- int Ndot = floor(value * dot_max / H_max);
- for (int i = 0; i < Ndot; i++) {
- switch_GS = 1;
- wait(dot_last);
- switch_GS = 0;
- }
+ now_z = z;
}
-/*
-void dealdata(){
- ser2usb.printf("getdata: %s \r\n",sdata);
- int data_size = 0;//strlen(pch);
- char * pch;
- pch = strtok (sdata,"\r\n ,.-)(");
- while (pch != NULL)
- {
- thedata[data_size/3][data_size%3]=atof(pch);
- pch = strtok (NULL, "\r\n ,.-)(");
- data_size++;
- }
- Nofdata = (data_size)/3;
-}
-void usbprintdata(){
- ser2usb.printf("dealdata: \r\n <<<<<<<<<<<<<<<<");
- for(int i=0;i<Nofdata;i++){
- ser2usb.printf("<<< i: %i x: %f y: %f v: %f \r\n",i,thedata[i][0],thedata[i][1],thedata[i][2]);
- }
- ser2usb.printf("dealdata done! \r\n >>>>>>>>>>>>");
-}
-*/
+
+
void draw()
{
- unit_xy=unit_xy*k;
- float x, y, v;
- fp_drawing = fopen("/sd/write.txt", "r");
+ //unit_xy=unit_xy*k;
+ float x, y, z,a;
+ FILE *fp_drawing = fopen("/sd/write.txt", "r");
pc.printf("reading\r\n");
- for (int i = 0; fscanf(fp_drawing, "(%f,%f,%f)", &x, &y, &v) == 3; i++)
+ flagp=0;
+ for (int i = 0; fscanf(fp_drawing, "(%f %f %f %f)", &x, &y, &z,&a) == 4&&flagp==0; i++)
{
- pc.printf("(%f,%f,%f)|%d|%d\r\n", x, y, v,i,dot);
- moveto(x, y);
- markdot(v);
+ pc.printf("(%f,%f,%f)|%d\r\n", x, y, z,i);
+ moveto(x, y,z);
// moveto(thedata[i][0],thedata[i][1]);
// markd??>ot(thedata[i][2]);
}
fclose(fp_drawing);
+ flagw = false;
+ flagp = false;
+ pc.printf("Here4!\r\n");
}
-void on_control_cmd(const char* actuator_name, const char* control_value)
+void btInterrupt() //中断接收函数
{
- //pc.printf("Received [%s] [%s]\r\n", actuator_name, control_value);
- if (strcmp(actuator_name, "dat") == 0) {
- //接受到的坐标数据写入存储卡
- //fprintf(fp_drawing, "%s", control_value);
- fprintf(fp_drawing, "%s", control_value);
- dot++;
- if(strcmp(control_value, "(0,0,1)") == 0)
- {
- flag=1;
- pc.printf("received (0,0,1) %d|%d\r\n",status,dot);
- }
- }
- else if (strcmp(actuator_name, "cmd") == 0) {
- int the_command = atoi(control_value);
- //
- if(key!=0)
+ 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)
{
- if (key==1) {
- k=the_command*0.1;
- pc.printf("Received k %f\n",k);
-
- }
- if (key==2) {
- x=the_command;
- pc.printf("Received x %f\n",x);
-
- }
- if (key==3) {
- y=the_command;
- pc.printf("Received y %f\n",y);
- }
-
- key=0;
+ 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;
+
}
- else
- {
- if (the_command == 1&&status==2&&key==0) {
- Getawork = 1;
- }
- if (the_command == 2&&status==3&&key==0) {
- Getdata = 1;
- pc.printf("Received Getdata %d\n",Getdata);
- }
- /*else if (the_command == 3&&status==2&&key==0) {//传k
- key=1;
- }*/
- else if (the_command == 4&&status==2&&key==0) {//传x
- key=2;
- }
- else if (the_command == 5&&status==2&&key==0) {//传y
- key=3;
- }
- }
- //
-
+ //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;
}
}
int main()
{
- init(); //初始化
-
- //建立通信
- status = 1;
-
- pc.printf("starting\r\n");
-
- MQTTSocket sock;
- MClient client(sock);
-
- //声明所有的传感器，每行一个，每个由名字、单位两部分组成，最后一行必须为空指针作为结尾
- const char* sensors[][2] = {
- "report", "",
- NULL, NULL //最后一行以空指针作为结束标记
- };
-
- //声明所有的执行器，每行一个，每个由名字、参数类型两部分组成，最后一行必须为空指针作为结尾
- const char* actuators[][2] = {
- "dat", "",
- "cmd", "",
- NULL, NULL //最后一行以空指针作为结束标记
- };
- pc.printf("connecting...\r\n");
-
- networking_init(sock, client, "192.168.1.100", sensors, actuators, on_control_cmd);
-
- pc.printf("Initialization done.\r\n");
-
- status = 2;
-
-
- pc.printf("laser printer send ready.haha\r\n");
-
- Getawork = 0;
- Getdata = 0;
-
- //pc.printf("yes%f|%f|%f\r\n",y,k,y*k);
- //pc.printf("ready.\r\n");
-
-
- pc.printf("biginning\r\n");
- biginning();
+ init_zero(); //初始化
+ bt.attach(btInterrupt);
+ pc.printf("beginning\r\n");
+ beginning();
pc.printf("finish\r\n");
-
- //draw();
-
- while (true)
- {
- if(x==0||y==0&&status==2)
+ while(1) {
+// 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)
{
- while(x==0&&status==2)
- {
- pc.printf("waiting x\r\n");
- publish_value(client, "report", "input x:");
- client.yield(5000);
- }
- while(y==0&&status==2)
- {
- pc.printf("waiting y\r\n");
- publish_value(client, "report", "input y:");
- client.yield(5000);
- }
- publish_value(client, "report", "ready.");
+ moveto(X1,now_y,now_z);
+ flagx = false;
}
- while(y*k>widthy&&status==2)
+ if (flagy)
+ {
+ moveto(now_x,Y1,now_z);
+ flagy = false;
+ }
+ if (flagz)
+ {
+ moveto(now_x,now_y,Z);
+ flagz = false;
+ }
+ if(flagw)
{
- k-=0.01;
- pc.printf("no%f|%f|%f\r\n",y,k,y*k);
- wait(0.001);
- }
- if (status == 2)
- {
-
- if (Getawork)
- { //收到新任务
- Getawork = 0;
- pc.printf("Begin receiving... %d\r\n",status);
- // 打开文件，准备接受坐标数据
- fp_drawing = fopen("/sd/write.txt", "w");
- if (fp_drawing)
- {
- pc.printf("File opened\r\n");
- status = 3;
- publish_value(client, "report", "wait data.");
- pc.printf("wait data.%d\r\n",status);
- }
+
+
+ FILE *fp = fopen("/sd/write.txt", "r"); //打开文件，路径以“/sd/”开头
+
+ if (fp == NULL) //打开失败，原因可能是文件不存在，或卡没有连接好
+ {
+ printf("open error!!\r\n");
+ return 1;
}
-
- }
-
-
- if (status == 3)
- {
- if(flag==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)
{
- //wait(15);
-
- publish_value(client, "report", "all wrote.");
- pc.printf("wrote finish %d\r\n",status);
+ 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);
}
- if (Getdata)
- { //数据接收完
- Getdata = 0;
- pc.printf("End receiving... %d\r\n",status);
- fclose(fp_drawing);
- publish_value(client, "report", "received.");
- status = 4;
- }
- }
- if (status == 4)
+ flagw = false;
+ pc.printf("%d\r\n",flagw);
+ }
+ /*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("debug4 \r\n");
- // dealdata();
- pc.printf("Drawing.. %d |%d\r\n",status,dot);
- // usbprintdata();
- draw();
- publish_value(client, "report", "done.");
- pc.printf("done. %d \r\n",status);
- status = 2;
- x=0;
- y=0;
- Getawork = 0;
- Getdata = 0;
- unit_xy=200;
- k=1.0;
- flag=0;
- dot=0;
- //strcpy(w,"");
+ 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);
}
-
+
}