API tools faq
paste
Advertisement
Guest User

sroorkosh

a guest
Jan 26th, 2020
110
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 13.04 KB | None | 0 0
raw download clone embed print report
  1. //
  2.  
  3. #include <LiquidCrystal_I2C.h>
  4. LiquidCrystal_I2C lcd(0x27, 20, 4);
  5.  
  6. #define DirL 9
  7. #define PwmL 8
  8. #define DirR 11
  9. #define PwmR 10
  10.  
  11. #define s0 32
  12. #define s1 34
  13. #define s2 36
  14. #define s3 38
  15. #define out 2
  16.  
  17. #define ls0 40
  18. #define ls1 42
  19. #define ls2 44
  20. #define ls3 46
  21. #define lout 3
  22.  
  23. bool calibrated = false;
  24. bool lineFollowing = false;
  25. bool calibrateRight = false;
  26. bool Mode = false;
  27. bool Mode1 = false;
  28. bool Mode2 = false;
  29.  
  30.  
  31. int GR = 0;
  32. int GG = 0;
  33. int GB = 0;
  34.  
  35.  
  36. int red = 0;
  37. int green = 0;
  38. int blue = 0;
  39.  
  40. int lred = 0;
  41. int lgreen = 0;
  42. int lblue = 0;
  43.  
  44. int colorValue = 60;
  45. int colorInterValue = 65;
  46. int colorValue2 = 40;
  47. int colorInterValue2 = 40;
  48.  
  49. int inPin = 50;
  50.  
  51. int Speed = 0;
  52.  
  53. int Direction;
  54. int obs = 0;
  55.  
  56. int state = HIGH;
  57. int reading;
  58. int previous = LOW;
  59.  
  60.  
  61. long ttime = 0;
  62. long debounce = 200;
  63.  
  64.  
  65. void setup() {
  66. delay(1000);
  67. pinMode(inPin, INPUT);
  68. pinMode(55, INPUT);
  69.  
  70. pinMode(53, INPUT);
  71.  
  72.  
  73. pinMode(52 , INPUT);
  74.  
  75. pinMode(22, INPUT);
  76. pinMode(24, INPUT);
  77. pinMode(26, INPUT);
  78. pinMode(23, INPUT);
  79. pinMode(25, INPUT);
  80. pinMode(DirL, OUTPUT);
  81. pinMode(PwmL, OUTPUT);
  82. pinMode(DirR, OUTPUT);
  83. pinMode(PwmR, OUTPUT);
  84.  
  85. pinMode(s0, OUTPUT);
  86. pinMode(s1, OUTPUT);
  87. pinMode(s2, OUTPUT);
  88. pinMode(s3, OUTPUT);
  89. pinMode(out, INPUT);
  90. digitalWrite(s0, LOW);
  91. digitalWrite(s1, HIGH);
  92. pinMode(ls0, OUTPUT);
  93. pinMode(ls1, OUTPUT);
  94. pinMode(ls2, OUTPUT);
  95. pinMode(ls3, OUTPUT);
  96. pinMode(lout, INPUT);
  97. digitalWrite(ls0, LOW);
  98. digitalWrite(ls1, HIGH);
  99.  
  100.  
  101. pinMode(52, INPUT);
  102.  
  103. Serial.begin (9600);
  104.  
  105. lcd.init();
  106. lcd.backlight();
  107.  
  108. lcd.print("EL line da bt3na");
  109. lcd.setCursor(0, 1);
  110. lcd.print("we next da dl3na");
  111. }
  112.  
  113. void loop() {
  114.  
  115. while (calibrated == false) { colorCalibrate();
  116. }
  117.  
  118. if (digitalRead(52) == HIGH) {
  119. lineFollowing = true;
  120. lcd.clear();
  121. lcd.setCursor(0, 0);
  122. lcd.print("Linefolwing");
  123. delay(500);
  124. lcd.clear();
  125. lcd.print("EL line da bt3na");
  126. lcd.setCursor(0, 1);
  127. lcd.print("we next da dl3na");
  128.  
  129. }
  130.  
  131. while (lineFollowing == true) {
  132.  
  133. // LimitCheck();
  134.  
  135. int SL = digitalRead(22);
  136. int SCL = digitalRead(24);
  137. int SC = digitalRead(26);
  138. int SCR = digitalRead(23);
  139. int SR = digitalRead(25);
  140.  
  141.  
  142. ///////////////////////Forward///////////////////////////////
  143. if ((SL == 0 && SCL == 0 && SC == 1 && SCR == 0 && SR == 0) || (SL == 0 && SCL == 0 && SC == 0 && SCR == 0 && SR == 0) || (SL == 0 && SCL == 1 && SC == 1 && SCR == 1 && SR == 0) || (SL == 0 && SCL == 1 && SC == 1 && SCR == 1 && SR == 0)) {
  144. // checkForObstacle();
  145. Forward();
  146. }
  147. ///////////////////////Turn Left/////////////////////////////
  148. else if (SL == 1 && SCL == 1 && SC == 1 && SCR == 0 && SR == 0) {
  149. colorRead();
  150.  
  151. if (red >= GR - colorValue2 && red <= GR + colorValue2 && green >= GG - colorValue2 && green <= GG + colorValue2 && blue >= GB - colorValue2 && blue <= GB + colorValue2)
  152. {
  153. colorCheck2();
  154. } else
  155. {
  156. Left();
  157. }
  158. }
  159. ///////////////////////Turn Right/////////////////////////////
  160. else if (SL == 0 && SCL == 0 && SC == 1 && SCR == 1 && SR == 1) {
  161. colorRead();
  162. Right();
  163. if (lred >= GR - colorValue2 && lred <= GR + colorValue2 && lgreen >= GG - colorValue2 && lgreen <= GG + colorValue2 && lblue >= GB - colorValue2 && lblue <= GB + colorValue2)
  164. {
  165. colorCheck2();
  166. } else {
  167. Right();
  168. }
  169. }
  170.  
  171. ///////////////////////Turn Left/////////////////////////////
  172. else if ((SL == 0 && SCL == 1 && SCR == 0 && SR == 0) || (SL == 1 && SCL == 1 && SCR == 0 && SR == 0) || (SL == 1 && SCL == 0 && SCR == 0 && SR == 0) || (SL == 1 && SCL == 1 && SCR == 1 && SR == 0) ) {
  173. Left();
  174. }
  175. ///////////////////////Turn Right/////////////////////////////
  176. else if ((SL == 0 && SCL == 0 && SCR == 1 && SR == 0) || (SL == 0 && SCL == 0 && SCR == 1 && SR == 1) || (SL == 0 && SCL == 0 && SCR == 0 && SR == 1) || (SL == 0 && SCL == 1 && SCR == 1 && SR == 1) ) {
  177. Right();
  178. }
  179. else if ((SL == 1 && SCL == 1 && SC == 1 && SCR == 1 && SR == 1)) {
  180. lcd.clear();
  181. lcd.print("Intersection");
  182. Stop();
  183. delay(500);
  184. colorRead();
  185. colorCheck();
  186. }
  187. }
  188. }
  189.  
  190. void Forward() {
  191. digitalWrite(DirL, LOW);
  192. analogWrite(PwmL, 50);
  193. digitalWrite(DirR, LOW);
  194. analogWrite(PwmR, 50);
  195. Serial.println("Forward");
  196. }
  197. void Backward() {
  198. digitalWrite(DirL, HIGH);
  199. analogWrite(PwmL, 40);
  200. digitalWrite(DirR, HIGH);
  201. analogWrite(PwmR, 40);
  202. Serial.println("Forward");
  203. }
  204. void Right() {
  205. digitalWrite(DirL, LOW);
  206. analogWrite(PwmL, 60);
  207. digitalWrite(DirR, HIGH);
  208. analogWrite(PwmR, 60);
  209. Serial.println("Right");
  210. }
  211. void Left() {
  212. digitalWrite(DirL, HIGH);
  213. analogWrite(PwmL, 60);
  214. digitalWrite(DirR, LOW);
  215. analogWrite(PwmR, 60);
  216. Serial.println("Left");
  217. }
  218. void Stop() {
  219. digitalWrite(DirL, LOW);
  220. analogWrite(PwmL, 0);
  221. digitalWrite(DirR, LOW);
  222. analogWrite(PwmR, 0);
  223. Serial.println("Stop");
  224. }
  225.  
  226. void colorRead()
  227. {
  228. digitalWrite(s2, LOW);
  229. digitalWrite(s3, LOW);
  230. red = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);
  231. digitalWrite(s3, HIGH);
  232. blue = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);
  233. digitalWrite(s2, HIGH);
  234. green = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);
  235.  
  236. digitalWrite(ls2, LOW);
  237. digitalWrite(ls3, LOW);
  238. lred = pulseIn(lout, digitalRead(lout) == HIGH ? LOW : HIGH);
  239. digitalWrite(ls3, HIGH);
  240. lblue = pulseIn(lout, digitalRead(lout) == HIGH ? LOW : HIGH);
  241. digitalWrite(ls2, HIGH);
  242. lgreen = pulseIn(lout, digitalRead(lout) == HIGH ? LOW : HIGH);
  243.  
  244. lred = lred - 20;
  245. lblue = lblue - 20;
  246. lgreen = lgreen - 20;
  247.  
  248. }
  249.  
  250. void colorCalibrate()
  251. {
  252. while (Mode == false) {
  253.  
  254. reading = digitalRead(inPin);
  255. if (reading == HIGH && previous == LOW && millis() - ttime > debounce) {
  256. if (state == HIGH)
  257. state = LOW;
  258. else
  259. state = HIGH;
  260.  
  261. ttime = millis();
  262. }
  263.  
  264. while (state == 0 && reading == HIGH) {
  265. lcd.clear();
  266. lcd.print("Calibtrate on");
  267. lcd.setCursor(0, 1);
  268. lcd.print(">> LEFT SESNOR");
  269. Mode1 = true;
  270. Serial.print(Mode1);
  271. break;
  272. }
  273.  
  274. while (state == 1 && reading == HIGH ) {
  275. lcd.clear();
  276. lcd.print("Calibtrate on");
  277. lcd.setCursor(0, 1);
  278. lcd.print(">> RIGHT SENSOR");
  279. Mode1 = false;
  280. Serial.print(Mode1);
  281. break;
  282. }
  283. previous = reading;
  284.  
  285. if (digitalRead(52) == HIGH) {
  286. Mode = true;
  287. break;
  288. }
  289. }
  290. int SL = digitalRead(22);
  291. int SCL = digitalRead(24);
  292. int SC = digitalRead(26);
  293. int SCR = digitalRead(23);
  294. int SR = digitalRead(25);
  295.  
  296. if ((SL == 0 && SCL == 0 && SC == 1 && SCR == 0 && SR == 0) || (SL == 0 && SCL == 0 && SC == 0 && SCR == 0 && SR == 0) || (SL == 0 && SCL == 1 && SC == 1 && SCR == 1 && SR == 0) || (SL == 0 && SCL == 1 && SC == 1 && SCR == 1 && SR == 0)) {
  297. Forward();
  298. }
  299. else if ((SL == 0 && SCL == 1 && SCR == 0 && SR == 0) || (SL == 1 && SCL == 1 && SCR == 0 && SR == 0) || (SL == 1 && SCL == 0 && SCR == 0 && SR == 0) || (SL == 1 && SCL == 1 && SCR == 1 && SR == 0) ) {
  300. Left();
  301. }
  302. ///////////////////////Turn Right/////////////////////////////
  303. else if ((SL == 0 && SCL == 0 && SCR == 1 && SR == 0) || (SL == 0 && SCL == 0 && SCR == 1 && SR == 1) || (SL == 0 && SCL == 0 && SCR == 0 && SR == 1) || (SL == 0 && SCL == 1 && SCR == 1 && SR == 1) ) {
  304. Right();
  305. }
  306. else if ((SL == 0 && SCL == 1 && SCR == 0 && SR == 0) || (SL == 1 && SCL == 1 && SCR == 0 && SR == 0) || (SL == 1 && SCL == 0 && SCR == 0 && SR == 0) || (SL == 1 && SCL == 1 && SCR == 1 && SR == 0) ) {
  307. Left();
  308. }
  309. ///////////////////////Turn Right/////////////////////////////
  310. else if ((SL == 0 && SCL == 0 && SCR == 1 && SR == 0) || (SL == 0 && SCL == 0 && SCR == 1 && SR == 1) || (SL == 0 && SCL == 0 && SCR == 0 && SR == 1) || (SL == 0 && SCL == 1 && SCR == 1 && SR == 1) ) {
  311. Right();
  312. }
  313.  
  314. else {
  315. digitalWrite(DirL, LOW);
  316. analogWrite(PwmL, 0);
  317. digitalWrite(DirR, LOW);
  318. analogWrite(PwmR, 0);
  319. }
  320. if (SL == 1 && SCL == 1 && SC == 1 && SCR == 1 && SR == 1) {
  321.  
  322. Stop();
  323. delay(200);
  324. colorRead();
  325.  
  326. if (Mode1 == true) {
  327. GR = red;
  328. GG = green;
  329. GB = blue;
  330. lcd.clear();
  331. lcd.print("Calibrated on left");
  332. lcd.setCursor(0, 1);
  333. int Cvalue = (GR + GG + GB) / 3;
  334. lcd.print("Cvalue= ");
  335. lcd.print(Cvalue);
  336. delay(4000);
  337.  
  338.  
  339. lcd.clear();
  340. lcd.print("EL line da bt3na");
  341. lcd.setCursor(0, 1);
  342. lcd.print("we next da dl3na");
  343. calibrated = true;
  344. }
  345.  
  346. else if (Mode1 == false) {
  347. GR = lred;
  348. GG = lgreen;
  349. GB = lblue;
  350. lcd.clear();
  351. lcd.print("Calibrated on right");
  352. lcd.setCursor(0, 1);
  353. int Cvalue = (GR + GG + GB) / 3;
  354. lcd.print("Cvalue= ");
  355. lcd.print(Cvalue);
  356. delay(4000);
  357. lcd.clear();
  358. lcd.print("EL line da bt3na");
  359. lcd.setCursor(0, 1);
  360. lcd.print("we next da dl3na");
  361. calibrated = true;
  362. }
  363. }
  364. else {
  365. lcd.clear();
  366. lcd.print("Not Calibrated");
  367. calibrated = false;
  368. }
  369. }
  370.  
  371.  
  372. void colorCheck() {
  373.  
  374. for (int i = 0; i <= 10 ; i++ ) {
  375. colorRead();
  376. lcd.clear();
  377. lcd.print(i);
  378. if ((lred >= GR - colorInterValue && lred <= GR + colorInterValue && lgreen >= GG - colorInterValue && lgreen <= GG + colorInterValue && lblue >= GB - colorInterValue && lblue <= GB + colorInterValue && red >= GR - colorInterValue && red <= GR + colorInterValue && green >= GG - colorInterValue && green <= GG + colorInterValue && blue >= GB - colorInterValue && blue <= GB + colorInterValue)) {
  379. lcd.clear();
  380. lcd.print("Turn 180 Right");
  381. Right();
  382. delay(1300);
  383. break;
  384. }
  385.  
  386. else if (red >= GR - colorValue && red <= GR + colorValue && green >= GG - colorValue && green <= GG + colorValue && blue >= GB - colorValue && blue <= GB + colorValue) {
  387. lcd.clear();
  388. lcd.println("Green Left");
  389. Left();
  390. delay(600);
  391. break;
  392. }
  393. else if (lred >= GR - colorValue && lred <= GR + colorValue && lgreen >= GG - colorValue && lgreen <= GG + colorValue && lblue >= GB - colorValue && lblue <= GB + colorValue) {
  394. lcd.clear();
  395. lcd.println("Green Right");
  396. Right();
  397. delay(600);
  398. break;
  399. }
  400. delay(100);
  401. }
  402. Forward();
  403. delay(300);
  404.  
  405. }
  406.  
  407. void colorCheck2() {
  408.  
  409. for (int i = 0; i <= 10 ; i++ ) {
  410. colorRead();
  411. lcd.clear();
  412. lcd.print(i);
  413. int SL = digitalRead(22);
  414. int SCL = digitalRead(24);
  415. int SC = digitalRead(26);
  416. int SCR = digitalRead(28);
  417. int SR = digitalRead(30);
  418. if ((lred >= GR - colorInterValue2 && lred <= GR + colorInterValue2 && lgreen >= GG - colorInterValue2 && lgreen <= GG + colorInterValue2 && lblue >= GB - colorInterValue2 && lblue <= GB + colorInterValue2 && red >= GR - colorInterValue2 && red <= GR + colorInterValue2 && green >= GG - colorInterValue2 && green <= GG + colorInterValue2 && blue >= GB - colorInterValue2 && blue <= GB + colorInterValue2)) {
  419. if ((SL == 1 && SCL == 1 && SC == 1 && SCR == 1 && SR == 1)) {
  420. break;
  421. }
  422. lcd.clear();
  423. lcd.print("Turn 180 Right");
  424. Right();
  425. delay(1500);
  426. break;
  427. }
  428.  
  429. else if (red >= GR - colorValue2 && red <= GR + colorValue2 && green >= GG - colorValue2 && green <= GG + colorValue2 && blue >= GB - colorValue2 && blue <= GB + colorValue2) {
  430. if ((SL == 1 && SCL == 1 && SC == 1 && SCR == 1 && SR == 1)) {
  431. break;
  432. }
  433. lcd.clear();
  434. lcd.println("Green Left");
  435. Forward();
  436. delay(100);
  437. Left();
  438. delay(600);
  439. Forward();
  440. delay(100);
  441. break;
  442. }
  443. else if (lred >= GR - colorValue2 && lred <= GR + colorValue2 && lgreen >= GG - colorValue2 && lgreen <= GG + colorValue2 && lblue >= GB - colorValue2 && lblue <= GB + colorValue2) {
  444. if ((SL == 1 && SCL == 1 && SC == 1 && SCR == 1 && SR == 1)) {
  445. break;
  446. }
  447. lcd.clear();
  448. lcd.println("Green Right");
  449. Forward();
  450. delay(100);
  451. Right();
  452. delay(600);
  453. Forward();
  454. delay(200);
  455. break;
  456. }
  457. if ((SL == 1 && SCL == 1 && SC == 1 && SCR == 1 && SR == 1)) {
  458. break;
  459. }
  460. delay(100);
  461. }
  462. int SL = digitalRead(22);
  463. int SCL = digitalRead(24);
  464. int SC = digitalRead(26);
  465. int SCR = digitalRead(28);
  466. int SR = digitalRead(30);
  467. if ((SL == 0 && SCL == 1 && SCR == 0 && SR == 0) || (SL == 1 && SCL == 1 && SCR == 0 && SR == 0) || (SL == 1 && SCL == 0 && SCR == 0 && SR == 0) || (SL == 1 && SCL == 1 && SCR == 1 && SR == 0) ) {
  468. Left();
  469.  
  470. }
  471. else if ((SL == 0 && SCL == 0 && SCR == 1 && SR == 0) || (SL == 0 && SCL == 0 && SCR == 1 && SR == 1) || (SL == 0 && SCL == 0 && SCR == 0 && SR == 1) || (SL == 0 && SCL == 1 && SCR == 1 && SR == 1) ) {
  472. Right();
  473.  
  474. }
  475. }
  476.  
  477. void LimitCheck() {
  478. if(digitalRead(53) == HIGH){
  479. Left();
  480. delay(1000);
  481. Forward();
  482. }
  483. }
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!