![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ddd
Fork of whouse by
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
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&°1<0.00000001) - { - deg1=0; - if(deg2>-0.00000001&°2<0.00000001) - { - deg2=0; - } - } - else if (deg2>-0.00000001&°2<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; - } -}