API tools faq
paste
Advertisement
safwan092

Untitled

safwan092
Apr 8th, 2022
48
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 13.33 KB | None | 0 0
raw download clone embed print report
  1. #define S0 2
  2. #define S1 3
  3. #define S2 4
  4. #define S3 A0
  5. #define sensorOut A1
  6. #define sensorOut2 11
  7. #define S22 12
  8. #define S23 13
  9.  
  10.  
  11. int frequency = 0;
  12. int color = 0;
  13. int frequency2 = 0;
  14. int color2 = 0;
  15.  
  16. #define BSW A2
  17. #define RSW A3
  18. #define GSW A4
  19.  
  20.  
  21.  
  22. #define MOTOR_SPEED 188 //180 0-255
  23.  
  24. //Right motor
  25. int enableRightMotor = 6;
  26. int rightMotorPin1 = 7;
  27. int rightMotorPin2 = 8;
  28.  
  29. //Left motor
  30. int enableLeftMotor = 5;
  31. int leftMotorPin1 = 9;
  32. int leftMotorPin2 = 10;
  33.  
  34. int redON = 0;
  35. int greenON = 0;
  36. int blackON = 0;
  37.  
  38. int BSW_status = 0;
  39. int RSW_status = 0;
  40. int GSW_status = 0;
  41.  
  42. void setup()
  43. {
  44. Serial.begin(9600);
  45. pinMode(S0 , OUTPUT);
  46. pinMode(S1 , OUTPUT);
  47. pinMode(S2 , OUTPUT);
  48. pinMode(S3 , OUTPUT);
  49. pinMode(S22 , OUTPUT);
  50. pinMode(S23 , OUTPUT);
  51. pinMode(sensorOut , INPUT);
  52. pinMode(sensorOut2 , INPUT);
  53. //Setting frequency-scaling
  54. digitalWrite(S0, HIGH);
  55. digitalWrite(S1, LOW);
  56.  
  57. pinMode(BSW, INPUT);
  58. pinMode(RSW, INPUT);
  59. pinMode(GSW, INPUT);
  60.  
  61. //The problem with TT gear motors is that, at very low pwm value it does not even rotate.
  62. //If we increase the PWM value then it rotates faster and our robot is not controlled in that speed and goes out of line.
  63. //For that we need to increase the frequency of analogWrite.
  64. //Below line is important to change the frequency of PWM signal on pin D5 and D6
  65. //Because of this, motor runs in controlled manner (lower speed) at high PWM value.
  66. //This sets frequecny as 7812.5 hz.
  67. TCCR0B = TCCR0B & B11111000 | B00000010 ;
  68.  
  69. // put your setup code here, to run once:
  70. pinMode(enableRightMotor, OUTPUT);
  71. pinMode(rightMotorPin1, OUTPUT);
  72. pinMode(rightMotorPin2, OUTPUT);
  73.  
  74. pinMode(enableLeftMotor, OUTPUT);
  75. pinMode(leftMotorPin1, OUTPUT);
  76. pinMode(leftMotorPin2, OUTPUT);
  77.  
  78. rotateMotor(0, 0);
  79. }
  80.  
  81.  
  82. void loop()
  83. {
  84.  
  85. color = readColor();// 0 1 2 3
  86. color2 = readColor2();
  87. delay(10);
  88. // 1 = RED
  89. // 2 = BLACK
  90. // 3 = GREEN
  91. BSW_status = digitalRead(BSW);
  92. RSW_status = digitalRead(RSW);
  93. GSW_status = digitalRead(GSW);
  94. Serial.print(BSW_status);
  95. Serial.print(RSW_status);
  96. Serial.println(GSW_status);
  97.  
  98. ////////////////////////////////////////////////////////////////////////
  99.  
  100. ////////////////////////////////////////////////////////////////////////
  101. if (BSW_status == 1 && RSW_status == 0 && GSW_status == 0) {
  102. follow3colorLine();
  103. //search();
  104. //Follow BLACK line
  105. // 2 = BLACK
  106. if (color == 2 && color2 == 2 && redON == 1) {
  107. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);//FRONT
  108. //delay(500);
  109. }
  110. else if (color == 2 && color2 != 2 && redON == 1) {
  111. //rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);//LEFT
  112. //delay(100);
  113. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);
  114. delay(50);
  115. rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);
  116. delay(50);
  117. rotateMotor(-MOTOR_SPEED, -MOTOR_SPEED);
  118. delay(50);
  119. rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);
  120. delay(50);
  121. }
  122. else if (color != 2 && color2 == 2 && redON == 1) {
  123. //rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);//RIGHT
  124. //delay(100);
  125. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);
  126. delay(50);
  127. rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);
  128. delay(50);
  129. rotateMotor(-MOTOR_SPEED, -MOTOR_SPEED);
  130. delay(50);
  131. rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);
  132. delay(50);
  133. }
  134. else if (color != 2 && color2 != 2 && redON == 1) {
  135. rotateMotor(0, 0);//STOP
  136. //redON = 0;
  137. //greenON = 0;
  138. //blackON = 0;
  139. }
  140. }
  141. else if (BSW_status == 0 && RSW_status == 1 && GSW_status == 0) {
  142. follow3colorLine();
  143. //search();
  144. //Follow RED line
  145. // 1 = RED
  146. if (color == 1 && color2 == 1 && redON == 1) {
  147. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);//FRONT
  148. //delay(500);
  149. }
  150. else if (color == 1 && color2 != 1 && redON == 1) {
  151. //rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);//LEFT
  152. //delay(100);
  153. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);
  154. delay(50);
  155. rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);
  156. delay(50);
  157. rotateMotor(-MOTOR_SPEED, -MOTOR_SPEED);
  158. delay(50);
  159. rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);
  160. delay(50);
  161. }
  162. else if (color != 1 && color2 == 1 && redON == 1) {
  163. //rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);//RIGHT
  164. //delay(100);
  165. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);
  166. delay(50);
  167. rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);
  168. delay(50);
  169. rotateMotor(-MOTOR_SPEED, -MOTOR_SPEED);
  170. delay(50);
  171. rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);
  172. delay(50);
  173. }
  174. else if (color != 1 && color2 != 1 && redON == 1) {
  175. rotateMotor(0, 0);//STOP
  176. //redON = 0;
  177. //greenON = 0;
  178. //blackON = 0;
  179. }
  180. }
  181.  
  182. else if (BSW_status == 0 && RSW_status == 0 && GSW_status == 1) {
  183. follow3colorLine();
  184. //search();
  185. //Follow GREEN line
  186. // 3 = GREEN
  187. if (color == 3 && color2 == 3 && redON == 1) {
  188. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);//FRONT
  189. //delay(500);
  190. }
  191. else if (color == 3 && color2 != 3 && redON == 1) {
  192. //rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);//LEFT
  193. //delay(100);
  194. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);
  195. delay(50);
  196. rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);
  197. delay(50);
  198. rotateMotor(-MOTOR_SPEED, -MOTOR_SPEED);
  199. delay(50);
  200. rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);
  201. delay(50);
  202. }
  203. else if (color != 3 && color2 == 3 && redON == 1) {
  204. //rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);//RIGHT
  205. //delay(100);
  206. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);
  207. delay(50);
  208. rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);
  209. delay(50);
  210. rotateMotor(-MOTOR_SPEED, -MOTOR_SPEED);
  211. delay(50);
  212. rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);
  213. delay(50);
  214. }
  215. else if (color != 3 && color2 != 3 && redON == 1) {
  216. rotateMotor(0, 0);//STOP
  217. //redON = 0;
  218. //greenON = 0;
  219. //blackON = 0;
  220. }
  221. }
  222.  
  223. else {
  224. rotateMotor(0, 0);
  225. redON = 0;
  226. greenON = 0;
  227. blackON = 0;
  228. }
  229.  
  230.  
  231. }//end of LOOP
  232.  
  233.  
  234. void rotateMotor(int rightMotorSpeed, int leftMotorSpeed)
  235. {
  236.  
  237. if (rightMotorSpeed < 0)
  238. {
  239. digitalWrite(rightMotorPin1, LOW);
  240. digitalWrite(rightMotorPin2, HIGH);
  241. }
  242. else if (rightMotorSpeed > 0)
  243. {
  244. digitalWrite(rightMotorPin1, HIGH);
  245. digitalWrite(rightMotorPin2, LOW);
  246. }
  247. else
  248. {
  249. digitalWrite(rightMotorPin1, LOW);
  250. digitalWrite(rightMotorPin2, LOW);
  251. }
  252.  
  253. if (leftMotorSpeed < 0)
  254. {
  255. digitalWrite(leftMotorPin1, LOW);
  256. digitalWrite(leftMotorPin2, HIGH);
  257. }
  258. else if (leftMotorSpeed > 0)
  259. {
  260. digitalWrite(leftMotorPin1, HIGH);
  261. digitalWrite(leftMotorPin2, LOW);
  262. }
  263. else
  264. {
  265. digitalWrite(leftMotorPin1, LOW);
  266. digitalWrite(leftMotorPin2, LOW);
  267. }
  268. analogWrite(enableRightMotor, abs(rightMotorSpeed));
  269. analogWrite(enableLeftMotor, abs(leftMotorSpeed));
  270. }
  271.  
  272.  
  273.  
  274. //Read-Color [1] Function
  275.  
  276. int readColor() {
  277.  
  278. ////////////////////////////////////////////////////////
  279.  
  280. //Setting red filtered photodiodes to be read
  281.  
  282. digitalWrite(S2, LOW);
  283. digitalWrite(S3, LOW);
  284.  
  285.  
  286. //Reading the output frequency
  287. frequency = pulseIn(sensorOut, LOW);
  288. int R = frequency;
  289. //Printing the value on the serial monitor
  290. Serial.print("R= "); //printing name
  291. Serial.print(frequency); //printing RED color frequency
  292. Serial.print(" ");
  293. delay(50);
  294.  
  295. ////////////////////////////////////////////////////////
  296.  
  297. //Setting Green filtered photodiodes to be read
  298.  
  299. digitalWrite(S2, HIGH);
  300. digitalWrite(S3, HIGH);
  301.  
  302. //Reading the output frequency
  303. frequency = pulseIn(sensorOut, LOW);
  304. int G = frequency;
  305.  
  306. //Printing the value on the serial monitor
  307. Serial.print("G= "); //printing name
  308. Serial.print(frequency); //printing RED color frequency
  309. Serial.print(" ");
  310. delay(50);
  311.  
  312. ////////////////////////////////////////////////////////
  313.  
  314. //Setting Blue filtered photodiodes to be read
  315.  
  316. digitalWrite(S2, LOW);
  317. digitalWrite(S3, HIGH);
  318.  
  319. //Reading the output frequency
  320. frequency = pulseIn(sensorOut, LOW);
  321. int B = frequency;
  322.  
  323. //Printing the value on the serial monitor
  324. Serial.print("B= "); //printing name
  325. Serial.print(frequency); //printing RED color frequency
  326. Serial.println(" ");
  327. delay(50);
  328.  
  329. ////////////////////////////////////////////////////////
  330. //20-68-68
  331. if (R < 30 && R > 10 && G < 78 && G > 40 && B < 78 && B > 40 ) {
  332. color = 1; // Red ✔[08-4-2022]
  333. Serial.println("\t\t\tRED detected!");
  334. }
  335. //29-33-33
  336. else if (R < 39 && R > 19 && G < 43 && G > 23 && B < 43 && B > 23 ) {
  337. color = 3; // Green ✔[08-4-2022]
  338. Serial.println("\t\t\tGREEN detected!");
  339. }
  340. //108-127-128
  341. else if (R < 130 && R > 20 && G < 150 && G > 50 && B < 150 && B > 50 ) {
  342. color = 2; // Black ✔[08-4-2022]
  343. Serial.println("\t\t\tBLACK detected!");
  344. }
  345. else {
  346. color = 0;
  347. }
  348. return color;
  349. }
  350.  
  351. ///////////////////////////////////////////////////////////////////////////////////////////////
  352.  
  353. //Read-Color [2] Function
  354.  
  355. int readColor2() {
  356.  
  357. ////////////////////////////////////////////////////////
  358.  
  359. //Setting red filtered photodiodes to be read
  360.  
  361. digitalWrite(S22, LOW);
  362. digitalWrite(S23, LOW);
  363.  
  364.  
  365. //Reading the output frequency
  366. frequency2 = pulseIn(sensorOut2, LOW);
  367. int R2 = frequency2;
  368. //Printing the value on the serial monitor
  369. Serial.print("R2= "); //printing name
  370. Serial.print(frequency2); //printing RED color frequency
  371. Serial.print(" ");
  372. delay(50);
  373.  
  374. ////////////////////////////////////////////////////////
  375.  
  376. //Setting Green filtered photodiodes to be read
  377.  
  378. digitalWrite(S22, HIGH);
  379. digitalWrite(S23, HIGH);
  380.  
  381. //Reading the output frequency
  382. frequency2 = pulseIn(sensorOut2, LOW);
  383. int G2 = frequency2;
  384.  
  385. //Printing the value on the serial monitor
  386. Serial.print("G2= "); //printing name
  387. Serial.print(frequency2); //printing RED color frequency
  388. Serial.print(" ");
  389. delay(50);
  390.  
  391. ////////////////////////////////////////////////////////
  392.  
  393. //Setting Blue filtered photodiodes to be read
  394.  
  395. digitalWrite(S22, LOW);
  396. digitalWrite(S23, HIGH);
  397.  
  398. //Reading the output frequency
  399. frequency2 = pulseIn(sensorOut2, LOW);
  400. int B2 = frequency2;
  401.  
  402. //Printing the value on the serial monitor
  403. Serial.print("B2= "); //printing name
  404. Serial.print(frequency2); //printing RED color frequency
  405. Serial.println(" ");
  406. delay(50);
  407.  
  408. ////////////////////////////////////////////////////////
  409. //21-64-56
  410. if (R2 < 31 && R2 > 11 && G2 < 74 && G2 > 40 && B2 < 66 && B2 > 35 ) {
  411. color2 = 1; // Red ✔[08-4-2022]
  412. Serial.println("\t\t\tRED detected! 2");
  413. }
  414. //29-31-39
  415. else if (R2 < 39 && R2 > 19 && G2 < 41 && G2 > 21 && B2 < 49 && B2 > 29 ) {
  416. color2 = 3; // Green ✔[08-4-2022]
  417. Serial.println("\t\t\tGREEN detected! 2");
  418. }
  419. //86-101-95
  420. else if (R2 < 96 && R2 > 70 && G2 < 111 && G2 > 80 && B2 < 105 && B2 > 70 ) {
  421. color2 = 2; // Black ✔[08-4-2022]
  422. Serial.println("\t\t\tBLACK detected! 2");
  423. }
  424. else {
  425. color2 = 0;
  426. }
  427. return color2;
  428. }
  429. ///////////////////////////////////////////////////////////////////////////////////////////////
  430.  
  431.  
  432.  
  433. void follow3colorLine() {
  434. if (redON == 0 || greenON == 0 || blackON == 0) {
  435. //Follow 3 color line
  436. // black(LEFT) red(RIGHT)
  437. // 2 = BLACK 1 = RED
  438. if (color == 2 && color2 == 1) {
  439. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);//FRONT
  440. //delay(500);
  441. //Serial.println("you are here");
  442. }
  443. /*
  444. else if (color == 2 && color2 != 1) {
  445. //followRED();
  446. //rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);//LEFT
  447. }
  448.  
  449.  
  450. else if (color != 2 && color2 == 1) {
  451. //followRED();
  452. //rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);//RIGHT
  453. }
  454. */
  455. else if (color != 2 && color2 != 1) {
  456. //followRED();
  457. rotateMotor(0, 0);//STOP
  458. search();
  459. //redON = 1;
  460. //greenON = 1;
  461. //blackON = 1;
  462. }
  463. }
  464. }
  465.  
  466. //(BSW_status == 0 && RSW_status == 0 && GSW_status == 1)
  467. void search() {
  468. if (BSW_status == 1 && redON == 0 ) {
  469. if (color != 2 && color2 != 2) {
  470. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);
  471. delay(50);
  472. rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);
  473. delay(50);
  474. rotateMotor(-MOTOR_SPEED, -MOTOR_SPEED);
  475. delay(50);
  476. rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);
  477. delay(50);
  478. }
  479. else {
  480. redON = 1;
  481. greenON = 1;
  482. blackON = 1;
  483. }
  484. }
  485. else if (RSW_status == 1 && redON == 0) {
  486. if (color != 1 && color2 != 1) {
  487. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);
  488. delay(50);
  489. rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);
  490. delay(50);
  491. rotateMotor(-MOTOR_SPEED, -MOTOR_SPEED);
  492. delay(50);
  493. rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);
  494. delay(50);
  495. }
  496. else {
  497. redON = 1;
  498. greenON = 1;
  499. blackON = 1;
  500. }
  501. }
  502. else if (GSW_status == 1 && redON == 0) {
  503. if (color != 3 && color2 != 3) {
  504. rotateMotor(MOTOR_SPEED, MOTOR_SPEED);
  505. delay(50);
  506. rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);
  507. delay(50);
  508. rotateMotor(-MOTOR_SPEED, -MOTOR_SPEED);
  509. delay(50);
  510. rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);
  511. delay(50);
  512. }
  513. else {
  514. redON = 1;
  515. greenON = 1;
  516. blackON = 1;
  517. }
  518. }
  519.  
  520. }
  521.  
  522.  
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!