API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Mar 24th, 2018
65
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 21.00 KB | None | 0 0
raw download clone embed print report
  1. /* Includes ------------------------------------------------------------------*/
  2. #include <Servo.h> //to define and control servos
  3. #include <FlexiTimer2.h>//to set a timer to manage all servos
  4. /* Servos --------------------------------------------------------------------*/
  5. //define 12 servos for 4 legs
  6. Servo servo[4][3];
  7. //define servos' ports
  8. const int servo_pin[4][3] = { {9, 10, 8}, {4, 3, 2}, {12, 13, 11}, {6, 7, 5} };
  9. /* Size of the robot ---------------------------------------------------------*/
  10. const float length_a = 55;
  11. const float length_b = 77.5;
  12. const float length_c = 27.5;
  13. const float length_side = 71;
  14. const float z_absolute = 0;
  15. /* Constants for movement ----------------------------------------------------*/
  16. const float z_default = -70, z_up = -30, z_boot = z_absolute;
  17. const float x_default = 62, x_offset = 0;
  18. const float y_start = 0, y_step = 40;
  19. /* variables for movement ----------------------------------------------------*/
  20. volatile float site_now[4][3]; //real-time coordinates of the end of each leg
  21. volatile float site_expect[4][3]; //expected coordinates of the end of each leg
  22. float temp_speed[4][3]; //each axis' speed, needs to be recalculated before each movement
  23. float move_speed; //movement speed
  24. float speed_multiple = 1; //movement speed multiple
  25. const float spot_turn_speed = 8;//4
  26. const float leg_move_speed = 15;//8
  27. const float body_move_speed = 10;//3
  28. const float stand_seat_speed = 5;
  29. volatile int rest_counter; //+1/0.02s, for automatic rest
  30. //functions' parameter
  31. const float KEEP = 255;
  32. //define PI for calculation
  33. const float pi = 3.1415926;
  34. /* Constants for turn --------------------------------------------------------*/
  35. //temp length
  36. const float temp_a = sqrt(pow(2 * x_default + length_side, 2) + pow(y_step, 2));
  37. const float temp_b = 2 * (y_start + y_step) + length_side;
  38. const float temp_c = sqrt(pow(2 * x_default + length_side, 2) + pow(2 * y_start + y_step + length_side, 2));
  39. const float temp_alpha = acos((pow(temp_a, 2) + pow(temp_b, 2) - pow(temp_c, 2)) / 2 / temp_a / temp_b);
  40. //site for turn
  41. const float turn_x1 = (temp_a - length_side) / 2;
  42. const float turn_y1 = y_start + y_step / 2;
  43. const float turn_x0 = turn_x1 - temp_b * cos(temp_alpha);
  44. const float turn_y0 = temp_b * sin(temp_alpha) - turn_y1 - length_side;
  45. /* ---------------------------------------------------------------------------*/
  46.  
  47. /*
  48. - setup function
  49. ---------------------------------------------------------------------------*/
  50. void setup()
  51. {
  52. //start serial for debug
  53. Serial.begin(115200);
  54. Serial.println("Robot starts initialization");
  55. //initialize default parameter
  56. set_site(0, x_default - x_offset, y_start + y_step, z_boot);
  57. set_site(1, x_default - x_offset, y_start + y_step, z_boot);
  58. set_site(2, x_default + x_offset, y_start, z_boot);
  59. set_site(3, x_default + x_offset, y_start, z_boot);
  60. for (int i = 0; i < 4; i++)
  61. {
  62. for (int j = 0; j < 3; j++)
  63. {
  64. site_now[i][j] = site_expect[i][j];
  65. }
  66. }
  67. //start servo service
  68. FlexiTimer2::set(20, servo_service);
  69. FlexiTimer2::start();
  70. Serial.println("Servo service started");
  71. //initialize servos
  72. servo_attach();
  73. Serial.println("Servos initialized");
  74. Serial.println("Robot initialization Complete");
  75. delay(2000);
  76. }
  77.  
  78.  
  79. void servo_attach(void)
  80. {
  81. for (int i = 0; i < 4; i++)
  82. {
  83. for (int j = 0; j < 3; j++)
  84. {
  85. servo[i][j].attach(servo_pin[i][j]);
  86. delay(100);
  87. }
  88. }
  89. }
  90.  
  91. void servo_detach(void)
  92. {
  93. for (int i = 0; i < 4; i++)
  94. {
  95. for (int j = 0; j < 3; j++)
  96. {
  97. servo[i][j].detach();
  98. delay(100);
  99. }
  100. }
  101. }
  102. /*
  103. - loop function
  104. ---------------------------------------------------------------------------*/
  105. void loop()
  106. {
  107. Serial.println("Stand");
  108. stand();
  109. delay(2000);
  110. Serial.println("Step forward");
  111. step_forward(15);
  112. delay(2000);
  113. Serial.println("Step back");
  114. step_back(15);
  115. delay(2000);
  116. Serial.println("Turn left");
  117. turn_left(5);
  118. delay(2000);
  119. Serial.println("Turn right");
  120. turn_right(5);
  121. delay(2000);
  122. Serial.println("Hand wave");
  123. hand_wave(3);
  124. delay(2000);
  125. Serial.println("Hand wave");
  126. hand_shake(3);
  127. delay(2000);
  128. Serial.println("Sit");
  129. sit();
  130. delay(5000);
  131. }
  132. /*
  133. - sit
  134. - blocking function
  135. ---------------------------------------------------------------------------*/
  136. void sit(void)
  137. {
  138. move_speed = stand_seat_speed;
  139. for (int leg = 0; leg < 4; leg++)
  140. {
  141. set_site(leg, KEEP, KEEP, z_boot);
  142. }
  143. wait_all_reach();
  144. }
  145.  
  146. /*
  147. - stand
  148. - blocking function
  149. ---------------------------------------------------------------------------*/
  150. void stand(void)
  151. {
  152. move_speed = stand_seat_speed;
  153. for (int leg = 0; leg < 4; leg++)
  154. {
  155. set_site(leg, KEEP, KEEP, z_default);
  156. }
  157. wait_all_reach();
  158. }
  159.  
  160.  
  161. /*
  162. - spot turn to left
  163. - blocking function
  164. - parameter step steps wanted to turn
  165. ---------------------------------------------------------------------------*/
  166. void turn_left(unsigned int step)
  167. {
  168. move_speed = spot_turn_speed;
  169. while (step-- > 0)
  170. {
  171. if (site_now[3][1] == y_start)
  172. {
  173. //leg 3&1 move
  174. set_site(3, x_default + x_offset, y_start, z_up);
  175. wait_all_reach();
  176.  
  177. set_site(0, turn_x1 - x_offset, turn_y1, z_default);
  178. set_site(1, turn_x0 - x_offset, turn_y0, z_default);
  179. set_site(2, turn_x1 + x_offset, turn_y1, z_default);
  180. set_site(3, turn_x0 + x_offset, turn_y0, z_up);
  181. wait_all_reach();
  182.  
  183. set_site(3, turn_x0 + x_offset, turn_y0, z_default);
  184. wait_all_reach();
  185.  
  186. set_site(0, turn_x1 + x_offset, turn_y1, z_default);
  187. set_site(1, turn_x0 + x_offset, turn_y0, z_default);
  188. set_site(2, turn_x1 - x_offset, turn_y1, z_default);
  189. set_site(3, turn_x0 - x_offset, turn_y0, z_default);
  190. wait_all_reach();
  191.  
  192. set_site(1, turn_x0 + x_offset, turn_y0, z_up);
  193. wait_all_reach();
  194.  
  195. set_site(0, x_default + x_offset, y_start, z_default);
  196. set_site(1, x_default + x_offset, y_start, z_up);
  197. set_site(2, x_default - x_offset, y_start + y_step, z_default);
  198. set_site(3, x_default - x_offset, y_start + y_step, z_default);
  199. wait_all_reach();
  200.  
  201. set_site(1, x_default + x_offset, y_start, z_default);
  202. wait_all_reach();
  203. }
  204. else
  205. {
  206. //leg 0&2 move
  207. set_site(0, x_default + x_offset, y_start, z_up);
  208. wait_all_reach();
  209.  
  210. set_site(0, turn_x0 + x_offset, turn_y0, z_up);
  211. set_site(1, turn_x1 + x_offset, turn_y1, z_default);
  212. set_site(2, turn_x0 - x_offset, turn_y0, z_default);
  213. set_site(3, turn_x1 - x_offset, turn_y1, z_default);
  214. wait_all_reach();
  215.  
  216. set_site(0, turn_x0 + x_offset, turn_y0, z_default);
  217. wait_all_reach();
  218.  
  219. set_site(0, turn_x0 - x_offset, turn_y0, z_default);
  220. set_site(1, turn_x1 - x_offset, turn_y1, z_default);
  221. set_site(2, turn_x0 + x_offset, turn_y0, z_default);
  222. set_site(3, turn_x1 + x_offset, turn_y1, z_default);
  223. wait_all_reach();
  224.  
  225. set_site(2, turn_x0 + x_offset, turn_y0, z_up);
  226. wait_all_reach();
  227.  
  228. set_site(0, x_default - x_offset, y_start + y_step, z_default);
  229. set_site(1, x_default - x_offset, y_start + y_step, z_default);
  230. set_site(2, x_default + x_offset, y_start, z_up);
  231. set_site(3, x_default + x_offset, y_start, z_default);
  232. wait_all_reach();
  233.  
  234. set_site(2, x_default + x_offset, y_start, z_default);
  235. wait_all_reach();
  236. }
  237. }
  238. }
  239.  
  240. /*
  241. - spot turn to right
  242. - blocking function
  243. - parameter step steps wanted to turn
  244. ---------------------------------------------------------------------------*/
  245. void turn_right(unsigned int step)
  246. {
  247. move_speed = spot_turn_speed;
  248. while (step-- > 0)
  249. {
  250. if (site_now[2][1] == y_start)
  251. {
  252. //leg 2&0 move
  253. set_site(2, x_default + x_offset, y_start, z_up);
  254. wait_all_reach();
  255.  
  256. set_site(0, turn_x0 - x_offset, turn_y0, z_default);
  257. set_site(1, turn_x1 - x_offset, turn_y1, z_default);
  258. set_site(2, turn_x0 + x_offset, turn_y0, z_up);
  259. set_site(3, turn_x1 + x_offset, turn_y1, z_default);
  260. wait_all_reach();
  261.  
  262. set_site(2, turn_x0 + x_offset, turn_y0, z_default);
  263. wait_all_reach();
  264.  
  265. set_site(0, turn_x0 + x_offset, turn_y0, z_default);
  266. set_site(1, turn_x1 + x_offset, turn_y1, z_default);
  267. set_site(2, turn_x0 - x_offset, turn_y0, z_default);
  268. set_site(3, turn_x1 - x_offset, turn_y1, z_default);
  269. wait_all_reach();
  270.  
  271. set_site(0, turn_x0 + x_offset, turn_y0, z_up);
  272. wait_all_reach();
  273.  
  274. set_site(0, x_default + x_offset, y_start, z_up);
  275. set_site(1, x_default + x_offset, y_start, z_default);
  276. set_site(2, x_default - x_offset, y_start + y_step, z_default);
  277. set_site(3, x_default - x_offset, y_start + y_step, z_default);
  278. wait_all_reach();
  279.  
  280. set_site(0, x_default + x_offset, y_start, z_default);
  281. wait_all_reach();
  282. }
  283. else
  284. {
  285. //leg 1&3 move
  286. set_site(1, x_default + x_offset, y_start, z_up);
  287. wait_all_reach();
  288.  
  289. set_site(0, turn_x1 + x_offset, turn_y1, z_default);
  290. set_site(1, turn_x0 + x_offset, turn_y0, z_up);
  291. set_site(2, turn_x1 - x_offset, turn_y1, z_default);
  292. set_site(3, turn_x0 - x_offset, turn_y0, z_default);
  293. wait_all_reach();
  294.  
  295. set_site(1, turn_x0 + x_offset, turn_y0, z_default);
  296. wait_all_reach();
  297.  
  298. set_site(0, turn_x1 - x_offset, turn_y1, z_default);
  299. set_site(1, turn_x0 - x_offset, turn_y0, z_default);
  300. set_site(2, turn_x1 + x_offset, turn_y1, z_default);
  301. set_site(3, turn_x0 + x_offset, turn_y0, z_default);
  302. wait_all_reach();
  303.  
  304. set_site(3, turn_x0 + x_offset, turn_y0, z_up);
  305. wait_all_reach();
  306.  
  307. set_site(0, x_default - x_offset, y_start + y_step, z_default);
  308. set_site(1, x_default - x_offset, y_start + y_step, z_default);
  309. set_site(2, x_default + x_offset, y_start, z_default);
  310. set_site(3, x_default + x_offset, y_start, z_up);
  311. wait_all_reach();
  312.  
  313. set_site(3, x_default + x_offset, y_start, z_default);
  314. wait_all_reach();
  315. }
  316. }
  317. }
  318.  
  319. /*
  320. - go forward
  321. - blocking function
  322. - parameter step steps wanted to go
  323. ---------------------------------------------------------------------------*/
  324. void step_forward(unsigned int step)
  325. {
  326. move_speed = leg_move_speed;
  327. while (step-- > 0)
  328. {
  329. if (site_now[2][1] == y_start)
  330. {
  331. //leg 2&1 move
  332. set_site(2, x_default + x_offset, y_start, z_up);
  333. wait_all_reach();
  334. set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up);
  335. wait_all_reach();
  336. set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default);
  337. wait_all_reach();
  338.  
  339. move_speed = body_move_speed;
  340.  
  341. set_site(0, x_default + x_offset, y_start, z_default);
  342. set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default);
  343. set_site(2, x_default - x_offset, y_start + y_step, z_default);
  344. set_site(3, x_default - x_offset, y_start + y_step, z_default);
  345. wait_all_reach();
  346.  
  347. move_speed = leg_move_speed;
  348.  
  349. set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up);
  350. wait_all_reach();
  351. set_site(1, x_default + x_offset, y_start, z_up);
  352. wait_all_reach();
  353. set_site(1, x_default + x_offset, y_start, z_default);
  354. wait_all_reach();
  355. }
  356. else
  357. {
  358. //leg 0&3 move
  359. set_site(0, x_default + x_offset, y_start, z_up);
  360. wait_all_reach();
  361. set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up);
  362. wait_all_reach();
  363. set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default);
  364. wait_all_reach();
  365.  
  366. move_speed = body_move_speed;
  367.  
  368. set_site(0, x_default - x_offset, y_start + y_step, z_default);
  369. set_site(1, x_default - x_offset, y_start + y_step, z_default);
  370. set_site(2, x_default + x_offset, y_start, z_default);
  371. set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default);
  372. wait_all_reach();
  373.  
  374. move_speed = leg_move_speed;
  375.  
  376. set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up);
  377. wait_all_reach();
  378. set_site(3, x_default + x_offset, y_start, z_up);
  379. wait_all_reach();
  380. set_site(3, x_default + x_offset, y_start, z_default);
  381. wait_all_reach();
  382. }
  383. }
  384. }
  385.  
  386. /*
  387. - go back
  388. - blocking function
  389. - parameter step steps wanted to go
  390. ---------------------------------------------------------------------------*/
  391. void step_back(unsigned int step)
  392. {
  393. move_speed = leg_move_speed;
  394. while (step-- > 0)
  395. {
  396. if (site_now[3][1] == y_start)
  397. {
  398. //leg 3&0 move
  399. set_site(3, x_default + x_offset, y_start, z_up);
  400. wait_all_reach();
  401. set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up);
  402. wait_all_reach();
  403. set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default);
  404. wait_all_reach();
  405.  
  406. move_speed = body_move_speed;
  407.  
  408. set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default);
  409. set_site(1, x_default + x_offset, y_start, z_default);
  410. set_site(2, x_default - x_offset, y_start + y_step, z_default);
  411. set_site(3, x_default - x_offset, y_start + y_step, z_default);
  412. wait_all_reach();
  413.  
  414. move_speed = leg_move_speed;
  415.  
  416. set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up);
  417. wait_all_reach();
  418. set_site(0, x_default + x_offset, y_start, z_up);
  419. wait_all_reach();
  420. set_site(0, x_default + x_offset, y_start, z_default);
  421. wait_all_reach();
  422. }
  423. else
  424. {
  425. //leg 1&2 move
  426. set_site(1, x_default + x_offset, y_start, z_up);
  427. wait_all_reach();
  428. set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up);
  429. wait_all_reach();
  430. set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default);
  431. wait_all_reach();
  432.  
  433. move_speed = body_move_speed;
  434.  
  435. set_site(0, x_default - x_offset, y_start + y_step, z_default);
  436. set_site(1, x_default - x_offset, y_start + y_step, z_default);
  437. set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default);
  438. set_site(3, x_default + x_offset, y_start, z_default);
  439. wait_all_reach();
  440.  
  441. move_speed = leg_move_speed;
  442.  
  443. set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up);
  444. wait_all_reach();
  445. set_site(2, x_default + x_offset, y_start, z_up);
  446. wait_all_reach();
  447. set_site(2, x_default + x_offset, y_start, z_default);
  448. wait_all_reach();
  449. }
  450. }
  451. }
  452.  
  453. // add by RegisHsu
  454.  
  455. void body_left(int i)
  456. {
  457. set_site(0, site_now[0][0] + i, KEEP, KEEP);
  458. set_site(1, site_now[1][0] + i, KEEP, KEEP);
  459. set_site(2, site_now[2][0] - i, KEEP, KEEP);
  460. set_site(3, site_now[3][0] - i, KEEP, KEEP);
  461. wait_all_reach();
  462. }
  463.  
  464. void body_right(int i)
  465. {
  466. set_site(0, site_now[0][0] - i, KEEP, KEEP);
  467. set_site(1, site_now[1][0] - i, KEEP, KEEP);
  468. set_site(2, site_now[2][0] + i, KEEP, KEEP);
  469. set_site(3, site_now[3][0] + i, KEEP, KEEP);
  470. wait_all_reach();
  471. }
  472.  
  473. void hand_wave(int i)
  474. {
  475. float x_tmp;
  476. float y_tmp;
  477. float z_tmp;
  478. move_speed = 1;
  479. if (site_now[3][1] == y_start)
  480. {
  481. body_right(15);
  482. x_tmp = site_now[2][0];
  483. y_tmp = site_now[2][1];
  484. z_tmp = site_now[2][2];
  485. move_speed = body_move_speed;
  486. for (int j = 0; j < i; j++)
  487. {
  488. set_site(2, turn_x1, turn_y1, 50);
  489. wait_all_reach();
  490. set_site(2, turn_x0, turn_y0, 50);
  491. wait_all_reach();
  492. }
  493. set_site(2, x_tmp, y_tmp, z_tmp);
  494. wait_all_reach();
  495. move_speed = 1;
  496. body_left(15);
  497. }
  498. else
  499. {
  500. body_left(15);
  501. x_tmp = site_now[0][0];
  502. y_tmp = site_now[0][1];
  503. z_tmp = site_now[0][2];
  504. move_speed = body_move_speed;
  505. for (int j = 0; j < i; j++)
  506. {
  507. set_site(0, turn_x1, turn_y1, 50);
  508. wait_all_reach();
  509. set_site(0, turn_x0, turn_y0, 50);
  510. wait_all_reach();
  511. }
  512. set_site(0, x_tmp, y_tmp, z_tmp);
  513. wait_all_reach();
  514. move_speed = 1;
  515. body_right(15);
  516. }
  517. }
  518.  
  519. void hand_shake(int i)
  520. {
  521. float x_tmp;
  522. float y_tmp;
  523. float z_tmp;
  524. move_speed = 1;
  525. if (site_now[3][1] == y_start)
  526. {
  527. body_right(15);
  528. x_tmp = site_now[2][0];
  529. y_tmp = site_now[2][1];
  530. z_tmp = site_now[2][2];
  531. move_speed = body_move_speed;
  532. for (int j = 0; j < i; j++)
  533. {
  534. set_site(2, x_default - 30, y_start + 2 * y_step, 55);
  535. wait_all_reach();
  536. set_site(2, x_default - 30, y_start + 2 * y_step, 10);
  537. wait_all_reach();
  538. }
  539. set_site(2, x_tmp, y_tmp, z_tmp);
  540. wait_all_reach();
  541. move_speed = 1;
  542. body_left(15);
  543. }
  544. else
  545. {
  546. body_left(15);
  547. x_tmp = site_now[0][0];
  548. y_tmp = site_now[0][1];
  549. z_tmp = site_now[0][2];
  550. move_speed = body_move_speed;
  551. for (int j = 0; j < i; j++)
  552. {
  553. set_site(0, x_default - 30, y_start + 2 * y_step, 55);
  554. wait_all_reach();
  555. set_site(0, x_default - 30, y_start + 2 * y_step, 10);
  556. wait_all_reach();
  557. }
  558. set_site(0, x_tmp, y_tmp, z_tmp);
  559. wait_all_reach();
  560. move_speed = 1;
  561. body_right(15);
  562. }
  563. }
  564.  
  565.  
  566.  
  567. /*
  568. - microservos service /timer interrupt function/50Hz
  569. - when set site expected,this function move the end point to it in a straight line
  570. - temp_speed[4][3] should be set before set expect site,it make sure the end point
  571. move in a straight line,and decide move speed.
  572. ---------------------------------------------------------------------------*/
  573. void servo_service(void)
  574. {
  575. sei();
  576. static float alpha, beta, gamma;
  577.  
  578. for (int i = 0; i < 4; i++)
  579. {
  580. for (int j = 0; j < 3; j++)
  581. {
  582. if (abs(site_now[i][j] - site_expect[i][j]) >= abs(temp_speed[i][j]))
  583. site_now[i][j] += temp_speed[i][j];
  584. else
  585. site_now[i][j] = site_expect[i][j];
  586. }
  587.  
  588. cartesian_to_polar(alpha, beta, gamma, site_now[i][0], site_now[i][1], site_now[i][2]);
  589. polar_to_servo(i, alpha, beta, gamma);
  590. }
  591.  
  592. rest_counter++;
  593. }
  594.  
  595. /*
  596. - set one of end points' expect site
  597. - this founction will set temp_speed[4][3] at same time
  598. - non - blocking function
  599. ---------------------------------------------------------------------------*/
  600. void set_site(int leg, float x, float y, float z)
  601. {
  602. float length_x = 0, length_y = 0, length_z = 0;
  603.  
  604. if (x != KEEP)
  605. length_x = x - site_now[leg][0];
  606. if (y != KEEP)
  607. length_y = y - site_now[leg][1];
  608. if (z != KEEP)
  609. length_z = z - site_now[leg][2];
  610.  
  611. float length = sqrt(pow(length_x, 2) + pow(length_y, 2) + pow(length_z, 2));
  612.  
  613. temp_speed[leg][0] = length_x / length * move_speed * speed_multiple;
  614. temp_speed[leg][1] = length_y / length * move_speed * speed_multiple;
  615. temp_speed[leg][2] = length_z / length * move_speed * speed_multiple;
  616.  
  617. if (x != KEEP)
  618. site_expect[leg][0] = x;
  619. if (y != KEEP)
  620. site_expect[leg][1] = y;
  621. if (z != KEEP)
  622. site_expect[leg][2] = z;
  623. }
  624.  
  625. /*
  626. - wait one of end points move to expect site
  627. - blocking function
  628. ---------------------------------------------------------------------------*/
  629. void wait_reach(int leg)
  630. {
  631. while (1)
  632. if (site_now[leg][0] == site_expect[leg][0])
  633. if (site_now[leg][1] == site_expect[leg][1])
  634. if (site_now[leg][2] == site_expect[leg][2])
  635. break;
  636. }
  637.  
  638. /*
  639. - wait all of end points move to expect site
  640. - blocking function
  641. ---------------------------------------------------------------------------*/
  642. void wait_all_reach(void)
  643. {
  644. for (int i = 0; i < 4; i++)
  645. wait_reach(i);
  646. }
  647.  
  648. /*
  649. - trans site from cartesian to polar
  650. - mathematical model 2/2
  651. ---------------------------------------------------------------------------*/
  652. void cartesian_to_polar(volatile float &alpha, volatile float &beta, volatile float &gamma, volatile float x, volatile float y, volatile float z)
  653. {
  654. //calculate w-z degree
  655. float v, w;
  656. w = (x >= 0 ? 1 : -1) * (sqrt(pow(x, 2) + pow(y, 2)));
  657. v = w - length_c;
  658. alpha = atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2)));
  659. beta = acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) / 2 / length_a / length_b);
  660. //calculate x-y-z degree
  661. gamma = (w >= 0) ? atan2(y, x) : atan2(-y, -x);
  662. //trans degree pi->180
  663. alpha = alpha / pi * 180;
  664. beta = beta / pi * 180;
  665. gamma = gamma / pi * 180;
  666. }
  667.  
  668. /*
  669. - trans site from polar to microservos
  670. - mathematical model map to fact
  671. - the errors saved in eeprom will be add
  672. ---------------------------------------------------------------------------*/
  673. void polar_to_servo(int leg, float alpha, float beta, float gamma)
  674. {
  675. if (leg == 0)
  676. {
  677. alpha = 90 - alpha;
  678. beta = beta;
  679. gamma += 90;
  680. }
  681. else if (leg == 1)
  682. {
  683. alpha += 90;
  684. beta = 180 - beta;
  685. gamma = 90 - gamma;
  686. }
  687. else if (leg == 2)
  688. {
  689. alpha += 90;
  690. beta = 180 - beta;
  691. gamma = 90 - gamma;
  692. }
  693. else if (leg == 3)
  694. {
  695. alpha = 90 - alpha;
  696. beta = beta;
  697. gamma += 90;
  698. }
  699.  
  700. servo[leg][0].write(alpha);
  701. servo[leg][1].write(beta);
  702. servo[leg][2].write(gamma);
  703. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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!