const int SENSOR_COUNT = 3; const int sensorPins[SENSOR_COUNT] = {A1, A0, A2}; const int sensorPositions[SENSOR_COUNT] = {0, 1000, 2000}; // Thay bằng profile calibration thật từ RLF05. int sensorMin[SENSOR_COUNT] = {240, 210, 300}; int sensorMax[SENSOR_COUNT] = {850, 780, 910}; const bool LINE_IS_LOW = true; const int LINE_PRESENT_MIN_SUM = 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++) { int raw = analogRead(sensorPins[i]); values[i] = normalizeSensor(raw, sensorMin[i], sensorMax[i], LINE_IS_LOW); } } 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 < LINE_PRESENT_MIN_SUM) return -1; return weightedSum / sum; } long strengthSum(int values[]) { long sum = 0; for (int i = 0; i < SENSOR_COUNT; i++) sum += values[i]; return 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); } int baseSpeed = 70; int turnLow = 35; int turnHigh = 85; int deadband = 120; bool followEnabled = false; long lastError = 0; void setup() { Serial.begin(9600); setupMotorPins(); stopRobot(); printHelp(); } void loop() { handleSerial(); if (!followEnabled) { stopRobot(); delay(80); return; } int values[SENSOR_COUNT]; readAllNormalized(values); long position = linePosition(values); bool lostLine = position < 0; long error = lastError; if (lostLine) { stopRobot(); printLog(position, error, "STOP_LOST", 0, 0); } else { error = position - SETPOINT; lastError = error; int leftSpeed = baseSpeed; int rightSpeed = baseSpeed; const char* action = "FORWARD"; if (error < -deadband) { leftSpeed = turnLow; rightSpeed = turnHigh; action = "TURN_LEFT"; } else if (error > deadband) { leftSpeed = turnHigh; rightSpeed = turnLow; action = "TURN_RIGHT"; } setDrive(leftSpeed, rightSpeed); printLog(position, error, action, leftSpeed, rightSpeed); } delay(40); } void handleSerial() { if (Serial.available() <= 0) return; char c = Serial.read(); if (c == 'F' || c == 'f') { followEnabled = true; Serial.println("follow=ON"); } else if (c == 'S' || c == 's') { followEnabled = false; stopRobot(); Serial.println("follow=OFF"); } else if (c == '+') { deadband += 20; printConfig(); } else if (c == '-') { deadband -= 20; if (deadband < 20) deadband = 20; printConfig(); } else if (c == '1') { baseSpeed = 60; turnLow = 30; turnHigh = 75; printConfig(); } else if (c == '2') { baseSpeed = 70; turnLow = 35; turnHigh = 85; printConfig(); } else if (c == '3') { baseSpeed = 90; turnLow = 45; turnHigh = 105; printConfig(); } else if (c == 'H' || c == 'h') { printHelp(); } } void printConfig() { Serial.print("base="); Serial.print(baseSpeed); Serial.print(",turnLow="); Serial.print(turnLow); Serial.print(",turnHigh="); Serial.print(turnHigh); Serial.print(",deadband="); Serial.println(deadband); } void printHelp() { Serial.println("RLF08C Serial tuning: F=follow, S=stop, +/-=deadband, 1/2/3=speed, H=help"); printConfig(); } void printLog(long position, long error, const char* action, int leftSpeed, int rightSpeed) { Serial.print("position="); Serial.print(position); Serial.print(",error="); Serial.print(error); Serial.print(",action="); Serial.print(action); Serial.print(",left="); Serial.print(leftSpeed); Serial.print(",right="); Serial.println(rightSpeed); }

Điều khiển on/off và phân tích hạn chế khi robot chạy nhanh

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