From 84e054a5e5e087cf141cfee430cf07cf7c7f17c1 Mon Sep 17 00:00:00 2001 From: "M. Umar Shahbaz" Date: Fri, 20 Sep 2024 23:42:19 +0500 Subject: [PATCH] Found and Edited Logical Errors --- LFR.ino => Line-Follower.ino | 45 +++++++++++++++++++----------------- 1 file changed, 24 insertions(+), 21 deletions(-) rename LFR.ino => Line-Follower.ino (66%) diff --git a/LFR.ino b/Line-Follower.ino similarity index 66% rename from LFR.ino rename to Line-Follower.ino index 8567b51..0ff3af2 100644 --- a/LFR.ino +++ b/Line-Follower.ino @@ -1,17 +1,17 @@ // Motor Connections -const int LF = 2; // Left Forward -const int LS = 3; // Left Speed -const int LB = 4; // Left Backward -const int RF = 5; // Right Forward -const int RS = 6; // Right Speed -const int RB = 7; // Right Backward +const int RF = 7; // Left Forward +const int RS = 6; // Left Speed +const int RB = 8; // Left Backward +const int LF = 9; // Right Forward +const int LS = 5; // Right Speed +const int LB = 10; // Right Backward // IR Array Connections -const int LH_IR = 8; // Very Left IR -const int LM_IR = 9; // Slight Left IR -const int M_IR = 10; // Middle IR -const int RM_IR = 11; // Slight Right IR -const int RH_IR = 12; // Very Right IR +const int LH_IR = 11; // Very Left IR +const int LM_IR = 12; // Slight Left IR +const int M_IR = 13; // Middle IR +const int RM_IR = 3; // Slight Right IR +const int RH_IR = 4; // Very Right IR // IR Value containers int LH_V; @@ -22,8 +22,8 @@ int RH_V; //Motor Speed Values const int Max = 255; //Maximum Speed -const int TurnSensitivity = 0.42; // Speed / Wobble -int TurnSpeed; +const int TurnSensitivity = 0.72; // Speed = k * Wobble +int TurnSpeed; // Speed of non-dominant side when turning void setup() { @@ -34,7 +34,8 @@ void setup() { pinMode(RF , OUTPUT); pinMode(RB , OUTPUT); pinMode(RS , OUTPUT); - TurnSpeed = TurnSensitivity * Max; + // TurnSpeed = TurnSensitivity * Max; Use with Gyro and Maths to caluclate on the run | Code for Gyro to be added soon + // TurnSpeed = 115; Use to manually set in case of use without sensors } void loop() { @@ -48,25 +49,27 @@ void loop() { if (M_V == 1) { // Middle sensor is not on track if (LM_V == 0) { // Slight Left IR is on track while(M_V != 0 && LH_V != 0) { // Turn slighly left until middles sensor is back on track or car has offshooted - left(Max, TurnSpeed); + forward(TurnSpeed, Max); M_V = digitalRead(M_IR); + LH_V = digitalRead(LH_IR); } } if (RM_V == 0) { // Slight Right IR is on track while(M_V != 0 && RH_V != 0) { // Turn slighly right until middles sensor is back on track or car has offshooted - right(TurnSpeed, Max); + forward(Max, TurnSpeed); M_V = digitalRead(M_IR); + RH_V = digitalRead(RH_IR); } } if (LH_V == 0) { // Very Left IR is on track - while(M_V != 0) { // Turn left at max speed until middles sensor is back on track - left(Max, Max); + while(M_V != 0) { // Rotate left at TurnSpeed until middles sensor is back on track + left(TurnSpeed, TurnSpeed); M_V = digitalRead(M_IR); } } if (RH_V == 0) { // Very Right IR is on track - while(M_V != 0) { // Turn right at max speed until middles sensor is back on track - right(Max, Max); + while(M_V != 0) { // Rotate right at TurnSpeed speed until middles sensor is back on track + right(TurnSpeed, TurnSpeed); M_V = digitalRead(M_IR); } } @@ -108,4 +111,4 @@ void right(int L_Speed, int R_Speed) { digitalWrite(LB , HIGH); digitalWrite(RF , HIGH); digitalWrite(RB , LOW); -} +} \ No newline at end of file