Paso 6: código de
El código para el robot básicamente sólo toma lecturas del sensor de la línea interpreta tiene donde está en la línea y le dice a cada uno de los motores lo rápidos para ejecutar. Nuestro robot tiene sensor 8 que significa que si suponemos es siempre en el mismo lado de la línea hay 9 posibles posiciones 0-8 que están representados por cuántos de los sensores son de.
Queremos que el robot se centra sobre el borde de la línea así que vamos a configurar la "consigna" para 4 que es el espacio entre el sensor de 4 º y 5 º. Trabajo de los controladores PID tomando algunos deseado valor y donde usted está y toma la diferencia de los dos que es el "error". El controlador de la porción entonces convertidores el error en una salida utilizable. En este caso el error es igual al valor de ajuste menos la posición. El paso es seleccionar la afinación llamada constantes Kp y Kd en el código. También son llamadas la proporcional y derivados constantes y será únicos a tu robot basado en sus motores y su medida exacta aunque nuestros números de deben llegar cerca. KP es la cantidad de diferencia entre los dos motores por unidad de error de línea de PWM y se multiplica por el error. Por ejemplo, si el sensor Lee 0 el error significado 4 el robot es demasiado extrema derecho que desea que el motor izquierdo se detenga un PWM de cero y el motor adecuado para funcionar a toda velocidad. Si Kp es 64 64 * error es 256 y resta de la velocidad de motor normal (255) la izquierda vendrá a una parada completa. Como el robot empieza a girar el error será menor y la diferencia del motor será menor hasta que el sensor ve 4 otra vez y es a toda velocidad.
¿Esto suena gran derecho? No es porque el robot ya es oscilante y tarda algunos parada hará pivotar derecho pasar el sensor de 4 y 5. Para evitar esto tenemos que frenar mucho más rápidamente a medida que nos acercamos al valor de ajuste. Esto es donde Kd para jugar. KD es multiplicado por el cambio en el error y agregado a la parte de Kp. En este ejemplo te recogemos donde nos quedamos arriba donde ahora el sensor es la lectura 1. Así que el error es 3 y el control proporcional es decir el motor izquierdo a 63 y en 255. El cambio en el error es -1 (3-4) y el Kd es 16 para un total de -16. Cuando añadir a la parte proporcional que obtienes una velocidad del motor de 47 en la rueda izquierda y 255 de la derecha. Todo el detalle puede verse en la función pid en el código y jugar con un Kd Kp valores para ver qué tan rápido puede hacer que tu bot.
Suerte para los hicimos la parte más difícil para usted. Copiar y pegar el siguiente código en el IDE de Arduino y subirlo a su tablero y su línea siguiente robot debe estar listo para rodar!
#define NUM_SENSORS 8 #define avgSpeed 255 int leftWheelf=3; int leftWheelr = 5; int rightWheelf=6; int rightWheelr = 9; int setpoint=4, val; unsigned long lastTime=0, timeChange=0; int Sampletime=20, outMax=255, outMin=-255; float error,sumerr,lastError,output,ITerm,DTerm; float Kp=avgSpeed/4, Ki=0, Kd=.25*Kp; int pos; unsigned int sensor[]={A0,A1,A2,A3,A4,A5,A6,A7}; unsigned int sensorValue[NUM_SENSORS]; int threshold = 200; byte incomingByte; int bias=5; void setup() { Serial.begin(115200); pinMode(4,INPUT); } void loop() { // Serial.println(digitalRead(4)); if (digitalRead(4)== true) { unsigned int Wsum=0; int sum=0; for (int i=0;i<NUM_SENSORS;i++) { sensorValue[i]=analogRead(sensor[i]); if (sensorValue[i] < threshold) sensorValue[i]=1; else sensorValue[i]=0; //Serial.print(i); Serial.print(": "); Serial.println(sensorValue[i]); //delay(250); } for (int i=0;i<NUM_SENSORS;i++) { sum=sensorValue[i]+sum; pos=sum; } // Serial.println(pos); //delay(100); timeChange = millis()-lastTime; if (timeChange >= Sampletime){ pid(pos); } } else { if (Serial.available() > 0) { incomingByte = Serial.read(); } if (incomingByte == 'w') { forward(); } else if (incomingByte == 's') { reverse(); } else if (incomingByte == 'd') { rightTurn(); } else if (incomingByte == 'a') { leftTurn(); } else { brake(); } } } void pid (int val) { error=setpoint-val; ITerm+=(Ki*error); DTerm=Kd*(error-lastError)/(Sampletime/1000.0); lastError=error; if(ITerm > outMax) ITerm=outMax; else if (ITerm < outMin) ITerm=outMin; output=Kp*error+ITerm+DTerm; if (output>outMax) output=outMax; else if (output<outMin) output=outMin; lastTime=millis(); Serial.println(val); if (output>0) { analogWrite(leftWheelf,avgSpeed); analogWrite(rightWheelf,avgSpeed); analogWrite(leftWheelr,abs(output)); analogWrite(rightWheelr,0); } else if (output<0) { analogWrite(leftWheelf,avgSpeed); analogWrite(rightWheelf,avgSpeed); analogWrite(leftWheelr,0); analogWrite(rightWheelr,abs(output)); } else { analogWrite(leftWheelf,avgSpeed); analogWrite(rightWheelf,avgSpeed); analogWrite(leftWheelr,0); analogWrite(rightWheelr,0); } } void forward() { analogWrite(leftWheelf, avgSpeed - bias); analogWrite(leftWheelr, 0); analogWrite(rightWheelr, 0); analogWrite(rightWheelf, avgSpeed); } void leftTurn() { analogWrite(leftWheelf,0); analogWrite(leftWheelr, 0); analogWrite(rightWheelr,0); analogWrite(rightWheelf, avgSpeed); } void rightTurn() { analogWrite(leftWheelf,avgSpeed); analogWrite(leftWheelr, 0); analogWrite(rightWheelr,0); analogWrite(rightWheelf, 0); } void reverse() { analogWrite(leftWheelf, 0); analogWrite(leftWheelr, avgSpeed-bias); analogWrite(rightWheelr, avgSpeed); analogWrite(rightWheelf, 0); } void brake() { analogWrite(leftWheelf, 0); analogWrite(leftWheelr, 0); analogWrite(rightWheelr, 0); analogWrite(rightWheelf, 0); }
