File content as of revision 14:6a6c6def773a:

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

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

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


#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);
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(PA_3);  //P25
 DigitalIn switch_pos2(PA_6);  //P26
 DigitalIn switch_pos3(PC_6);  //P27
 
//与实际有关参数
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 dir_x = 0;///调试时调整
int dir_y = 0;///调试时调整
int dir_z = 1;///调试时调整

FILE *fp_drawing;// 存储图案的文件

double theta=0,alpha=0;

char buf[256];
int cur;
bool ready;
bool received;
bool start;
bool done;
bool panduan;

void rotate(int id ,int pix);
void init_zero();
void moveto(double x, double y,double z);


void beginning()
{
                                 //                   
                               //                 
    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())  // 
    {         
        //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()
{
    now_x = 0;
    now_y = 0;
    now_z = 0;
    now_theta=0;
    now_alpha=1.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(double x, double y,double z)
{
    scaraArmCal(x, y , theta, alpha);
    if(theta>=0&&alpha>=1.0&&((x*x+y*y)<160000))
    {
        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("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;
    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 on_control_cmd(const char* actuator_name, const char* control_value)
{

    if (strcmp(actuator_name, "create") == 0) 
    {
        int the_command = atoi(control_value);
        //
        if(the_command==0)
        {
            ready=1;
            pc.printf("get ready\r\n");
        }
        else if(the_command==1)
        {
            start=1;
            pc.printf("get start\r\n");
        }
        else if(the_command==2)
        {
            done=1;
            pc.printf("get done\r\n");
        }
        //
        
    }
}
*/

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, "dat") == 0) 
    {
        //接受到的坐标数据写入存储卡
        
            received=1;
            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()
{
    MQTTSocket sock;
    MClient client(sock);
    //声明所有的传感器，每行一个，每个由名字、单位两部分组成，最后一行必须为空指针作为结尾
    const char* sensors[][2] = {
        "report", "",
        NULL, NULL //最后一行以空指针作为结束标记
    };

    //声明所有的执行器，每行一个，每个由名字、参数类型两部分组成，最后一行必须为空指针作为结尾
    const char* actuators[][2] = {
        "dat", "",
        "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");
    
    
    init_zero();     //初始化
    pc.printf("beginning\r\n");
    beginning();
    pc.printf("finish\r\n");


    ready=0;
    received=0;
    start=0;
    done=0;
    panduan=0;
    
    
    while(1) 
    {   
        if(!ready)
        {
            publish_value(client, "report", "ready.");
            
        }
        else if(ready&&!received&&panduan==0)
        {
            pc.printf("send wait.\r\n");
            fp_drawing = fopen("/sd/write.txt", "w");
            pc.printf("opened\r\n");
            publish_value(client, "report", "wait data.");
            panduan=1;
        }
        
        else if(received&&!start)
        {
            publish_value(client, "report", "received.");
            pc.printf("send received.\r\n");
            received=0;
        }
        
        else if(start)
        {
            fclose(fp_drawing);
            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++) 
            {
                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);
            }
            fclose(fp_drawing);
            ready=0;
            received=0;
            start=0;
            done=0;
            panduan=0;
            publish_value(client, "report", "done.");
            pc.printf("done.\r\n");
        }
        
        

        client.yield(1000);
    }
}