API tools faq
paste
Advertisement
partti

Läby väistöllä v2.1 ei toimi

partti
Dec 12th, 2019
142
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 9.32 KB | None | 0 0
raw download clone embed print report
  1. #if 1
  2. // Labyrintti 1.2 väistöllä ja osittain toimii.
  3. void zmain(void)
  4. {
  5. TickType_t start; // Aloita ajanlasku
  6. start = xTaskGetTickCount();
  7. IR_Start();
  8. Ultra_Start();
  9.  
  10. struct sensors_ ref; // Sensorit päälle
  11. struct sensors_ dig;
  12. reflectance_start();
  13. reflectance_set_threshold(10000, 10000, 12000, 12000, 10000, 10000);
  14.  
  15. motor_start(); // Käynnistetään moottori
  16. motor_forward(0,0);
  17.  
  18. int x = 0; // Tarvittavat muuttujat
  19. int y = -1;
  20. int este = 0;
  21. int nappi = 0;
  22. int nappi1 = 0;
  23. int kytkin = 0;
  24. int risteys = 0;
  25. int risteysp= 0;
  26. int valiaika;
  27. int risteyslupa = 0;
  28. int suuntaluku = 400;
  29. int suunta; // 0 = ylös, 1 = oikea, 2 = alas, 3 = vasen,
  30. int d = Ultra_GetDistance();
  31.  
  32. while(nappi1 == 0) {
  33. vTaskDelay(10);
  34. if(SW1_Read() == 0){ // Liikkeellelähtö napista
  35. printf("nappi painettu, lahen menee \n");
  36. nappi = 1;
  37. nappi1 = 1;
  38. }
  39. }
  40.  
  41. while(nappi == 1 && kytkin == 0){ // Odotus ensimmäisellä viivalla kunnes IR
  42. reflectance_digital(&dig);
  43. printf("ajossa\n");
  44. motor_forward(50, 10);
  45. if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 1) {
  46. motor_forward(0,0);
  47.  
  48. printf("olen pauses\n"); //send_mqtt(ready)
  49. IR_flush();
  50. IR_wait();
  51. printf("nappia painettu\n");
  52. valiaika = xTaskGetTickCount();
  53. printf("%d\n", valiaika); //stoptime print
  54. motor_forward(60, 300);
  55. kytkin = 1;
  56. }
  57. }
  58. while(kytkin == 1 && nappi == 1) {
  59. suunta = suuntaluku %4;
  60. printf("eka suuntaluku: %d\n", suuntaluku);
  61. int arvo = 0;
  62.  
  63. while(kytkin == 1 && nappi == 1 && risteys == 0) { // Eteenpäin ajo, kunnes viiva.
  64.  
  65. reflectance_digital(&dig);
  66.  
  67. if ((dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 1) ||(dig.l1 == 1 && dig.l2 == 1 && dig.l3 == 1)) {
  68. motor_forward(0, 10); // Risteykseen ajo
  69. d = Ultra_GetDistance();
  70.  
  71. while(dig.l2 == 1 && dig.r2 == 1){
  72. reflectance_digital(&dig);
  73. motor_forward(60, 10);
  74. }
  75. motor_forward(60, 50);
  76. if(suunta == 0){
  77. y++;
  78. }
  79. else if(suunta == 1){
  80. x++;
  81. }
  82. else if(suunta == 2){
  83. y--;
  84. }
  85. else if(suunta == 3){
  86. x--;
  87. }
  88. printf("X: %d, Y: %d\n", x, y);
  89. Beep(100, 100);BatteryLed_Write(1);
  90. risteys = 1;
  91. }
  92.  
  93. else if(dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 0 && dig.r3 == 0) {
  94. motor_forward(50, 10);
  95. }
  96.  
  97. else if(dig.l2 == 1) {
  98. motor_turn(10, 60, 10); // Korjaus vasemmalle
  99. }
  100.  
  101. else if(dig.r2 == 1) {
  102. motor_turn(60, 10, 10); // Korjaus oikealle
  103. }
  104. }
  105.  
  106. while((kytkin == 1 && nappi == 1 && risteys == 1 && y<12 && d<16 && suunta == 0 && x != -3 && este ==0) || ( x == 0 && suunta == 1 && risteys == 1 && este ==0 )) { // Risteys vasemalle || (x == 0 && suunta == 1 && kytkin == 1 && nappi == 1 && risteys == 1) || (d<11 && kytkin == 1 && nappi == 1 && risteys == 1)
  107.  
  108. reflectance_digital(&dig);
  109.  
  110. if(arvo == 0){
  111. motor_hardturnleft(60, 90, 300);
  112. printf("Vasemmalle eka if\n");
  113. arvo = 1;
  114. }
  115. else if (dig.r1 == 0 || dig.l3 == 1 || dig.l2 == 1){
  116. motor_hardturnleft(60, 90, 1);
  117. printf("Vasemmalle toka if\n");
  118.  
  119. }
  120. else if (dig.r1 == 1){
  121. risteys = 0;
  122. suuntaluku--;
  123. d = Ultra_GetDistance();
  124. printf("Vasemmalle valmis: %d, %d\n", suuntaluku, suunta);
  125. if(d<16){
  126. // print_mqtt("Zumo046/info","este menee");
  127. este = 1;
  128. risteys = 1;
  129. arvo = 0; //lisäsin arvot tähän ja toho alempaa samaa kohtaa. nyt alempaa whileen kans
  130. }
  131. }
  132. }
  133. while((kytkin == 1 && nappi == 1 && risteys == 1 && x == -3 && suunta == 3 && este ==0 )|| (d<16 && suunta == 3 && risteys == 1 && este ==0 ) || (risteys == 1 && x == -3 && d<16 && este ==0 ) || (risteys ==1 && suunta ==0 && x<0 && y>=11 && este ==0)) { // Risteys oikealle
  134. //oikeelle
  135. reflectance_digital(&dig);
  136. if (arvo == 0){
  137. motor_hardturnright(90, 60, 300);
  138. arvo = 1;
  139. }
  140. else if (dig.l1 == 0 || dig.r3 == 1 || dig.r2 == 1){
  141. motor_hardturnright(90, 60, 1);
  142. }
  143. else if ((dig.l1 == 1) || (dig.l1 == 1 && risteys ==0) ){
  144. risteys = 0;
  145. suuntaluku++;
  146. d = Ultra_GetDistance();
  147. // print_mqtt("Zumo046/info","oikeelle valmis: %d, %d\n", suuntaluku, suunta);
  148.  
  149. if(d<16){
  150. // print_mqtt("Zumo046/info","este havaittu oikealle kaantumis \n ");
  151. risteys = 1;
  152. arvo=0;
  153. este = 1;
  154.  
  155. }
  156. }
  157.  
  158. }
  159. while (este ==1){
  160. reflectance_digital(&dig);
  161. suunta = suuntaluku %4;
  162. //print_mqtt("Zumo046/info","suunta %d\n",suunta);
  163.  
  164. if(risteys ==1 && risteysp==0 ){ // takas ympäri
  165. reflectance_digital(&dig);
  166. if (arvo == 0){
  167. motor_backward(60,100 );
  168. motor_hardturnright(100, 100, 400);
  169. arvo = 1;
  170. }
  171. else if (dig.l1 == 0 || dig.r3 == 1 || dig.r2 == 1){
  172. motor_hardturnright(90, 60, 10);
  173. }
  174. else if (dig.l1 == 1){
  175. suuntaluku++;
  176. --arvo;
  177. risteysp = 1 ;
  178. risteys = 0;
  179. --arvo;
  180. // print_mqtt("Zumo046/info","oikeamalle poikkeus kannos valmis: %d, %d\n", suuntaluku, suunta);
  181.  
  182. }
  183.  
  184. }
  185.  
  186. reflectance_digital(&dig);
  187.  
  188. if ((dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 1) ||(dig.l1 == 1 && dig.l2 == 1 && dig.l3 == 1)) {
  189. motor_forward(0, 10); // cord luku
  190.  
  191.  
  192. while(dig.l2 == 1 && dig.r2 == 1){
  193. reflectance_digital(&dig);
  194. motor_forward(60, 10);
  195.  
  196. }
  197. motor_forward(60, 50);
  198. if(suunta == 0){
  199. y++;
  200. }
  201. else if(suunta == 1){
  202. x++;
  203. }
  204. else if(suunta == 2){
  205. y--;
  206. }
  207. else if(suunta == 3){
  208. x--;
  209. }
  210. printf("X: %d, Y: %d\n", x, y);
  211. Beep(200, 100);BatteryLed_Write(1);
  212. risteys = 1;
  213. vTaskDelay(100);
  214. }
  215. else if(dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 0 && dig.r3 == 0) {
  216. motor_forward(50, 10);
  217. }
  218.  
  219. else if(dig.l2 == 1) {
  220. motor_turn(10, 60, 10); // Korjaus vasemmalle
  221. }
  222.  
  223. else if(dig.r2 == 1) {
  224. motor_turn(60, 10, 10); // Korjaus oikealle
  225. }
  226. else if(risteysp== 1 && risteys==1){
  227. motor_forward(0,0);
  228. if(arvo==0){
  229. motor_hardturnleft(60, 90, 300);
  230. // print_mqtt("Zumo046/info","Vasemmalle eka if");
  231. arvo = 1;
  232. }
  233. else if (dig.r1 == 0 || dig.l3 == 1 || dig.l2 == 1){
  234. motor_hardturnleft(60, 90, 1);
  235. printf("Vasemmalle toka if");
  236. }
  237. else if (dig.r1 == 1){
  238. risteys = 0;
  239. suuntaluku--;
  240. d = Ultra_GetDistance();
  241. // print_mqtt("Zumo046/info","Vasemmalle valmis: %d, %d\n", suuntaluku, suunta);
  242. --arvo;
  243. --risteysp;
  244. if(d<16){
  245. este = 1;
  246. risteys = 1;
  247. arvo =0;
  248. }
  249. else if(d>16){
  250. este = 0;
  251. // print_mqtt("Zumo046/info","este ohitettu este =0");
  252. }
  253.  
  254. }
  255. }
  256. motor_forward(60, 100);
  257. }
  258. risteys = 0;
  259. motor_forward(60, 100);
  260. }
  261. }
  262. #endif
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!