File content as of revision 14:350e03215b0e:

/************************************************************************************************
File name:          main.cpp
Description:        项目激光部分客户端代码
Author:             秦智
Date:               June 4, 2018
Others:             本程序编写在mbed平台(https://os.mbed.com/)上

*************************************************************************************************/

#include "mbed.h"
#include <math.h>
#include <cstring>
#include <stdlib.h>
#include "SDFileSystem.h"
#include "networking.h"



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);
/////

//硬件接口
// mosi, miso, sclk, cs, name
SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
//步进电机
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_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
///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_z = 1;///调试时调整

bool ready = 0;
bool start = 0;
bool done = 0;

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 init_zero();


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");           //  
    while(switch_pos1.read())  // 
    {                           //
    rotate(0,-5);                //
    }          
    init_zero();
}

void init_zero()
{
    now_x = 0;
    now_y = 0;
    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 ,2--z pix=32000为一圈
{
    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;
        wait(step_halfperiod);
        step[id] = 0;
        wait(step_halfperiod);
    }
}

void moveto(float x, float y,float 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");
    }
}



void draw()
{
    //unit_xy=unit_xy*k;
    float x, y, z,a;
    FILE *fp_drawing = fopen("/sd/write.txt", "r");
    pc.printf("reading\r\n");
    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\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 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)
{

    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);
    //声明所有的传感器，每行一个，每个由名字、单位两部分组成，最后一行必须为空指针作为结尾
    const char* sensors[][2] = {
        "report", "",
        NULL, NULL //最后一行以空指针作为结束标记
    };

    //声明所有的执行器，每行一个，每个由名字、参数类型两部分组成，最后一行必须为空指针作为结尾
    const char* actuators[][2] = {
        "command", "",
        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");
    pc.printf("laser printer send ready.haha\r\n");
    ready=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)
        {
            moveto(X1,now_y,now_z);
            flagx = false;
        }
        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)
            {
                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);
            }
            flagw = false;
            pc.printf("%d\r\n",flagw);
            if(!done)
            {
                publish_value(client, "report", "done.");
                pc.printf("send done.%d\r\n",flagw);
                
            }
            ready=0;
            start=0;
            done=0;
        }
        /*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);
    } 
}