Hauptprojekt.ino 23 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716
  1. //Verfassser: Felix Stange, 4056379, MET2016
  2. //Datum: erstellt am 19.07.2017, zuletzt geändert am 04.05.2018
  3. //Projektbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe
  4. //der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen).
  5. //Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es
  6. //dennoch zu einer Kollision, wird diese durch den Beschleunigungssensor (LSM303) erkannt.
  7. //Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G)
  8. //genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden.
  9. //Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die
  10. //Kommunikation erfolgt seriell ('SERIAL' für USB, 'SERIAL1' für Bluetooth).
  11. //Das LCD kann bei Bluetoothnutzung nicht verwendet werden.
  12. #include <Wire.h>
  13. #include <Zumo32U4.h>
  14. Zumo32U4ProximitySensors proxSensors; //Abstandssensoren
  15. Zumo32U4LineSensors lineSensors; //Liniensensoren
  16. Zumo32U4Motors motors; //Motoren
  17. Zumo32U4ButtonA buttonA; //Taste A
  18. Zumo32U4Encoders encoders; //Motorencoder
  19. LSM303 compass; //Beschleunigungssensor x-Achse
  20. L3G gyro; //Drehbewegungssensor z-Achse
  21. #define MARKLINE0 150
  22. #define MARKLINE1 100
  23. #define MARKLINE2 120
  24. #define SIGN0 500
  25. #define SIGN1 300
  26. #define SIGN2 500
  27. #define MAXSPEED 400
  28. #define FULLSPEEDLEFT 106
  29. #define HALFSPEEDLEFT 53
  30. #define SLOWSPEEDLEFT 27
  31. #define FULLSPEEDRIGHT 100
  32. #define HALFSPEEDRIGHT 50
  33. #define SLOWSPEEDRIGHT 25
  34. int16_t lineValue[3]; //Liniensensoren
  35. uint16_t lineOffset[3]; //von links (0) nach rechts (2)
  36. uint8_t proxValue[4]; //Abstandssensoren v.l. (0) n.r. (3)
  37. int16_t encoderCounts[2]; //Anzahl der Umdrehungen
  38. int16_t driveRotation[2]; //von links (0) nach rechts (1)
  39. int32_t rotationAngle = 0; //Drehwinkel
  40. int32_t turnAngle = 0;
  41. int16_t turnRate;
  42. int16_t gyroOffset;
  43. uint16_t gyroLastUpdate;
  44. //int32_t moveDisplay = 0; //Beschleunigung
  45. //int32_t moveDistance = 0;
  46. int16_t moveRate;
  47. int16_t compassOffset;
  48. //uint16_t compassLastUpdate;
  49. uint16_t lastUpdate = 0; //Systemzeit
  50. uint16_t eventTime = 0; //Zeit seit letzter Geschwindigkeitsänderung
  51. char report[200]; //Ausgabe
  52. float eventSpeed = 1; //vermindert die Geschwindigkeit bei Manövern
  53. int btData = 0; //Gelesene Daten aus Bluetooth
  54. bool alarm = false; //zeigt Manöver abhängig von btData
  55. /*-----------------------------------------------------------------*/
  56. //Setup Bluetoothverbindung
  57. void BlueSetup()
  58. {
  59. Serial1.begin(9600); //Initialisierung mit Datengeschwindigkeit(Baud)
  60. Serial1.setTimeout(10); //verkürzt Serial(1).read auf 10 ms statt 1000 ms
  61. if(Serial1.available()) Serial.println("Bluetoothverbindung hergestellt");
  62. if(Serial1.available() > 0) Serial1.read(); //Verwerfen der alten Informationen aus dem Puffer
  63. }
  64. //Setup Liniensensoren
  65. void LineSetup()
  66. {
  67. ledYellow(1);
  68. lineSensors.initThreeSensors(); //Initialisierung von 3 Sensoren (max 5)
  69. //Kalibrierung
  70. uint32_t total[3] = {0, 0, 0};
  71. for(uint8_t i = 0; i < 120; i++)
  72. {
  73. if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEEDLEFT, -FULLSPEEDRIGHT);
  74. else motors.setSpeeds(-FULLSPEEDLEFT, FULLSPEEDRIGHT);
  75. lineSensors.read(lineOffset);
  76. total[0] += lineOffset[0];
  77. total[1] += lineOffset[1];
  78. total[2] += lineOffset[2];
  79. lineSensors.calibrate();
  80. }
  81. motors.setSpeeds(0, 0);
  82. lineOffset[0] = total[0] / 120;
  83. lineOffset[1] = total[1] / 120;
  84. lineOffset[2] = total[2] / 120;
  85. ledYellow(0);
  86. // lineOffset[0] = 240;
  87. // lineOffset[1] = 120;
  88. // lineOffset[2] = 220;
  89. }
  90. //Setup Abstandssensoren
  91. void ProxSetup()
  92. {
  93. proxSensors.initThreeSensors();
  94. }
  95. //Setup Drehbewegungssensor
  96. void GyroSetup()
  97. {
  98. ledYellow(1);
  99. gyro.init(); //Initialisierung
  100. if(!gyro.init()) //Fehlerabfrage
  101. {
  102. //Fehler beim Initialisieren des Drehbewegungssensors
  103. ledRed(1);
  104. while(1)
  105. {
  106. Serial.println("Fehler Drehbewegungssensors");
  107. delay(5000);
  108. }
  109. }
  110. gyro.writeReg(L3G::CTRL1, 0b11111111); //Ausgaberate 800Hz, Tiefpassfilter 100Hz
  111. gyro.writeReg(L3G::CTRL4, 0b00100000); //2000dps Auflösung
  112. gyro.writeReg(L3G::CTRL5, 0b00000000); //Hochpassfilter ausgeschaltet
  113. //Kalibrierung
  114. int32_t total = 0;
  115. for(uint16_t i = 0; i < 1024; i++)
  116. {
  117. while(!gyro.readReg(L3G::STATUS_REG) & 0x08); //Fehlerabfrage
  118. gyro.read();
  119. total += gyro.g.z;
  120. }
  121. gyroOffset = total / 1024;
  122. gyroLastUpdate = micros();
  123. ledYellow(0);
  124. }
  125. //Setup Beschleunigungssensor
  126. void CompassSetup()
  127. {
  128. ledYellow(1);
  129. compass.init(); //Initialisierung
  130. if(!compass.init()) //Fehlerabfrage
  131. {
  132. //Fehler beim Initialisieren des Beschleunigungssensors
  133. ledRed(1);
  134. while(1)
  135. {
  136. Serial.println("Fehler Beschleunigungssensors");
  137. delay(5000);
  138. }
  139. }
  140. compass.enableDefault();
  141. //Kalibrierung
  142. int32_t total = 0;
  143. for (uint16_t i = 0; i < 1024; i++)
  144. {
  145. compass.read();
  146. total += compass.a.x;
  147. }
  148. compassOffset = total / 1024;
  149. //compassLastUpdate = micros();
  150. ledYellow(0);
  151. }
  152. /*-----------------------------------------------------------------*/
  153. void setup()
  154. {
  155. //Initialisierung der Bluetoothverbindung
  156. BlueSetup();
  157. //Initialisierung und Kalibrierung der Sensoren
  158. //Serial1.println("Sensorkalibrierung");
  159. Wire.begin();
  160. delay(500);
  161. ProxSetup();
  162. LineSetup();
  163. GyroSetup();
  164. CompassSetup();
  165. //Zumo bereit zu starten
  166. //Serial1.println("Zumo bereit, starte mit A");
  167. ledGreen(1);
  168. buttonA.waitForButton();
  169. randomSeed(millis());
  170. delay(500);
  171. //Serial1.println("Start");
  172. }
  173. /*-----------------------------------------------------------------*/
  174. //Update Durchlaufzeit
  175. void TimeUpdate()
  176. {
  177. uint16_t m = millis();
  178. eventTime = m - lastUpdate;
  179. }
  180. void LoopTiming()
  181. {
  182. const int AL = 100; // Arraylänge, NUR GERADE Zahlen verwenden!
  183. static unsigned long LoopTime[AL];
  184. static unsigned int Index=0, Messung=0, Min=0xFFFF, Max, Avg;
  185. if (Messung % 2 == 0) // wenn Messung X gerade (0,2,4,6 usw.), entspricht immer Anfang der Loop
  186. {
  187. LoopTime[Index] = millis();
  188. Messung++;
  189. Index++;
  190. return; // Funktion sofort beenden, spart etwas Zeit
  191. }
  192. if (Messung % 2 == 1) // wenn Messung X ungerade (1,3,5,7 usw.), entspricht immer Ende der Loop
  193. {
  194. LoopTime[Index] = millis();
  195. LoopTime[Index-1] = LoopTime[Index] - LoopTime[Index-1]; // Loopdauer einen Index niedriger einspeichern wie aktuell
  196. Messung++;
  197. }
  198. if (Index >= AL) // Array voll, Daten auswerten
  199. {
  200. for (int i = 0; i<AL; i++)
  201. {
  202. Min = min(Min, LoopTime[i]);
  203. Max = max(Max, LoopTime[i]);
  204. Avg += LoopTime[i];
  205. }
  206. Avg = Avg / AL;
  207. Serial1.print(F("Minimal "));Serial1.print(Min);Serial1.println(" ms ");
  208. Serial1.print(F("Durchschnitt "));Serial1.print(Avg);Serial1.println(" ms ");
  209. Serial1.print(F("Maximal "));Serial1.print(Max);Serial1.println(" ms ");
  210. SerielleAusgabe();
  211. Min = 0xFFFF;
  212. Max = 0;
  213. Avg = 0;
  214. Messung = 0;
  215. Index = 0;
  216. }
  217. }
  218. //Update Abstandssensoren
  219. void ProxUpdate()
  220. {
  221. proxSensors.read();
  222. proxValue[0] = proxSensors.countsLeftWithLeftLeds();
  223. proxValue[1] = proxSensors.countsFrontWithLeftLeds();
  224. proxValue[2] = proxSensors.countsFrontWithRightLeds();
  225. proxValue[3] = proxSensors.countsRightWithRightLeds();
  226. }
  227. //Update Liniensensoren
  228. void LineUpdate()
  229. {
  230. uint16_t lineRaw[3];
  231. lineSensors.read(lineRaw);
  232. lineValue[0] = lineRaw[0] - lineOffset[0];
  233. lineValue[1] = lineRaw[1] - lineOffset[1];
  234. lineValue[2] = lineRaw[2] - lineOffset[2];
  235. }
  236. //Update Drehbewegungssensor
  237. void GyroUpdate()
  238. {
  239. gyro.read(); //Rohwert 10285 entspricht 90° bzw.
  240. turnRate = gyro.g.z - gyroOffset; //8,75mdps/LSB
  241. uint16_t m = micros();
  242. uint16_t dt = m - gyroLastUpdate;
  243. gyroLastUpdate = m;
  244. int32_t d = (int32_t)turnRate * dt;
  245. turnAngle += (int64_t)d * 14680064 / 17578125;
  246. rotationAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
  247. }
  248. //Update Beschleunigungssensor
  249. void CompassUpdate()
  250. {
  251. compass.read(); //Rohwert 16384 entspricht 1g (9,81m/s²) bzw.
  252. moveRate = compass.a.x - compassOffset; //bei +/-2g Messbereich 0,61mg/LSB
  253. // uint16_t m = micros();
  254. // uint16_t dt = m - compassLastUpdate;
  255. // compassLastUpdate = m;
  256. // int32_t d = (int32_t)moveRate * dt;
  257. // moveDistance += (int64_t)d * dt >> 14;
  258. // moveDisplay = moveDistance * 9,81 / 1000;
  259. }
  260. //Update Encoder
  261. void EncoderUpdate()
  262. {
  263. encoderCounts[0] = encoders.getCountsLeft();
  264. driveRotation[0] = encoderCounts[0] / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
  265. encoderCounts[1] = encoders.getCountsRight();
  266. driveRotation[1] = encoderCounts[1] / 910;
  267. }
  268. /*-----------------------------------------------------------------*/
  269. //Funktion Kontaktvermeiden
  270. void Kontaktvermeiden()
  271. {
  272. //Serial1.println("Kontaktvermeiden");
  273. Serial1.print(1);
  274. ledRed(1);
  275. motors.setSpeeds(0, 0);
  276. delay(1000);
  277. while(proxValue[1] == 0 || proxValue[2] == 0)
  278. {
  279. ProxUpdate();
  280. motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
  281. if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) break;
  282. }
  283. motors.setSpeeds(0, 0);
  284. //Serial1.println("Vermeiden beendet");
  285. Serial1.print(0);
  286. }
  287. //Funktion Hindernisumfahrung
  288. void Hindernisumfahren()
  289. {
  290. //Serial1.println("Hindernisumfahren");
  291. Serial1.print(1);
  292. ledYellow(1);
  293. //Schritt 1: Spurwechsel links
  294. //links drehen
  295. turnAngle = 0;
  296. rotationAngle = 0;
  297. GyroUpdate();
  298. while(rotationAngle < 45)
  299. {
  300. GyroUpdate();
  301. LineUpdate();
  302. motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
  303. if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
  304. }
  305. //geradeaus über Mittellinie fahren
  306. LineUpdate();
  307. while(lineValue[2] < MARKLINE2)
  308. {
  309. LineUpdate();
  310. motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
  311. //if(lineValue[0] > MARKLINE0) break;
  312. }
  313. //rechts drehen
  314. GyroUpdate();
  315. while(rotationAngle > 10)
  316. {
  317. GyroUpdate();
  318. LineUpdate();
  319. if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, 0);
  320. else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
  321. else motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
  322. }
  323. //geradeaus fahren
  324. motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
  325. //Gegenverkehr beachten
  326. ProxUpdate();
  327. //if(proxValue[1] < 5 || proxValue[2] < 5)
  328. //{
  329. //Schritt 2: Hindernis passieren
  330. //Serial1.println("Aufholen");
  331. lastUpdate = millis();
  332. while(proxValue[3] < 6 && eventTime < 3000)
  333. {
  334. TimeUpdate();
  335. ProxUpdate();
  336. LineUpdate();
  337. Spurhalten();
  338. }
  339. //Serial1.println("Überholen");
  340. ProxUpdate();
  341. while(proxValue[3] == 6)
  342. {
  343. ProxUpdate();
  344. LineUpdate();
  345. Spurhalten();
  346. }
  347. //Serial1.println("Abstand gewinnen");
  348. lastUpdate = millis();
  349. TimeUpdate();
  350. while(eventTime < 3000)
  351. {
  352. //Serial1.println(eventTime);
  353. TimeUpdate();
  354. LineUpdate();
  355. Spurhalten();
  356. }
  357. //}
  358. //Schritt 3: Spurwechsel rechts
  359. //rechts drehen
  360. turnAngle = 0;
  361. rotationAngle = 0;
  362. GyroUpdate();
  363. while(rotationAngle > -45)
  364. {
  365. GyroUpdate();
  366. LineUpdate();
  367. motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
  368. if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
  369. }
  370. //geradeaus über Mittellinie fahren
  371. LineUpdate();
  372. while(lineValue[0] < MARKLINE0)
  373. {
  374. LineUpdate();
  375. motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
  376. //if(lineValue[0] > MARKLINE0) break;
  377. }
  378. //links drehen
  379. GyroUpdate();
  380. while(rotationAngle < -10)
  381. {
  382. GyroUpdate();
  383. LineUpdate();
  384. if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(0, FULLSPEEDRIGHT);
  385. else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
  386. else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
  387. }
  388. //geradeaus fahren
  389. //Serial1.println("Umfahren beendet");
  390. Serial1.print(0);
  391. motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
  392. }
  393. //Funktion Abbiegen
  394. void Abbiegen()
  395. {
  396. ledYellow(1);
  397. //Serial1.println("Abbiegen");
  398. Serial1.print(1);
  399. //Markierung analysieren
  400. LineUpdate();
  401. bool leftCode; //links => 1
  402. bool rightCode; //rechts => 2
  403. if((lineValue[0] > SIGN0 && lineValue[0] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) leftCode = true;
  404. else leftCode = false;
  405. if((lineValue[2] > SIGN2 && lineValue[2] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) rightCode = true;
  406. else rightCode = false;
  407. //Zufallszahl erzeugen
  408. uint8_t randy; //geradeaus => 0
  409. if(leftCode == true && rightCode == true) randy = random(1, 3); //1, 2
  410. else if(leftCode == true && rightCode == false) //randy = 1;
  411. {
  412. randy = random(0, 2); //0, 1
  413. //if(randy == 0) randy = random(0, 2); //erhöht Wahrscheinlickeit abzubiegen
  414. }
  415. else if(leftCode == false && rightCode == true) //randy = 2;
  416. {
  417. randy = random(0, 2); //0, 1
  418. //if(randy == 0) randy = random(0, 2); //erhöht Wahrscheinlickeit abzubiegen
  419. if(randy == 1) randy = 2; //!1 => 2
  420. }
  421. //links Abbiegen (von der Verbindungsstrecke)
  422. if(randy == 1 && rightCode == true)
  423. {
  424. //Serial1.println("links Abbiegen von der Verbindungsstrecke");
  425. //geradeaus zur Mittellinie fahren
  426. motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10);
  427. lastUpdate = millis();
  428. TimeUpdate();
  429. while(eventTime < 300)
  430. {
  431. TimeUpdate();
  432. motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10);
  433. }
  434. LineUpdate();
  435. while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
  436. {
  437. LineUpdate();
  438. motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
  439. }
  440. //links drehen
  441. turnAngle = 0;
  442. rotationAngle = 0;
  443. GyroUpdate();
  444. while(rotationAngle < 90)
  445. {
  446. GyroUpdate();
  447. LineUpdate();
  448. if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(0, FULLSPEEDRIGHT);
  449. else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(HALFSPEEDLEFT, FULLSPEEDRIGHT);
  450. else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
  451. }
  452. //geradeaus fahren
  453. motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
  454. }
  455. //links Abbiegen (vom Rundkurs)
  456. else if(randy == 1 && leftCode == true)
  457. {
  458. //Serial1.println("links Abbiegen vom Rundkurs");
  459. //links drehen
  460. turnAngle = 0;
  461. rotationAngle = 0;
  462. driveRotation[1] = 0;
  463. GyroUpdate();
  464. while(rotationAngle < 40)
  465. {
  466. GyroUpdate();
  467. EncoderUpdate();
  468. motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
  469. //if(driveRotation[1] > 1) break;
  470. }
  471. driveRotation[1] = 0;
  472. GyroUpdate();
  473. while(rotationAngle < 85)
  474. {
  475. GyroUpdate();
  476. EncoderUpdate();
  477. motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
  478. //if(driveRotation[1] > 1) break;
  479. }
  480. //geradeaus fahren
  481. motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
  482. lastUpdate = millis();
  483. TimeUpdate();
  484. while(eventTime < 1000)
  485. {
  486. TimeUpdate();
  487. LineUpdate();
  488. if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
  489. }
  490. lastUpdate = millis();
  491. TimeUpdate();
  492. while(eventTime < 1000)
  493. {
  494. TimeUpdate();
  495. LineUpdate();
  496. Spurhalten();
  497. }
  498. }
  499. //rechts Abbiegen
  500. else if(randy == 2 && rightCode == true)
  501. {
  502. //Serial1.println("rechts Abbiegen");
  503. //rechts drehen
  504. turnAngle = 0;
  505. rotationAngle = 0;
  506. driveRotation[0] = 0;
  507. GyroUpdate();
  508. while(rotationAngle > -40)
  509. {
  510. GyroUpdate();
  511. EncoderUpdate();
  512. LineUpdate();
  513. motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
  514. if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
  515. }
  516. GyroUpdate();
  517. lastUpdate = millis();
  518. while(rotationAngle > -85)
  519. {
  520. TimeUpdate();
  521. GyroUpdate();
  522. LineUpdate();
  523. if(eventTime > 3000) break;
  524. if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, 0);
  525. //else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) motors.setSpeeds(-SLOWSPEEDLEFT, -SLOWSPEEDRIGHT);
  526. else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(FULLSPEEDLEFT, HALFSPEEDRIGHT);
  527. else motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
  528. }
  529. }
  530. //nicht Abbiegen, geradeaus fahren
  531. else
  532. {
  533. //Serial1.println("nicht Abbiegen");
  534. motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
  535. lastUpdate = millis();
  536. TimeUpdate();
  537. while(eventTime < 1000)
  538. {
  539. TimeUpdate();
  540. LineUpdate();
  541. Spurhalten();
  542. }
  543. }
  544. //Serial1.println("Abbiegen beendet");
  545. Serial1.print(0);
  546. }
  547. //Funktion Spurhalten
  548. void Spurhalten()
  549. {
  550. //linke Linie erreicht, nach rechts fahren
  551. if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0)
  552. {
  553. ledYellow(1);
  554. //Serial1.println("Spur nach rechts korrigieren");
  555. motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
  556. lastUpdate = millis();
  557. TimeUpdate();
  558. while(eventTime < 100)
  559. {
  560. TimeUpdate();
  561. LineUpdate();
  562. if(lineValue[2] > MARKLINE2) break;
  563. }
  564. }
  565. //rechte Linie erreicht, nach links fahren
  566. else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2)
  567. {
  568. ledYellow(1);
  569. //Serial1.println("Spur nach links korrigieren");
  570. motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);
  571. lastUpdate = millis();
  572. TimeUpdate();
  573. while(eventTime < 100)
  574. {
  575. TimeUpdate();
  576. LineUpdate();
  577. if(lineValue[0] > MARKLINE0) break;
  578. }
  579. }
  580. //normale Fahrt
  581. else
  582. {
  583. ledGreen(1);
  584. motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);
  585. }
  586. }
  587. //Funktion Spurfinden
  588. void Spurfinden()
  589. {
  590. ledRed(1);
  591. //Serial1.println("Spurfinden");
  592. Serial1.print(1);
  593. lastUpdate = millis();
  594. while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
  595. {
  596. TimeUpdate();
  597. LineUpdate();
  598. motors.setSpeeds(-FULLSPEEDLEFT, -FULLSPEEDRIGHT);
  599. if(eventTime > 3000) break;
  600. }
  601. //Serial1.println("Spur gefunden");
  602. Serial1.print(0);
  603. }
  604. //Funktion Serielle Ausgabe
  605. void SerielleAusgabe()
  606. {
  607. snprintf_P(report, sizeof(report),
  608. PSTR("Abstand: %d %d %d %d Linie: %d %d %d"),
  609. proxValue[0], proxValue[1], proxValue[2], proxValue[3],
  610. lineValue[0], lineValue[1], lineValue[2]);
  611. Serial1.println(report);
  612. snprintf_P(report, sizeof(report),
  613. PSTR("Weg: %d %d Winkel: %d Beschleunigung: %d"),
  614. driveRotation[0], driveRotation[1], rotationAngle, moveRate);
  615. Serial1.println(report);
  616. }
  617. /*-----------------------------------------------------------------*/
  618. void loop()
  619. {
  620. //LoopTiming(); //Zykluszeit beträgt max. 20ms im Idle
  621. ledGreen(0);
  622. ledYellow(0);
  623. ledRed(0);
  624. //Abfragen der Sensordaten
  625. ProxUpdate();
  626. EncoderUpdate();
  627. GyroUpdate();
  628. CompassUpdate();
  629. LineUpdate();
  630. TimeUpdate();
  631. //Funktionsauswahl
  632. //btData = Serial1.readString();
  633. if(Serial1.available() > 0) btData = Serial1.read();
  634. if(btData == '1') alarm = true;
  635. else if(btData == '0') alarm = false;
  636. //verfügbare Funktionen bei laufenden Manövern
  637. //if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kontaktvermeiden"|| btData == "Spurfinden")
  638. if(alarm == true)
  639. {
  640. //Serial1.println("Verstanden");
  641. eventSpeed = 1.4;
  642. if(moveRate > 1500 || (proxValue[1] == 6 && proxValue[2] == 6) || lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);
  643. else Spurhalten();
  644. }
  645. //verfügbare Funktionen im Normalfall
  646. else
  647. {
  648. eventSpeed = 1.0;
  649. //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();
  650. if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
  651. else if((lineValue[0] > SIGN0 && lineValue[0] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) Abbiegen();
  652. else if((lineValue[2] > SIGN2 && lineValue[2] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) Abbiegen();
  653. else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) Spurfinden();
  654. else Spurhalten();
  655. }
  656. //LoopTiming();
  657. }