Robot Car Project

Hauptprojekt.ino.cpp 28KB

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