API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Nov 13th, 2019
102
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 12.01 KB | None | 0 0
raw download clone embed print report
  1. #define leftCenterSensor 4
  2. #define leftNearSensor 3
  3. #define leftFarSensor 12
  4. #define rightCenterSensor 5
  5. #define rightNearSensor 6
  6. #define rightFarSensor 7
  7.  
  8. //for ultrasonic
  9. int trigPin = 1;
  10. int echoPin = 2;
  11.  
  12.  
  13. int leftCenterReading;
  14. int leftNearReading;
  15. int leftFarReading;
  16. int rightCenterReading;
  17. int rightNearReading;
  18. int rightFarReading;
  19.  
  20. int leftNudge;
  21. int replaystage;
  22. int rightNudge;
  23.  
  24. #define leapTime 500
  25.  
  26. #define leftMotor1 8
  27. #define leftMotor2 9
  28.  
  29. #define rightMotor1 10
  30. #define rightMotor2 11
  31.  
  32. #define led 13
  33.  
  34. char path[30] = {};
  35. int pathLength;
  36. int readLength;
  37.  
  38.  
  39. void setup(){
  40.  
  41. pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  42. pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  43.  
  44. pinMode(leftCenterSensor, INPUT);
  45. pinMode(leftNearSensor, INPUT);
  46. pinMode(leftFarSensor, INPUT);
  47. pinMode(rightCenterSensor, INPUT);
  48. pinMode(rightNearSensor, INPUT);
  49. pinMode(rightFarSensor, INPUT);
  50.  
  51. pinMode(leftMotor1, OUTPUT);
  52. pinMode(leftMotor2, OUTPUT);
  53. pinMode(rightMotor1, OUTPUT);
  54. pinMode(rightMotor2, OUTPUT);
  55. pinMode(led, OUTPUT);
  56. //Serial.begin(115200);
  57. digitalWrite(led, HIGH);
  58. delay(1000);
  59. }
  60.  
  61.  
  62. void loop(){
  63.  
  64. float distance = getDistance();
  65.  
  66. if(distance < 30) {
  67. turnAround();
  68. }
  69.  
  70. readSensors();
  71.  
  72. if(leftFarReading==0 && rightFarReading==0 &&
  73. (leftCenterReading==1 || rightCenterReading==1) ){
  74. straight();
  75. }
  76. else{
  77. leftHandWall();
  78. }
  79.  
  80. }
  81.  
  82.  
  83. void readSensors(){
  84.  
  85. leftCenterReading = digitalRead(leftCenterSensor)^1;
  86. leftNearReading = digitalRead(leftNearSensor)^1;
  87. leftFarReading = digitalRead(leftFarSensor)^1;
  88. rightCenterReading = digitalRead(rightCenterSensor)^1;
  89. rightNearReading = digitalRead(rightNearSensor)^1;
  90. rightFarReading = digitalRead(rightFarSensor)^1;
  91.  
  92.  
  93.  
  94.  
  95.  
  96. }
  97.  
  98.  
  99. void leftHandWall(){
  100.  
  101.  
  102. if( leftFarReading==1 && rightFarReading==1){
  103. digitalWrite(leftMotor1, HIGH);
  104. digitalWrite(leftMotor2, LOW);
  105. digitalWrite(rightMotor1, HIGH);
  106. digitalWrite(rightMotor2, LOW);
  107. delay(leapTime);
  108. readSensors();
  109.  
  110. if(leftFarReading==1 || rightFarReading==1){
  111. done();
  112. }
  113. if(leftFarReading==0 && rightFarReading==0){
  114. turnLeft();
  115. }
  116.  
  117. }
  118.  
  119. if(leftFarReading==1){ // if you can turn left then turn left
  120. digitalWrite(leftMotor1, HIGH);
  121. digitalWrite(leftMotor2, LOW);
  122. digitalWrite(rightMotor1, HIGH);
  123. digitalWrite(rightMotor2, LOW);
  124. delay(leapTime);
  125. readSensors();
  126.  
  127. if(leftFarReading==0 && rightFarReading==0){
  128. turnLeft();
  129. }
  130. else{
  131. done();
  132. }
  133. }
  134.  
  135. if(rightFarReading==1){
  136. digitalWrite(leftMotor1, HIGH);
  137. digitalWrite(leftMotor2, LOW);
  138. digitalWrite(rightMotor1, HIGH);
  139. digitalWrite(rightMotor2, LOW);
  140. delay(30);
  141. readSensors();
  142.  
  143. if(leftFarReading==1){
  144. delay(leapTime-30);
  145. readSensors();
  146.  
  147. if(rightFarReading==1 && leftFarReading==1){
  148. done();
  149. }
  150. else{
  151. turnLeft();
  152. return;
  153. }
  154. }
  155. delay(leapTime-30);
  156. readSensors();
  157. if(leftFarReading==0 && leftCenterReading==0 &&
  158. rightCenterReading==0 && rightFarReading==0){
  159. turnRight();
  160. return;
  161. }
  162. path[pathLength]='S';
  163. // Serial.println("s");
  164. pathLength++;
  165. //Serial.print("Path length: ");
  166. //Serial.println(pathLength);
  167. if(path[pathLength-2]=='B'){
  168. //Serial.println("shortening path");
  169. shortPath();
  170. }
  171. straight();
  172. }
  173. readSensors();
  174. if(leftFarReading==0 && leftCenterReading==0 && rightCenterReading==0
  175. && rightFarReading==0 && leftNearReading==0 && rightNearReading==0){
  176. turnAround();
  177. }
  178.  
  179. }
  180. void done(){
  181. digitalWrite(leftMotor1, LOW);
  182. digitalWrite(leftMotor2, LOW);
  183. digitalWrite(rightMotor1, LOW);
  184. digitalWrite(rightMotor2, LOW);
  185. replaystage=1;
  186. path[pathLength]='D';
  187. pathLength++;
  188. while(digitalRead(leftFarSensor)==1){
  189. digitalWrite(led, LOW);
  190. delay(150);
  191. digitalWrite(led, HIGH);
  192. delay(150);
  193. }
  194. delay(500);
  195. replay();
  196. }
  197.  
  198. void turnLeft(){
  199.  
  200.  
  201.  
  202. while(digitalRead(rightCenterSensor)==1||digitalRead(leftCenterSensor)==1){
  203. digitalWrite(leftMotor1, LOW);
  204. digitalWrite(leftMotor2, HIGH);
  205. digitalWrite(rightMotor1, HIGH);
  206. digitalWrite(rightMotor2, LOW);
  207. delay(3);
  208. digitalWrite(leftMotor1, LOW);
  209. digitalWrite(leftMotor2, LOW);
  210. digitalWrite(rightMotor1, LOW);
  211. digitalWrite(rightMotor2, LOW);
  212. delay(1);
  213. }
  214.  
  215. while(digitalRead(rightCenterSensor)==0){
  216. digitalWrite(leftMotor1, LOW);
  217. digitalWrite(leftMotor2, HIGH);
  218. digitalWrite(rightMotor1, HIGH);
  219. digitalWrite(rightMotor2, LOW);
  220. delay(3);
  221. digitalWrite(leftMotor1, LOW);
  222. digitalWrite(leftMotor2, LOW);
  223. digitalWrite(rightMotor1, LOW);
  224. digitalWrite(rightMotor2, LOW);
  225. delay(1);
  226. }
  227.  
  228. if(replaystage==1){
  229. path[pathLength]='L';
  230. //Serial.println("l");
  231. pathLength++;
  232. //Serial.print("Path length: ");
  233. //Serial.println(pathLength);
  234. if(path[pathLength-2]=='B'){
  235. //Serial.println("shortening path");
  236. shortPath();
  237. }
  238. }
  239. }
  240.  
  241. void turnRight(){
  242.  
  243.  
  244. while(digitalRead(rightCenterSensor)==1){
  245. digitalWrite(leftMotor1, HIGH);
  246. digitalWrite(leftMotor2, LOW);
  247. digitalWrite(rightMotor1, LOW);
  248. digitalWrite(rightMotor2, HIGH);
  249. delay(3);
  250. digitalWrite(leftMotor1, LOW);
  251. digitalWrite(leftMotor2, LOW);
  252. digitalWrite(rightMotor1, LOW);
  253. digitalWrite(rightMotor2, LOW);
  254. delay(1);
  255. }
  256. while(digitalRead(rightCenterSensor)==0){
  257. digitalWrite(leftMotor1, HIGH);
  258. digitalWrite(leftMotor2, LOW);
  259. digitalWrite(rightMotor1, LOW);
  260. digitalWrite(rightMotor2, HIGH);
  261. delay(3);
  262. digitalWrite(leftMotor1, LOW);
  263. digitalWrite(leftMotor2, LOW);
  264. digitalWrite(rightMotor1, LOW);
  265. digitalWrite(rightMotor2, LOW);
  266. delay(1);
  267. }
  268. while(digitalRead(leftCenterSensor)==0){
  269. digitalWrite(leftMotor1, HIGH);
  270. digitalWrite(leftMotor2, LOW);
  271. digitalWrite(rightMotor1, LOW);
  272. digitalWrite(rightMotor2, HIGH);
  273. delay(3);
  274. digitalWrite(leftMotor1, LOW);
  275. digitalWrite(leftMotor2, LOW);
  276. digitalWrite(rightMotor1, LOW);
  277. digitalWrite(rightMotor2, LOW);
  278. delay(1);
  279. }
  280.  
  281. if(replaystage==1){
  282. path[pathLength]='R';
  283. Serial.println("r");
  284. pathLength++;
  285. Serial.print("Path length: ");
  286. Serial.println(pathLength);
  287. if(path[pathLength-2]=='B'){
  288. Serial.println("shortening path");
  289. shortPath();
  290. }
  291. }
  292.  
  293. }
  294.  
  295. void straight(){
  296. if( digitalRead(leftCenterSensor)==0){
  297. digitalWrite(leftMotor1, HIGH);
  298. digitalWrite(leftMotor2, LOW);
  299. digitalWrite(rightMotor1, HIGH);
  300. digitalWrite(rightMotor2, LOW);
  301. delay(1);
  302. digitalWrite(leftMotor1, HIGH);
  303. digitalWrite(leftMotor2, LOW);
  304. digitalWrite(rightMotor1, LOW);
  305. digitalWrite(rightMotor2, LOW);
  306. delay(5);
  307. return;
  308. }
  309. if(digitalRead(rightCenterSensor)==0){
  310. digitalWrite(leftMotor1, HIGH);
  311. digitalWrite(leftMotor2, LOW);
  312. digitalWrite(rightMotor1, HIGH);
  313. digitalWrite(rightMotor2, LOW);
  314. delay(1);
  315. digitalWrite(leftMotor1, LOW);
  316. digitalWrite(leftMotor2, LOW);
  317. digitalWrite(rightMotor1, HIGH);
  318. digitalWrite(rightMotor2, LOW);
  319. delay(5);
  320. return;
  321. }
  322.  
  323. digitalWrite(leftMotor1, HIGH);
  324. digitalWrite(leftMotor2, LOW);
  325. digitalWrite(rightMotor1, HIGH);
  326. digitalWrite(rightMotor2, LOW);
  327. delay(4);
  328. digitalWrite(leftMotor1, LOW);
  329. digitalWrite(leftMotor2, LOW);
  330. digitalWrite(rightMotor1, LOW);
  331. digitalWrite(rightMotor2, LOW);
  332. delay(1);
  333.  
  334. }
  335.  
  336. void turnAround(){
  337. digitalWrite(leftMotor1, HIGH);
  338. digitalWrite(leftMotor2, LOW);
  339. digitalWrite(rightMotor1, HIGH);
  340. digitalWrite(rightMotor2, LOW);
  341. delay(150);
  342. while(digitalRead(rightCenterSensor)==0){
  343. digitalWrite(leftMotor1, LOW);
  344. digitalWrite(leftMotor2, HIGH);
  345. digitalWrite(rightMotor1, HIGH);
  346. digitalWrite(rightMotor2, LOW);
  347. delay(3);
  348. digitalWrite(leftMotor1, LOW);
  349. digitalWrite(leftMotor2, LOW);
  350. digitalWrite(rightMotor1, LOW);
  351. digitalWrite(rightMotor2, LOW);
  352. delay(1);
  353. }
  354. path[pathLength]='B';
  355. pathLength++;
  356. straight();
  357. //Serial.println("b");
  358. //Serial.print("Path length: ");
  359. //Serial.println(pathLength);
  360. }
  361.  
  362. void shortPath(){
  363. int shortDone=0;
  364. if(path[pathLength-3]=='L' && path[pathLength-1]=='R'){
  365. pathLength-=3;
  366. path[pathLength]='B';
  367. //Serial.println("test1");
  368. shortDone=1;
  369. }
  370.  
  371. if(path[pathLength-3]=='L' && path[pathLength-1]=='S' && shortDone==1){
  372. pathLength-=3;
  373. path[pathLength]='R';
  374. //Serial.println("test2");
  375. shortDone=1;
  376. }
  377.  
  378. if(path[pathLength-3]=='R' && path[pathLength-1]=='L' && shortDone==1){
  379. pathLength-=3;
  380. path[pathLength]='B';
  381. //Serial.println("test3");
  382. shortDone=1;
  383. }
  384.  
  385.  
  386. if(path[pathLength-3]=='S' && path[pathLength-1]=='L' && shortDone==1){
  387. pathLength-=3;
  388. path[pathLength]='R';
  389. //Serial.println("test4");
  390. shortDone=1;
  391. }
  392.  
  393. if(path[pathLength-3]=='S' && path[pathLength-1]=='S' && shortDone==1){
  394. pathLength-=3;
  395. path[pathLength]='B';
  396. //Serial.println("test5");
  397. shortDone=1;
  398. }
  399. if(path[pathLength-3]=='L' && path[pathLength-1]=='L' && shortDone==1){
  400. pathLength-=3;
  401. path[pathLength]='S';
  402. //Serial.println("test6");
  403. shortDone=1;
  404. }
  405.  
  406. path[pathLength+1]='D';
  407. path[pathLength+2]='D';
  408. pathLength++;
  409. //Serial.print("Path length: ");
  410. //Serial.println(pathLength);
  411. //printPath();
  412. }
  413.  
  414.  
  415.  
  416.  
  417. void printPath(){
  418. Serial.println("+++++++++++++++++");
  419. int x;
  420. while(x<=pathLength){
  421. Serial.println(path[x]);
  422. x++;
  423. }
  424. Serial.println("+++++++++++++++++");
  425. }
  426.  
  427.  
  428. void replay(){
  429. readSensors();
  430. if(leftFarReading==0 && rightFarReading==0){
  431. straight();
  432. }
  433. else{
  434. if(path[readLength]=='D'){
  435. digitalWrite(leftMotor1, HIGH);
  436. digitalWrite(leftMotor2, LOW);
  437. digitalWrite(rightMotor1, HIGH);
  438. digitalWrite(rightMotor2, LOW);
  439. delay(100);
  440. digitalWrite(leftMotor1, LOW);
  441. digitalWrite(leftMotor2, LOW);
  442. digitalWrite(rightMotor1, LOW);
  443. digitalWrite(rightMotor2, LOW);
  444. endMotion();
  445. }
  446. if(path[readLength]=='L'){
  447. digitalWrite(leftMotor1, HIGH);
  448. digitalWrite(leftMotor2, LOW);
  449. digitalWrite(rightMotor1, HIGH);
  450. digitalWrite(rightMotor2, LOW);
  451. delay(leapTime);
  452. turnLeft();
  453. }
  454. if(path[readLength]=='R'){
  455. digitalWrite(leftMotor1, HIGH);
  456. digitalWrite(leftMotor2, LOW);
  457. digitalWrite(rightMotor1, HIGH);
  458. digitalWrite(rightMotor2, LOW);
  459. delay(leapTime);
  460. turnRight();
  461. }
  462. if(path[readLength]=='S'){
  463. digitalWrite(leftMotor1, HIGH);
  464. digitalWrite(leftMotor2, LOW);
  465. digitalWrite(rightMotor1, HIGH);
  466. digitalWrite(rightMotor2, LOW);
  467. delay(leapTime);
  468. straight();
  469. }
  470.  
  471. readLength++;
  472. }
  473.  
  474. replay();
  475.  
  476. }
  477.  
  478. void endMotion(){
  479. digitalWrite(led, LOW);
  480. delay(500);
  481. digitalWrite(led, HIGH);
  482. delay(200);
  483. digitalWrite(led, LOW);
  484. delay(200);
  485. digitalWrite(led, HIGH);
  486. delay(500);
  487. endMotion();
  488. }
  489.  
  490. float getDistance(){
  491.  
  492.  
  493. // Clears the trigPin
  494. digitalWrite(trigPin, LOW);
  495. delayMicroseconds(2);
  496. // Sets the trigPin on HIGH state for 10 micro seconds
  497. digitalWrite(trigPin, HIGH);
  498. delayMicroseconds(10);
  499. digitalWrite(trigPin, LOW);
  500. // Reads the echoPin, returns the sound wave travel time in microseconds
  501. duration = pulseIn(echoPin, HIGH);
  502. // Calculating the distance
  503. return distance;
  504. }
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!