• Author / Uploaded
• Fernando Gomez

Citation preview

#include const int trigPin = 9; const int echoPin = 10; const int servoPin = 6; long duration; //Tiempo que toma el eco para la vuelta en el ultrasonico. int distance; //Ubicacion de la bola en el sensor. int pos = 0; //Posicion del servomotor. double StartAngle = 112; //Angulo del servomotor cuando la bola esta al paralelo de la superficie. double pError,iError,dError; //Valores de error asociado con el PID. double Output; //Suma de valores de error multiplicados con su correspondiente ganancia. double prevError; //Error de previa prevencion de error. unsigned long prevTime,now; //Tiempo previo y tiempo corriendo. double dt; //Cambio del tiempo. int Setpoint = 0; //Punto deseado de la pelota. float Kp = 3.400; //Constante proporcional. float Ki = 0.159; //Constante de integracion. float Kd = 17.836; //Constante derivativa. Servo servo;

//Crea la variable servo para controlar el servomotor.

void setup() { pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); servo.attach(servoPin); servo.write(StartAngle); Serial.begin(9600);

//Configura el trigPin como salida. //Configura el echoPin como una entrada //Configura el pin 6 como salida del servomotor. //Iniciar el servomotor en el angulo horizontal. //Inicia la comunicacion serial.

} void loop() { digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); duration = pulseIn(echoPin, HIGH); distance= duration*0.034/2 - 15; //Imprime la distancia en el monitor serial. if (distance > 40 or distance < -40) { distance = -12; //pone el valor de distancia como 12 para evitar que genere valores aleatorios. } if (distance == 0 or distance == 0) { distance = 0; //pone el valor de distancia como 0 para evitar que genere valores aleatorios. } //Algoritmo PID now = millis(); dt = (now - prevTime); pError = Setpoint - distance; dError = (pError - prevError) / dt; iError = iError + (pError * dt); if (iError > 10) {

iError = 10; es ruido. }

//Si el error es mas grande que 10 que no haga nada, porque

Output = Kp * pError + Ki * iError + Kd * dError; //Aqui se evita que se generen valores aleatorios. if (Output >40) { Output = 40; } if (Output