|
|
@ -36,6 +36,9 @@ float kp=.5; |
|
|
|
float ki=0; |
|
|
|
float kd=0; |
|
|
|
int velocidad=100; |
|
|
|
int ultimaPosicion=0; |
|
|
|
int errorAnterior=0; |
|
|
|
int P,I,D; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@ -80,6 +83,12 @@ void setup() { |
|
|
|
//------------------------------------------------------------
|
|
|
|
void loop() { |
|
|
|
int pos=getPosicion(); |
|
|
|
int pos=getPosicion(); |
|
|
|
if(pos!=0){ |
|
|
|
ultimaPosicion=pos; |
|
|
|
}else{ |
|
|
|
pos=ultimaPosicion; |
|
|
|
} |
|
|
|
/*
|
|
|
|
//PINTAR LOS VALORES
|
|
|
|
for(int x=0;x<numSensores;x++){ |
|
|
@ -102,8 +111,13 @@ void loop() { |
|
|
|
* TODO: se debe agregar el código para incluir los valores de |
|
|
|
* D e I, pues actualmante solo tutiliza control P |
|
|
|
*/ |
|
|
|
float pid=kp*error; |
|
|
|
acelerar(70+pid,70-pid); |
|
|
|
P=error; |
|
|
|
D=error-errorAnterior; |
|
|
|
I=I+error; |
|
|
|
errorAnterior=error; |
|
|
|
float pid=(kp*P)+(ki*I)+(kd*D); |
|
|
|
|
|
|
|
acelerar(velocidad-pid,velocidad+pid); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|