diff --git a/PravegaCode/Line_follower_code.cpp b/PravegaCode/Line_follower_code.cpp new file mode 100644 index 0000000..f0b8bb8 --- /dev/null +++ b/PravegaCode/Line_follower_code.cpp @@ -0,0 +1,430 @@ +//#include +#include +#include + +// === BLE Setup === +/*BluetoothSerial SerialBT; +bool bleActive = true;*/ +// === Last Pivot Direction === +bool lastPivotLeft = true; // default to left +bool pivotPending = false; // flag to execute pivot even if overshoot + + +// === QTR Sensor Setup === +QTRSensors qtr; +const uint8_t sensorPins[] = {26, 27, 14, 15, 13, 4, 35, 34}; +const uint8_t sensorCount = 8; +uint16_t sensorValues[sensorCount]; + +// === Motor Pins (TB6612FNG) === +#define AIN1 21 +#define AIN2 22 +#define PWMA 23 +#define BIN1 25 +#define BIN2 33 +#define PWMB 32 +#define STBY 19 + +// === Create Motors using TB6612 library === +Motor motorLeft = Motor(AIN1, AIN2, PWMA, 1,STBY); +Motor motorRight = Motor(BIN1, BIN2, PWMB, 1,STBY); + +// === PID Control Variables === +float Kp = 2.5, Ki = 0.0, Kd = 0.0; +int baseSpeed = 120; +int maxSpeed = 255; +float error = 0, lastError = 0, integral = 0, derivative = 0; + +// === Turn Constants === +const int TURN_SPEED = 100; +const int TURN_45_TIME = 250; +const int TURN_90_TIME = 450; +const float BLACK_THRESHOLD = 0.90; +const float WHITE_THRESHOLD = 0.80; + +// === Turn Detection Variables === +unsigned long lastTurnTime = 0; +const unsigned long TURN_COOLDOWN = 2000; +int consecutiveTurnDetections = 0; +const int REQUIRED_CONSECUTIVE_DETECTIONS = 5; + +// === Time-Based Pattern Filtering === +unsigned long firstTurnDetectionTime = 0; +const unsigned long MIN_PATTERN_DURATION = 150; +const unsigned long MAX_PATTERN_DURATION = 800; +bool patternTimingActive = false; +int totalDetectionsInWindow = 0; +const int MIN_DETECTIONS_IN_WINDOW = 8; + +// === Flags === +enum Mode { WAITING, RUNNING }; +Mode mode = WAITING; + +// === Motor Function (Wrapper for TB6612 library) === +void setMotor(int leftSpeed, int rightSpeed) { + motorLeft.drive(leftSpeed); + motorRight.drive(rightSpeed); +} + +// === BLE Setup === +void setupBLE() { + if (!SerialBT.begin("LineBot")) { + Serial.println("Failed to start BLE"); + return; + } + Serial.println("BLE started. Awaiting commands..."); + bleActive = true; +} + +// === BLE Stop === +void stopBLE() { + if (bleActive) { + SerialBT.end(); + Serial.println("BLE stopped. Freed ADC2 for sensors."); + bleActive = false; + } +} + +// === PID Update === +void updatePID(String cmd) { + if (cmd.startsWith("kp")) { + Kp = cmd.substring(2).toFloat(); + Serial.printf("Updated Kp = %.2f\n", Kp); + } else if (cmd.startsWith("kd")) { + Kd = cmd.substring(2).toFloat(); + Serial.printf("Updated Kd = %.2f\n", Kd); + } else if (cmd.startsWith("ki")) { + Ki = cmd.substring(2).toFloat(); + Serial.printf("Updated Ki = %.2f\n", Ki); + } else if (cmd.startsWith("base")) { + baseSpeed = cmd.substring(4).toInt(); + Serial.printf("Updated Base Speed = %d\n", baseSpeed); + } +} + +// === Reset PID State === +void resetPIDState() { + integral = 0; + lastError = 0; + error = 0; + + patternTimingActive = false; + totalDetectionsInWindow = 0; + consecutiveTurnDetections = 0; + + Serial.println("PID state and turn detection state reset"); +} + +// === Pivot Turn Function === +void pivotTurn(bool turnLeft, bool is90Degree = false) { + int turnTime = is90Degree ? TURN_90_TIME : TURN_45_TIME; + + Serial.printf("Pivot turning %s (%d degrees)\n", + turnLeft ? "LEFT" : "RIGHT", + is90Degree ? 90 : 45); + + setMotor(0, 0); + delay(350); + + if (turnLeft) { + setMotor(-TURN_SPEED, TURN_SPEED); + } else { + setMotor(TURN_SPEED, -TURN_SPEED); + } + + delay(turnTime); + setMotor(0, 0); + delay(100); + + resetPIDState(); + lastTurnTime = millis(); + + Serial.printf("Turn completed. Next turn allowed after %lu ms\n", TURN_COOLDOWN); +} + +// === Normalize Sensor Values === +void normalizeSensorValues(float* normalized) { + for (int i = 0; i < sensorCount; i++) { + // Serial.print(sensorValues[i]); + // Serial.print(" "); + normalized[i] = sensorValues[i] / 1000.0; + } + + static unsigned long lastSensorPrint = 0; + if (millis() - lastSensorPrint > 500) { + for (int i = 0; i < sensorCount; i++) { + //Serial.print(normalized[i], 2); + // Serial.print(" "); + } + lastSensorPrint = millis(); + } +} + +// === Check for All Black === +bool isAllBlack(float* normalized) { + int blackCount = 0; + for (int i = 0; i < sensorCount; i++) { + if (normalized[i] > BLACK_THRESHOLD) { + blackCount++; + } + } + return blackCount >= 8; +} + +// === Time-Based Pattern Stability Check === +bool isPatternStable(bool currentDetection) { + unsigned long currentTime = millis(); + + if (currentDetection) { + if (!patternTimingActive) { + firstTurnDetectionTime = currentTime; + patternTimingActive = true; + totalDetectionsInWindow = 1; + return false; + } + + totalDetectionsInWindow++; + unsigned long patternDuration = currentTime - firstTurnDetectionTime; + + if (patternDuration >= MIN_PATTERN_DURATION) { + float detectionRate = (float)totalDetectionsInWindow / (patternDuration / 50.0); + if (totalDetectionsInWindow >= MIN_DETECTIONS_IN_WINDOW && detectionRate > 0.7) { + Serial.printf("Pattern stable: %lums duration, %d detections, rate: %.2f\n", + patternDuration, totalDetectionsInWindow, detectionRate); + return true; + } + } + + if (patternDuration > MAX_PATTERN_DURATION) { + Serial.println("Pattern timeout - resetting (possible false positive)"); + patternTimingActive = false; + totalDetectionsInWindow = 0; + return false; + } + return false; + } else { + if (patternTimingActive) { + unsigned long patternDuration = currentTime - firstTurnDetectionTime; + Serial.printf("Pattern broken after %lums, %d detections\n", + patternDuration, totalDetectionsInWindow); + patternTimingActive = false; + totalDetectionsInWindow = 0; + } + return false; + } +} + +// === Sharp Turn Detection === +bool detectSharpTurn(float* normalized, bool* isLeft) +{ + if (millis() - lastTurnTime < TURN_COOLDOWN) { + return false; + } + + bool currentTurnDetected = false; + bool currentIsLeft = false; + + if (normalized[7] > BLACK_THRESHOLD && normalized[6] > BLACK_THRESHOLD && normalized[5] > BLACK_THRESHOLD && normalized[5] > BLACK_THRESHOLD && normalized[0] < WHITE_THRESHOLD) + { + currentIsLeft = true; + currentTurnDetected = true; + } + + else if (normalized[0] > BLACK_THRESHOLD && normalized[1] > BLACK_THRESHOLD && normalized[3] > BLACK_THRESHOLD && normalized[2] > BLACK_THRESHOLD && normalized[7] < WHITE_THRESHOLD) + { + currentIsLeft = false; + currentTurnDetected = true; + } + + if (isPatternStable(currentTurnDetected)) { + *isLeft = currentIsLeft; + patternTimingActive = false; + totalDetectionsInWindow = 0; + consecutiveTurnDetections = 0; + + Serial.printf("TURN CONFIRMED: %s turn after stable pattern detection\n", + currentIsLeft ? "LEFT" : "RIGHT"); + lastPivotLeft = currentIsLeft; + pivotPending = true; + return true; + } + return false; +} + +// === Check if on line === +bool isOnLine(float* normalized) { + int blackCount = 0; + for (int i = 0; i < sensorCount; i++) { + if (normalized[i] > BLACK_THRESHOLD) { + blackCount++; + } + } + return blackCount >= 2; +} + +// === Line Recovery === +void lineRecovery(float* normalized) { + Serial.println("Line lost → starting recovery..."); + unsigned long startTime = millis(); + const unsigned long RECOVERY_TIMEOUT = 1000; + + while (millis() - startTime < RECOVERY_TIMEOUT) { + qtr.read(sensorValues); + normalizeSensorValues(normalized); + uint16_t position = qtr.readLineBlack(sensorValues); + if (!((position-1000)<=3000)) { + Serial.println("Line recovered!"); + return; + } + setMotor(-70, 70); + //just spin left if the bot loses the line + + delay(10); + } + + /* if(millis() - startTime > RECOVERY_TIMEOUT){ + startTime = millis(); + + while (millis() - startTime < RECOVERY_TIMEOUT) { + qtr.read(sensorValues); + normalizeSensorValues(normalized); + uint16_t position = qtr.readLineBlack(sensorValues); + if (!((position-1000)<=3000)) { + Serial.println("Line recovered!"); + return; + } + + setMotor(100,-100); + //just spin right if the bot loses the line + + delay(15); + } + }*/ + + + + Serial.println("Recovery failed → stopping"); + setMotor(0, 0); +} + +// === Line Following === +void lineFollow() { + uint16_t position = qtr.readLineBlack(sensorValues); + + float normalized[sensorCount]; + normalizeSensorValues(normalized); + + // === 1. If pivot was detected earlier, execute it even if overshoot === + if (pivotPending) { + Serial.println(lastPivotLeft); + pivotTurn(lastPivotLeft, false); + pivotPending = false; + return; + } + + // === 2. If line completely lost (white) === + if (!isOnLine(normalized)) { + Serial.println("Line lost → continuing last pivot direction"); + if (lastPivotLeft) { + setMotor(-70, 70); // keep pivoting left + } else { + setMotor(70, -70); // keep pivoting right + } + return; + } + + // === 3. If all sensors see black === + if (isAllBlack(normalized)) { + setMotor(baseSpeed, baseSpeed); + Serial.println("All black detected → Going straight"); + return; + } + + bool isLeftTurn; + if (detectSharpTurn(normalized, &isLeftTurn)) { + pivotTurn(isLeftTurn, false); + return; + } + // === 4. Normal line following (PID) === + error = ((int)position - 3500) / 1000.0; + integral += error; + integral = constrain(integral, -50, 50); + + derivative = error - lastError; + lastError = error; + + float correction = Kp * error + Ki * integral + Kd * derivative; + + int leftSpeed = constrain(baseSpeed - correction * 100, -maxSpeed, maxSpeed); + int rightSpeed = constrain(baseSpeed + correction * 100, -maxSpeed, maxSpeed); + + setMotor(leftSpeed, rightSpeed); + + static unsigned long lastPrint = 0; + if (millis() - lastPrint > 300) { + Serial.printf("Pos:%d Err:%.2f L:%d R:%d Corr:%.2f\n", + position, error, leftSpeed, rightSpeed, correction); + lastPrint = millis(); + } +} + + + +// === Calibration === +void calibrateSensors() { + Serial.println("Calibrating sensors..."); + Serial.println("Move the robot over the line during calibration..."); + + for (int i = 0; i < 400; i++) { + qtr.calibrate(); + if (i % 100 == 0) { + Serial.printf("Calibration progress: %d%%\n", (i * 100) / 400); + } + delay(10); + } + Serial.println("Calibration complete."); + + Serial.println("Calibration values:"); + for (int i = 0; i < sensorCount; i++) { + Serial.printf("Sensor %d: Min=%d, Max=%d\n", + i, qtr.calibrationOn.minimum[i], qtr.calibrationOn.maximum[i]); + } +} + +// === Setup === +void setup() { + Serial.begin(115200); + + pinMode(STBY, OUTPUT); + digitalWrite(STBY, HIGH); + + qtr.setTypeAnalog(); + qtr.setSensorPins(sensorPins, sensorCount); + + setupBLE(); + + Serial.println("Commands: kpX, kiX, kdX, baseX, calibrate"); + Serial.println("Example: kp1.5, ki0.1, kd0.2, base80"); +} + +// === Loop === +void loop() { + if (mode == WAITING && bleActive && SerialBT.available()) { + String cmd = SerialBT.readStringUntil('\n'); + cmd.trim(); + Serial.printf("Received: %s\n", cmd.c_str()); + + if (cmd == "calibrate") { + stopBLE(); + calibrateSensors(); + mode = RUNNING; + Serial.println("Calibration done → RUNNING mode"); + } else { + updatePID(cmd); + } + } + + if (mode == RUNNING) { + lineFollow(); + } +}