const int SENSOR_COUNT = 3; const int sensorPins[SENSOR_COUNT] = {A1, A0, A2}; const int sensorPositions[SENSOR_COUNT] = {0, 1000, 2000}; int sensorMin[SENSOR_COUNT] = {240, 210, 300}; int sensorMax[SENSOR_COUNT] = {850, 780, 910}; const bool LINE_IS_LOW = true; int linePresentMinSum = 100; const int SETPOINT = 1000; const int STBY_PIN = 4; const int LEFT_PWM = 5; const int LEFT_IN1 = 7; const int LEFT_IN2 = 8; const int RIGHT_PWM = 6; const int RIGHT_IN1 = 9; const int RIGHT_IN2 = 10; const bool LEFT_MOTOR_INVERTED = false; const bool RIGHT_MOTOR_INVERTED = false; int normalizeSensor(int raw, int minValue, int maxValue, bool lineIsLow) { if (maxValue <= minValue) return 0; int value = map(raw, minValue, maxValue, 0, 1000); value = constrain(value, 0, 1000); if (lineIsLow) value = 1000 - value; return value; } void readAllNormalized(int values[]) { for (int i = 0; i < SENSOR_COUNT; i++) { values[i] = normalizeSensor(analogRead(sensorPins[i]), sensorMin[i], sensorMax[i], LINE_IS_LOW); } } long strengthSum(int values[]) { long sum = 0; for (int i = 0; i < SENSOR_COUNT; i++) sum += values[i]; return sum; } long linePosition(int values[]) { long sum = 0; long weightedSum = 0; for (int i = 0; i < SENSOR_COUNT; i++) { sum += values[i]; weightedSum += (long)values[i] * sensorPositions[i]; } if (sum < linePresentMinSum) return -1; return weightedSum / sum; } void setupMotorPins() { pinMode(STBY_PIN, OUTPUT); pinMode(LEFT_PWM, OUTPUT); pinMode(LEFT_IN1, OUTPUT); pinMode(LEFT_IN2, OUTPUT); pinMode(RIGHT_PWM, OUTPUT); pinMode(RIGHT_IN1, OUTPUT); pinMode(RIGHT_IN2, OUTPUT); digitalWrite(STBY_PIN, HIGH); } void setDrive(int leftSpeed, int rightSpeed) { setLeftMotor(leftSpeed); setRightMotor(rightSpeed); } void setLeftMotor(int speed) { if (LEFT_MOTOR_INVERTED) speed = -speed; setMotorRaw(LEFT_PWM, LEFT_IN1, LEFT_IN2, speed); } void setRightMotor(int speed) { if (RIGHT_MOTOR_INVERTED) speed = -speed; setMotorRaw(RIGHT_PWM, RIGHT_IN1, RIGHT_IN2, speed); } void setMotorRaw(int pwmPin, int in1Pin, int in2Pin, int speed) { speed = constrain(speed, -255, 255); if (speed > 0) { digitalWrite(in1Pin, HIGH); digitalWrite(in2Pin, LOW); analogWrite(pwmPin, speed); } else if (speed < 0) { digitalWrite(in1Pin, LOW); digitalWrite(in2Pin, HIGH); analogWrite(pwmPin, -speed); } else { digitalWrite(in1Pin, LOW); digitalWrite(in2Pin, LOW); analogWrite(pwmPin, 0); } } void stopRobot() { setDrive(0, 0); } enum RobotState { FOLLOW_LINE, SEARCH_LINE, STOPPED }; RobotState state = STOPPED; float kp = 0.08; float kd = 0.10; int baseSpeed = 70; int searchSpeed = 70; int maxCorrection = 110; long lastError = 0; String lastDirection = "CENTER"; unsigned long searchStartTime = 0; unsigned long searchTimeoutMs = 1500; void setup() { Serial.begin(9600); setupMotorPins(); stopRobot(); printHelp(); } void loop() { handleSerial(); int values[SENSOR_COUNT]; readAllNormalized(values); long position = linePosition(values); if (state == FOLLOW_LINE) { if (position < 0) { state = SEARCH_LINE; searchStartTime = millis(); } else { followLine(position); } } else if (state == SEARCH_LINE) { if (position >= 0) { state = FOLLOW_LINE; followLine(position); } else if (millis() - searchStartTime > searchTimeoutMs) { state = STOPPED; stopRobot(); } else { searchLine(); } } else { stopRobot(); } printLog(position); delay(40); } void handleSerial() { if (Serial.available() <= 0) return; char c = Serial.read(); if (c == 'F' || c == 'f') { state = FOLLOW_LINE; Serial.println("follow=ON"); } else if (c == 'S' || c == 's') { state = STOPPED; stopRobot(); Serial.println("follow=OFF"); } else if (c == '+') { kp += 0.01; printConfig(); } else if (c == '-') { kp -= 0.01; if (kp < 0) kp = 0; printConfig(); } else if (c == ']') { kd += 0.02; printConfig(); } else if (c == '[') { kd -= 0.02; if (kd < 0) kd = 0; printConfig(); } else if (c == 'R') { searchSpeed += 5; printConfig(); } else if (c == 'r') { searchSpeed -= 5; if (searchSpeed < 30) searchSpeed = 30; printConfig(); } else if (c == 'T') { searchTimeoutMs += 250; printConfig(); } else if (c == 't') { if (searchTimeoutMs > 500) searchTimeoutMs -= 250; printConfig(); } else if (c == 'H' || c == 'h') { printHelp(); } } void followLine(long position) { long error = position - SETPOINT; long derivative = error - lastError; int correction = (int)(kp * error + kd * derivative); correction = constrain(correction, -maxCorrection, maxCorrection); int leftSpeed = constrain(baseSpeed + correction, -255, 255); int rightSpeed = constrain(baseSpeed - correction, -255, 255); setDrive(leftSpeed, rightSpeed); lastError = error; if (error < -80) lastDirection = "LEFT"; else if (error > 80) lastDirection = "RIGHT"; } void searchLine() { if (lastDirection == "LEFT") setDrive(-searchSpeed, searchSpeed); else setDrive(searchSpeed, -searchSpeed); } void printConfig() { Serial.print("kp="); Serial.print(kp, 3); Serial.print(",kd="); Serial.print(kd, 3); Serial.print(",searchSpeed="); Serial.print(searchSpeed); Serial.print(",timeout="); Serial.println(searchTimeoutMs); } void printHelp() { Serial.println("RLF13E recovery tuning: F=follow, S=stop, +/-=Kp, ]/[=Kd, R/r=search speed, T/t=timeout, H=help"); printConfig(); } void printLog(long position) { Serial.print("state="); Serial.print(stateName(state)); Serial.print(",position="); Serial.print(position); Serial.print(",lastDirection="); Serial.print(lastDirection); Serial.print(",timeout="); Serial.println(searchTimeoutMs); } const char* stateName(RobotState s) { if (s == FOLLOW_LINE) return "FOLLOW_LINE"; if (s == SEARCH_LINE) return "SEARCH_LINE"; return "STOPPED"; }

Xử lý mất line, cua gấp và chiến lược tìm lại line

Đăng nhập để học miễn phí