API tools faq
paste
Guest User

Untitled

a guest
Mar 23rd, 2018
119
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 23.32 KB | None | 0 0
raw download clone embed print report
  1. #include "stdafx.h"
  2.  
  3. #include<iostream>
  4. using namespace std;
  5. #include <stdio.h>
  6. #include <stdlib.h>
  7. #include <stdlib.h>
  8. #include"math.h"
  9. float swap(float a, float b)
  10. {
  11. float c;
  12. c = a;
  13. a = b;
  14. b = c;
  15. return 0;
  16.  
  17. }
  18.  
  19.  
  20. //二分法求根
  21. double boots_q[20];
  22. float coef[20];//系数
  23. float *p;//系数
  24. double fabs(double x)
  25. {
  26. return (x < 0) ? -x : x;
  27. }
  28. double Equation(double x)
  29.  
  30. {
  31.  
  32. double z;
  33. z = pow(x, 4)**p+ pow(x, 3)**(p+1)+ pow(x, 2)**(p+2) + x**(p+3) + *(p+4);/*更改为实际多项式*/
  34.  
  35. return(z);
  36.  
  37. }
  38.  
  39. int BinSearchRoot(double a, double b, double h, double eps, double x[], int m) /*用二分法计算非线性方程的实根*/
  40.  
  41. /*参数意义：
  42.  
  43. a 要求的根的下界
  44.  
  45. b 要求的根的上界，即：所求的根落在区间 [a,b]之内
  46.  
  47. h 递进的步长
  48.  
  49. eps 精度
  50.  
  51. x 根的值
  52.  
  53. m 预计的根的个数*/
  54.  
  55. {
  56.  
  57. int n, js;
  58.  
  59. double z, y, z1, y1, z0, y0;
  60.  
  61. n = 0; z = a; y = Equation(z);
  62.  
  63. while ((z <= b + h / 2.0) && (n != m)) /*对给定步长的子区间进行搜索*/
  64.  
  65. {
  66.  
  67. if (fabs(y)<eps) /*当前的判定点是方程的根*/
  68.  
  69. {
  70.  
  71. n = n + 1;
  72.  
  73. x[n - 1] = z;
  74.  
  75. z = z + h / 2.0;
  76.  
  77. y = Equation(z);
  78.  
  79. }
  80.  
  81. else /*当前点不是方程的根*/
  82.  
  83. {
  84.  
  85. z1 = z + h;
  86.  
  87. y1 = Equation(z1);
  88.  
  89. if (fabs(y1)<eps) /*下一个点是方程的根*/
  90.  
  91. {
  92.  
  93. n = n + 1;
  94.  
  95. x[n - 1] = z1;
  96.  
  97. z = z1 + h / 2.0;
  98.  
  99. y = Equation(z);
  100.  
  101. }
  102.  
  103. else if (y*y1>0.0) /*该区间内无根*/
  104.  
  105. {
  106. y = y1; z = z1;
  107. }
  108.  
  109. else /*该区间内有根*/
  110.  
  111. {
  112.  
  113. js = 0;/*标志，0表示未找到根，1表示已经确定了根*/
  114.  
  115. while (js == 0)
  116.  
  117. {
  118.  
  119. if (fabs(z1 - z)<eps) /*区间的长度小于给定的精度，可以当作已经找到了根*/
  120.  
  121. {
  122.  
  123. n = n + 1;
  124.  
  125. x[n - 1] = (z1 + z) / 2.0; /*把区间的中位值作为根*/
  126.  
  127. z = z1 + h / 2.0; /*把寻找的位置放到下一个区间内*/
  128.  
  129. y = Equation(z);
  130.  
  131. js = 1; /*在当前区间内已经找到了根*/
  132.  
  133. }
  134.  
  135. else /*区间比给定的精度大，则进行二分*/
  136.  
  137. {
  138.  
  139. z0 = (z1 + z) / 2.0; /*区间二分*/
  140.  
  141. y0 = Equation(z0);
  142.  
  143. if (fabs(y0)<eps) /*z0位置为根*/
  144.  
  145. {
  146.  
  147. x[n] = z0;
  148.  
  149. n = n + 1;
  150.  
  151. js = 1;
  152.  
  153. z = z0 + h / 2.0;
  154.  
  155. y = Equation(z);
  156.  
  157. }
  158.  
  159. else if ((y*y0)<0.0) /*[z,z0]内有根*/
  160.  
  161. {
  162. z1 = z0; y1 = y0;
  163. }
  164.  
  165. else { z = z0; y = y0; }
  166.  
  167. }
  168.  
  169. }
  170.  
  171. }
  172.  
  173. }
  174.  
  175. }
  176.  
  177. return(n); /*返回根的个数*/
  178.  
  179. }
  180.  
  181. int boot_q()
  182.  
  183. {
  184.  
  185. int i, n;
  186.  
  187. static int m = 6;
  188.  
  189. static double x[6];
  190. // system("cls");
  191. n = BinSearchRoot(-100.0, 150.0, 0.1, 0.000001, x, m);
  192.  
  193. printf("\nThe function has %d roots, they are:\n", n);/*输出根的个数*/
  194.  
  195. for (i = 0; i <= n - 1; i++)
  196. {
  197. printf("x(%d)=%10.7f\n", i, x[i]);
  198. boots_q[i] = x[i];
  199. }
  200. //system("pause");
  201.  
  202. return 0;
  203.  
  204. }
  205. //二分法求多项式的
  206.  
  207.  
  208.  
  209.  
  210.  
  211.  
  212.  
  213.  
  214.  
  215. /*float Va = 6.0, Acca = 3.0, Deca = 2.0, J1 = 3.0, J2 = 2.0 ,step = 0.01;*/
  216. float motioncase1(float Va,float Acca,float Deca,float Tacc,float Tdec,float S,float step)
  217. {
  218. float vmax;
  219. float J1 = Acca / Tacc;
  220. float J2 = Deca / Tdec;
  221. Tacc = ceil(Acca / (J1*step))*step;
  222. Tdec = ceil(Deca / (J2*step))*step;
  223. float T1 = ceil((Va / (Tacc*J1) - Tacc) / step)*step;
  224. J1 = Va / (pow(Tacc,2) + T1*Tacc);
  225. float T2 = ceil((Va / (Tdec*J2) - Tdec) / step)*step;
  226. J2 = Va / (pow(Tdec,2) + T2*Tdec);//step 2
  227. vmax = (pow(Tacc, 2) + T1*Tacc)*J1;
  228. float T3 = ceil((S / (pow(Tacc,2) + T1*Tacc) / J1 - (Tacc + T1 / 2 + Tdec + T2 / 2)) / step)*step;
  229. // float S1 = (Tacc + T1 / 2)*vmax + (Tdec + T2 / 2)*vmax + T3*vmax;
  230. J1 = S / (pow(Tacc, 2) + T1*Tacc) / (Tacc + T1 / 2 + Tdec + T2 / 2 + T3);
  231. J2 = (pow(Tacc, 2) + T1*Tacc)*J1 / (pow(Tdec, 2) + T2*Tdec);
  232. float C1 = Va*Tacc + Va*T1 / 2 + Va*Tdec + Va*T2 / 2;
  233. // cout << "临界位移C1" << endl;
  234. // cout << C1<< endl;
  235. T1 = (Va - Acca*Tacc) / Acca;
  236. T2 = (Va - Deca*Tdec) / Deca;
  237. T3 = (S - C1) / Va;
  238. float T = 2 * (Tacc + Tdec) + T1 + T2 + T3;
  239. /*if (rev == 1)
  240. {
  241. swap(J1, J2);
  242. swap(T1,T2);
  243. swap(Dec,Acc);
  244. swap(Tacc, Tdec);
  245. }*/
  246. int size = T / step;
  247. float t[10000],a[10000] = { 0 };
  248. t[0] = 0;
  249. for (int i = 0; i <= size; i++)
  250. {
  251. t[i + 1] = t[i] + step;
  252. }
  253.  
  254. //cout << T1 << endl;; cout << T2 << endl;
  255. for (int i = 0; i <= size; i++)
  256. {
  257. if (t[i] <= Tacc)
  258. a[i] = t[i]*J1;
  259. if(Tacc<t[i] && t[i]<= Tacc + T1)
  260. a[i] = Acca;
  261. if(Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1)
  262. a[i] = Acca - J1*(t[i] - Tacc - T1);
  263. if(2 * Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1 + T3)
  264. a[i] = 0;
  265. if(2 * Tacc + T1 + T3<t[i] && t[i] <= 2 * Tacc + T1 + T3 + Tdec)
  266. a[i] = -J2*(t[i] - 2 * Tacc - T1 - T3);
  267. if(2 * Tacc + T1 + T3 + Tdec<t[i]&& t[i] <= 2 * Tacc + T1 + T2 + Tdec + T3)
  268. a[i] = -Deca;
  269. if(2 * Tacc + T1 + T2 + Tdec + T3<t[i])
  270. a[i] = -Deca + J2*(t[i] - (2 * Tacc + T1 + T2 + Tdec + T3));
  271. //printf("%f+", a[i]);
  272.  
  273. }
  274. float v[10000];
  275. for (int i = 0; i <= size; i++)
  276. {
  277. float ss=0;
  278. for (int j = 0; j <= i; j++)
  279. {
  280. ss = ss + a[j] * step;
  281. }
  282. v[i] = ss;
  283. //printf("%f+", v[i]);
  284. }
  285. float d[10000];
  286. printf("位移变化\n");
  287. for (int i = 0; i <= size; i++)
  288. {
  289. float ss = 0;
  290. for (int j = 0; j <= i; j++)
  291. {
  292. ss = ss + v[j] * step;
  293. }
  294. d[i] = ss;
  295. printf("%f__", d[i]);
  296. }
  297.  
  298. return 0;
  299. }
  300. float motioncase2(float Va, float Acca, float Deca, float Tacc, float Tdec, float S, float step)
  301. {
  302. float vmax,T1,T2,T3;
  303. float J1 = Acca / Tacc;
  304. float J2 = Deca / Tdec;
  305. Tacc = ceil(Acca / (J1*step))*step;
  306. Tdec = ceil(Deca / (J2*step))*step;
  307. float pa = 1 / Acca + 1 / Deca;
  308. float pb = Tacc + Tdec;
  309. float pc = -2 * S;
  310. vmax = (-pb +pow((pow(pb, 2) - 4 * pa*pc), 0.5)) / (2 * pa);
  311. cout << "\nvmax的值为" << endl;
  312. cout << vmax << endl;
  313. T1 = ceil((vmax / Acca - Tacc)/step)*step;
  314. T2 = ceil((vmax / Deca - Tdec)/step)*step;
  315. vmax = S / (Tacc + T1 / 2 + Tdec + Tdec + T2 / 2);
  316. J1 = vmax / (Tacc*Tacc + T1*Tacc);
  317. J2 = vmax / (Tdec*Tdec + T2*Tdec);
  318. float C2 = Acca*Tacc*Tdec + Acca*Tacc*T2 / 2 + Acca*pow(Tacc, 2);
  319. cout << "C2的值为" << endl;
  320. cout << C2<< endl;
  321. //%求解C2完成
  322. T3 = 0;
  323. float T = 2 * (Tacc + Tdec) + T1 + T2 + T3;
  324.  
  325. int size = T / step;
  326. float t[10000], a[10000] = { 0 };
  327. t[0] = 0;
  328. for (int i = 0; i <= size; i++)
  329. {
  330. t[i + 1] = t[i] + step;
  331. }
  332. printf("加速度变化\n");
  333. //cout << T1 << endl;; cout << T2 << endl;
  334. for (int i = 0; i <= size; i++)
  335. {
  336. if (t[i] <= Tacc)
  337. a[i] = t[i] * J1;
  338. if (Tacc<t[i] && t[i] <= Tacc + T1)
  339. a[i] = Acca;
  340. if (Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1)
  341. a[i] = Acca - J1*(t[i] - Tacc - T1);
  342. if (2 * Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1 + T3)
  343. a[i] = 0;
  344. if (2 * Tacc + T1 + T3<t[i] && t[i] <= 2 * Tacc + T1 + T3 + Tdec)
  345. a[i] = -J2*(t[i] - 2 * Tacc - T1 - T3);
  346. if (2 * Tacc + T1 + T3 + Tdec<t[i] && t[i] <= 2 * Tacc + T1 + T2 + Tdec + T3)
  347. a[i] = -Deca;
  348. if (2 * Tacc + T1 + T2 + Tdec + T3<t[i])
  349. a[i] = -Deca + J2*(t[i] - (2 * Tacc + T1 + T2 + Tdec + T3));
  350. printf("%f__", a[i]);
  351.  
  352. }
  353. printf("速度变化\n");
  354. float v[10000];
  355. for (int i = 0; i <= size; i++)
  356. {
  357. float ss = 0;
  358. for (int j = 0; j <= i; j++)
  359. {
  360. ss = ss + a[j] * step;
  361. }
  362. v[i] = ss;
  363. printf("%f__", v[i]);
  364. }
  365. float d[10000];
  366. printf("位移变化\n");
  367. for (int i = 0; i <= size; i++)
  368. {
  369. float ss = 0;
  370. for (int j = 0; j <= i; j++)
  371. {
  372. ss = ss + v[j] * step;
  373. }
  374. d[i] = ss;
  375. printf("%f__", d[i]);
  376. }
  377.  
  378. return 0;
  379. }
  380.  
  381. float motioncase3(float Va, float Acca, float Deca, float Tacc, float Tdec, float S, float step)
  382. {
  383.  
  384. float vmax, T1, T2, T3,J1,J2;
  385. J2 = Deca / Tdec;
  386. J1=Acca/Tacc;/////////
  387. Tdec = ceil(Deca / (J2*step))*step;
  388. p = coef;
  389. *p = pow(J1,2)/(2*J2*Tdec);
  390. *(p+1)=J1;
  391. *(p+2) = J1*Tdec/2;
  392. *(p+3)=0;
  393. *(p+4) = -S;
  394.  
  395. boot_q();
  396. double p= boots_q[1];
  397. Tacc = p;
  398. T2 = ceil((J1*pow(Tacc, 2) / (J2*Tdec) - Tdec)/step)*step;
  399. J2 = pow(Tacc, 2)*J1 / (Tdec*(Tdec + T2));
  400. J1 = S / (pow(Tacc, 2)*(Tacc + Tdec + T2 / 2));
  401. //T2 = J1*pow(Tacc, 2) / (J2*Tdec) - Tdec;
  402. cout << "TAcc=" << endl;
  403. cout << p << endl;
  404. //float T2 = ceil((J1*pow(Tacc,2) / J2*Tdec - Tdec) / step)*step;
  405. T1 = 0;
  406. T3 = 0;
  407. float T = 2 * (Tacc + Tdec) + T1 + T2 + T3;
  408. float Acc = Tacc*J1;
  409. float Dec = J2*Tdec;
  410. int size = T / step;
  411. float t[10000], a[10000] = { 0 };
  412. t[0] = 0;
  413. for (int i = 0; i <= size; i++)
  414. {
  415. t[i + 1] = t[i] + step;
  416. }
  417. printf("加速度变化\n");
  418. //cout << T1 << endl;; cout << T2 << endl;
  419. for (int i = 0; i <= size; i++)
  420. {
  421. if (t[i] <= Tacc)
  422. a[i] = t[i] * J1;
  423. if (Tacc<t[i] && t[i] <= Tacc + T1)
  424. a[i] = Acc;
  425. if (Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1)
  426. a[i] = Acc - J1*(t[i] - Tacc - T1);
  427. if (2 * Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1 + T3)
  428. a[i] = 0;
  429. if (2 * Tacc + T1 + T3<t[i] && t[i] <= 2 * Tacc + T1 + T3 + Tdec)
  430. a[i] = -J2*(t[i] - 2 * Tacc - T1 - T3);
  431. if (2 * Tacc + T1 + T3 + Tdec<t[i] && t[i] <= 2 * Tacc + T1 + T2 + Tdec + T3)
  432. a[i] = -Dec;
  433. if (2 * Tacc + T1 + T2 + Tdec + T3<t[i])
  434. a[i] = -Dec + J2*(t[i] - (2 * Tacc + T1 + T2 + Tdec + T3));
  435. printf("%f+", a[i]);
  436.  
  437. }
  438. printf("速度变化\n");
  439. float v[10000];
  440. for (int i = 0; i <= size; i++)
  441. {
  442. float ss = 0;
  443. for (int j = 0; j <= i; j++)
  444. {
  445. ss = ss + a[j] * step;
  446. }
  447. v[i] = ss;
  448. printf("%f+", v[i]);
  449. }
  450. float d[10000];
  451. printf("位移变化\n");
  452. for (int i = 0; i <= size; i++)
  453. {
  454. float ss = 0;
  455. for (int j = 0; j <= i; j++)
  456. {
  457. ss = ss + v[j] * step;
  458. }
  459. d[i] = ss;
  460. printf("%f__", d[i]);
  461. }
  462. return 0;
  463. }
  464. float motioncase4(float Va, float Acca, float Deca, float Tacc, float Tdec, float S, float step)
  465. {
  466.  
  467. float vmax,Acc,Dec;
  468. float g = 0.3333333;
  469. float J1 = Acca / Tacc;
  470. float J2 = Deca / Tdec;
  471. Tdec = ceil( ( pow( S /(J2*(1 + pow((J2 / J1) ,0.5))) , g) )/ step )*step;
  472. Tacc = ceil(pow((J2 / J1) , 0.5)*Tdec / step)*step;
  473. cout << Tacc << endl;
  474. cout << Tdec << endl;
  475. J1 = S / (pow(Tacc, 3) + pow(Tacc, 2)*Tdec);
  476. J2 = pow(Tacc, 2)*J1 / pow(Tdec, 2);
  477. //Tdec = ceil((S / pow((J2*(1 + pow((J2 / J1), 0.5))), 1 / 3)));
  478. Acc = Tacc*J1;
  479. Dec = Tdec*J2;
  480. float T1 = 0, T2 = 0, T3 = 0;
  481. float T = 2 * (Tacc + Tdec) + T1 + T2 + T3;
  482. int size = T / step;
  483. float t[10000], a[10000] = { 0 };
  484. t[0] = 0;
  485. for (int i = 0; i <= size; i++)
  486. {
  487. t[i + 1] = t[i] + step;
  488. }
  489. printf("加速度变化\n");
  490. //cout << T1 << endl;; cout << T2 << endl;
  491. for (int i = 0; i <= size; i++)
  492. {
  493. if (t[i] <= Tacc)
  494. a[i] = t[i] * J1;
  495. if (Tacc<t[i] && t[i] <= Tacc + T1)
  496. a[i] = Acc;
  497. if (Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1)
  498. a[i] = Acc - J1*(t[i] - Tacc - T1);
  499. if (2 * Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1 + T3)
  500. a[i] = 0;
  501. if (2 * Tacc + T1 + T3<t[i] && t[i] <= 2 * Tacc + T1 + T3 + Tdec)
  502. a[i] = -J2*(t[i] - 2 * Tacc - T1 - T3);
  503. if (2 * Tacc + T1 + T3 + Tdec<t[i] && t[i] <= 2 * Tacc + T1 + T2 + Tdec + T3)
  504. a[i] = -Dec;
  505. if (2 * Tacc + T1 + T2 + Tdec + T3<t[i])
  506. a[i] = -Dec + J2*(t[i] - (2 * Tacc + T1 + T2 + Tdec + T3));
  507. printf("%f__", a[i]);
  508.  
  509. }
  510. printf("速度变化\n");
  511. float v[10000];
  512. for (int i = 0; i <= size; i++)
  513. {
  514. float ss = 0;
  515. for (int j = 0; j <= i; j++)
  516. {
  517. ss = ss + a[j] * step;
  518. }
  519. v[i] = ss;
  520. printf("%f__", v[i]);
  521. }
  522. float d[10000];
  523. printf("位移变化\n");
  524. for (int i = 0; i <= size; i++)
  525. {
  526. float ss = 0;
  527. for (int j = 0; j <= i; j++)
  528. {
  529. ss = ss + v[j] * step;
  530. }
  531. d[i] = ss;
  532. printf("%f__", d[i]);
  533. }
  534.  
  535.  
  536.  
  537. return 0;
  538. }
  539. float motioncase5(float Va, float Acca, float Deca, float Tacc, float Tdec , float S, float step)
  540. {
  541. float J1,J2,vmax;
  542. J1 =Va/ Tacc/Tacc;
  543. Tacc = ceil(pow((Va / J1) , 0.5) / step)*step;
  544. J2 = Deca / Tdec;
  545.  
  546. Tdec = ceil(Deca / J2 / step)*step;
  547. float T2 = ceil((Va / (Tdec*J2) - Tdec) / step)*step;
  548.  
  549. J2 = Va / (Tdec*(T2 + Tdec));
  550. float T3 = ceil((S / (pow(Tacc,2) * J1) - Tacc - Tdec - T2 / 2) / step)*step;
  551. J1 = S / (pow(Tacc, 2)*(T3 + Tacc + Tdec + T2 / 2));
  552. J2 =pow( Tacc ,2) * J1 / (Tdec*(T2 + Tdec));
  553. float T1 = 0;
  554. Tacc = pow(Va/ J1,0.5);
  555. T2 = (Va - Tdec*Deca) / Deca;
  556. float C4 = Va*Tdec+Va*T2/2+pow(Va,1.5)/pow(J1,0.5);
  557. //C4 = 6.34808;
  558. T3 = (S-C4)/Va;
  559.  
  560.  
  561. //J1 = S / (pow(Tacc , 2) * (T3 + Tacc + Tdec + T2 / 2));
  562. //J2 = pow(Tacc,2) * J1 / (Tdec*(T2 + Tdec));
  563. cout << T3 << endl;
  564. cout << C4<< endl;
  565.  
  566. float T = 2 * (Tacc + Tdec) + T1 + T2 + T3;
  567. int size = T / step;
  568. float t[10000], a[10000] = { 0 };
  569. t[0] = 0;
  570. for (int i = 0; i <= size; i++)
  571. {
  572. t[i + 1] = t[i] + step;
  573. }
  574. printf("加速度变化\n");
  575. //cout << T1 << endl;; cout << T2 << endl;
  576. for (int i = 0; i <= size; i++)
  577. {
  578. if (t[i] <= Tacc)
  579. a[i] = t[i] * J1;
  580. if (Tacc<t[i] && t[i] <= Tacc + T1)
  581. a[i] = J1*Tacc;
  582. if (Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1)
  583. a[i] = J1*Tacc - J1*(t[i] - Tacc - T1);
  584. if (2 * Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1 + T3)
  585. a[i] = 0;
  586. if (2 * Tacc + T1 + T3<t[i] && t[i] <= 2 * Tacc + T1 + T3 + Tdec)
  587. a[i] = -J2*(t[i] - 2 * Tacc - T1 - T3);
  588. if (2 * Tacc + T1 + T3 + Tdec<t[i] && t[i] <= 2 * Tacc + T1 + T2 + Tdec + T3)
  589. a[i] = -J2*Tdec;
  590. if (2 * Tacc + T1 + T2 + Tdec + T3<t[i])
  591. a[i] = -J2*Tdec + J2*(t[i] - (2 * Tacc + T1 + T2 + Tdec + T3));
  592. printf("%f__", a[i]);
  593.  
  594. }
  595. printf("速度变化\n");
  596. float v[10000];
  597. for (int i = 0; i <= size; i++)
  598. {
  599. float ss = 0;
  600. for (int j = 0; j <= i; j++)
  601. {
  602. ss = ss + a[j] * step;
  603. }
  604. v[i] = ss;
  605. printf("%f__", v[i]);
  606. }
  607. float d[10000];
  608. printf("位移变化\n");
  609. for (int i = 0; i <= size; i++)
  610. {
  611. float ss = 0;
  612. for (int j = 0; j <= i; j++)
  613. {
  614. ss = ss + v[j] * step;
  615. }
  616. d[i] = ss;
  617. printf("%f__", d[i]);
  618. }
  619.  
  620.  
  621.  
  622. return 0;
  623. }
  624. float motioncase6(float Va, float Acca, float Deca, float Tacc, float Tdec, float S, float step)
  625. {
  626. float vmax,J1,J2;
  627. J1 = Deca / Tdec;
  628. J2 = Deca / Tdec;
  629. Tdec = ceil(Deca / J2/step)*step;
  630. p = coef;
  631. *p = pow(J1,2) / (2 * J2*Tdec);
  632. *(p + 1) =J1 ;
  633. *(p + 2) = J1*Tdec / 2;
  634. *(p + 3) = 0;
  635. *(p + 4) = -S;
  636. //
  637. boot_q();
  638. Tacc = boots_q[1];
  639. cout << Tacc << endl;
  640. float T2 = ceil((J1*pow(Tacc, 2) / J2 / Tdec - Tdec) / step)*step;
  641. J2 = Tacc*Tacc*J1 / (Tdec*(Tdec + T2));
  642. J1 = S / (Tacc*Tacc*(Tacc + Tdec + T2 / 2));
  643.  
  644. float T1 = 0; float T3 = 0;
  645. float T = 2 * (Tacc + Tdec) + T1 + T2 + T3;
  646. int size = T / step;
  647. float t[10000], a[10000] = { 0 };
  648. t[0] = 0;
  649. for (int i = 0; i <= size; i++)
  650. {
  651. t[i + 1] = t[i] + step;
  652. }
  653. printf("加速度变化\n");
  654. //cout << T1 << endl;; cout << T2 << endl;
  655. for (int i = 0; i <= size; i++)
  656. {
  657. if (t[i] <= Tacc)
  658. a[i] = t[i] * J1;
  659. if (Tacc<t[i] && t[i] <= Tacc + T1)
  660. a[i] = J1*Tacc;
  661. if (Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1)
  662. a[i] = J1*Tacc - J1*(t[i] - Tacc - T1);
  663. if (2 * Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1 + T3)
  664. a[i] = 0;
  665. if (2 * Tacc + T1 + T3<t[i] && t[i] <= 2 * Tacc + T1 + T3 + Tdec)
  666. a[i] = -J2*(t[i] - 2 * Tacc - T1 - T3);
  667. if (2 * Tacc + T1 + T3 + Tdec<t[i] && t[i] <= 2 * Tacc + T1 + T2 + Tdec + T3)
  668. a[i] = -J2*Tdec;
  669. if (2 * Tacc + T1 + T2 + Tdec + T3<t[i])
  670. a[i] = -J2*Tdec + J2*(t[i] - (2 * Tacc + T1 + T2 + Tdec + T3));
  671. printf("%f__", a[i]);
  672.  
  673. }
  674. printf("速度变化\n");
  675. float v[10000];
  676. for (int i = 0; i <= size; i++)
  677. {
  678. float ss = 0;
  679. for (int j = 0; j <= i; j++)
  680. {
  681. ss = ss + a[j] * step;
  682. }
  683. v[i] = ss;
  684. printf("%f__", v[i]);
  685. }
  686. float d[10000];
  687. printf("位移变化\n");
  688. for (int i = 0; i <= size; i++)
  689. {
  690. float ss = 0;
  691. for (int j = 0; j <= i; j++)
  692. {
  693. ss = ss + v[j] * step;
  694. }
  695. d[i] = ss;
  696. printf("%f__", d[i]);
  697. }
  698.  
  699. return 0;
  700. }
  701. float motioncase7(float Va, float Acca, float Deca, float Tacc, float Tdec, float S, float step)
  702. {
  703. float Acc ,Dec,vmax,J1,J2;
  704. J1 = Acca / Tacc; J2 = Deca / Tdec;
  705. Tdec = ceil(pow(S / J2 / (1 + pow(J2 / J1, 0.5)),0.33333333) / step)*step;
  706. Tacc = ceil(pow(J2 / J1, 0.5)*pow(S / J2 / (1 + pow(J2 / J1, 0.5)), 0.333333333) / step)*step;
  707. J1 = S / (pow(Tacc, 3) + pow(Tacc, 2)*Tdec);
  708. J2 = J1*pow(Tacc, 2) / pow(Tdec, 2);
  709.  
  710. Tdec = pow(S / J2 / (1 + pow(J2 / J1, 0.5)), 0.3333333);
  711. Tacc = pow(J2 / J1, 0.5)*Tdec;
  712. Acc = Tacc*J1;
  713. Dec = Tdec*J2;
  714.  
  715. cout << Tacc << endl;
  716. cout << Tdec << endl;
  717. float T1 = 0; float T2 = 0; float T3 = 0;
  718. float T = 2 * (Tacc + Tdec) + T1 + T2 + T3;
  719. cout << T<< endl;
  720. int size = T / step;
  721. float t[10000], a[10000] = { 0 };
  722. t[0] = 0;
  723. for (int i = 0; i <= size; i++)
  724. {
  725. t[i + 1] = t[i] + step;
  726. }
  727. printf("加速度变化\n");
  728. //cout << T1 << endl;; cout << T2 << endl;
  729. for (int i = 0; i <= size; i++)
  730. {
  731. if (t[i] <= Tacc)
  732. a[i] = t[i] * J1;
  733. if (Tacc<t[i] && t[i] <= Tacc + T1)
  734. a[i] = Acc;
  735. if (Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1)
  736. a[i] = Acc - J1*(t[i] - Tacc - T1);
  737. if (2 * Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1 + T3)
  738. a[i] = 0;
  739. if (2 * Tacc + T1 + T3<t[i] && t[i] <= 2 * Tacc + T1 + T3 + Tdec)
  740. a[i] = -J2*(t[i] - 2 * Tacc - T1 - T3);
  741. if (2 * Tacc + T1 + T3 + Tdec<t[i] && t[i] <= 2 * Tacc + T1 + T2 + Tdec + T3)
  742. a[i] = -Dec;
  743. if (2 * Tacc + T1 + T2 + Tdec + T3<t[i])
  744. a[i] = -Dec + J2*(t[i] - (2 * Tacc + T1 + T2 + Tdec + T3));
  745. printf("%f__", a[i]);
  746.  
  747. }
  748. printf("速度变化\n");
  749. float v[10000];
  750. for (int i = 0; i <= size; i++)
  751. {
  752. float ss = 0;
  753. for (int j = 0; j <= i; j++)
  754. {
  755. ss = ss + a[j] * step;
  756. }
  757. v[i] = ss;
  758. printf("%f__", v[i]);
  759. }
  760. float d[10000];
  761. printf("位移变化\n");
  762. for (int i = 0; i <= size; i++)
  763. {
  764. float ss = 0;
  765. for (int j = 0; j <= i; j++)
  766. {
  767. ss = ss + v[j] * step;
  768. }
  769. d[i] = ss;
  770. printf("%f__", d[i]);
  771. }
  772.  
  773. return 0;
  774. }
  775. float motioncase8(float Va, float Acca, float Deca, float Tacc, float Tdec, float S, float step)
  776. {
  777. float J1 = Va/Tacc/Tacc;float J2=Va/Tdec/Tdec;
  778. Tacc = ceil(pow(Va / J1, 0.5) / step)*step;
  779. Tdec = ceil(pow(Va / J2, 0.5) / step)*step;
  780. float T1 = 0; float T2 = 0;
  781. float T3 = ceil((S - J1*pow(Tacc, 3) - J2*pow(Tdec, 3)) / J1 / pow(Tacc, 2) / step)*step;
  782. J1 = S / (Tacc + Tdec + T3) / pow(Tacc, 2);
  783. J2 = J1*pow(Tacc, 2) / pow(Tdec, 2);
  784. Tacc = pow(Va / J1, 0.5) ;
  785. Tdec = pow(Va / J2, 0.5);
  786. float C6 = pow(Va,1.5) / (pow(J1,-0.5) + pow(J2,-0.5));
  787. //T3= (S - C6) / Va;
  788. float Acc = J1*Tacc;
  789. float Dec = J2*Tdec;
  790. float T = 2 * (Tacc + Tdec) + T1 + T2 + T3;
  791. cout << S << endl;
  792. cout << C6 << endl;
  793. int size = T / step;
  794. float t[10000], a[10000] = { 0 };
  795. t[0] = 0;
  796. for (int i = 0; i <= size; i++)
  797. {
  798. t[i + 1] = t[i] + step;
  799. }
  800. printf("加速度变化\n");
  801. //cout << T1 << endl;; cout << T2 << endl;
  802. for (int i = 0; i <= size; i++)
  803. {
  804. if (t[i] <= Tacc)
  805. a[i] = t[i] * J1;
  806. if (Tacc<t[i] && t[i] <= Tacc + T1)
  807. a[i] = Acc;
  808. if (Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1)
  809. a[i] = Acc - J1*(t[i] - Tacc - T1);
  810. if (2 * Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1 + T3)
  811. a[i] = 0;
  812. if (2 * Tacc + T1 + T3<t[i] && t[i] <= 2 * Tacc + T1 + T3 + Tdec)
  813. a[i] = -J2*(t[i] - 2 * Tacc - T1 - T3);
  814. if (2 * Tacc + T1 + T3 + Tdec<t[i] && t[i] <= 2 * Tacc + T1 + T2 + Tdec + T3)
  815. a[i] = -Dec;
  816. if (2 * Tacc + T1 + T2 + Tdec + T3<t[i])
  817. a[i] = -Dec + J2*(t[i] - (2 * Tacc + T1 + T2 + Tdec + T3));
  818. printf("%f__", a[i]);
  819.  
  820. }
  821. printf("速度变化\n");
  822. float v[10000];
  823. for (int i = 0; i <= size; i++)
  824. {
  825. float ss = 0;
  826. for (int j = 0; j <= i; j++)
  827. {
  828. ss = ss + a[j] * step;
  829. }
  830. v[i] = ss;
  831. printf("%f__", v[i]);
  832. }
  833. float d[10000];
  834. printf("位移变化\n");
  835. for (int i = 0; i <= size; i++)
  836. {
  837. float ss = 0;
  838. for (int j = 0; j <= i; j++)
  839. {
  840. ss = ss + v[j] * step;
  841. }
  842. d[i] = ss;
  843. printf("%f__", d[i]);
  844. }
  845.  
  846. return 0;
  847.  
  848.  
  849. }
  850. float motioncase9(float Va, float Acca, float Deca, float Tacc, float Tdec, float S, float step)
  851. {
  852.  
  853. float Acc , Dec;
  854. float J1=Acca / Tacc;
  855. float J2 = Deca / Tdec;
  856. Tdec = ceil(pow(S / J2 / (1 + pow(J2 / J1, 0.5)), 0.33333333) / step)*step;
  857.  
  858. Tacc = ceil(pow(J2 / J1, 0.5)*pow(S / J2 / (1 + pow(J2 / J1, 0.5)), 0.333333333) / step)*step;
  859.  
  860. J1 = S / (pow(Tacc, 3) + pow(Tacc, 2)*Tdec);
  861. J2 = J1*pow(Tacc, 2) / pow(Tdec, 2);
  862.  
  863. Tdec = pow(S / J2 / (1 + pow(J2 / J1, 0.5)), 0.3333333);
  864. Tacc = pow(J2 / J1, 0.5)*Tdec;
  865. Acc = Tacc*J1;
  866. Dec = Tdec*J2;
  867.  
  868. cout << Tacc << endl;
  869. cout << Tdec << endl;
  870. float T1 = 0; float T2 = 0; float T3 = 0;
  871. float T = 2 * (Tacc + Tdec) + T1 + T2 + T3;
  872. cout << T << endl;
  873. int size = T / step;
  874. float t[10000], a[10000] = { 0 };
  875. t[0] = 0;
  876. for (int i = 0; i <= size; i++)
  877. {
  878. t[i + 1] = t[i] + step;
  879. }
  880. printf("加速度变化\n");
  881. //cout << T1 << endl;; cout << T2 << endl;
  882. for (int i = 0; i <= size; i++)
  883. {
  884. if (t[i] <= Tacc)
  885. a[i] = t[i] * J1;
  886. if (Tacc<t[i] && t[i] <= Tacc + T1)
  887. a[i] = Acc;
  888. if (Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1)
  889. a[i] = Acc - J1*(t[i] - Tacc - T1);
  890. if (2 * Tacc + T1<t[i] && t[i] <= 2 * Tacc + T1 + T3)
  891. a[i] = 0;
  892. if (2 * Tacc + T1 + T3<t[i] && t[i] <= 2 * Tacc + T1 + T3 + Tdec)
  893. a[i] = -J2*(t[i] - 2 * Tacc - T1 - T3);
  894. if (2 * Tacc + T1 + T3 + Tdec<t[i] && t[i] <= 2 * Tacc + T1 + T2 + Tdec + T3)
  895. a[i] = -Dec;
  896. if (2 * Tacc + T1 + T2 + Tdec + T3<t[i])
  897. a[i] = -Dec + J2*(t[i] - (2 * Tacc + T1 + T2 + Tdec + T3));
  898. printf("%f__", a[i]);
  899.  
  900. }
  901. printf("速度变化\n");
  902. float v[10000];
  903. for (int i = 0; i <= size; i++)
  904. {
  905. float ss = 0;
  906. for (int j = 0; j <= i; j++)
  907. {
  908. ss = ss + a[j] * step;
  909. }
  910. v[i] = ss;
  911. printf("%f__", v[i]);
  912. }
  913. float d[10000];
  914. printf("位移变化\n");
  915. for (int i = 0; i <= size; i++)
  916. {
  917. float ss = 0;
  918. for (int j = 0; j <= i; j++)
  919. {
  920. ss = ss + v[j] * step;
  921. }
  922. d[i] = ss;
  923. printf("%f__", d[i]);
  924. }
  925.  
  926. return 0;
  927.  
  928.  
  929. }
  930.  
  931. float critical(float Va,float Acca,float Deca,float Tacc,float Tdec,float S )
  932. {
  933. float T1;
  934. float T2;
  935.  
  936. if (Va > Tacc*Acca&&Tacc*Acca > Tdec*Deca)
  937. {
  938. float J1 = Acca /Tacc ;
  939. float J2 = Deca /Tdec ;
  940. T1 = (Va - Acca*Tacc) / Acca;
  941. T2 = (Va - Deca*Tdec) / Deca;
  942. float C1 = Va*Tacc + Va*T1 / 2 + Va*Tdec + Va*T2 / 2;
  943. cout << C1 << endl;
  944. T2 = (Acca*Tacc - Deca*Tdec) / Deca;
  945. float C2 = Acca*Tacc*Tdec + Acca*Tacc*T2 / 2 + Acca*Tacc*Tacc;
  946. cout << C2 << endl;
  947. J2= Deca /Tdec;
  948. Tacc = Tdec*pow(J2 / J1, 0.5);
  949. float C3 = J1*pow(Deca*Tdec / J1, 1.5) + Deca*Tdec*Tdec;
  950. cout << C3 << endl;
  951. if(S>C1)
  952. return 1;
  953. if (S <= C1&&C2 < S)
  954. return 2;
  955. if (S <= C2&&S > C3)
  956. return 3;
  957. if (S <= C3)
  958. return 4;
  959. }
  960. if (Tacc*Acca >= Va&&Va > Tdec*Deca)
  961. {
  962. float J1= Acca/Tacc ;
  963. T2 = (Va - Deca*Tdec)/Deca;
  964. float C4 = pow(Va, 1.5) / pow(J1, 0.5) + Va*Tdec + Va*T2 / 2;
  965. cout << C4 << endl;
  966. J1=Acca/Tacc;
  967. float J2 = pow(Tacc / Tdec, 2)*J1;
  968. float C5 = Deca*Tdec*pow(Deca*Tdec / J1, 0.5)+Deca*Tdec*Tdec;
  969. cout << C5 << endl;
  970. if (S>C4)
  971. return 5;
  972. if (S <= C4&&C5 < S)
  973. return 6;
  974. if (S <= C5)
  975. return 7;
  976. }
  977. if (Tacc*Acca >Tdec*Deca&&Tdec*Deca >=Va)
  978. {
  979. float J1 = Acca / Tacc;
  980. float J2 = Deca / Tdec;
  981. float C6 = pow(Va, 1.5)*(pow(J1, -0.5) + pow(J2, -0.5));
  982. cout << C6 << endl;
  983. if(S>C6)
  984. return 8;
  985. if (S <= C6)
  986. return 9;
  987. }
  988. if (Tacc*Acca<=Tdec*Deca)
  989. {
  990.  
  991. cout <<"需要逆向规划" << endl;
  992. return 10;
  993. }
  994.  
  995. }
  996.  
  997.  
  998.  
  999. int main()//逆向规划的时候把a[0]和a[T/step]交换
  1000. {
  1001. //motioncase1();
  1002. //motioncase1();
  1003. float Va = 3;//3和2是出现第二和第三大类的边界
  1004. float Acca = 4;
  1005. float Deca = 2;
  1006. float Tacc = 1;
  1007. float Tdec = 1;
  1008. float S =5;
  1009. float step = 0.01;
  1010. int flag = critical(Va, Acca, Deca, Tacc, Tdec, S);
  1011. cout<<flag<<endl;
  1012.  
  1013. switch (flag)
  1014. {
  1015. case 1:motioncase1(Va, Acca, Deca, Tacc, Tdec, S, step);break;
  1016. case 2:motioncase2(Va, Acca, Deca, Tacc, Tdec, S, step); break;
  1017. case 3:motioncase3(Va, Acca, Deca, Tacc, Tdec, S, step); break;
  1018. case 4:motioncase4(Va, Acca, Deca, Tacc, Tdec, S, step); break;
  1019. case 5:motioncase5(Va, Acca, Deca, Tacc, Tdec, S, step); break;
  1020. case 6:motioncase6(Va, Acca, Deca, Tacc, Tdec, S, step); break;
  1021. case 7:motioncase7(Va, Acca, Deca, Tacc, Tdec, S, step); break;
  1022. case 8:motioncase8(Va, Acca, Deca, Tacc, Tdec, S, step); break;
  1023. case 9:motioncase9(Va, Acca, Deca, Tacc, Tdec, S, step); break;
  1024. case 10:; break;
  1025. default:;break;
  1026. }
  1027.  
  1028.  
  1029. return 0;
  1030. }
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!