ddd

Dependencies:   a4998ss mbed

Fork of whouse by Keegan Hu

Files at this revision

API Documentation at this revision

Comitter:
glintligo
Date:
Sun Jun 03 10:18:11 2018 +0000
Parent:
3:09f6061e6e5d
Commit message:
ddd

Changed in this revision

Stepper_motor.cpp Show annotated file Show diff for this revision Revisions of this file
Stepper_motor.h Show annotated file Show diff for this revision Revisions of this file
a4998.lib Show annotated file Show diff for this revision Revisions of this file
esp8266.cpp Show annotated file Show diff for this revision Revisions of this file
esp8266.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
robo.h Show diff for this revision Revisions of this file
diff -r 09f6061e6e5d -r d66b6f331b74 Stepper_motor.cpp
--- 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
diff -r 09f6061e6e5d -r d66b6f331b74 Stepper_motor.h
--- 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:
diff -r 09f6061e6e5d -r d66b6f331b74 a4998.lib
--- 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
diff -r 09f6061e6e5d -r d66b6f331b74 esp8266.cpp
--- /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
diff -r 09f6061e6e5d -r d66b6f331b74 esp8266.h
--- /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
diff -r 09f6061e6e5d -r d66b6f331b74 main.cpp
--- 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
+}
diff -r 09f6061e6e5d -r d66b6f331b74 robo.h
--- 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;
-    }
-}