Revision 4:d66b6f331b74, committed 2018-06-03

Comitter:

glintligo

Date:

Sun Jun 03 10:18:11 2018 +0000

Parent:

3:09f6061e6e5d

Commit message:

ddd

Changed in this revision

--- a/Stepper_motor.cpp	Fri May 11 10:42:09 2018 +0000
+++ b/Stepper_motor.cpp	Sun Jun 03 10:18:11 2018 +0000
@@ -85,15 +85,20 @@
  */
 void Stepper_motor::set()
 {
+      wait(0.01);
+      if(this->read() == 1)
+        {
     this -> Config(0,0);//停止运动
     wait(0.01);
     this -> enable();
-    this -> Config(-5,7);
+    this -> Config(-5,10);
+        }
 }
+        
 
 /** [Stepper_motor::Init 步进电机初始化] */
 void Stepper_motor::Init()
 {
     this->Config(10,-1);
-    this->fall(this,&Stepper_motor::set); 
+    this->rise(this,&Stepper_motor::set); 
 }
\ No newline at end of file

--- a/Stepper_motor.h	Fri May 11 10:42:09 2018 +0000
+++ b/Stepper_motor.h	Sun Jun 03 10:18:11 2018 +0000
@@ -2,6 +2,7 @@
 #define _Stepper_motor_H
 #include "a4988.h"
 #include "mbed.h"
+
 class Stepper_motor:public Stepper,public InterruptIn
 {
 public:

--- a/a4998.lib	Fri May 11 10:42:09 2018 +0000
+++ b/a4998.lib	Sun Jun 03 10:18:11 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/Bilybill/code/stepper/#88f743f3e7a7
+https://os.mbed.com/users/glintligo/code/a4998ss/#e9699816ff84

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/esp8266.cpp	Sun Jun 03 10:18:11 2018 +0000
@@ -0,0 +1,236 @@
+/***************************************************************
+功能 : ESP8266物联车接口函数
+作者 : 马晓健
+邮箱 : jeasinema[at]gmail[dot]com
+声明 : 
+本程序仅供学习与交流使用,如需他用,须联系作者
+本程序可以随意更改,但须保留本信息页
+All rights reserved
+2017.6.16
+***************************************************************/
+
+#include "esp8266.h"
+
+#include <cstdarg>
+#include <cstring>
+#include <stdint.h>
+#include "mbed.h"
+
+extern Serial ser2usb;
+
+static int ser_baud = 9600;
+
+//定义了一个调试的宏,C语言语法
+#define ESP_CMD(format, ...) do{\
+    char cmdbuf[128], *p;\
+    ser2esp8266.printf("\r"); \
+    sprintf(cmdbuf, format "\r", ##__VA_ARGS__);\
+    for(p=cmdbuf;*p;p++){\
+        ser2esp8266.putc(*p);\
+        wait(0.02);\
+    }\
+    wait(0.3);\
+}while(0)
+
+
+
+void Esp8266::gotResponse(char *token, char *param)
+{
+    if(*token<'a' || *token>'z') return;
+    ser2usb.printf("gotResponse %s %s\r\n", token, param);
+    if(strcmp(token, "connected") == 0)
+        mqtt_start = true;
+    else if(strcmp(token, "control") == 0){
+        if(!control_cmd){
+            strncpy(control_buf, param, sizeof(control_buf));
+            control_cmd = true;
+        }
+    }
+    else if(strcmp(token, "wifi") == 0){
+        if(*param == '5')
+            network_start = true;
+    }
+}
+
+bool Esp8266::get_control_cmd(char* actuator, char* value)
+{
+    if(!control_cmd)
+        return false;
+    
+    char* plus = strchr(control_buf, '+');
+    if(!plus){
+        control_cmd = false;
+        return false;
+    }
+    *plus = '\0';
+    strcpy(actuator, control_buf);
+    strcpy(value, plus+1);
+    control_cmd = false;
+    return true;
+}
+
+// 接收 esp8266 侧数据的回调函数, 每次仅接收一个8位字符
+// 数据格式约定: #token+data
+void Esp8266::esp8266_rxCallback() {
+    char in = ser2esp8266.getc();
+//    ser2usb.putc(in);
+    enum{STATE_WAIT, STATE_TOKEN, STATE_PARAM};
+    static uint8_t state = STATE_WAIT;
+    static int tokenLen, paramLen;
+    switch(state){
+    case STATE_WAIT:
+        if(in == '#'){
+            tokenLen = 0;
+            state = STATE_TOKEN;
+        }
+        break;
+    case STATE_TOKEN:
+        if(in == '+' || in == '\r' || in == '\n'){
+            esp_tokenBuf[tokenLen] = '\0';
+            if(in == '+'){
+                paramLen = 0;
+                state = STATE_PARAM;
+            }else{
+                gotResponse(esp_tokenBuf, NULL);
+                //memcpy(esp_token, esp_tokenBuf, tokenLen);
+                //esp_token[tokenLen] = '\0';
+                esp_buf_ready = true;
+                state = STATE_WAIT;
+            }
+        }else if(tokenLen+1 < sizeof(esp_tokenBuf)){
+            esp_tokenBuf[tokenLen++] = in;
+        }
+        break;
+    case STATE_PARAM:
+        if(in == '\r' || in == '\n'){
+            esp_paramBuf[paramLen] = '\0';
+            gotResponse(esp_tokenBuf, esp_paramBuf);
+            //memcpy(esp_token, esp_tokenBuf, tokenLen);
+            //memcpy(esp_param, esp_paramBuf, paramLen);
+            //esp_token[tokenLen] = '\0';
+            //esp_param[paramLen] = '\0';
+            //ser2usb.putc('?');
+            esp_buf_ready = true;
+            state = STATE_WAIT;
+        }else if(paramLen+1 < sizeof(esp_paramBuf)){
+            esp_paramBuf[paramLen++] = in;
+        }
+        break;
+    }
+}
+
+
+Esp8266::Esp8266(PinName TX, PinName RX, const char *wifi_ssid, const char *wifi_passwd)       //定义类的函数
+    : network_start(false), mqtt_start(false), control_cmd(false), esp_buf_ready(false), ser2esp8266(TX, RX)
+{
+    // serial to esp8266 init
+    ser2esp8266.baud(ser_baud);
+    ser2esp8266.attach(callback(this,&Esp8266::esp8266_rxCallback), Serial::RxIrq);    
+    //if (mode == 0) {                                                            // client mode 
+        this->reset();
+        this->connect_wifi(wifi_ssid, wifi_passwd);
+        while(!is_connected()){
+            wait(0.5);
+        }
+        this->weblogin();
+    //} else {
+    //    
+    //}
+}
+
+bool Esp8266::reset() {                                                         //定义类的函数
+    ESP_CMD("node.restart()");
+    wait(2);                                                                    // 延迟2s
+    return true;
+}   
+
+bool Esp8266::connect_wifi(const char *wifi_ssid, const char *wifi_passwd) {                                                  //定义类的函数
+    ESP_CMD("wifi.setmode(wifi.STATION)");
+    ESP_CMD("wifi.sta.config([[%s]],[[%s]])", wifi_ssid, wifi_passwd);
+    wait(2);
+    // set auto autoconnect
+    ESP_CMD("wifi.sta.autoconnect(1)");
+    return true;
+}
+
+bool Esp8266::is_connected()
+{
+    ESP_CMD("print('\\035wifi+'..wifi.sta.status())");
+    wait(0.4);
+    return network_start;
+}
+
+bool Esp8266::weblogin() {                                                      //定义类的函数
+    // not implemented yet
+    return true;
+}
+
+void Esp8266::buildCapability(char *out, const char* infoList[][2])
+{
+    out[0] = '\0';
+    for (int i = 0; infoList[i][0]; ++i)
+    {
+        strcat(out, infoList[i][0]);
+        strcat(out, ",");
+        strcat(out, infoList[i][1]);
+        strcat(out, "\\n");
+    }
+}
+
+bool Esp8266::connect_mqtt_broker(char *ip, const char *node_name, const char* sensors[][2], const char* actuator[][2]) {  //定义类的函数
+
+    ESP_CMD("node_name = '%s'", node_name);
+    ESP_CMD("m = mqtt.Client('i_' .. node.chipid(), 120)");
+    ESP_CMD("m:connect(\"%s\",1883,0,function(conn)print (\"\\035connected\"); end)", ip);
+
+    do{    
+        wait(0.5);
+    }while(!mqtt_start);
+
+    ESP_CMD("m:on(\"message\", function(conn, topic, data)");
+    ESP_CMD("if topic:find(\"^/control/\") then");
+    ESP_CMD("local tok = topic:match(\"^/control/.+/(.+)\")");
+    ESP_CMD("if tok then print(\"\\035control+\"..tok..\"+\"..data) end");
+    ESP_CMD("end");
+    ESP_CMD("end)");
+
+    ESP_CMD("m:publish('/events/'..node_name..'/online','',1,0)");
+    wait(0.1);
+
+    char * capabilities = new char[512];
+
+    if(sensors){
+        buildCapability(capabilities, sensors);
+        ESP_CMD("m:publish('/capability/'..node_name..'/values','%s',1,1)", capabilities);
+        wait(0.1);
+    }
+    if(actuator){
+        buildCapability(capabilities, actuator);
+        ESP_CMD("m:publish('/capability/'..node_name..'/control','%s',1,1)", capabilities);
+        wait(0.1);
+        for (int i = 0; actuator[i][0]; ++i)
+            subscribe_control(actuator[i][0]);
+    }
+
+    delete[ ] capabilities;
+
+    return true;
+}
+    
+bool Esp8266::publish_value(const char *topic, const char *data) {                      //定义类的函数
+    //if (mqtt_start) {
+        ESP_CMD("m:publish('/values/'..node_name..'/%s',\"%s\",0,1)", topic, data);
+        wait(0.1);
+    //}
+    return true;
+}
+
+bool Esp8266::subscribe_control(const char *topic, const char *data) {               //定义类的函数
+    //if (mqtt_start) {
+        ESP_CMD("m:subscribe('/control/'..node_name..'/%s', 0)", topic);
+        wait(0.1);
+    //}
+
+    // ESP_CMD("m:unsubscribe(\"%s\")", topic);
+    return true;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/esp8266.h	Sun Jun 03 10:18:11 2018 +0000
@@ -0,0 +1,34 @@
+
+#include "mbed.h"
+
+class Esp8266 {                                                                 //声明一个类
+    volatile bool network_start;
+    volatile bool mqtt_start;
+    volatile bool control_cmd;
+    char esp_tokenBuf[32], esp_paramBuf[32];   // recv from esp8266
+    char control_buf[32];
+    bool esp_buf_ready;
+    Serial ser2esp8266;
+    
+protected:
+    void esp8266_rxCallback();
+    void gotResponse(char *token, char *param);
+    void buildCapability(char *out, const char* infoList[][2]);
+public:
+    Esp8266(PinName TX, PinName RX, const char *wifi_ssid, const char *wifi_passwd);
+    
+    // 通用
+    bool reset();
+    
+    // 连接模式
+    bool connect_wifi(const char *wifi_ssid, const char *wifi_passwd);
+    bool weblogin();
+    bool connect_mqtt_broker(char *ip, const char *node_name, const char* sensors[][2], const char* actuator[][2]);
+    bool is_connected();
+    
+    bool publish_value(const char *topic, const char *data);
+    bool subscribe_control(const char *topic, const char *data = NULL);
+    bool is_control_available(void) { return control_cmd; }
+    bool get_control_cmd(char* actuator, char* value);
+    // 热点模式
+};
\ No newline at end of file

--- a/main.cpp	Fri May 11 10:42:09 2018 +0000
+++ b/main.cpp	Sun Jun 03 10:18:11 2018 +0000
@@ -1,62 +1,129 @@
 #include "mbed.h"
 #include "Stepper_motor.h"
-#include "robo.h"
-Serial Info(PB_10,PB_11);
-Ticker t1;
-void serial_ticker();//蓝牙串口中断函数
-// puma black
-/*Stepper_motor s1(PB_9,PB_0,PB_7,PA_11,10,16,1);
-Stepper_motor s2(PB_6,PA_7,PB_4,PA_15,10,16,1);
-Stepper_motor s3(PB_3,PA_6,PA_12,PB_1,10,16,1);
-*/
+#include "esp8266.h"
+#include <cstdarg>
+#include <cstring>
+
 //                en  step  dir button 减速比 细分数 初始方向
-Stepper_motor s1(PA_6,PC_5,PC_4,PB_14,10,16,0);//x  puma white
-Stepper_motor s2(PA_2,PA_5,PA_4,PC_6,10,16,1);//y
-Stepper_motor s3(PC_13,PC_3,PC_2,PB_15,10,16,0);//z
-//PwmOut pwm2(PA_1);
-//DigitalOut button(PA_0);
-
-float moterXdeg, moterYdeg, moterZdeg;
+Stepper_motor s1(PB_9,PB_0,PB_7,PC_15,10,16,1);
+Stepper_motor s2(PB_6,PA_7,PB_4,PB_13,10,16,1);
+Stepper_motor s3(PB_3,PA_6,PA_12,PB_5,10,16,0);
+DigitalOut air(PC_13);
+Serial ser2usb(PB_10, PB_11, 115200); 
+int stepok = 0;
+int actionok = 0;
+int state;
+int high;
 int main()
 { 
-    {//初始化
-        Info.attach(&serial_ticker);
         s1.Init();
         wait(5);
         s2.Init();
         wait(5);
         s3.Init();
         wait(5);
-    }
-    
-    {//计算电机角度
-     moterXdeg = 1;
-     moterYdeg = 2;
-     moterZdeg = 3; 
-     //Info.printf("moterXdeg:%f moterYdeg:%f moterZdeg:%f\n",moterXdeg, moterYdeg,moterZdeg);  
-    }
+     ser2usb.printf("starting\r\n");
     
-    {   //运动
-        /*若转动的角度为-1 则会一直转，此时rec_rate需大于0
-         * 除此之外，若转动角度或转速小于0，则电机反转
-         * 如在电机尚未停止前再次调用该函数，则会立刻开始新的电机运动
-         */
-        s1.Config(5,moterXdeg);
-        //wait(5);
-        //如果运动出现bug,可将上方注释去掉
-        s2.Config(5,moterYdeg);
-        //wait(5);
-        s3.Config(5,moterZdeg);
-        //wait(5);
+    // 选定与 esp8266 相连接的串口，WiFi 名称和密码
+    Esp8266 client(PA_9, PA_10, "iot_b827ebd838be", "ab087c9e");// 参数分别为 TX pin / RX pin / SSID / Password
+
+    //声明所有的传感器，每行一个，每个由名字、单位两部分组成，最后一行必须为空指针作为结尾
+    const char* sensors[][2] = {
+        "actionok", "",
+        NULL, NULL //最后一行以空指针作为结束标记
+    };
+
+    //声明所有的执行器，每行一个，每个由名字、参数类型两部分组成，最后一行必须为空指针作为结尾
+    const char* actuators[][2] = {
+        "do", "int",
+        "high","int",
+        NULL, NULL //最后一行以空指针作为结束标记
+    };
+    ser2usb.printf("connecting...\r\n");
+
+    //连接到服务器
+    client.connect_mqtt_broker("192.168.12.1", "warehouse", sensors, actuators);
+
+    ser2usb.printf("Initialization done.\r\n");
+
+    char actuator_name[32], control_value[32];
+    while(1) {
+        //检查有没有收到新的执行器控制指令
+        if(client.get_control_cmd(actuator_name, control_value)){
+            ser2usb.printf("Received CMD %s %s\r\n", actuator_name, control_value);
+            //判断哪个执行器收到命令
+            if (actuator_name == "high")
+            {
+                high = atoi(control_value);
+            }
+            else if(actuator_name == "do")
+            {
+                state = atoi(control_value);
+                switch (state)
+                {
+                    case 1: //forward1
+                        s2.Config(10,10);
+                        break;
+                    case 2: //back1
+                        s2.Config(-10,10);
+                    case 3: //forward2
+                        s2.Config(10,20);
+                    case 4: //back2
+                        s2.Config(-10,20);
+                    case 5:
+                        s3.Config(10,20);   //到方块上方
+                        wait(3);
+//                        s1.Config(high,30); //下降
+//                        wait(4);
+//                        air = 1;            //开气泵
+//                        s1.Config(-high,30);//上升
+//                        wait(3);
+//                        s3.Config(-10,20);  //到小车上方
+//                        wait(3);
+//                        s1.Config(20,20);   //下降
+//                        wait(3);                  
+//                        air = 0;            //关气泵
+//                        wait(2);
+//                        s1.Config(-20,20);  //上升 
+                        client.publish_value("actionok", "0");   
+                    case 6:
+                        s1.Config(20,20);   //下降
+                        wait(3);                  
+//                        air = 1;            //开气泵
+//                        wait(2);
+//                        s1.Config(-20,20);  //上升
+//                        wait(3);
+//                        s3.Config(10,20);   //到方块上方
+//                        wait(3);
+//                        s1.Config(high,30); //下降
+//                        wait(4);
+//                        air = 0;            //关气泵
+//                        s1.Config(-high,30);//上升
+//                        wait(3);
+//                        s3.Config(-10,20);  //到小车上方
+                        client.publish_value("actionok", "0");
+                    default:
+                        break;
+                    
+                }
+            }
         }
-    while(1)
-    {
-   
- 
-    }
+        if (stepok == 1)
+        {
+            switch (state)
+            {
+                case 1: 
+                case 2: 
+                case 3:
+                case 4:
+                    client.publish_value("actionok", "0");
+                    stepok = 0; 
+                    break;                
+                default:
+                    break;
+                
+            }
+            actionok = 0;
+        }
 }
-
-void serial_ticker()
-{
-    
-}
\ No newline at end of file
+}

--- a/robo.h	Fri May 11 10:42:09 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,386 +0,0 @@
-#include<iostream>
-#include<cmath>
-
-#include "mbed.h"
-
-const double pi=3.1415;
-class robo
-{
-public:
-    robo(double a, double b,double x1,double y1,double z1);
-    void setrobo(double *dest,int c);
-    void syschange();//坐标变换
-    void degcalculate();//三舵机角度计算
-    void droute(double a);//直线路径
-    void croute(double a);//曲线路径
-    void dexcute(double *p);//直线执行
-    void cexcute(double *p);//曲线执行
-    double argcalculate(double x, double y);//求反正切对应角度
-private://两组六个原始坐标，两组六个柱坐标下参数，两个机械臂常数，两组六个个机械臂转角，三个转角差，中间过程点直角坐标与柱坐标
-    double X1;
-    double Y1;
-    double Z1;
-    double X2;
-    double Y2;
-    double Z2;
-    
-    double P1;
-    double H1;
-    double Fai1;
-    double P2;
-    double H2;
-    double Fai2;
-    
-    double A;
-    double B;
-    
-    double faia1;
-    double faia2;
-    double faia3;
-    double faib1;
-    double faib2;
-    double faib3;
-    double fai1;
-    double fai2;
-    double fai3;
-    
-    double delta1;
-    double delta2;
-    double delta3;
-    
-    double P;
-    double fai;
-    double H;
-    
-    double x;
-    double y;
-    double z;
-
-    
-};
-
-robo::robo(double a, double b,double x1,double y1,double z1)
-{
-    A=a;
-    B=b;
-    X1=x1;
-    Y1=y1;
-    Z1=z1;
-    X2=x1;
-    Y2=y1;
-    Z2=z1;
-/*    cout<<"please input the first parameter A:";
-    cin>>A;
-    cout<<"please input the second parameter B:";
-    cin>>B;
-    */
-};
-void robo::setrobo(double *dest,int c)
-{
-    X1 = X2;
-    Y1 = Y2;
-    Z1 = Z2;    
-    X2=dest[0];
-    Y2=dest[1];
-    Z2=dest[2];
-    syschange();
-    degcalculate();
-    if(c==1)
-    {
-        dexcute(dest);
-    }
-    else cexcute(dest);
-}
-double robo::argcalculate(double x, double y)
-{
-    double a;
-    if(x>0.000001||x<-0.000001)
-    {
-        if(x>0&&y>=0)
-        {
-            a=atan(y/x);
-        }
-        else if (x>0&&y<0)
-        {
-            a=2*pi+atan(y/x);
-        }
-        else
-        {
-            a=pi+atan(y/x);
-        }
-    }
-    else
-    {
-        if(y>0)
-        {
-            a=pi/2;
-        }
-        else
-        {
-            a=3*pi/2;
-        }
-    }
-  /*  cout<<"输入坐标为："<<x<<","<<y<<endl;
-    cout<<"对应柱坐标弧度为："<<a<<endl;
-    cout<<"对应角度为："<<a/pi*180<<endl;
-    cout<<endl;
-    */return a;
-}
-void robo::syschange()//Fai1、Fai2范围0-360°
-{
-    P1=sqrt(X1*X1+Y1*Y1);
-    P2=sqrt(X2*X2+Y2*Y2);
-    H1=Z1;
-    H2=Z2;
-    Fai1=argcalculate(X1,Y1);
-    Fai2=argcalculate(X2,Y2);
-  /*  cout<<"起始点对应柱坐标为："<<P1<<","<<Fai1/pi*180<<","<<H1<<endl;
-    cout<<endl;
-    cout<<"终止点对应柱坐标为："<<P2<<","<<Fai2/pi*180<<","<<H2<<endl;
-    cout<<endl;
-*/
-}
-void robo::degcalculate()
-{
-    double p2_h2_1=P1*P1+H1*H1;//计算始末的角度参数
-    double sqrT1=sqrt(p2_h2_1);
-    double a1=(A*A+P1*P1+H1*H1-B*B)/(2*A*sqrT1);
-    double a2=P1/sqrT1;
-    double b1=a1;
-    double b2=(A*A-B*B-P1*P1-H1*H1)/(2*B*sqrT1);
-    faia2=pi-asin(a1)-asin(a2);
-    faia3=asin(b1)-asin(b2);
-    faia1=Fai1;
-    double p2_h2_2=P2*P2+H2*H2;
-    double sqrT2=sqrt(p2_h2_2);
-    double c1=(A*A+P2*P2+H2*H2-B*B)/(2*A*sqrT2);
-    double c2=P2/sqrT2;
-    double d1=c1;
-    double d2=(A*A-B*B-P2*P2-H2*H2)/(2*B*sqrT2);
-    faib2=pi-asin(c1)-asin(c2);
-    faib3=asin(d1)-asin(d2);
-    faib1=Fai2;
-/*    cout<<"起始点对应姿态为："<<faia1<<" "<<faia2<<" "<<faia3<<endl;
-    cout<<endl;
-    cout<<"终止点对应姿态为："<<faib1<<" "<<faib2<<" "<<faib3<<endl;
-    cout<<endl;
-*/
-}
-void robo::droute(double a)
-{
-    if(a<0||a>1){
-//        cout<<"WRONG!"<<endl;
-    }
-    else
-    {
-  //      cout<<"开始直线规划！"<<endl;
-    P=sqrt(a*a*P2*P2+(1-a)*(1-a)*P1*P1+2*a*(1-a)*P1*P2*cos(Fai2-Fai1));
-    H=H1+a*(H2-H1);
-    double deg1=a*P2*sin(Fai2-pi/2)+(1-a)*P1*sin(Fai1-pi/2);
-    double deg2=a*P2*cos(Fai2-pi/2)+(1-a)*P1*cos(Fai1-pi/2);
-    if(deg1>-0.00000001&&deg1<0.00000001)
-    {
-        deg1=0;
-        if(deg2>-0.00000001&&deg2<0.00000001)
-        {
-            deg2=0;
-        }
-    }
-    else if (deg2>-0.00000001&&deg2<0.00000001)
-    {
-        deg2=0;
-    }
-//  cout<<cos(Fai1)<<endl; 角度累计计算有误差
-    if(deg1>=0)
-    {
-        if(deg2==0)
-        {
-            fai=pi/2;
-        }
-        else
-        {
-            if(deg2>0)
-            {
-                fai=atan(deg1/deg2);
-            }
-            else
-            {
-                fai=pi+atan(deg1/deg2);
-            }
-        }
-    }
-    else
-    {
-        if(deg2==0)
-        {
-            fai=3*pi/2;
-        }
-        else
-        {
-            if(deg2>0)
-            {
-                fai=2*pi+atan(deg1/deg2);
-            }
-            else
-            {
-                fai=pi+atan(deg1/deg2);
-            }
-        }
-    }
-  //  cout<<"两个参数deg1、deg2分别为："<<deg1<<" "<<deg2<<endl;
-    double p2_h2=P*P+H*H;//计算末端三个角度参数
-    double sqrT=sqrt(p2_h2);
-    double a1=(A*A+P*P+H*H-B*B)/(2*A*sqrT);
-    double a2=P/sqrT;
-    double b1=(A*A+P*P+H*H-B*B)/(2*A*sqrT);
-    double b2=(A*A-B*B-P*P-H*H)/(2*B*sqrT);
-    faia1=fai1;
-    faia2=fai2;
-    faia3=fai3;
-    fai2=pi-asin(a1)-asin(a2);
-    fai3=asin(b1)-asin(b2);
-    fai1=fai;
-    delta1=fai1-faia1;
-    delta2=fai2-faia2;
-    delta3=fai3-faia3;
-/*    cout<<"运动百分比："<<a*100<<"%"<<endl;
-    cout<<"末端位置为："<<P<<" "<<fai<<" "<<H<<endl;
-    cout<<"末端姿态为："<<fai1<<" "<<fai2<<" "<<fai3<<endl;
-    cout<<"转动弧度为："<<delta1<<" "<<delta2<<" "<<delta3<<endl;
-    cout<<"转动角度为："<<delta1*180/pi<<" "<<delta2*180/pi<<" "<<delta3*180/pi<<endl;
-  */  }
-    
-}
-void robo::croute(double a)
-{
-    if(a<0||a>1){
-//        cout<<"WRONG!"<<endl;
-    }
-    else
-    {
-        cout<<endl;
-        cout<<endl;
-  //      cout<<"开始曲线规划！"<<endl;
-        double alpha;
-        double r=sqrt((X2-X1)*(X2-X1)+(Y2-Y1)*(Y2-Y1))/2;
-        if((X1-X2)!=0)
-        {
-            if(Y1>Y2)
-            {
-                if(X1>X2)
-                {
-                    alpha=pi+atan((Y1-Y2)/(X1-X2));
-                }
-                else
-                {
-                    alpha=2*pi+atan((Y1-Y2)/(X1-X2));
-                }
-            }
-            else
-            {
-                if(X1<X2)
-                {
-                    alpha=atan((Y1-Y2)/(X1-X2));
-                }
-                else
-                {
-                    alpha=pi+atan((Y1-Y2)/(X1-X2));
-                }
-            }
-        }
-        else
-        {
-            if(Y1>Y2)
-            {
-                alpha=3*pi/2;
-            }
-            else
-            {
-                alpha=pi/2;
-            }
-        }
-  //      cout<<"夹角参数为："<<alpha<<endl;
-        double xita=a*pi;
-        if(alpha>=0&&alpha<=(pi/2))
-        {
-            x=(X1+X2)/2-r*cos(xita-alpha);
-            y=(Y1+Y2)/2+r*sin(xita-alpha);
-            z=Z1;
-        }
-        else
-        {
-            x=(X1+X2)/2-r*cos(xita+alpha);
-            y=(Y1+Y2)/2-r*sin(xita+alpha);
-            z=Z1;
-        }
-        if(x>-0.00000001&&x<0.00000001)
-        {
-            x=0;
-        }
-   //     cout<<"圆弧直角坐标为："<<x<<" "<<y<<" "<<z<<" "<<endl;
-        fai=argcalculate(x,y);
-        double p2=x*x+y*y;
-        P=sqrt(p2);
-        H=z;
-        double p2_h2=P*P+H*H;//计算末端三个角度参数
-        double sqrT=sqrt(p2_h2);
-        double a1=(A*A+P*P+H*H-B*B)/(2*A*sqrT);
-        double a2=P/sqrT;
-        double b1=(A*A+P*P+H*H-B*B)/(2*A*sqrT);
-        double b2=(A*A-B*B-P*P-H*H)/(2*B*sqrT);
-        fai2=pi-asin(a1)-asin(a2);
-        fai3=asin(b1)-asin(b2);
-        fai1=fai;
-        delta1=fai1-faia1;
-        delta2=fai2-faia2;
-        delta3=fai3-faia3;
- /*       cout<<"运动百分比："<<a*100<<"%"<<endl;
-        cout<<"中间圆弧位置为："<<P<<" "<<fai<<" "<<H<<endl;
-        cout<<"中间姿态为："<<fai1<<" "<<fai2<<" "<<fai3<<endl;
-        cout<<"转动角度为："<<delta1<<" "<<delta2<<" "<<delta3<<endl;
- */       }
-}
-
-void robo::dexcute(double *p)
-{
-    double i=0;
-    int j = 0;
-    while(i<=1)
-    {
-        droute(i);
-        //myServo1.rotate(fai1/pi*180);
-        //myServo2.rotate(fai2/pi*180);
-        //myServo3.rotate(fai3/pi*180);
-        //wait(0.5);
-        p[j]=delta1;
-        p[j+1]=delta2;
-        p[j+2]=delta3;
-        j = j + 3;
-        
-        i=i+0.1;
-    //    cout<<"正在执行步数："<<i*10<<endl;
-    }
-}
-void robo::cexcute(double *p)
-{
-    double i=0;
-    int j = 0;
-    while(i<=1)
-    {
-        croute(i);
-       // double deg1=fai1/pi*180;
-        //myServo1.rotate(delta/pi*180);
-        //myServo2.rotate(fai2/pi*180);
-        //myServo3.rotate(fai3/pi*180);
-        //wait(0.5);
-//      cout<<"正在执行步数："<<i*10<<endl;
-        p[j]=delta1;
-        p[j+1]=delta2;
-        p[j+2]=delta3;
-        j = j + 3;
-        
-        i=i+0.1;
-        
-//      cout<<"已旋转角度："<<deg1<<endl;
-    }
-}