fork(1) download
copy 
  1. #pragma config(Sensor, S1,     soundSensor,    sensorSoundDB)
  2. #pragma config(Sensor, S2,     touchSensor,    sensorTouch)
  3. #pragma config(Sensor, S3,     sonarSensor,    sensorSONAR)
  4. #pragma config(Sensor, S4,     lightSensor,    sensorLightActive)
  5. /* Programul in limbajul NXC
  6. /***********************************************************************************************
  7. Prin acest program se pozitioneaza bratul de robot NXT, format din doua segmente rigide
  8. de motoare NXT, intr-un punct din planul de lucru. In acest program demonstrativ se
  9. bratul in cadranul I.
  10. Inainte de a rula programul, trebuie sa se stabileasca orientarea fizica a celor doua
  11. axe ale planului de lucru si sa se pozitioneze manual bratul in pozitia initiala
  12. stabilita: cu cele doua segmente in prelungire, de-a lungul axei OX.
  13. Nota: Compilatorul NXC folosit de BricxCC poate sa nu "inteleaga" expresiile
  14. complexe semnaland o eroare la compilare: "error parsing expression". Din acest
  15. motiv e nevoie ca astfel de expresii sa se descompuna in expresii simple si sa se
  16. faca in mai multi pasi.*/
  17. /**************************************************************************************/
  18. // Constante
  19. /*************************************************************************************/
  20. // Valoarea de raportare a marimilor de intrare.
  21. // Trebuie sa fie identica cu cea introdusa in fisierul in care s-au scalat coeficientii
  22. #include <math.h>
  23. #define DIM_MAX 20 // [cm]
  24. // Coeficientii scalati
  25. #define C1 5461
  26. #define C2 27306
  27. #define C3 8874
  28. #define C4 17066
  29. #define SH_C1C2 0
  30. #define SH_C3C4 2
  31. //#define COEF_C1C2 1
  32. //#define COEF_C3C4 3
  33. // Macro pentru scalarea variabilelor de intrare
  34. #define Ks (32767/DIM_MAX)
  35. #define SCAL(x) (x*Ks)
  36. // "Viteza" stabilita pentru rotirea motoarelor
  37. #define pwr 20
  38. /**********************************************************************************************/
  39. // Macrouri pentru multiplicare si impartire in C, pentru formatul Q15
  40. /**********************************************************************************************/
  41. #define MUL(a,b) ((a*b)>>15)
  42. #define DIV(a,b) ((a<<15)/b)
  43. // Macro pt trecerea din fractionar Q15 in procente, folosit pentru argumentele functiei acos din NXC
  44. /* cu rotunjire, tinand cont de semn */
  45.  
  46. int sign(int a) {
  47.     if (a > 0)
  48.         return 1;
  49.     else if (a < 0)
  50.         return -1;
  51.     else
  52.         return 0;
  53. }
  54.  
  55. #define SCALARE_100(a) (((a*100)>>14)+1*sign(a))>>1
  56. // Macro-ul pt "breakpoint", realizat in lucrarea 1.
  57. #define BREAKPOINT_AFISARE_VAR(string_x,x){\
  58. TextOut(0,LCD_LINE1,string_x,true);\
  59. NumOut(0,LCD_LINE2,x);\
  60. Wait(1000);\
  61. while(ButtonPressed(BTNCENTER,true)==0)\
  62. {\
  63. }\
  64. }
  65. short x_real, y_real; // coordonatele reale, in aceeasi unitate de masura
  66. // folosita pentru DIM_MAX
  67. short x, y; // coordonatele scalate
  68. short r; // modulul vectorul de pozitie al punctului tinta
  69. short r_patrat; // r la patrat
  70. // Unghiurile, calculate cu functia acos din NXC rezulta in [grade].
  71. // Tot in grade se vor introduce si unghiurile corespunzatoare pozitiei initiale
  72. short arg1, arg2; // argumente ale functiei Acos, pt calculul
  73. // unghiurilor gama si theta2
  74. short theta1, theta2; // unghiurile celor doua segmente ale bratului
  75. short theta1_ant, theta2_ant; // valorile unghiurlor coresp. poz.ant.
  76. short D_theta1, D_theta2; // unghiurile cu care tb. rotite motoarele
  77. // pentru obtinerea pozitiei
  78. short theta, gama; // unghiuri auxiliare
  79. short temp; // variabila pentru calcule intermediare
  80. long temp_long; // variabila pentru calcule intermediare
  81. int main()
  82. {
  83. // Initializare unghiuri coresp. pozitiei initiale a bratului
  84. theta1_ant = 0; // [grade]
  85. theta2_ant = 180; // [grade]
  86. // Variabilele de intrare: coordonatele dorite, in [cm]
  87. x_real = 14;
  88. y_real = 0;
  89. // Scalare variabile de intrare
  90. x = SCAL(x_real);
  91. y = SCAL(y_real);
  92. // r^2 = x^2 + y^2.
  93. // Variabila r^2 se pastreaza intr-o variabila, pt ca se refolosi in program.
  94. r_patrat = MUL(x,x) + MUL(y,y);
  95. temp_long = r_patrat << 15;
  96. r = (sqrt(temp_long));
  97. // Calcul theta = arccos(x/r). Inainte de impartirea x/r se testeaza daca x>=r.
  98. // Aceasta deoarece numaratorul trebuie sa fie mai mic decat numitorul, pt a nu
  99. // avea depasire in formatul Q15.
  100. if (x>=r) // daca x>=r
  101. temp = 32767; // rezultatul se satureaza la ~1, ie 32767
  102. else
  103. temp = DIV(x,r); // altfel, se face impartirea
  104. temp = SCALARE_100(temp); // scalare in procente
  105. theta = acos(temp);
  106. // BREAKPOINT_AFISARE_VAR("theta= ",theta); //pt. debug
  107. // Calcul argument arg1 = C1/r + C2*r (pentru unghiul gama)
  108. // Ca sa evitam depasirea la impartire rearanjam expresia astfel:
  109. /* arg1 = (C1+C2*r^2)/r, adica facem impartirea la sfarsit, stiind ca arg1 trebuie sa
  110. rezulte geometric, in intervalul [-1,1].*/
  111. temp = MUL(r,r);
  112. temp = MUL(C2,temp)+ C1;
  113. /* In situatia, posibila, in care numaratorul si numitorul ar fi egali, rezultatul ar trebui
  114. sa fie ~1. Facem deci acelasi test ca la impartirea anterioara:*/
  115. if (temp>=r) // daca x>=r
  116. temp = 32767; // rezultatul se satureaza la ~1, ie 32767
  117. else
  118. temp = DIV(temp,r); // altfel, se face impartirea
  119. arg1 = temp;
  120. /* Inmultirea directa a argumentului arg1 cu coeficientul COEF_C1C2 folosit
  121. la scalare. Daca COEF_C1C2 s-a ales ca putere a lui 2, se face deplasare (shift) la
  122. stanga, in loc de inmultire directa */
  123. arg1 = arg1 << SH_C1C2;
  124. //arg1 = arg1 * COEF_C1C2;
  125. // Calcul unghi gama:
  126. arg1 = SCALARE_100(arg1); // scalare in procente (arg
  127. // functiei Acos din NXC)
  128. gama = acos(arg1);
  129. // BREAKPOINT_AFISARE_VAR("gama= ",gama); // pt. debug
  130. // Calcul arg2 = C3 - C4*r^2, pt unghiul theta2:
  131. temp = MUL(r,r);
  132. arg2 = C3 - MUL(C4,temp);
  133. arg2 = arg2 << SH_C3C4;
  134. // arg2 = arg2 * COEF_C3C4;
  135. // Calcul theta2:
  136. arg2 = SCALARE_100(arg2); // scalare in procente (arg
  137. // functiei Acos din NXC)
  138. theta2 = acos(arg2);
  139. // BREAKPOINT_AFISARE_VAR("theta2= ",theta2); // pt. debug
  140. // Calcul theta1 = gama + theta:
  141. theta1 = gama + theta;
  142. }
  143. // BREAKPOINT_AFISARE_VAR("theta1= ",theta1); // pt. debug
  144. // Calcul unghiurilor cu care trebuie rotite motoarele, fata de pozitia anterioara:
  145. //D_theta1 = theta1 - theta1_ant;
  146. //D_theta2 = theta2 - theta2_ant;
  147.  
  148.  
Success #stdin #stdout 0s 5276KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
Standard output is empty
https://ideone.com/pb88Sw
language:
C (gcc 8.3)
created:
3 days ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!