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; 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++) { values[i] = normalizeSensor(analogRead(sensorPins[i]), 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; } 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); } float kp = 0.08; float ki = 0.0; float kd = 0.10; int baseSpeed = 70; int maxCorrection = 120; long lastError = 0; long integral = 0; const long INTEGRAL_LIMIT = 3000; bool followEnabled = false; 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); if (position < 0) { integral = 0; stopRobot(); printLog(-1, 0, 0, 0, 0, 0, "LOST_LINE"); } else { long error = position - SETPOINT; long derivative = error - lastError; integral += error; integral = constrain(integral, -INTEGRAL_LIMIT, INTEGRAL_LIMIT); int correction = (int)(kp * error + ki * integral + kd * derivative); correction = constrain(correction, -maxCorrection, maxCorrection); int leftSpeed = constrain(baseSpeed + correction, -255, 255); int rightSpeed = constrain(baseSpeed - correction, -255, 255); setDrive(leftSpeed, rightSpeed); printLog(position, error, integral, derivative, correction, leftSpeed, "LINE_FOUND"); lastError = error; } 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 == '+') { 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 == '}') { ki += 0.0001; printConfig(); } else if (c == '{') { ki -= 0.0001; if (ki < 0) ki = 0; printConfig(); } else if (c == '1') { baseSpeed = 60; printConfig(); } else if (c == '2') { baseSpeed = 70; printConfig(); } else if (c == '3') { baseSpeed = 85; printConfig(); } else if (c == 'R' || c == 'r') { integral = 0; Serial.println("integral=0"); } else if (c == 'H' || c == 'h') { printHelp(); } } void printConfig() { Serial.print("base="); Serial.print(baseSpeed); Serial.print(",kp="); Serial.print(kp, 3); Serial.print(",ki="); Serial.print(ki, 5); Serial.print(",kd="); Serial.println(kd, 3); } void printHelp() { Serial.println("RLF15D sweep: F=follow, S=stop, +/-=Kp, ]/[=Kd, }/{=Ki, 1/2/3=speed, R=reset integral, H=help"); printConfig(); } void printLog(long position, long error, long integralValue, long derivative, int correction, int leftSpeed, const char* state) { int rightSpeed = constrain(baseSpeed - correction, -255, 255); Serial.print("position="); Serial.print(position); Serial.print(",error="); Serial.print(error); Serial.print(",integral="); Serial.print(integralValue); Serial.print(",derivative="); Serial.print(derivative); Serial.print(",kp="); Serial.print(kp, 3); Serial.print(",ki="); Serial.print(ki, 5); Serial.print(",kd="); Serial.print(kd, 3); Serial.print(",correction="); Serial.print(correction); Serial.print(",left="); Serial.print(leftSpeed); Serial.print(",right="); Serial.print(rightSpeed); Serial.print(",state="); Serial.println(state); }

Tuning Kp, Kd, Ki bằng bảng thử nghiệm và dữ liệu vòng chạy

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