Advertisement
Guest User

Untitled

a guest
Oct 14th, 2019
120
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 20.33 KB | None | 0 0
  1. #include <MeAurigaLab.h> // Contains sensor and port handler
  2. #include "scheduler.h" // Contains FreeRTOS and ESFree
  3. #include <Wire.h>
  4. #include "queue.h"
  5. #include "semphr.h"
  6.  
  7. MeEncoderOnBoard Encoder_1(SLOT1); //Knyter motor encodern
  8. MeEncoderOnBoard Encoder_2(SLOT2); // till rätt motor kontakt
  9.  
  10.  
  11. //Pekare till tasks
  12. TaskHandle_t xHandle1 = NULL; // varibles containing pointers
  13. TaskHandle_t xHandle2 = NULL; // to tasks
  14. TaskHandle_t xHandle3 = NULL; // to tasks
  15. TaskHandle_t xHandle4 = NULL; // to tasks
  16. TaskHandle_t xHandle5 = NULL; // to tasks
  17. TaskHandle_t xHandle6 = NULL; // to tasks
  18. TaskHandle_t xHandle7 = NULL; // to tasks
  19. TaskHandle_t xHandle8 = NULL; // to tasks
  20.  
  21. //Global linesensor
  22. int sensorState;
  23. //Global stop bool says if ulta sensor is within range or not
  24. bool stay = false;
  25. //The range for to close
  26. const int AvoidDist = 25;
  27.  
  28. /*const int ult = 200;
  29. const int high = 90;
  30. const int low = 60;
  31. const int off = 0;*/
  32.  
  33. int speedLeft;
  34. int speedRight;
  35.  
  36. //Globala lägeskontroller
  37. int count = 0;
  38. //Flaga som säger om rob ska backa eller köra används när linjen ska hittas
  39. int flag = 0;
  40. //Tells task to perform linefollow task
  41. bool go = false;
  42. //Tells task to avoid current object
  43. bool avoid=false;
  44. //Strike to tell if programm needs to find line once again
  45. int strike=0;
  46.  
  47.  
  48. //flag sem
  49. SemaphoreHandle_t FlagSem;
  50. //Turnflag
  51. SemaphoreHandle_t TurnSem;
  52. //Sem that hold linefinder task
  53. SemaphoreHandle_t FindSem;
  54. //Sem that holds linefollower task
  55. SemaphoreHandle_t GoSem;
  56. //For motor speed task
  57. SemaphoreHandle_t speedSem;
  58. //For line sensor task
  59. SemaphoreHandle_t LineSem;
  60. //For ultra sonic task
  61. SemaphoreHandle_t UltSonSem;
  62. //For count
  63. SemaphoreHandle_t CountSem;
  64. //For avoid
  65. SemaphoreHandle_t AvoidSem;
  66. //For strike count
  67. SemaphoreHandle_t StrikeSem;
  68.  
  69. char c1 = 'a';
  70. char c2 = 'b';
  71. char c3 = 'c';
  72. char c4 = 'd';
  73. char c5 = 'e';
  74. char c6 = 'f';
  75. char c7 = 'g';
  76. char c8 = 'h';
  77.  
  78. /*Interupt processer som behövs för att hantera positioner i motorerna
  79. använd alltid dessa när motorerna används */
  80. void isr_process_encoder1(void)
  81. {
  82. if (digitalRead(Encoder_1.getPortB()) == 0)
  83. Encoder_1.pulsePosMinus();
  84. else
  85. Encoder_1.pulsePosPlus();
  86. }
  87.  
  88. void isr_process_encoder2(void)
  89. {
  90. if (digitalRead(Encoder_2.getPortB()) == 0)
  91. Encoder_2.pulsePosMinus();
  92. else
  93. Encoder_2.pulsePosPlus();
  94. }
  95.  
  96. //Sensor ports
  97. MeLineFollower lineFinder(PORT_6);//Linefollower
  98. MeUltrasonicSensor ultraSensor(PORT_7);//Obstecle sensor
  99.  
  100. //***********************************************************************************************************************************
  101. //***********************************************************************************************************************************
  102. //Strike count task
  103. void StrikeCount(void *pvParameters)
  104. {
  105. if (xSemaphoreTake(StrikeSem, 0) == pdTRUE)
  106. {
  107. switch(sensorState)
  108. {
  109. //Strike gets +1 if outside of line for to long
  110. case S1_OUT_S2_OUT:
  111. strike++;
  112. break;
  113. //Strike resets if black line is found again
  114. default:
  115. strike=0;
  116. break;
  117. }
  118. xSemaphoreGive(StrikeSem);
  119. }
  120. }
  121.  
  122. //***********************************************************************************************************************************
  123. //***********************************************************************************************************************************
  124. //Store Linesensor task
  125. void StoreLineSensor(void *pvParameters)
  126. {
  127. if (xSemaphoreTake(LineSem, 0) == pdTRUE)
  128. {
  129. sensorState = lineFinder.readSensors();
  130. xSemaphoreGive(LineSem);
  131. }
  132. }
  133. //***********************************************************************************************************************************
  134. //***********************************************************************************************************************************
  135. //Store ultsensor task
  136. void StoreUltSensor(void *pvParameters)
  137. {
  138. if (xSemaphoreTake(UltSonSem, 0) == pdTRUE)
  139. {
  140. if (ultraSensor.distanceCm() <= AvoidDist)
  141. stay=true;
  142. else
  143. stay=false;
  144. xSemaphoreGive(UltSonSem);
  145. }
  146. }
  147. //***********************************************************************************************************************************
  148. //***********************************************************************************************************************************
  149. //Speed set task
  150. void TaskSetSpeed(void *pvParameters)
  151. {
  152. if (xSemaphoreTake(speedSem, 3) == pdTRUE)
  153. {
  154. Encoder_1.setMotorPwm(-speedRight);
  155. Encoder_2.setMotorPwm(speedLeft);
  156. xSemaphoreGive(speedSem);
  157. }
  158. }
  159. //***********************************************************************************************************************************
  160. //***********************************************************************************************************************************
  161. //Speed set task
  162. void TaskCount(void *pvParameters)
  163. {
  164. if(xSemaphoreTake(CountSem,3)== pdTRUE)
  165. {
  166. if(xSemaphoreTake(FlagSem,3)== pdTRUE)
  167. {
  168.  
  169. if(flag==0)
  170. count++;
  171. else
  172. count--;
  173. xSemaphoreGive(FlagSem);
  174. }
  175. xSemaphoreGive(CountSem);
  176. }
  177. }
  178. //***********************************************************************************************************************************
  179. //***********************************************************************************************************************************
  180. //Line follower task
  181. void TaskFollowLine(void *pvParameters)
  182. {
  183. if(xSemaphoreTake(GoSem,0)== pdTRUE)
  184. {
  185. //Flaga som säger hur roboten ska svänga när båda sensorna kommer ur bana
  186. static int turnflag;
  187. if (go)
  188. {
  189. count=0;
  190. if(!avoid)
  191. {
  192. if(xSemaphoreTake(StrikeSem, 5) == pdTRUE)
  193. {
  194. //Undvik om objekt förekommer
  195. if (stay)
  196. {
  197. go=false;
  198. avoid=true;
  199. }
  200. //If car lose line it will start linesearch algorithm again
  201. else if(strike>2000)
  202. {
  203. avoid=false;
  204. go=false;
  205. }
  206. else
  207. {
  208. switch (sensorState)
  209. {
  210.  
  211. //Drive straight when both sensors are inside of black line
  212. case S1_IN_S2_IN:
  213. if (xSemaphoreTake(speedSem, 3) == pdTRUE)
  214. {
  215. speedLeft= 120;
  216. speedRight= 120;
  217. turnflag=0;
  218.  
  219. xSemaphoreGive(speedSem);
  220. }
  221. break;
  222.  
  223. //Turn slightly to the right if sensor 2 is outside black line
  224. case S1_IN_S2_OUT:
  225. if (xSemaphoreTake(speedSem, 3) == pdTRUE)
  226. {
  227. speedLeft= -120;
  228. speedRight= 120;
  229. turnflag=1;
  230. xSemaphoreGive(speedSem);
  231. }
  232. break;
  233.  
  234. //Turn slightly to the left if sensor 1 is outside of black line
  235. case S1_OUT_S2_IN:
  236. if (xSemaphoreTake(speedSem, 3) == pdTRUE)
  237. {
  238. speedLeft= 120;
  239. speedRight= - 120;
  240. turnflag=2;
  241. xSemaphoreGive(speedSem);
  242. }
  243. break;
  244.  
  245. //Booth in white extra turn or back
  246. case S1_OUT_S2_OUT:
  247. switch (turnflag)
  248. {
  249. case 0:
  250. if (xSemaphoreTake(speedSem, 3) == pdTRUE)
  251. {
  252. speedLeft= - 90;
  253. speedRight= - 90;
  254. xSemaphoreGive(speedSem);
  255. }
  256. break;
  257.  
  258. case 1:
  259. if (xSemaphoreTake(speedSem, 3) == pdTRUE)
  260. {
  261. speedLeft= -160;
  262. speedRight= 160;
  263. xSemaphoreGive(speedSem);
  264. }
  265. break;
  266.  
  267. case 2:
  268. if (xSemaphoreTake(speedSem, 3) == pdTRUE)
  269. {
  270. speedLeft= 160;
  271. speedRight= - 160;
  272. xSemaphoreGive(speedSem);
  273. }
  274. break;
  275. default:break;
  276. }
  277. break;
  278.  
  279. default: break;
  280. }
  281. }
  282. xSemaphoreGive(StrikeSem);
  283. }
  284. }
  285. }
  286. xSemaphoreGive(GoSem);
  287. }
  288. }
  289. //***********************************************************************************************************************************
  290. //***********************************************************************************************************************************
  291. //Object avoidence task
  292. void TaskAvoidObject(void *pvParameters)
  293. {
  294. if(xSemaphoreTake(AvoidSem,0)== pdTRUE)
  295. {
  296. if(avoid)
  297. {
  298. if (xSemaphoreTake(StrikeSem, 5) == pdTRUE)
  299. {
  300. if (stay)
  301. {
  302. if (xSemaphoreTake(speedSem, 5) == pdTRUE)
  303. {
  304. speedLeft = -150;
  305. speedRight = 150;
  306. xSemaphoreGive(speedSem);
  307. }
  308. delay(800);
  309. }
  310. else if(strike>5000)
  311. {
  312. avoid=false;
  313. go=false;
  314. }
  315. else
  316. {
  317. switch (sensorState)
  318. {
  319. case S1_IN_S2_IN:
  320. if (xSemaphoreTake(speedSem, 5) == pdTRUE)
  321. {
  322. speedLeft = -180;
  323. speedRight = 180;
  324. xSemaphoreGive(speedSem);
  325. }
  326. delay(200);
  327.  
  328. go=true;
  329. avoid=false;
  330. break;
  331. default:
  332. if (xSemaphoreTake(speedSem, 5) == pdTRUE)
  333. {
  334. speedLeft = 150;
  335. speedRight = 90;
  336. xSemaphoreGive(speedSem);
  337. }
  338. break;
  339. }
  340. }
  341.  
  342. xSemaphoreGive(StrikeSem);
  343. }
  344. }
  345.  
  346. xSemaphoreGive(AvoidSem);
  347. }
  348. }
  349. //***********************************************************************************************************************************
  350. //***********************************************************************************************************************************
  351. //Find the black line task!
  352. void TaskFindLine(void *pvParameters)
  353. {
  354. if(xSemaphoreTake(FindSem,0)== pdTRUE)
  355. {
  356. if (!go)
  357. {
  358. if (stay)
  359. {
  360. //Stop and back up
  361. if(xSemaphoreTake(FlagSem,5)== pdTRUE)
  362. {
  363. flag = 1;
  364. xSemaphoreGive(FlagSem);
  365. }
  366.  
  367. //Start back up
  368. if (xSemaphoreTake(speedSem, 5) == pdTRUE)
  369. {
  370. speedLeft = -80;
  371. speedRight = -80;
  372. xSemaphoreGive(speedSem);
  373. }
  374. //start counting down
  375. }
  376. else
  377. {
  378. switch (sensorState)
  379. {
  380. //Stop when both sensors are inside of black line
  381. case S1_IN_S2_IN:
  382. //Turn slightly when line is found
  383. if (xSemaphoreTake(speedSem, 5) == pdTRUE)
  384. {
  385. speedLeft=-150;
  386. speedRight=150;
  387. xSemaphoreGive(speedSem);
  388. }
  389. //Set go bool to true
  390. go = true;
  391. delay(100);
  392. break;
  393.  
  394. //Turn slightly to the left if sensor 2 is outside black line
  395. case S1_IN_S2_OUT:
  396. if (xSemaphoreTake(speedSem, 5) == pdTRUE)
  397. {
  398. //Turn to the left
  399. speedLeft=-150;
  400. speedRight=150;
  401. xSemaphoreGive(speedSem);
  402. }
  403. go = true;
  404. delay(100);
  405. break;
  406.  
  407. //Turn slightly to the left if sensor 1 is outside of black line
  408. case S1_OUT_S2_IN:
  409. if (xSemaphoreTake(speedSem, 5) == pdTRUE)
  410. {
  411. //Turn to the left
  412. speedLeft=150;
  413. speedRight=-150;
  414. xSemaphoreGive(speedSem);
  415. }
  416. //Set go bool to true
  417. go = true;
  418. delay(100);
  419. break;
  420.  
  421. default:
  422. if(xSemaphoreTake(FlagSem,5)== pdTRUE)
  423. {
  424. if (flag == 0)
  425. {
  426. if(xSemaphoreTake(CountSem,5)== pdTRUE)
  427. {
  428. if (count <= 3500)
  429. {
  430. if (xSemaphoreTake(speedSem, 5) == pdTRUE)
  431. {
  432. //Kör frammåt leta efter bana
  433. speedLeft=90;
  434. speedRight=90;
  435. xSemaphoreGive(speedSem);
  436. }
  437. }
  438.  
  439. else
  440. {
  441. flag = 1;
  442. }
  443. xSemaphoreGive(CountSem);
  444. }
  445. }
  446.  
  447. else
  448. {
  449. if(xSemaphoreTake(CountSem,5)== pdTRUE)
  450. {
  451. if (count >= 0)
  452. {
  453. if (xSemaphoreTake(speedSem, 5) == pdTRUE)
  454. {
  455. speedLeft=-90;
  456. speedRight=-90;
  457. xSemaphoreGive(speedSem);
  458. }
  459. }
  460.  
  461. else
  462. {
  463.  
  464. flag = 0;
  465. if (xSemaphoreTake(speedSem, 5) == pdTRUE)
  466. {
  467. speedLeft=-150;
  468. speedRight=150;
  469. xSemaphoreGive(speedSem);
  470. }
  471. delay(500); //Svänger för att testa om bana finns drygt 45 grader åt vänster
  472. }
  473. xSemaphoreGive(CountSem);
  474. }
  475. }
  476. //Release flag sem
  477. xSemaphoreGive(FlagSem);
  478. }
  479. break;
  480. }
  481. }
  482. }
  483. xSemaphoreGive(FindSem);
  484. }
  485. }
  486. //***********************************************************************************************************************************
  487. //***********************************************************************************************************************************
  488.  
  489. void setup()
  490. {
  491. attachInterrupt(Encoder_1.getIntNum(), isr_process_encoder1, RISING);
  492. attachInterrupt(Encoder_2.getIntNum(), isr_process_encoder2, RISING);
  493.  
  494. Serial.begin(9600); // Initiating Serial port bps needs to be the same as parameter in IDE Serial monitor.
  495.  
  496. vSchedulerInit(); // Initiate the scheduler.
  497.  
  498. //Set PWM 8KHz (sätter modulationsfrekvensen inkludera alltid med motorn)
  499. TCCR1A = _BV(WGM10);
  500. TCCR1B = _BV(CS11) | _BV(WGM12);
  501. TCCR2A = _BV(WGM21) | _BV(WGM20);
  502. TCCR2B = _BV(CS21);
  503.  
  504. //Create the semaphores
  505. CountSem=xSemaphoreCreateMutex();
  506. FlagSem=xSemaphoreCreateMutex();
  507. TurnSem=xSemaphoreCreateMutex();
  508. FindSem = xSemaphoreCreateMutex();
  509. GoSem = xSemaphoreCreateMutex();
  510. speedSem = xSemaphoreCreateMutex();
  511. LineSem = xSemaphoreCreateMutex();
  512. UltSonSem = xSemaphoreCreateMutex();
  513. CountSem = xSemaphoreCreateMutex();
  514. AvoidSem = xSemaphoreCreateMutex();
  515. StrikeSem = xSemaphoreCreateMutex();
  516.  
  517. /* Creates a periodic task.(TaskBlip: The task function.
  518. "t2": Name of the task.
  519. configMINIMAL_STACK_SIZE: Stack size of the task in words, not bytes.
  520. &c1: Parameters to the task function.
  521. 1: Priority of the task. (Only used when scheduling policy is set to
  522. manual)
  523. &xHandle1: Pointer to the task handle.
  524. pdMS_TO_TICKS(0): Phase given in software ticks. Counted from when
  525. vSchedulerStart is called. pdMS_TO_TICKS() converts microseconds to ticks.
  526. pdMS_TO_TICKS(100): Period given in software ticks.
  527. pdMS_TO_TICKS(100): Worst-case execution time given in software ticks.
  528. pdMS_TO_TICKS(100): Relative deadline given in software ticks.):
  529. * */
  530. //Ultra sonic task
  531. vSchedulerPeriodicTaskCreate(StoreUltSensor,
  532. "t1",
  533. configMINIMAL_STACK_SIZE,
  534. &c1,
  535. 1,
  536. &xHandle1,
  537. pdMS_TO_TICKS(0),
  538. pdMS_TO_TICKS(10),
  539. pdMS_TO_TICKS(10),
  540. pdMS_TO_TICKS(10));
  541. //Get line task
  542. vSchedulerPeriodicTaskCreate(StoreLineSensor,
  543. "t2",
  544. configMINIMAL_STACK_SIZE,
  545. &c2,
  546. 2,
  547. &xHandle2,
  548. pdMS_TO_TICKS(0),
  549. pdMS_TO_TICKS(10),
  550. pdMS_TO_TICKS(10),
  551. pdMS_TO_TICKS(10));
  552. // Motor task
  553. vSchedulerPeriodicTaskCreate(TaskSetSpeed,
  554. "t3",
  555. configMINIMAL_STACK_SIZE,
  556. &c3,
  557. 3,
  558. &xHandle3,
  559. pdMS_TO_TICKS(0),
  560. pdMS_TO_TICKS(10),
  561. pdMS_TO_TICKS(10),
  562. pdMS_TO_TICKS(10));
  563. // Object avoidence task
  564. vSchedulerPeriodicTaskCreate(TaskAvoidObject,
  565. "t4",
  566. configMINIMAL_STACK_SIZE,
  567. &c4,
  568. 4,
  569. &xHandle4,
  570. pdMS_TO_TICKS(0),
  571. pdMS_TO_TICKS(10),
  572. pdMS_TO_TICKS(10),
  573. pdMS_TO_TICKS(10));
  574. //Line follow task
  575. vSchedulerPeriodicTaskCreate(TaskFollowLine,
  576. "t5",
  577. configMINIMAL_STACK_SIZE,
  578. &c5,
  579. 5,
  580. &xHandle5,
  581. pdMS_TO_TICKS(0),
  582. pdMS_TO_TICKS(10),
  583. pdMS_TO_TICKS(10),
  584. pdMS_TO_TICKS(10));
  585.  
  586. //Line finder task
  587. vSchedulerPeriodicTaskCreate(TaskFindLine,
  588. "t6",
  589. configMINIMAL_STACK_SIZE,
  590. &c6,
  591. 6,
  592. &xHandle6,
  593. pdMS_TO_TICKS(0),
  594. pdMS_TO_TICKS(10),
  595. pdMS_TO_TICKS(10),
  596. pdMS_TO_TICKS(10));
  597.  
  598. //Count task
  599. vSchedulerPeriodicTaskCreate(TaskCount,
  600. "t7",
  601. configMINIMAL_STACK_SIZE,
  602. &c7,
  603. 7,
  604. &xHandle7,
  605. pdMS_TO_TICKS(0),
  606. pdMS_TO_TICKS(10),
  607. pdMS_TO_TICKS(10),
  608. pdMS_TO_TICKS(10));
  609.  
  610. //Count task
  611. vSchedulerPeriodicTaskCreate(StrikeCount,
  612. "t8",
  613. configMINIMAL_STACK_SIZE,
  614. &c8,
  615. 8,
  616. &xHandle8,
  617. pdMS_TO_TICKS(0),
  618. pdMS_TO_TICKS(10),
  619. pdMS_TO_TICKS(10),
  620. pdMS_TO_TICKS(10));
  621. vSchedulerStart(); // Start the program.
  622. }
  623.  
  624. //***********************************************************************************************************************************
  625. //***********************************************************************************************************************************
  626. //DONT CARE
  627.  
  628. void loop()
  629. {
  630. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement