API tools faq
paste
pleasedontcode

**Maze Navigator** rev_03

pleasedontcode
Mar 10th, 2026
34
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Arduino 26.42 KB | None | 0 0
raw download clone embed print report
  1. /********* Pleasedontcode.com **********
  2.  
  3.     Pleasedontcode thanks you for automatic code generation! Enjoy your code!
  4.  
  5.     - Terms and Conditions:
  6.     You have a non-exclusive, revocable, worldwide, royalty-free license
  7.     for personal and commercial use. Attribution is optional; modifications
  8.     are allowed, but you're responsible for code maintenance. We're not
  9.     liable for any loss or damage. For full terms,
  10.     please visit pleasedontcode.com/termsandconditions.
  11.  
  12.     - Project: **Maze Navigator**
  13.     - Version: 003
  14.     - Source Code NOT compiled for: Arduino Mega
  15.     - Source Code created on: 2026-03-10 05:41:56
  16.  
  17. ********* Pleasedontcode.com **********/
  18.  
  19. /****** SYSTEM REQUIREMENTS *****/
  20. /****** SYSTEM REQUIREMENT 1 *****/
  21.     /* Read 5 IR sensors (Left, FrontLeft, Center, */
  22.     /* FrontRight, Right) to detect path edges and walls */
  23.     /* using analog input */
  24. /****** SYSTEM REQUIREMENT 2 *****/
  25.     /* Use PID controller to calculate steering */
  26.     /* correction based on sensor difference between left */
  27.     /* and right sides to keep car centered on path */
  28. /****** SYSTEM REQUIREMENT 3 *****/
  29.     /* Control two DC motors (left and right) with PWM */
  30.     /* speed and direction pins to execute steering and */
  31.     /* forward movement */
  32. /****** SYSTEM REQUIREMENT 4 *****/
  33.     /* Display current sensor readings, PID error, and */
  34.     /* motor speeds on 16x4 I2C LCD in real-time for */
  35.     /* debugging */
  36. /****** SYSTEM REQUIREMENT 5 *****/
  37.     /* When front sensor detects wall (distance < 18cm), */
  38.     /* car STOPS and scans all sensors 5 times with 100ms */
  39.     /* delays. Only turn if side opening detected. */
  40.     /* Diagonal (FL/FR) and side (L/R) sensors must both */
  41.     /* confirm opening before turning. */
  42. /****** SYSTEM REQUIREMENT 6 *****/
  43.     /* Car must maintain 8-12cm distance from walls while */
  44.     /* moving. Center between walls if corridor width */
  45.     /* allows. Use low PID gains (Kp=3) to prevent jerky */
  46.     /* corrections that cause corner hits. */
  47. /****** SYSTEM REQUIREMENT 7 *****/
  48.     /* Fix turnLeft and turnRight: turnLeft uses */
  49.     /* setMotorSpeeds(TURN_SPEED, -TURN_SPEED), turnRight */
  50.     /* uses setMotorSpeeds(-TURN_SPEED, TURN_SPEED). */
  51.     /* Monitor front sensor every 50ms and abort early if */
  52.     /* front clears. */
  53. /****** SYSTEM REQUIREMENT 8 *****/
  54.     /* Maintain 8-12cm distance from walls while moving. */
  55.     /* Center between walls in corridors using Kp=3 PID */
  56.     /* for gentle smooth corrections without jerky */
  57.     /* movements at corners. */
  58. /****** END SYSTEM REQUIREMENTS *****/
  59.  
  60.  
  61.  
  62.  
  63.  
  64. // ===================================================
  65. // MAZE ROBOT – ADVANCED CORNER-SAFE NAVIGATION
  66. // Features: Pre-turn scanning, dual-sensor validation
  67. // Reduced PID Kp=3, Turn monitoring with early abort
  68. // ===================================================
  69.  
  70. #include <Wire.h>
  71. #include <LiquidCrystal_I2C.h>
  72.  
  73. LiquidCrystal_I2C lcd(0x27, 16, 4);
  74.  
  75. // -------- Sensors --------
  76. #define IR_LEFT        A0   // 180° (left side)
  77. #define IR_FRONTLEFT   A1   // 135° (front-left diagonal)
  78. #define IR_FRONT       A2   // 0°   (straight ahead)
  79. #define IR_FRONTRIGHT  A3   // 45°  (front-right diagonal)
  80. #define IR_RIGHT       A4   // 90°  (right side)
  81.  
  82. // -------- Motors --------
  83. #define ENA 5   // Right motor enable
  84. #define IN1 8   // Right motor input 1
  85. #define IN2 9   // Right motor input 2
  86. #define ENB 6   // Left motor enable
  87. #define IN3 10  // Left motor input 1
  88. #define IN4 11  // Left motor input 2
  89.  
  90. // ===================================================
  91. // ROBOT PARAMETERS - CORNER-SAFE TUNING
  92. // ===================================================
  93.  
  94. // Speed settings
  95. #define BASE_SPEED       80  // Normal cruising speed
  96. #define MIN_SPEED        65  // Minimum speed during correction
  97. #define CORNER_SPEED     65  // Speed at corners
  98. #define TURN_SPEED       60  // Speed during turns (REDUCED for safety)
  99.  
  100. // Distance thresholds (in cm)
  101. #define WALL_DETECT      20  // Distance to detect a wall
  102. #define FRONT_CLEAR      18  // Front is clear if > this
  103. #define FRONT_TURN       18  // Distance to prepare for turn
  104. #define FRONT_STOP       20  // Emergency stop distance
  105. #define WALL_IDEAL       8   // TARGET distance from side wall
  106. #define WALL_DEADBAND    2   // Deadband for corrections
  107. #define WALL_DANGER      4   // Dangerously close to wall
  108. #define WALL_FAR         12  // Too far from wall
  109. #define OPEN_SPACE       28  // Consider as open space
  110.  
  111. // PID constants - REDUCED FOR GENTLE CORRECTIONS
  112. #define KP               3.0 // Proportional gain (REDUCED from 5 to 3 for smooth gentle corrections)
  113. #define KD               2.0 // Derivative gain (REDUCED for smoothness)
  114. #define KI               0.05 // Integral gain (REDUCED for smoothness)
  115. #define CORRECTION_LIMIT 8   // Max correction per cycle (REDUCED from 10 to 8)
  116.  
  117. // Motor trim
  118. #define LEFT_MOTOR_TRIM  0
  119. #define RIGHT_MOTOR_TRIM 0
  120.  
  121. // Turn timing
  122. #define TURN_90_TIME     450
  123. #define TURN_PAUSE       200
  124.  
  125. // Pre-turn scanning parameters
  126. #define PRE_TURN_SCANS   5   // Number of consecutive reads for noise elimination
  127. #define SCAN_DELAY       100 // Milliseconds between scans (100ms as specified)
  128.  
  129. // Turn monitoring timeout
  130. #define TURN_TIMEOUT     500 // Maximum milliseconds for a 90 degree turn
  131. #define TURN_CHECK_INTERVAL 50 // Check front sensor every 50ms during turn
  132.  
  133. // ===================================================
  134. // GLOBAL VARIABLES
  135. // ===================================================
  136.  
  137. float L, FL, F, FR, R;
  138. float filteredL, filteredR;
  139.  
  140. // Arrays for pre-turn scanning - stores multiple sensor readings
  141. float scanL[PRE_TURN_SCANS];
  142. float scanFL[PRE_TURN_SCANS];
  143. float scanF[PRE_TURN_SCANS];
  144. float scanFR[PRE_TURN_SCANS];
  145. float scanR[PRE_TURN_SCANS];
  146.  
  147. // Averaged scan values
  148. float avgL, avgFL, avgF, avgFR, avgR;
  149.  
  150. // PID variables
  151. float wallError = 0;
  152. float prevWallError = 0;
  153. float wallIntegral = 0;
  154. float prevCorrection = 0;
  155. float correction = 0;
  156.  
  157. // Wall following state
  158. int followingWall = 0;  // 0 = none, 1 = left wall, 2 = right wall, 3 = both
  159. float wallDistance = 0;
  160. float targetDistance = WALL_IDEAL;
  161.  
  162. // Status strings
  163. String moveStr = "STOP";
  164. String stateStr = "IDLE";
  165. String turnStr = "-";
  166. String wallStr = "NONE";
  167. int currentLeftSpeed = 0;
  168. int currentRightSpeed = 0;
  169.  
  170. // Turn decision variables
  171. bool turning = false;
  172. unsigned long turnStartTime = 0;
  173. int turnDecision = 0;
  174. bool frontClearedDuringTurn = false;
  175.  
  176. // Anti-stall protection
  177. unsigned long lastMoveTime = 0;
  178. int stallCounter = 0;
  179.  
  180. // ===================================================
  181. // DISTANCE MEASUREMENT
  182. // ===================================================
  183.  
  184. float getDist(int pin) {
  185.   int adc = analogRead(pin);
  186.   if (adc < 30) adc = 30;
  187.   return 4800.0 / (adc - 20.0);
  188. }
  189.  
  190. // ===================================================
  191. // READ ALL SENSORS (SINGLE READ)
  192. // ===================================================
  193.  
  194. void readAllSensors() {
  195.   L  = getDist(IR_LEFT);
  196.   FL = getDist(IR_FRONTLEFT);
  197.   F  = getDist(IR_FRONT);
  198.   FR = getDist(IR_FRONTRIGHT);
  199.   R  = getDist(IR_RIGHT);
  200.  
  201.   // Simple filtering
  202.   static float lastL = 0, lastR = 0;
  203.   if (lastL == 0) {
  204.     lastL = L;
  205.     lastR = R;
  206.   } else {
  207.     L = (L + lastL) / 2;
  208.     R = (R + lastR) / 2;
  209.     lastL = L;
  210.     lastR = R;
  211.   }
  212.  
  213.   // Cap values for display
  214.   if (L > 99) L = 99;
  215.   if (FL > 99) FL = 99;
  216.   if (F > 99) F = 99;
  217.   if (FR > 99) FR = 99;
  218.   if (R > 99) R = 99;
  219. }
  220.  
  221. // ===================================================
  222. // PRE-TURN SENSOR SCANNING (NEW)
  223. // Performs 5 consecutive reads with 100ms delays
  224. // to eliminate noise and confirm wall detection
  225. // ===================================================
  226.  
  227. void performPreTurnScanning() {
  228.   Serial.println("\n=== PRE-TURN SCANNING (Noise Elimination) ===");
  229.  
  230.   // Perform 5 consecutive sensor reads with 100ms delays
  231.   for (int i = 0; i < PRE_TURN_SCANS; i++) {
  232.     readAllSensors();
  233.    
  234.     // Store readings in arrays
  235.     scanL[i] = L;
  236.     scanFL[i] = FL;
  237.     scanF[i] = F;
  238.     scanFR[i] = FR;
  239.     scanR[i] = R;
  240.    
  241.     Serial.print("Scan "); Serial.print(i + 1);
  242.     Serial.print(": F="); Serial.print(F);
  243.     Serial.print(" FL="); Serial.print(FL);
  244.     Serial.print(" FR="); Serial.print(FR);
  245.     Serial.print(" L="); Serial.print(L);
  246.     Serial.print(" R="); Serial.println(R);
  247.    
  248.     // Delay 100ms between scans (except after last scan)
  249.     if (i < PRE_TURN_SCANS - 1) {
  250.       delay(SCAN_DELAY);
  251.     }
  252.   }
  253.  
  254.   // Calculate averages from the 5 scans
  255.   avgL = 0, avgFL = 0, avgF = 0, avgFR = 0, avgR = 0;
  256.  
  257.   for (int i = 0; i < PRE_TURN_SCANS; i++) {
  258.     avgL += scanL[i];
  259.     avgFL += scanFL[i];
  260.     avgF += scanF[i];
  261.     avgFR += scanFR[i];
  262.     avgR += scanR[i];
  263.   }
  264.  
  265.   avgL /= PRE_TURN_SCANS;
  266.   avgFL /= PRE_TURN_SCANS;
  267.   avgF /= PRE_TURN_SCANS;
  268.   avgFR /= PRE_TURN_SCANS;
  269.   avgR /= PRE_TURN_SCANS;
  270.  
  271.   Serial.println("\n=== AVERAGED RESULTS (After noise elimination) ===");
  272.   Serial.print("Avg F="); Serial.print(avgF);
  273.   Serial.print(" Avg FL="); Serial.print(avgFL);
  274.   Serial.print(" Avg FR="); Serial.print(avgFR);
  275.   Serial.print(" Avg L="); Serial.print(avgL);
  276.   Serial.print(" Avg R="); Serial.println(avgR);
  277. }
  278.  
  279. // ===================================================
  280. // DUAL-SENSOR VALIDATION (NEW)
  281. // Requires BOTH diagonal (FL/FR) AND side (L/R) sensors
  282. // to confirm opening before allowing turn
  283. // ===================================================
  284.  
  285. bool validateOpeningDualSensor(bool leftTurn) {
  286.   Serial.println("\n=== DUAL-SENSOR VALIDATION ===");
  287.  
  288.   if (leftTurn) {
  289.     // For LEFT turn, check LEFT side
  290.     // BOTH diagonal (FL) AND side (L) must confirm opening
  291.     bool diagonalOpen = (avgFL > OPEN_SPACE);
  292.     bool sideOpen = (avgL > OPEN_SPACE);
  293.    
  294.     Serial.print("LEFT TURN - Diagonal (FL)="); Serial.print(avgFL);
  295.     Serial.print(" ["); Serial.print(diagonalOpen ? "OPEN" : "BLOCKED");
  296.     Serial.print("], Side (L)="); Serial.print(avgL);
  297.     Serial.print(" ["); Serial.print(sideOpen ? "OPEN" : "BLOCKED");
  298.     Serial.println("]");
  299.    
  300.     bool valid = (diagonalOpen && sideOpen);
  301.     Serial.print("LEFT TURN VALIDATION: ");
  302.     Serial.println(valid ? "VALID - PROCEED" : "INVALID - CANCEL");
  303.     return valid;
  304.   } else {
  305.     // For RIGHT turn, check RIGHT side
  306.     // BOTH diagonal (FR) AND side (R) must confirm opening
  307.     bool diagonalOpen = (avgFR > OPEN_SPACE);
  308.     bool sideOpen = (avgR > OPEN_SPACE);
  309.    
  310.     Serial.print("RIGHT TURN - Diagonal (FR)="); Serial.print(avgFR);
  311.     Serial.print(" ["); Serial.print(diagonalOpen ? "OPEN" : "BLOCKED");
  312.     Serial.print("], Side (R)="); Serial.print(avgR);
  313.     Serial.print(" ["); Serial.print(sideOpen ? "OPEN" : "BLOCKED");
  314.     Serial.println("]");
  315.    
  316.     bool valid = (diagonalOpen && sideOpen);
  317.     Serial.print("RIGHT TURN VALIDATION: ");
  318.     Serial.println(valid ? "VALID - PROCEED" : "INVALID - CANCEL");
  319.     return valid;
  320.   }
  321. }
  322.  
  323. // ===================================================
  324. // MOTOR CONTROL
  325. // ===================================================
  326.  
  327. void setMotorSpeeds(int leftSpeed, int rightSpeed) {
  328.   currentLeftSpeed = leftSpeed;
  329.   currentRightSpeed = rightSpeed;
  330.  
  331.   leftSpeed += LEFT_MOTOR_TRIM;
  332.   rightSpeed += RIGHT_MOTOR_TRIM;
  333.  
  334.   // ANTI-STALL PROTECTION - NEVER go below MIN_SPEED when moving forward
  335.   if (leftSpeed > 0 && leftSpeed < MIN_SPEED) leftSpeed = MIN_SPEED;
  336.   if (rightSpeed > 0 && rightSpeed < MIN_SPEED) rightSpeed = MIN_SPEED;
  337.  
  338.   leftSpeed = constrain(leftSpeed, -255, 255);
  339.   rightSpeed = constrain(rightSpeed, -255, 255);
  340.  
  341.   // Left motor
  342.   if (leftSpeed >= 0) {
  343.     digitalWrite(IN3, HIGH);
  344.     digitalWrite(IN4, LOW);
  345.     analogWrite(ENB, leftSpeed);
  346.   } else {
  347.     digitalWrite(IN3, LOW);
  348.     digitalWrite(IN4, HIGH);
  349.     analogWrite(ENB, -leftSpeed);
  350.   }
  351.  
  352.   // Right motor
  353.   if (rightSpeed >= 0) {
  354.     digitalWrite(IN1, HIGH);
  355.     digitalWrite(IN2, LOW);
  356.     analogWrite(ENA, rightSpeed);
  357.   } else {
  358.     digitalWrite(IN1, LOW);
  359.     digitalWrite(IN2, HIGH);
  360.     analogWrite(ENA, -rightSpeed);
  361.   }
  362.  
  363.   // Update last move time for anti-stall
  364.   if (leftSpeed != 0 || rightSpeed != 0) {
  365.     lastMoveTime = millis();
  366.   }
  367. }
  368.  
  369. void stopCar() {
  370.   setMotorSpeeds(0, 0);
  371.   moveStr = "STOP";
  372. }
  373.  
  374. void moveForward(int leftSpeed, int rightSpeed) {
  375.   setMotorSpeeds(leftSpeed, rightSpeed);
  376.   moveStr = "FORWARD";
  377. }
  378.  
  379. // ===================================================
  380. // ANTI-STALL CHECK
  381. // ===================================================
  382.  
  383. void checkForStall() {
  384.   // If we're trying to move but not making progress (detected by sensors)
  385.   if (!turning && (currentLeftSpeed > 0 || currentRightSpeed > 0)) {
  386.    
  387.     // Check if we're stuck against a wall
  388.     if (F < FRONT_STOP && currentLeftSpeed > 0 && currentRightSpeed > 0) {
  389.       stallCounter++;
  390.       if (stallCounter > 5) {
  391.         Serial.println("STALL DETECTED - Backing up");
  392.         // Back up a little
  393.         setMotorSpeeds(-MIN_SPEED, -MIN_SPEED);
  394.         delay(300);
  395.         stopCar();
  396.         delay(200);
  397.         stallCounter = 0;
  398.       }
  399.     } else {
  400.       stallCounter = 0;
  401.     }
  402.   }
  403. }
  404.  
  405. // ===================================================
  406. // TURN FUNCTIONS WITH MONITORING (ENHANCED)
  407. // Monitor front sensor during turn
  408. // If front clears, stop turning and resume wall-following
  409. //
  410. // CRITICAL FIX: Motor commands corrected
  411. // turnLeft: setMotorSpeeds(TURN_SPEED, -TURN_SPEED)
  412. //   = Left motor fast forward, Right motor reverse = PIVOT LEFT
  413. // turnRight: setMotorSpeeds(-TURN_SPEED, TURN_SPEED)
  414. //   = Left motor reverse, Right motor fast forward = PIVOT RIGHT
  415. // ===================================================
  416.  
  417. void turnLeft() {
  418.   stopCar();
  419.   delay(100);
  420.   stateStr = "TURN LEFT";
  421.   turnStr = "LEFT";
  422.   Serial.println(">>> EXECUTING: LEFT TURN WITH MONITORING <<<");
  423.  
  424.   frontClearedDuringTurn = false;
  425.   turnStartTime = millis();
  426.   unsigned long lastCheckTime = turnStartTime;
  427.  
  428.   // Execute turn with periodic front sensor monitoring
  429.   while (millis() - turnStartTime < TURN_90_TIME) {
  430.     unsigned long currentTime = millis();
  431.    
  432.     // Check front sensor every 50ms during turn
  433.     if (currentTime - lastCheckTime >= TURN_CHECK_INTERVAL) {
  434.       readAllSensors();
  435.       lastCheckTime = currentTime;
  436.      
  437.       // If front clears during turn, abort and resume wall-following
  438.       if (F > FRONT_CLEAR + 5) {
  439.         Serial.println("FRONT CLEARED DURING LEFT TURN - ABORTING TURN");
  440.         frontClearedDuringTurn = true;
  441.         break;
  442.       }
  443.     }
  444.    
  445.     // CORRECTED: Left motor fast forward (TURN_SPEED), Right motor reverse (-TURN_SPEED)
  446.     // This pivots the car to the LEFT around its center
  447.     setMotorSpeeds(TURN_SPEED, -TURN_SPEED);
  448.   }
  449.  
  450.   stopCar();
  451.   delay(TURN_PAUSE);
  452.   turning = false;
  453.   turnDecision = 0;
  454.   turnStr = "-";
  455. }
  456.  
  457. void turnRight() {
  458.   stopCar();
  459.   delay(100);
  460.   stateStr = "TURN RIGHT";
  461.   turnStr = "RIGHT";
  462.   Serial.println(">>> EXECUTING: RIGHT TURN WITH MONITORING <<<");
  463.  
  464.   frontClearedDuringTurn = false;
  465.   turnStartTime = millis();
  466.   unsigned long lastCheckTime = turnStartTime;
  467.  
  468.   // Execute turn with periodic front sensor monitoring
  469.   while (millis() - turnStartTime < TURN_90_TIME) {
  470.     unsigned long currentTime = millis();
  471.    
  472.     // Check front sensor every 50ms during turn
  473.     if (currentTime - lastCheckTime >= TURN_CHECK_INTERVAL) {
  474.       readAllSensors();
  475.       lastCheckTime = currentTime;
  476.      
  477.       // If front clears during turn, abort and resume wall-following
  478.       if (F > FRONT_CLEAR + 5) {
  479.         Serial.println("FRONT CLEARED DURING RIGHT TURN - ABORTING TURN");
  480.         frontClearedDuringTurn = true;
  481.         break;
  482.       }
  483.     }
  484.    
  485.     // CORRECTED: Left motor reverse (-TURN_SPEED), Right motor fast forward (TURN_SPEED)
  486.     // This pivots the car to the RIGHT around its center
  487.     setMotorSpeeds(-TURN_SPEED, TURN_SPEED);
  488.   }
  489.  
  490.   stopCar();
  491.   delay(TURN_PAUSE);
  492.   turning = false;
  493.   turnDecision = 0;
  494.   turnStr = "-";
  495. }
  496.  
  497. void turnAround() {
  498.   stopCar();
  499.   delay(100);
  500.   stateStr = "U-TURN";
  501.   turnStr = "U";
  502.   Serial.println(">>> EXECUTING: U-TURN WITH MONITORING <<<");
  503.  
  504.   frontClearedDuringTurn = false;
  505.   turnStartTime = millis();
  506.   unsigned long lastCheckTime = turnStartTime;
  507.  
  508.   // Execute U-turn (180 degrees) with periodic monitoring
  509.   while (millis() - turnStartTime < (TURN_90_TIME * 2)) {
  510.     unsigned long currentTime = millis();
  511.    
  512.     // Check front sensor every 50ms during turn
  513.     if (currentTime - lastCheckTime >= TURN_CHECK_INTERVAL) {
  514.       readAllSensors();
  515.       lastCheckTime = currentTime;
  516.      
  517.       // For U-turn, only abort if front suddenly becomes very clear
  518.       if (F > FRONT_CLEAR + 10) {
  519.         Serial.println("FRONT CLEARED DURING U-TURN - CONTINUING (may be legitimate)");
  520.         // For U-turns, we don't abort since we're going backwards anyway
  521.       }
  522.     }
  523.    
  524.     // Continue U-turning at TURN_SPEED with left motor forward, right motor reverse
  525.     setMotorSpeeds(TURN_SPEED, -TURN_SPEED);
  526.   }
  527.  
  528.   stopCar();
  529.   delay(TURN_PAUSE);
  530.   turning = false;
  531.   turnDecision = 0;
  532.   turnStr = "-";
  533. }
  534.  
  535. // ===================================================
  536. // WALL DETECTION
  537. // ===================================================
  538.  
  539. void selectWallToFollow() {
  540.   bool leftWallDetected = (L < WALL_DETECT);
  541.   bool rightWallDetected = (R < WALL_DETECT);
  542.  
  543.   if (leftWallDetected && rightWallDetected) {
  544.     followingWall = 3;
  545.     wallStr = "BOTH";
  546.     wallDistance = (L + R) / 2;
  547.   }
  548.   else if (leftWallDetected) {
  549.     followingWall = 1;
  550.     wallStr = "LEFT";
  551.     wallDistance = L;
  552.   }
  553.   else if (rightWallDetected) {
  554.     followingWall = 2;
  555.     wallStr = "RIGHT";
  556.     wallDistance = R;
  557.   }
  558.   else {
  559.     followingWall = 0;
  560.     wallStr = "NONE";
  561.   }
  562. }
  563.  
  564. // ===================================================
  565. // CONTINUOUS CENTERING CORRECTION
  566. // Uses REDUCED Kp=3 for smooth gentle corrections
  567. // ===================================================
  568.  
  569. void applyContinuousCentering() {
  570.   // Don't apply centering if we're turning
  571.   if (turning) return;
  572.  
  573.   int leftSpeed = BASE_SPEED;
  574.   int rightSpeed = BASE_SPEED;
  575.  
  576.   // EMERGENCY: Dangerously close to wall
  577.   if (L < WALL_DANGER || R < WALL_DANGER) {
  578.     stateStr = "AVOID";
  579.     if (L < WALL_DANGER) {
  580.       // Too close to left wall - steer RIGHT
  581.       rightSpeed = BASE_SPEED - 20;
  582.     } else {
  583.       // Too close to right wall - steer LEFT
  584.       leftSpeed = BASE_SPEED - 20;
  585.     }
  586.     moveForward(leftSpeed, rightSpeed);
  587.     return;
  588.   }
  589.  
  590.   // CASE 1: BOTH WALLS - Center between them
  591.   if (followingWall == 3) {
  592.     stateStr = "CENTER";
  593.     float centerError = R - L;
  594.    
  595.     if (abs(centerError) < WALL_DEADBAND) {
  596.       correction = 0;
  597.       wallIntegral = 0;
  598.     } else {
  599.       // Smooth P control with reduced Kp for gentle corrections
  600.       correction = centerError * KP * 0.8;
  601.       correction = constrain(correction, -CORRECTION_LIMIT, CORRECTION_LIMIT);
  602.     }
  603.    
  604.     leftSpeed = BASE_SPEED - correction;
  605.     rightSpeed = BASE_SPEED + correction;
  606.   }
  607.  
  608.   // CASE 2: ONLY LEFT WALL - Follow at exact distance
  609.   else if (followingWall == 1) {
  610.     stateStr = "FOLLOW L";
  611.     wallDistance = L;
  612.     float error = wallDistance - targetDistance;
  613.    
  614.     if (abs(error) < WALL_DEADBAND) {
  615.       correction = 0;
  616.     } else {
  617.       // Reduced Kp for smooth gentle wall following
  618.       correction = error * KP * 0.8;
  619.       correction = constrain(correction, -CORRECTION_LIMIT, CORRECTION_LIMIT);
  620.      
  621.       // Apply correction for LEFT wall
  622.       rightSpeed = BASE_SPEED - correction;
  623.     }
  624.   }
  625.  
  626.   // CASE 3: ONLY RIGHT WALL - Follow at exact distance
  627.   else if (followingWall == 2) {
  628.     stateStr = "FOLLOW R";
  629.     wallDistance = R;
  630.     float error = wallDistance - targetDistance;
  631.    
  632.     if (abs(error) < WALL_DEADBAND) {
  633.       correction = 0;
  634.     } else {
  635.       // Reduced Kp for smooth gentle wall following
  636.       correction = error * KP * 0.8;
  637.       correction = constrain(correction, -CORRECTION_LIMIT, CORRECTION_LIMIT);
  638.      
  639.       // Apply correction for RIGHT wall
  640.       leftSpeed = BASE_SPEED - correction;
  641.     }
  642.   }
  643.  
  644.   // CASE 4: NO WALLS - Go straight
  645.   else {
  646.     stateStr = "OPEN";
  647.     leftSpeed = BASE_SPEED;
  648.     rightSpeed = BASE_SPEED;
  649.     wallIntegral = 0;
  650.   }
  651.  
  652.   // Ensure minimum speed
  653.   leftSpeed = max(leftSpeed, MIN_SPEED);
  654.   rightSpeed = max(rightSpeed, MIN_SPEED);
  655.  
  656.   // Apply movement
  657.   moveForward(leftSpeed, rightSpeed);
  658. }
  659.  
  660. // ===================================================
  661. // TURN DECISION - ONLY TURN WHEN FRONT IS BLOCKED
  662. // WITH DUAL-SENSOR VALIDATION AND PRE-SCAN NOISE ELIMINATION
  663. // ===================================================
  664.  
  665. bool shouldTurn() {
  666.   // Don't check if already turning
  667.   if (turning) return false;
  668.  
  669.   // ONLY turn if front is actually blocked
  670.   bool frontBlocked = (F < FRONT_TURN);
  671.  
  672.   // If front is clear, NEVER turn
  673.   if (!frontBlocked) {
  674.     return false;
  675.   }
  676.  
  677.   // Front is blocked - check if sides are open
  678.   bool leftOpen = (L > OPEN_SPACE || FL > OPEN_SPACE);
  679.   bool rightOpen = (R > OPEN_SPACE || FR > OPEN_SPACE);
  680.  
  681.   // If at least one side is open, it's a turn
  682.   if (leftOpen || rightOpen) {
  683.     Serial.println("TURN NEEDED - Front blocked, side open");
  684.     return true;
  685.   }
  686.  
  687.   // If both sides are blocked, it's a dead end
  688.   Serial.println("DEAD END - Front and sides blocked");
  689.   return true;
  690. }
  691.  
  692. int decideTurnDirection() {
  693.   // Perform pre-turn scanning (5 reads with 100ms delays)
  694.   // This eliminates noise and ensures wall detection is accurate
  695.   performPreTurnScanning();
  696.  
  697.   Serial.println("\n=== TURN DECISION WITH DUAL-SENSOR VALIDATION ===");
  698.   Serial.print("Avg F="); Serial.print(avgF);
  699.   Serial.print(" Avg FL="); Serial.print(avgFL);
  700.   Serial.print(" Avg FR="); Serial.print(avgFR);
  701.   Serial.print(" Avg L="); Serial.print(avgL);
  702.   Serial.print(" Avg R="); Serial.println(avgR);
  703.  
  704.   float leftSpace = max(avgFL, avgL);
  705.   float rightSpace = max(avgFR, avgR);
  706.  
  707.   Serial.print("Left Space: "); Serial.print(leftSpace);
  708.   Serial.print(" Right Space: "); Serial.println(rightSpace);
  709.  
  710.   // Dead end - all sides blocked
  711.   if (leftSpace < FRONT_STOP && rightSpace < FRONT_STOP && avgF < FRONT_STOP) {
  712.     Serial.println("DEAD END - U-TURN");
  713.     return 3;
  714.   }
  715.  
  716.   // Evaluate turn directions
  717.   bool canTurnLeft = false;
  718.   bool canTurnRight = false;
  719.  
  720.   // LEFT TURN: Validate using dual-sensor validation
  721.   if (leftSpace > rightSpace + 2) {
  722.     canTurnLeft = validateOpeningDualSensor(true);
  723.   }
  724.  
  725.   // RIGHT TURN: Validate using dual-sensor validation
  726.   if (rightSpace > leftSpace + 2) {
  727.     canTurnRight = validateOpeningDualSensor(false);
  728.   }
  729.  
  730.   // If equal space, check both directions
  731.   if (abs(leftSpace - rightSpace) <= 2) {
  732.     canTurnLeft = validateOpeningDualSensor(true);
  733.     canTurnRight = validateOpeningDualSensor(false);
  734.   }
  735.  
  736.   // Choose valid turn direction
  737.   if (canTurnLeft && !canTurnRight) {
  738.     Serial.println("TURN LEFT - Validated");
  739.     return 1;
  740.   } else if (canTurnRight && !canTurnLeft) {
  741.     Serial.println("TURN RIGHT - Validated");
  742.     return 2;
  743.   } else if (canTurnLeft && canTurnRight) {
  744.     // Both valid - choose direction with more space
  745.     if (leftSpace > rightSpace) {
  746.       Serial.println("TURN LEFT - Both valid, more space on left");
  747.       return 1;
  748.     } else {
  749.       Serial.println("TURN RIGHT - Both valid, more space on right");
  750.       return 2;
  751.     }
  752.   } else {
  753.     // No valid direction - perform U-turn as fallback
  754.     Serial.println("NO VALID DIRECTION - U-TURN");
  755.     return 3;
  756.   }
  757. }
  758.  
  759. // ===================================================
  760. // LCD DISPLAY
  761. // ===================================================
  762.  
  763. void updateLCD() {
  764.   lcd.clear();
  765.  
  766.   lcd.setCursor(0, 0);
  767.   lcd.print("F:");
  768.   lcd.print((int)F);
  769.   lcd.print(" L:");
  770.   lcd.print((int)L);
  771.   lcd.print(" R:");
  772.   lcd.print((int)R);
  773.  
  774.   lcd.setCursor(0, 1);
  775.   lcd.print("FL:");
  776.   lcd.print((int)FL);
  777.   lcd.print(" FR:");
  778.   lcd.print((int)FR);
  779.  
  780.   lcd.setCursor(0, 2);
  781.   lcd.print(moveStr);
  782.   lcd.print(" ");
  783.   lcd.print(stateStr);
  784.   lcd.print(" ");
  785.   lcd.print(wallStr);
  786.  
  787.   lcd.setCursor(0, 3);
  788.   lcd.print("SPD:");
  789.   lcd.print(currentLeftSpeed);
  790.   lcd.print("/");
  791.   lcd.print(currentRightSpeed);
  792.  
  793.   // Serial debug - less frequent
  794.   static unsigned long lastDebug = 0;
  795.   if (millis() - lastDebug > 500) {
  796.     Serial.print("State:");
  797.     Serial.print(stateStr);
  798.     Serial.print(" F:");
  799.     Serial.print(F);
  800.     Serial.print(" L:");
  801.     Serial.print(L);
  802.     Serial.print(" R:");
  803.     Serial.print(R);
  804.     Serial.print(" Speed:");
  805.     Serial.print(currentLeftSpeed);
  806.     Serial.print("/");
  807.     Serial.println(currentRightSpeed);
  808.     lastDebug = millis();
  809.   }
  810. }
  811.  
  812. // ===================================================
  813. // SETUP
  814. // ===================================================
  815.  
  816. void setup() {
  817.   pinMode(ENA, OUTPUT);
  818.   pinMode(IN1, OUTPUT);
  819.   pinMode(IN2, OUTPUT);
  820.   pinMode(ENB, OUTPUT);
  821.   pinMode(IN3, OUTPUT);
  822.   pinMode(IN4, OUTPUT);
  823.  
  824.   lcd.init();
  825.   lcd.backlight();
  826.   lcd.clear();
  827.   lcd.print("ADVANCED CORNER");
  828.   lcd.setCursor(0, 1);
  829.   lcd.print("SAFE NAVIGATOR");
  830.   lcd.setCursor(0, 2);
  831.   lcd.print("v5.0 CORRECTED");
  832.   delay(2500);
  833.   lcd.clear();
  834.  
  835.   Serial.begin(9600);
  836.   Serial.println("\n=== ADVANCED CORNER-SAFE NAVIGATION ===");
  837.   Serial.println("Features:");
  838.   Serial.println("- Pre-turn scanning (5 reads, 100ms delays)");
  839.   Serial.println("- Dual-sensor validation (diagonal + side)");
  840.   Serial.println("- Reduced Kp=3 for smooth corrections");
  841.   Serial.println("- Turn monitoring with early abort");
  842.   Serial.println("- CORRECTED MOTOR COMMANDS FOR TURNS");
  843.   Serial.println("====================================\n");
  844.  
  845.   readAllSensors();
  846.   delay(500);
  847. }
  848.  
  849. // ===================================================
  850. // MAIN LOOP - ADVANCED CORNER-SAFE NAVIGATION
  851. // ===================================================
  852.  
  853. void loop() {
  854.   // Read sensors
  855.   readAllSensors();
  856.  
  857.   // Select which wall to follow
  858.   selectWallToFollow();
  859.  
  860.   // ANTI-STALL: Check if we're stuck
  861.   checkForStall();
  862.  
  863.   // DECISION MAKING - WITH ADVANCED VALIDATION
  864.   if (!turning) {
  865.     // Check if we need to turn (ONLY when front is actually blocked)
  866.     if (F < FRONT_TURN) {
  867.       // Front is blocked - we need to make a decision
  868.       turning = true;
  869.       stopCar();
  870.       delay(300);
  871.      
  872.       // Decide turn direction with pre-scan and dual-sensor validation
  873.       turnDecision = decideTurnDirection();
  874.      
  875.       // Execute the turn with monitoring
  876.       if (turnDecision == 1) {
  877.         turnLeft();
  878.        
  879.         // If front cleared during turn, resume wall-following instead of continuing
  880.         if (frontClearedDuringTurn) {
  881.           Serial.println("Resuming wall-following after turn abort");
  882.           stateStr = "RESUME";
  883.         }
  884.       } else if (turnDecision == 2) {
  885.         turnRight();
  886.        
  887.         // If front cleared during turn, resume wall-following instead of continuing
  888.         if (frontClearedDuringTurn) {
  889.           Serial.println("Resuming wall-following after turn abort");
  890.           stateStr = "RESUME";
  891.         }
  892.       } else {
  893.         turnAround();
  894.       }
  895.     }
  896.     else {
  897.       // Front is clear - just do wall following/centering
  898.       applyContinuousCentering();
  899.     }
  900.   }
  901.  
  902.   // Update display
  903.   updateLCD();
  904.  
  905.   // Small delay for stability
  906.   delay(50);
  907. }
  908.  
  909. /* END CODE */
  910.  
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
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!