Keegan Hu / Mbed 2 deprecated whouse

dfd

Dependencies:   stepper mbed

Fork of puma_test by Keegan Hu

robo.h@3:09f6061e6e5d, 2018-05-11 (annotated)

Committer:
glintligo
Date:
Fri May 11 10:42:09 2018 +0000
Revision:
3:09f6061e6e5d
Parent:
0:2e00b81c9137 
???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Bilybill 0:2e00b81c9137 1 #include<iostream>
Bilybill 0:2e00b81c9137 2 #include<cmath>
Bilybill 0:2e00b81c9137 3
Bilybill 0:2e00b81c9137 4 #include "mbed.h"
Bilybill 0:2e00b81c9137 5
Bilybill 0:2e00b81c9137 6 const double pi=3.1415;
Bilybill 0:2e00b81c9137 7 class robo
Bilybill 0:2e00b81c9137 8 {
Bilybill 0:2e00b81c9137 9 public:
Bilybill 0:2e00b81c9137 10 robo(double a, double b,double x1,double y1,double z1);
Bilybill 0:2e00b81c9137 11 void setrobo(double *dest,int c);
Bilybill 0:2e00b81c9137 12 void syschange();//坐标变换
Bilybill 0:2e00b81c9137 13 void degcalculate();//三舵机角度计算
Bilybill 0:2e00b81c9137 14 void droute(double a);//直线路径
Bilybill 0:2e00b81c9137 15 void croute(double a);//曲线路径
Bilybill 0:2e00b81c9137 16 void dexcute(double *p);//直线执行
Bilybill 0:2e00b81c9137 17 void cexcute(double *p);//曲线执行
Bilybill 0:2e00b81c9137 18 double argcalculate(double x, double y);//求反正切对应角度
Bilybill 0:2e00b81c9137 19 private://两组六个原始坐标，两组六个柱坐标下参数，两个机械臂常数，两组六个个机械臂转角，三个转角差，中间过程点直角坐标与柱坐标
Bilybill 0:2e00b81c9137 20 double X1;
Bilybill 0:2e00b81c9137 21 double Y1;
Bilybill 0:2e00b81c9137 22 double Z1;
Bilybill 0:2e00b81c9137 23 double X2;
Bilybill 0:2e00b81c9137 24 double Y2;
Bilybill 0:2e00b81c9137 25 double Z2;
Bilybill 0:2e00b81c9137 26
Bilybill 0:2e00b81c9137 27 double P1;
Bilybill 0:2e00b81c9137 28 double H1;
Bilybill 0:2e00b81c9137 29 double Fai1;
Bilybill 0:2e00b81c9137 30 double P2;
Bilybill 0:2e00b81c9137 31 double H2;
Bilybill 0:2e00b81c9137 32 double Fai2;
Bilybill 0:2e00b81c9137 33
Bilybill 0:2e00b81c9137 34 double A;
Bilybill 0:2e00b81c9137 35 double B;
Bilybill 0:2e00b81c9137 36
Bilybill 0:2e00b81c9137 37 double faia1;
Bilybill 0:2e00b81c9137 38 double faia2;
Bilybill 0:2e00b81c9137 39 double faia3;
Bilybill 0:2e00b81c9137 40 double faib1;
Bilybill 0:2e00b81c9137 41 double faib2;
Bilybill 0:2e00b81c9137 42 double faib3;
Bilybill 0:2e00b81c9137 43 double fai1;
Bilybill 0:2e00b81c9137 44 double fai2;
Bilybill 0:2e00b81c9137 45 double fai3;
Bilybill 0:2e00b81c9137 46
Bilybill 0:2e00b81c9137 47 double delta1;
Bilybill 0:2e00b81c9137 48 double delta2;
Bilybill 0:2e00b81c9137 49 double delta3;
Bilybill 0:2e00b81c9137 50
Bilybill 0:2e00b81c9137 51 double P;
Bilybill 0:2e00b81c9137 52 double fai;
Bilybill 0:2e00b81c9137 53 double H;
Bilybill 0:2e00b81c9137 54
Bilybill 0:2e00b81c9137 55 double x;
Bilybill 0:2e00b81c9137 56 double y;
Bilybill 0:2e00b81c9137 57 double z;
Bilybill 0:2e00b81c9137 58
Bilybill 0:2e00b81c9137 59
Bilybill 0:2e00b81c9137 60 };
Bilybill 0:2e00b81c9137 61
Bilybill 0:2e00b81c9137 62 robo::robo(double a, double b,double x1,double y1,double z1)
Bilybill 0:2e00b81c9137 63 {
Bilybill 0:2e00b81c9137 64 A=a;
Bilybill 0:2e00b81c9137 65 B=b;
Bilybill 0:2e00b81c9137 66 X1=x1;
Bilybill 0:2e00b81c9137 67 Y1=y1;
Bilybill 0:2e00b81c9137 68 Z1=z1;
Bilybill 0:2e00b81c9137 69 X2=x1;
Bilybill 0:2e00b81c9137 70 Y2=y1;
Bilybill 0:2e00b81c9137 71 Z2=z1;
Bilybill 0:2e00b81c9137 72 /* cout<<"please input the first parameter A:";
Bilybill 0:2e00b81c9137 73 cin>>A;
Bilybill 0:2e00b81c9137 74 cout<<"please input the second parameter B:";
Bilybill 0:2e00b81c9137 75 cin>>B;
Bilybill 0:2e00b81c9137 76 */
Bilybill 0:2e00b81c9137 77 };
Bilybill 0:2e00b81c9137 78 void robo::setrobo(double *dest,int c)
Bilybill 0:2e00b81c9137 79 {
Bilybill 0:2e00b81c9137 80 X1 = X2;
Bilybill 0:2e00b81c9137 81 Y1 = Y2;
Bilybill 0:2e00b81c9137 82 Z1 = Z2;
Bilybill 0:2e00b81c9137 83 X2=dest[0];
Bilybill 0:2e00b81c9137 84 Y2=dest[1];
Bilybill 0:2e00b81c9137 85 Z2=dest[2];
Bilybill 0:2e00b81c9137 86 syschange();
Bilybill 0:2e00b81c9137 87 degcalculate();
Bilybill 0:2e00b81c9137 88 if(c==1)
Bilybill 0:2e00b81c9137 89 {
Bilybill 0:2e00b81c9137 90 dexcute(dest);
Bilybill 0:2e00b81c9137 91 }
Bilybill 0:2e00b81c9137 92 else cexcute(dest);
Bilybill 0:2e00b81c9137 93 }
Bilybill 0:2e00b81c9137 94 double robo::argcalculate(double x, double y)
Bilybill 0:2e00b81c9137 95 {
Bilybill 0:2e00b81c9137 96 double a;
Bilybill 0:2e00b81c9137 97 if(x>0.000001||x<-0.000001)
Bilybill 0:2e00b81c9137 98 {
Bilybill 0:2e00b81c9137 99 if(x>0&&y>=0)
Bilybill 0:2e00b81c9137 100 {
Bilybill 0:2e00b81c9137 101 a=atan(y/x);
Bilybill 0:2e00b81c9137 102 }
Bilybill 0:2e00b81c9137 103 else if (x>0&&y<0)
Bilybill 0:2e00b81c9137 104 {
Bilybill 0:2e00b81c9137 105 a=2*pi+atan(y/x);
Bilybill 0:2e00b81c9137 106 }
Bilybill 0:2e00b81c9137 107 else
Bilybill 0:2e00b81c9137 108 {
Bilybill 0:2e00b81c9137 109 a=pi+atan(y/x);
Bilybill 0:2e00b81c9137 110 }
Bilybill 0:2e00b81c9137 111 }
Bilybill 0:2e00b81c9137 112 else
Bilybill 0:2e00b81c9137 113 {
Bilybill 0:2e00b81c9137 114 if(y>0)
Bilybill 0:2e00b81c9137 115 {
Bilybill 0:2e00b81c9137 116 a=pi/2;
Bilybill 0:2e00b81c9137 117 }
Bilybill 0:2e00b81c9137 118 else
Bilybill 0:2e00b81c9137 119 {
Bilybill 0:2e00b81c9137 120 a=3*pi/2;
Bilybill 0:2e00b81c9137 121 }
Bilybill 0:2e00b81c9137 122 }
Bilybill 0:2e00b81c9137 123 /* cout<<"输入坐标为："<<x<<","<<y<<endl;
Bilybill 0:2e00b81c9137 124 cout<<"对应柱坐标弧度为："<<a<<endl;
Bilybill 0:2e00b81c9137 125 cout<<"对应角度为："<<a/pi*180<<endl;
Bilybill 0:2e00b81c9137 126 cout<<endl;
Bilybill 0:2e00b81c9137 127 */return a;
Bilybill 0:2e00b81c9137 128 }
Bilybill 0:2e00b81c9137 129 void robo::syschange()//Fai1、Fai2范围0-360°
Bilybill 0:2e00b81c9137 130 {
Bilybill 0:2e00b81c9137 131 P1=sqrt(X1*X1+Y1*Y1);
Bilybill 0:2e00b81c9137 132 P2=sqrt(X2*X2+Y2*Y2);
Bilybill 0:2e00b81c9137 133 H1=Z1;
Bilybill 0:2e00b81c9137 134 H2=Z2;
Bilybill 0:2e00b81c9137 135 Fai1=argcalculate(X1,Y1);
Bilybill 0:2e00b81c9137 136 Fai2=argcalculate(X2,Y2);
Bilybill 0:2e00b81c9137 137 /* cout<<"起始点对应柱坐标为："<<P1<<","<<Fai1/pi*180<<","<<H1<<endl;
Bilybill 0:2e00b81c9137 138 cout<<endl;
Bilybill 0:2e00b81c9137 139 cout<<"终止点对应柱坐标为："<<P2<<","<<Fai2/pi*180<<","<<H2<<endl;
Bilybill 0:2e00b81c9137 140 cout<<endl;
Bilybill 0:2e00b81c9137 141 */
Bilybill 0:2e00b81c9137 142 }
Bilybill 0:2e00b81c9137 143 void robo::degcalculate()
Bilybill 0:2e00b81c9137 144 {
Bilybill 0:2e00b81c9137 145 double p2_h2_1=P1*P1+H1*H1;//计算始末的角度参数
Bilybill 0:2e00b81c9137 146 double sqrT1=sqrt(p2_h2_1);
Bilybill 0:2e00b81c9137 147 double a1=(A*A+P1*P1+H1*H1-B*B)/(2*A*sqrT1);
Bilybill 0:2e00b81c9137 148 double a2=P1/sqrT1;
Bilybill 0:2e00b81c9137 149 double b1=a1;
Bilybill 0:2e00b81c9137 150 double b2=(A*A-B*B-P1*P1-H1*H1)/(2*B*sqrT1);
Bilybill 0:2e00b81c9137 151 faia2=pi-asin(a1)-asin(a2);
Bilybill 0:2e00b81c9137 152 faia3=asin(b1)-asin(b2);
Bilybill 0:2e00b81c9137 153 faia1=Fai1;
Bilybill 0:2e00b81c9137 154 double p2_h2_2=P2*P2+H2*H2;
Bilybill 0:2e00b81c9137 155 double sqrT2=sqrt(p2_h2_2);
Bilybill 0:2e00b81c9137 156 double c1=(A*A+P2*P2+H2*H2-B*B)/(2*A*sqrT2);
Bilybill 0:2e00b81c9137 157 double c2=P2/sqrT2;
Bilybill 0:2e00b81c9137 158 double d1=c1;
Bilybill 0:2e00b81c9137 159 double d2=(A*A-B*B-P2*P2-H2*H2)/(2*B*sqrT2);
Bilybill 0:2e00b81c9137 160 faib2=pi-asin(c1)-asin(c2);
Bilybill 0:2e00b81c9137 161 faib3=asin(d1)-asin(d2);
Bilybill 0:2e00b81c9137 162 faib1=Fai2;
Bilybill 0:2e00b81c9137 163 /* cout<<"起始点对应姿态为："<<faia1<<" "<<faia2<<" "<<faia3<<endl;
Bilybill 0:2e00b81c9137 164 cout<<endl;
Bilybill 0:2e00b81c9137 165 cout<<"终止点对应姿态为："<<faib1<<" "<<faib2<<" "<<faib3<<endl;
Bilybill 0:2e00b81c9137 166 cout<<endl;
Bilybill 0:2e00b81c9137 167 */
Bilybill 0:2e00b81c9137 168 }
Bilybill 0:2e00b81c9137 169 void robo::droute(double a)
Bilybill 0:2e00b81c9137 170 {
Bilybill 0:2e00b81c9137 171 if(a<0||a>1){
Bilybill 0:2e00b81c9137 172 // cout<<"WRONG!"<<endl;
Bilybill 0:2e00b81c9137 173 }
Bilybill 0:2e00b81c9137 174 else
Bilybill 0:2e00b81c9137 175 {
Bilybill 0:2e00b81c9137 176 // cout<<"开始直线规划！"<<endl;
Bilybill 0:2e00b81c9137 177 P=sqrt(a*a*P2*P2+(1-a)*(1-a)*P1*P1+2*a*(1-a)*P1*P2*cos(Fai2-Fai1));
Bilybill 0:2e00b81c9137 178 H=H1+a*(H2-H1);
Bilybill 0:2e00b81c9137 179 double deg1=a*P2*sin(Fai2-pi/2)+(1-a)*P1*sin(Fai1-pi/2);
Bilybill 0:2e00b81c9137 180 double deg2=a*P2*cos(Fai2-pi/2)+(1-a)*P1*cos(Fai1-pi/2);
Bilybill 0:2e00b81c9137 181 if(deg1>-0.00000001&&deg1<0.00000001)
Bilybill 0:2e00b81c9137 182 {
Bilybill 0:2e00b81c9137 183 deg1=0;
Bilybill 0:2e00b81c9137 184 if(deg2>-0.00000001&&deg2<0.00000001)
Bilybill 0:2e00b81c9137 185 {
Bilybill 0:2e00b81c9137 186 deg2=0;
Bilybill 0:2e00b81c9137 187 }
Bilybill 0:2e00b81c9137 188 }
Bilybill 0:2e00b81c9137 189 else if (deg2>-0.00000001&&deg2<0.00000001)
Bilybill 0:2e00b81c9137 190 {
Bilybill 0:2e00b81c9137 191 deg2=0;
Bilybill 0:2e00b81c9137 192 }
Bilybill 0:2e00b81c9137 193 // cout<<cos(Fai1)<<endl; 角度累计计算有误差
Bilybill 0:2e00b81c9137 194 if(deg1>=0)
Bilybill 0:2e00b81c9137 195 {
Bilybill 0:2e00b81c9137 196 if(deg2==0)
Bilybill 0:2e00b81c9137 197 {
Bilybill 0:2e00b81c9137 198 fai=pi/2;
Bilybill 0:2e00b81c9137 199 }
Bilybill 0:2e00b81c9137 200 else
Bilybill 0:2e00b81c9137 201 {
Bilybill 0:2e00b81c9137 202 if(deg2>0)
Bilybill 0:2e00b81c9137 203 {
Bilybill 0:2e00b81c9137 204 fai=atan(deg1/deg2);
Bilybill 0:2e00b81c9137 205 }
Bilybill 0:2e00b81c9137 206 else
Bilybill 0:2e00b81c9137 207 {
Bilybill 0:2e00b81c9137 208 fai=pi+atan(deg1/deg2);
Bilybill 0:2e00b81c9137 209 }
Bilybill 0:2e00b81c9137 210 }
Bilybill 0:2e00b81c9137 211 }
Bilybill 0:2e00b81c9137 212 else
Bilybill 0:2e00b81c9137 213 {
Bilybill 0:2e00b81c9137 214 if(deg2==0)
Bilybill 0:2e00b81c9137 215 {
Bilybill 0:2e00b81c9137 216 fai=3*pi/2;
Bilybill 0:2e00b81c9137 217 }
Bilybill 0:2e00b81c9137 218 else
Bilybill 0:2e00b81c9137 219 {
Bilybill 0:2e00b81c9137 220 if(deg2>0)
Bilybill 0:2e00b81c9137 221 {
Bilybill 0:2e00b81c9137 222 fai=2*pi+atan(deg1/deg2);
Bilybill 0:2e00b81c9137 223 }
Bilybill 0:2e00b81c9137 224 else
Bilybill 0:2e00b81c9137 225 {
Bilybill 0:2e00b81c9137 226 fai=pi+atan(deg1/deg2);
Bilybill 0:2e00b81c9137 227 }
Bilybill 0:2e00b81c9137 228 }
Bilybill 0:2e00b81c9137 229 }
Bilybill 0:2e00b81c9137 230 // cout<<"两个参数deg1、deg2分别为："<<deg1<<" "<<deg2<<endl;
Bilybill 0:2e00b81c9137 231 double p2_h2=P*P+H*H;//计算末端三个角度参数
Bilybill 0:2e00b81c9137 232 double sqrT=sqrt(p2_h2);
Bilybill 0:2e00b81c9137 233 double a1=(A*A+P*P+H*H-B*B)/(2*A*sqrT);
Bilybill 0:2e00b81c9137 234 double a2=P/sqrT;
Bilybill 0:2e00b81c9137 235 double b1=(A*A+P*P+H*H-B*B)/(2*A*sqrT);
Bilybill 0:2e00b81c9137 236 double b2=(A*A-B*B-P*P-H*H)/(2*B*sqrT);
Bilybill 0:2e00b81c9137 237 faia1=fai1;
Bilybill 0:2e00b81c9137 238 faia2=fai2;
Bilybill 0:2e00b81c9137 239 faia3=fai3;
Bilybill 0:2e00b81c9137 240 fai2=pi-asin(a1)-asin(a2);
Bilybill 0:2e00b81c9137 241 fai3=asin(b1)-asin(b2);
Bilybill 0:2e00b81c9137 242 fai1=fai;
Bilybill 0:2e00b81c9137 243 delta1=fai1-faia1;
Bilybill 0:2e00b81c9137 244 delta2=fai2-faia2;
Bilybill 0:2e00b81c9137 245 delta3=fai3-faia3;
Bilybill 0:2e00b81c9137 246 /* cout<<"运动百分比："<<a*100<<"%"<<endl;
Bilybill 0:2e00b81c9137 247 cout<<"末端位置为："<<P<<" "<<fai<<" "<<H<<endl;
Bilybill 0:2e00b81c9137 248 cout<<"末端姿态为："<<fai1<<" "<<fai2<<" "<<fai3<<endl;
Bilybill 0:2e00b81c9137 249 cout<<"转动弧度为："<<delta1<<" "<<delta2<<" "<<delta3<<endl;
Bilybill 0:2e00b81c9137 250 cout<<"转动角度为："<<delta1*180/pi<<" "<<delta2*180/pi<<" "<<delta3*180/pi<<endl;
Bilybill 0:2e00b81c9137 251 */ }
Bilybill 0:2e00b81c9137 252
Bilybill 0:2e00b81c9137 253 }
Bilybill 0:2e00b81c9137 254 void robo::croute(double a)
Bilybill 0:2e00b81c9137 255 {
Bilybill 0:2e00b81c9137 256 if(a<0||a>1){
Bilybill 0:2e00b81c9137 257 // cout<<"WRONG!"<<endl;
Bilybill 0:2e00b81c9137 258 }
Bilybill 0:2e00b81c9137 259 else
Bilybill 0:2e00b81c9137 260 {
Bilybill 0:2e00b81c9137 261 cout<<endl;
Bilybill 0:2e00b81c9137 262 cout<<endl;
Bilybill 0:2e00b81c9137 263 // cout<<"开始曲线规划！"<<endl;
Bilybill 0:2e00b81c9137 264 double alpha;
Bilybill 0:2e00b81c9137 265 double r=sqrt((X2-X1)*(X2-X1)+(Y2-Y1)*(Y2-Y1))/2;
Bilybill 0:2e00b81c9137 266 if((X1-X2)!=0)
Bilybill 0:2e00b81c9137 267 {
Bilybill 0:2e00b81c9137 268 if(Y1>Y2)
Bilybill 0:2e00b81c9137 269 {
Bilybill 0:2e00b81c9137 270 if(X1>X2)
Bilybill 0:2e00b81c9137 271 {
Bilybill 0:2e00b81c9137 272 alpha=pi+atan((Y1-Y2)/(X1-X2));
Bilybill 0:2e00b81c9137 273 }
Bilybill 0:2e00b81c9137 274 else
Bilybill 0:2e00b81c9137 275 {
Bilybill 0:2e00b81c9137 276 alpha=2*pi+atan((Y1-Y2)/(X1-X2));
Bilybill 0:2e00b81c9137 277 }
Bilybill 0:2e00b81c9137 278 }
Bilybill 0:2e00b81c9137 279 else
Bilybill 0:2e00b81c9137 280 {
Bilybill 0:2e00b81c9137 281 if(X1<X2)
Bilybill 0:2e00b81c9137 282 {
Bilybill 0:2e00b81c9137 283 alpha=atan((Y1-Y2)/(X1-X2));
Bilybill 0:2e00b81c9137 284 }
Bilybill 0:2e00b81c9137 285 else
Bilybill 0:2e00b81c9137 286 {
Bilybill 0:2e00b81c9137 287 alpha=pi+atan((Y1-Y2)/(X1-X2));
Bilybill 0:2e00b81c9137 288 }
Bilybill 0:2e00b81c9137 289 }
Bilybill 0:2e00b81c9137 290 }
Bilybill 0:2e00b81c9137 291 else
Bilybill 0:2e00b81c9137 292 {
Bilybill 0:2e00b81c9137 293 if(Y1>Y2)
Bilybill 0:2e00b81c9137 294 {
Bilybill 0:2e00b81c9137 295 alpha=3*pi/2;
Bilybill 0:2e00b81c9137 296 }
Bilybill 0:2e00b81c9137 297 else
Bilybill 0:2e00b81c9137 298 {
Bilybill 0:2e00b81c9137 299 alpha=pi/2;
Bilybill 0:2e00b81c9137 300 }
Bilybill 0:2e00b81c9137 301 }
Bilybill 0:2e00b81c9137 302 // cout<<"夹角参数为："<<alpha<<endl;
Bilybill 0:2e00b81c9137 303 double xita=a*pi;
Bilybill 0:2e00b81c9137 304 if(alpha>=0&&alpha<=(pi/2))
Bilybill 0:2e00b81c9137 305 {
Bilybill 0:2e00b81c9137 306 x=(X1+X2)/2-r*cos(xita-alpha);
Bilybill 0:2e00b81c9137 307 y=(Y1+Y2)/2+r*sin(xita-alpha);
Bilybill 0:2e00b81c9137 308 z=Z1;
Bilybill 0:2e00b81c9137 309 }
Bilybill 0:2e00b81c9137 310 else
Bilybill 0:2e00b81c9137 311 {
Bilybill 0:2e00b81c9137 312 x=(X1+X2)/2-r*cos(xita+alpha);
Bilybill 0:2e00b81c9137 313 y=(Y1+Y2)/2-r*sin(xita+alpha);
Bilybill 0:2e00b81c9137 314 z=Z1;
Bilybill 0:2e00b81c9137 315 }
Bilybill 0:2e00b81c9137 316 if(x>-0.00000001&&x<0.00000001)
Bilybill 0:2e00b81c9137 317 {
Bilybill 0:2e00b81c9137 318 x=0;
Bilybill 0:2e00b81c9137 319 }
Bilybill 0:2e00b81c9137 320 // cout<<"圆弧直角坐标为："<<x<<" "<<y<<" "<<z<<" "<<endl;
Bilybill 0:2e00b81c9137 321 fai=argcalculate(x,y);
Bilybill 0:2e00b81c9137 322 double p2=x*x+y*y;
Bilybill 0:2e00b81c9137 323 P=sqrt(p2);
Bilybill 0:2e00b81c9137 324 H=z;
Bilybill 0:2e00b81c9137 325 double p2_h2=P*P+H*H;//计算末端三个角度参数
Bilybill 0:2e00b81c9137 326 double sqrT=sqrt(p2_h2);
Bilybill 0:2e00b81c9137 327 double a1=(A*A+P*P+H*H-B*B)/(2*A*sqrT);
Bilybill 0:2e00b81c9137 328 double a2=P/sqrT;
Bilybill 0:2e00b81c9137 329 double b1=(A*A+P*P+H*H-B*B)/(2*A*sqrT);
Bilybill 0:2e00b81c9137 330 double b2=(A*A-B*B-P*P-H*H)/(2*B*sqrT);
Bilybill 0:2e00b81c9137 331 fai2=pi-asin(a1)-asin(a2);
Bilybill 0:2e00b81c9137 332 fai3=asin(b1)-asin(b2);
Bilybill 0:2e00b81c9137 333 fai1=fai;
Bilybill 0:2e00b81c9137 334 delta1=fai1-faia1;
Bilybill 0:2e00b81c9137 335 delta2=fai2-faia2;
Bilybill 0:2e00b81c9137 336 delta3=fai3-faia3;
Bilybill 0:2e00b81c9137 337 /* cout<<"运动百分比："<<a*100<<"%"<<endl;
Bilybill 0:2e00b81c9137 338 cout<<"中间圆弧位置为："<<P<<" "<<fai<<" "<<H<<endl;
Bilybill 0:2e00b81c9137 339 cout<<"中间姿态为："<<fai1<<" "<<fai2<<" "<<fai3<<endl;
Bilybill 0:2e00b81c9137 340 cout<<"转动角度为："<<delta1<<" "<<delta2<<" "<<delta3<<endl;
Bilybill 0:2e00b81c9137 341 */ }
Bilybill 0:2e00b81c9137 342 }
Bilybill 0:2e00b81c9137 343
Bilybill 0:2e00b81c9137 344 void robo::dexcute(double *p)
Bilybill 0:2e00b81c9137 345 {
Bilybill 0:2e00b81c9137 346 double i=0;
Bilybill 0:2e00b81c9137 347 int j = 0;
Bilybill 0:2e00b81c9137 348 while(i<=1)
Bilybill 0:2e00b81c9137 349 {
Bilybill 0:2e00b81c9137 350 droute(i);
Bilybill 0:2e00b81c9137 351 //myServo1.rotate(fai1/pi*180);
Bilybill 0:2e00b81c9137 352 //myServo2.rotate(fai2/pi*180);
Bilybill 0:2e00b81c9137 353 //myServo3.rotate(fai3/pi*180);
Bilybill 0:2e00b81c9137 354 //wait(0.5);
Bilybill 0:2e00b81c9137 355 p[j]=delta1;
Bilybill 0:2e00b81c9137 356 p[j+1]=delta2;
Bilybill 0:2e00b81c9137 357 p[j+2]=delta3;
Bilybill 0:2e00b81c9137 358 j = j + 3;
Bilybill 0:2e00b81c9137 359
Bilybill 0:2e00b81c9137 360 i=i+0.1;
Bilybill 0:2e00b81c9137 361 // cout<<"正在执行步数："<<i*10<<endl;
Bilybill 0:2e00b81c9137 362 }
Bilybill 0:2e00b81c9137 363 }
Bilybill 0:2e00b81c9137 364 void robo::cexcute(double *p)
Bilybill 0:2e00b81c9137 365 {
Bilybill 0:2e00b81c9137 366 double i=0;
Bilybill 0:2e00b81c9137 367 int j = 0;
Bilybill 0:2e00b81c9137 368 while(i<=1)
Bilybill 0:2e00b81c9137 369 {
Bilybill 0:2e00b81c9137 370 croute(i);
Bilybill 0:2e00b81c9137 371 // double deg1=fai1/pi*180;
Bilybill 0:2e00b81c9137 372 //myServo1.rotate(delta/pi*180);
Bilybill 0:2e00b81c9137 373 //myServo2.rotate(fai2/pi*180);
Bilybill 0:2e00b81c9137 374 //myServo3.rotate(fai3/pi*180);
Bilybill 0:2e00b81c9137 375 //wait(0.5);
Bilybill 0:2e00b81c9137 376 // cout<<"正在执行步数："<<i*10<<endl;
Bilybill 0:2e00b81c9137 377 p[j]=delta1;
Bilybill 0:2e00b81c9137 378 p[j+1]=delta2;
Bilybill 0:2e00b81c9137 379 p[j+2]=delta3;
Bilybill 0:2e00b81c9137 380 j = j + 3;
Bilybill 0:2e00b81c9137 381
Bilybill 0:2e00b81c9137 382 i=i+0.1;
Bilybill 0:2e00b81c9137 383
Bilybill 0:2e00b81c9137 384 // cout<<"已旋转角度："<<deg1<<endl;
Bilybill 0:2e00b81c9137 385 }
Bilybill 0:2e00b81c9137 386 }