API tools faq
paste
Guest User

Program

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