API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Mar 19th, 2019
72
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 8.52 KB | None | 0 0
raw download clone embed print report
  1. /*!
  2. * \file ctrl_main_gr2.cc
  3. * \brief Initialization, loop and finilization of the controller written in C (but compiled as C++)
  4. */
  5.  
  6. #include "ctrl_main_gr6.h"
  7. #include "namespace_ctrl.h"
  8. #include <stdio.h>
  9. #include "math.h"
  10. #include <stdlib.h>
  11. #include <string.h>
  12. #include "set_output.h"
  13.  
  14.  
  15. NAMESPACE_INIT(ctrlGr6);
  16.  
  17. /*! \brief initialize controller operations (called once)
  18. *
  19. * \param[in] cvs controller main structure
  20. */
  21. void controller_init(CtrlStruct *cvs)
  22. {
  23. cvs->l_sum = 0.0; // Somme integrale roue gauche ==> Controleur PI roues
  24. cvs->r_sum = 0.0; // Somme integrale roue droite ==> Controleur PI roues
  25. cvs->t_sum = 0.0; // Somme integrale tourelle ==> Controleur PI tourelle
  26. cvs->pos[0] = 0.0; // Position X odometrie
  27. cvs->pos[1] = 0.0; // Position Y odometrie
  28. cvs->postri[0] = 0.0; // Position X triangulation
  29. cvs->postri[1] = 0.0; // Position Y triangulation
  30. cvs->theta = 0.0; // Angle entre robot et inertiel
  31. cvs->oldang = 0.0; // Valeur de l'angle de la tourelle à l'iteration precedente ==> Controleur PI tourelle
  32. cvs->i1 = 1; // Indice pilone initialisation
  33. cvs->j1 = 0; // Indice position initialisation
  34. cvs->i2 = 1; // Indice pilone
  35. cvs->j2 = 0; // Indice position
  36. cvs->x_beam[0] = 0.0; //
  37. cvs->x_beam[1] = 0.0; //
  38. cvs->x_beam[2] = 0.0; // Coordonnees relatives aux 3 pilones
  39. cvs->y_beam[0] = 0.0; //
  40. cvs->y_beam[1] = 0.0; //
  41. cvs->y_beam[2] = 0.0; //
  42. }
  43.  
  44. /*! \brief controller loop (called every timestep)
  45. *
  46. * \param[in] cvs controller main structure
  47. */
  48. void controller_loop(CtrlStruct *cvs)
  49. {
  50.  
  51. // INITIALISATION DES VARIABLES
  52. double time = cvs->inputs->t; // Temps de la simulation
  53. double Omtow; // Vitesse de rotation de la tourelle (rad/s) ==> Controleur PI tourelle
  54. double V; // Vitesse d'avance du robot
  55. double Ki = 25.95; // Constante de l'intégrateur du PI des roues ==> Controleur PI roues
  56. double Kp = 2.72; // Constante du proportionnel du PI des roues ==> Controleur PI roues
  57. double Om[2] = { 0 , 0 }; // Commande de vitesse de rotation des roues (rad/s)
  58. double tow = 20.0; // Commande de vitesse de rotation de la tourelle (rad/s) !!! PAS OK !!!
  59. double delta_t = 0.001; // Pas de temps de la boucle
  60. double r = 0.03; // Rayon des roues ==> Odometrie
  61. double l = 0.225; // Distance entre les deux roues ==> Odometrie
  62. double realang; // Angle detecte dans le repere inertiel
  63. double tta; // Angle mesure entre la tourelle et un pilone ==> Triangulation
  64. double d; // Distance mesuree entre la tourelle et un pilone ==> Triangulation
  65. double trans12[2] = { 1.9 , 0.0 }; // Distance fixe entre les pilones 1 et 2 ==> Triangulation
  66. double trans13[2] = { 1.0 , 3.0 }; // Distance fixe entre les pilones 1 et 3 ==> Triangulation
  67.  
  68.  
  69. // CALCUL POSITION INITIALE
  70. if (time<=-10.0 && time>=-15)
  71. {
  72. // VITESSE NULLE IMPOSEE AUX ROUES
  73. Om[0] = 0.0;
  74. Om[1] = 0.0;
  75. cvs->outputs->tower_command = tow;
  76. if (cvs->inputs->falling_index_fixed == cvs->inputs->rising_index_fixed && cvs->inputs->falling_index_fixed != cvs->i1)
  77. {
  78. // CALCUL DE LA DISTANCE ET DE L'ANGLE RELATIF AU PILONE SUIVANT
  79.  
  80. tta = (cvs->inputs->last_rising_fixed[cvs->inputs->falling_index_fixed] + cvs->inputs->last_falling_fixed[cvs->inputs->falling_index_fixed]) / 2.0;
  81. d = 0.04 / (sin((cvs->inputs->last_falling_fixed[cvs->inputs->falling_index_fixed] - cvs->inputs->last_rising_fixed[cvs->inputs->falling_index_fixed]) / 2.0));
  82.  
  83. // ON RAMENE L'ANGLE AU REPERE INERTIEL
  84.  
  85. tta = tta + cvs->theta;
  86. if (tta > M_PI)
  87. {
  88. tta = tta - 2 * M_PI;
  89. }
  90. else if (tta <= -M_PI)
  91. {
  92. tta = tta + 2 * M_PI;
  93. }
  94. cvs->i1 = cvs->inputs->falling_index_fixed;
  95. cvs->x_beam[cvs->j1] = -d * sin(tta);
  96. cvs->y_beam[cvs->j1] = -d * cos(tta) - 0.083;
  97. cvs->j1 = cvs->j1 + 1;
  98.  
  99. // MISE A JOUR DE LA POSITION SUR BASE DES 3 POSITIONS CALCULEES A L'AIDE DES 3 PILONES
  100.  
  101. if (cvs->j1 >= 3)
  102. {
  103. cvs->j1 = 0.0;
  104. cvs->postri[0] = (cvs->x_beam[0] + cvs->x_beam[1] + trans12[0] + cvs->x_beam[2] + trans13[0]) / 3.0;
  105. cvs->postri[1] = (cvs->y_beam[0] + cvs->y_beam[1] + trans12[1] + cvs->y_beam[2] + trans13[1]) / 3.0;
  106. cvs->pos[0] = (cvs->x_beam[0] + cvs->x_beam[1] + trans12[0] + cvs->x_beam[2] + trans13[0]) / 3.0;
  107. cvs->pos[1] = (cvs->y_beam[0] + cvs->y_beam[1] + trans12[1] + cvs->y_beam[2] + trans13[1]) / 3.0;
  108.  
  109. }
  110. }
  111. cvs->finalpos[0] = (cvs->pos[0] + cvs->postri[0]) / 2.0;
  112. cvs->finalpos[1] = (cvs->pos[1] + cvs->postri[1]) / 2.0;
  113. }
  114. else
  115. {
  116. Om[0] = 4.0;
  117. Om[1] = 4.0;
  118.  
  119. // CONTROLEUR DE VITESSE PI
  120.  
  121. cvs->r_sum = cvs->r_sum + (Om[0] - cvs->inputs->r_wheel_speed) * delta_t;
  122. cvs->l_sum = cvs->l_sum + (Om[1] - cvs->inputs->l_wheel_speed) * delta_t;
  123. cvs->outputs->wheel_commands[0] =( Kp * (Om[0] - cvs->inputs->r_wheel_speed) + Ki * cvs->r_sum) * 100 / (24 * 0.9);
  124. cvs->outputs->wheel_commands[1] =( Kp * (Om[1] - cvs->inputs->l_wheel_speed) + Ki * cvs->l_sum) * 100 / (24 * 0.9);
  125.  
  126. // ODOMETRIE
  127.  
  128. V = (cvs->inputs->r_wheel_speed + cvs->inputs->l_wheel_speed) * r / 2.0;
  129. cvs->theta = cvs->theta + ((cvs->inputs->r_wheel_speed * r - cvs->inputs->l_wheel_speed * r) / l) * delta_t;
  130. cvs->pos[0] = cvs->pos[0] + (V * sin(cvs->theta)) * delta_t;
  131. cvs->pos[1] = cvs->pos[1] + (V * cos(cvs->theta)) * delta_t;
  132.  
  133. // TRIANGULATION
  134.  
  135. cvs->outputs->tower_command = tow;
  136. if (cvs->inputs->falling_index_fixed == cvs->inputs->rising_index_fixed && cvs->inputs->falling_index_fixed != cvs->i2)
  137. {
  138. // CALCUL DE LA DISTANCE ET DE L'ANGLE RELATIF AU PILONE SUIVANT
  139.  
  140. tta = (cvs->inputs->last_rising_fixed[cvs->inputs->falling_index_fixed] + cvs->inputs->last_falling_fixed[cvs->inputs->falling_index_fixed]) / 2.0;
  141. d = 0.04 / (sin((cvs->inputs->last_falling_fixed[cvs->inputs->falling_index_fixed] - cvs->inputs->last_rising_fixed[cvs->inputs->falling_index_fixed]) / 2.0));
  142.  
  143. // ON RAMENE L'ANGLE AU REPERE INERTIEL
  144.  
  145. tta = tta + cvs->theta;
  146. if (tta > M_PI)
  147. {
  148. tta = tta - 2 * M_PI;
  149. }
  150. else if(tta <= -M_PI)
  151. {
  152. tta = tta + 2 * M_PI;
  153. }
  154. cvs->i2 = cvs->inputs->falling_index_fixed;
  155. cvs->x_beam[cvs->j2] = -d * sin(tta) ;
  156. cvs->y_beam[cvs->j2] = -d * cos(tta) - 0.083 ;
  157. cvs->j2 = cvs->j2 + 1;
  158.  
  159. // MISE A JOUR DE LA POSITION SUR BASE DES 3 POSITIONS CALCULEES A L'AIDE DES 3 PILONES
  160.  
  161. if (cvs->j2 >= 3)
  162. {
  163. cvs->j2 = 0.0;
  164. cvs->postri[0] = (cvs->x_beam[0] + cvs->x_beam[1] + trans12[0] + cvs->x_beam[2] + trans13[0]) / 3.0;
  165. cvs->postri[1] = (cvs->y_beam[0] + cvs->y_beam[1] + trans12[1] + cvs->y_beam[2] + trans13[1]) / 3.0;
  166. }
  167. }
  168.  
  169. cvs->finalpos[0] = (cvs->pos[0] + cvs->postri[0]) / 2.0;
  170. cvs->finalpos[1] = (cvs->pos[1] + cvs->postri[1]) / 2.0;
  171.  
  172. }
  173. //printf("%f \n",cvs->inputs->target_detected);
  174. set_output(cvs->finalpos[0], "X");
  175. set_output(cvs->finalpos[1], "Y");
  176.  
  177. // CONTROLEUR PI TOURELLE
  178.  
  179. /*
  180. Omtow = (cvs->inputs->tower_pos - cvs->oldang) / delta_t;
  181. cvs->t_sum = cvs->t_sum + (tow - Omtow) * delta_t;
  182. cvs->outputs->tower_command = (1 * (tow - Omtow) + 1 * cvs->t_sum);
  183. cvs->oldang = cvs->inputs->tower_pos;
  184. */
  185.  
  186. // ECRITURE FICHIER
  187.  
  188. /*
  189.  
  190. if (time >= -15.0 && time <= 10.0)
  191. {
  192. char files[30];
  193. char buffer[10];
  194. strcpy(files, "x");
  195. FILE* fichier1 = NULL;
  196. fichier1 = fopen(files, "a");
  197. // On peut lire et écrire dans le fichier
  198. //fwrite(5.0, sizeof(double), 1, fichier);
  199. fprintf(fichier1, "%f", cvs->pos[0]);
  200. fclose(fichier1);
  201.  
  202. //char files[30];
  203. //char buffer[10];
  204. strcpy(files, "y");
  205. FILE* fichier2 = NULL;
  206. fichier2 = fopen(files, "a");
  207. // On peut lire et écrire dans le fichier
  208. //fwrite(5.0, sizeof(double), 1, fichier);
  209. fprintf(fichier2, "%f", cvs->pos[1]);
  210. fclose(fichier2);
  211.  
  212. }*/
  213. }
  214.  
  215. /*! \brief last controller operations (called once)
  216. *
  217. * \param[in] cvs controller main structure
  218. */
  219. void controller_finish(CtrlStruct *cvs)
  220. {
  221.  
  222. }
  223.  
  224. void rot(CtrlStruct *cvs, double Om[2], double side)
  225. {
  226. if (side == 0)
  227. {
  228. Om[0] = 2.0;
  229. Om[1] = 6.0;
  230. }
  231. if (side == 1)
  232. {
  233. Om[0] = 6.0;
  234. Om[1] = 2.0;
  235. }
  236. else
  237. {
  238. Om[0] = 0.0;
  239. Om[0] = 0.0;
  240. }
  241. }
  242.  
  243. void fwd(CtrlStruct *cvs, double Om[2], double spd)
  244. {
  245. Om[0] = spd;
  246. Om[1] = spd;
  247. }
  248. NAMESPACE_CLOSE();
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!