Robot Minisumo PID ~ Aprendiendo Fácil Electronica

Hace meses que no escribia un post sobre robots, y que mejor momento para hacerlo en estas vacaciones, asi que ahi vamos roboteros!
Nota: Este post lo estaré editando constantemente sobre la marcha.

La teoría con la cual vamos a partir es las de utilizar un controlador PID (controlador proporcional integral derivativo) para que el robot rastree a su oponente usando las características propias de un controlador PID, bien es sabido que un controlador PID es mas eficiente y mas preciso que un controlador ON/OFF, este ultimo es muy utilizado en las competencias de robots minisumos y sumos en sus algoritmos de detección. Al usar el controlador ON/OFF surgen muchos inconvenientes en el cual he visto que competidores se van topando para programar a sus robots, por ejemplo, que pasa si quiero subir la velocidad de los motores al giro? Pues el robot empezara a oscilar de un lado a otro como si se volviera loco. y en ese trance, pues el robot pierde energía y al estar asi de inestable, pues el contrincante le gana la partida, porque lo detecto mas rapido.
Con un controlador PID operando estos problemas son superados dando una ventaja sobre el oponente ala vez que el robot gasta menos energia.

Materiales:
-Chasis robot Minisumo + cuchillas(con medidas oficiales)
-llantas de goma + Aro de aluminio
-Arduino Nano
-Sensores Sharp 340K
-Driver TB6612FNG
-baterias lipo 500mA
-Motores fingertech 22:1
- resistencias de 1 ohm , 10 kohm, 330ohm, capacitore ceramico de 100nf , capacitor electrolitico 100uf, pulsadores, leds jumpers.

Para este Ejemplo se usan 3 Sensores digitales, Pero también se puede extender a muchos más sensores. Siguiendo la misma idea que es la de ponderar los pesos de los sensores en función del numero. Mientras mas sensores existan, la resolución es mayor, por ende la precisión del PID Aumenta. Si el numero de sensores es mínimo, pues vuelve a ser como un control ON/OFF.

Es cierto que las siempre sera necesario calibrar las contantes Integral, derivativa y proporcional que van acordes al sistema físico: Peso, tamaño, centro de gravedad, constante de rozamiento, relación par-motor, velocidad, etc... Modelar un sistema así es complejo... Una alternativa mas razonables es empezar calibrando con ensayo y error hasta paulatinamente ir reduciendo las oscilaciones. Empezar aumentando la constante proporcional, y luego el diferencial... y si es necesario (aunque no seria recomendable para cuando se usen pocos sensores) calibrar la constante integral.

ESQUEMÁTICO:

PID 3 SENSORES y MOTORES Mini MAXON (RUMI)

//Entradas
#define b_start 18
#define b_stop 17
#define boton1 14
#define boton2 15
#define boton3 16

#define sensor_i 4
#define sensor_c 3
#define sensor_d 2
//Salidasr
#define led1 5
#define mi1 9
#define mi2 8
#define pwmi 11 //~
#define md1 7
#define md2 6
#define pwmd 10 //~
//variables

////////////////CONFIGURACION DE VARIABLES DE ROBOT MINISUMO RUMI////////////////////
/////////////////////////MOTORES MAXON, 3 SENSORES //////////////////////////////////

/////////////////Delay de seguridad////////////////////////
long delay_salida=5000; //5 segundos
////////////////////////////////////////////////////////////////////
////////////////Salida giro tipo "licuadora"///////////////////////
int izq_l_i=-195; //por default: -195 , caso salida izquierda, motor izquierdo retrocede
int der_l_i=195; //por defaul: 195 ,caso salida izquierda, motor derecho avanza
int izq_l_d=195; //por default: 195 , caso salida derecha, motor izquierdo avanza
int der_l_d=-195; //por defaul: -195 , caso salida derecha, motor derecho retrocede
/////////////////////////////////////////////////////////////////////
//////////////// Salida giro tipo "coja"/////////////////////////////
int izq_c_i=20; //por default: 20 , caso salida izquierda, motor izquierdo avanza
int der_c_i=255; //por default:255 ,caso salida izquierda, motor derecho avanza
int izq_c_d=255; //por default: 255 ,caso salida derecha, motor izquierdo avanza
int der_c_d=20; //por default:20 ,caso salida derecha, motor derecho avanza
long tiempo_c=90000; //por default:90000 ,tiempo que permanece haciendo coja
int izq_c_i_l=-200; //default: -200 ,terminado el tiempo giro licuadora, caso salida izquierda, motor izquierdo retrocede
int der_c_i_l=200; //default: 200 ,terminado el tiempo giro licuadora, caso salida izquierda, motor derecho avanza
int izq_c_d_l=200; //default: 200 ,terminado el tiempo giro licuadora, caso salida derecha, motor izquierdo avanza
int der_c_d_l=-200; //default: -200 ,terminado el tiempo giro licuadora, caso salida derecha, motor derecho retrocede
//////////////////////////////////////////////////////////

////////////////Frenos de contorno////////////////////////
int izq_f_i=-120; ///Default:-120 retorno a la izquierda, motor izquierdo retrocede
int der_f_i=200; ///Default:200 retorno a la izquierda, motor derecho avanza
int izq_f_d=200; ///Default:200 retorno a la derecha, motor izquierdo avanza
int der_f_d=-120; ///Default:-120 retorno a la derecha, motor derecho retrocede
////////////////////Constantes PID////////////////////////
int velocidad=255; //aqui cambias velocidad 0-100 //
float KP=35.78; //25.78 , 35.78 //
float KD=110; //20 ,50 //
float KI=0; // ,0 //
//
////////////////////////////////////////////////////////

/////////////Otras Variables auxiliares///////////////////////////////////////
int _numSensors=3; //cantidad de sensores usados
int sensors_values[3]; //numero de sensores usados
int last_value=0;

int btn1=0,btn2=0,btn3=0;
int s1=0,s2=0,s3=0;
int posicion;
int proporcional=0;
int integral=0;
int derivativo=0;
int proporcional_pasado=0;
int salida_control=0;
int caso=0;
//////////////////////////////////////////////////////

void setup() {
delay(50);
motores(0,0);
pinMode(boton1,INPUT);
pinMode(boton2,INPUT);
pinMode(boton3,INPUT);
pinMode(boton3,INPUT);
pinMode(b_start,INPUT);
pinMode(b_stop,INPUT);

pinMode(sensor_i,INPUT);
pinMode(sensor_c,INPUT);
pinMode(sensor_d,INPUT);

pinMode(13,OUTPUT);
pinMode(led1,OUTPUT);
pinMode(mi1,OUTPUT);
pinMode(mi2,OUTPUT);
pinMode(pwmi,OUTPUT);
pinMode(md1,OUTPUT);
pinMode(md2,OUTPUT);
pinMode(pwmd,OUTPUT);
digitalWrite(13,HIGH);
Serial.begin(115200);

while(true)
{
botones();
if(btn1==1){caso=1;digitalWrite(13,LOW);delay(delay_salida);} //izquierda
if(btn2==1){caso=2;digitalWrite(13,LOW);delay(2000);}//estrategia
if(btn3==1){caso=3;digitalWrite(13,LOW);delay(delay_salida);} //derecha
botones();
int b_s=digitalRead(b_start);

if(b_s==1)
{
if(caso==1)
{
sensores();
motores(izq_l_i,der_l_i);
if(s1==1 || s2==1 || s3==1)
{
break;
}
}

if(caso==2)
{
s1=0,s2=0,s3=0;
while(true)
{
botones();
if(btn1==1)//izquierda coja
{
digitalWrite(13,HIGH);
delay(delay_salida);
long k=0;
motores(izq_c_i,der_c_i);
while(true)
{
sensores();
k++;
if(k==tiempo_c)
{
digitalWrite(led1,HIGH);
motores(izq_c_i_l,der_c_i_l);
k=0;
}
if(s1==1 || s2==1 || s3==1)
{
break;
}
}
break;
}
if(btn2==1)// defrente con con semicoja a la izquierda
{
digitalWrite(13,HIGH);
delay(delay_salida);
long k=0;
motores(140,250);
while(true)
{
sensores();
k++;
if(k==20000)
{
digitalWrite(led1,HIGH);
motores(-180,180);
k=0;
}
if(s1==1 || s2==1 || s3==1)
{
break;
}
}
break;
}

if(btn3==1)//derecha coja
{
digitalWrite(13,HIGH);
delay(delay_salida);
long k=0;
motores(izq_c_d,der_c_d);
while(true)
{
sensores();
k++;
if(k==tiempo_c)
{
digitalWrite(led1,HIGH);
motores(izq_c_d_l,der_c_d_l);
k=0;
}
if(s1==1 || s2==1 || s3==1)
{
break;
}
}
break;
}
}
break;
}

if(caso==3)
{
sensores();
motores(izq_l_d,der_l_d);
if(s1==1 || s2==1 || s3==1)
{
break;
}
}
}

}
}

void loop()
{
led_run(); //led palpitando
//Serial.println(posicion); //imprime posicion de robot
tracking3(); //seguimiento pid robot
frenos_contorno(); // retorno de robot cuando no se ha sensado
//delay(1);
if(s1==1&&s2==1&&s3==1){motores(255,255);}
}
int l;
void led_run()
{
l++;
if(l==15){ digitalWrite(led1,!digitalRead(led1));l=0;}

}

void frenos_contorno()
{
if(posicion>=20)
{
motores(izq_f_i,der_f_i);
while(true)
{
sensores();
if(s1==1 || s2==1 || s3==1) break;
}

}

if(posicion<=0)
{
motores(izq_f_d,der_f_d);
while(true)
{
sensores();
if(s1==1 || s2==1 || s3==1) break;
}

}

}
void test()
{
digitalWrite(led1,!digitalRead(led1));
botones();
sensores();
Serial.print(btn1);
Serial.print(" ");
Serial.print(btn2);
Serial.print(" ");
Serial.print(s1);
Serial.print(" ");
Serial.print(s2);
Serial.print(" ");
Serial.print(s3);
Serial.println(" ");
delay(100);
}

void motores(int motor_izq, int motor_der){
if(motor_izq >= 0)
{
digitalWrite(mi1,LOW); digitalWrite(mi2,HIGH); analogWrite(pwmi,motor_izq);
}
else
{
digitalWrite(mi1,HIGH); digitalWrite(mi2,LOW); motor_izq = motor_izq*(-1); analogWrite(pwmi,motor_izq);
}
if ( motor_der >= 0 ) //motor derecho
{
digitalWrite(md1,LOW); digitalWrite(md2,HIGH); analogWrite(pwmd,motor_der);
}
else
{
digitalWrite(md1,HIGH); digitalWrite(md2,LOW); motor_der= motor_der*(-1); analogWrite(pwmd,motor_der);
}
}
void botones()
{
btn1=digitalRead(boton1);
btn2=digitalRead(boton2);
btn3=digitalRead(boton3);
}

void sensores()
{
s3=!digitalRead(sensor_i);
s2=!digitalRead(sensor_c);
s1=!digitalRead(sensor_d);
}

void tracking()
{
posicion=ponderacion();
proporcional=(posicion) - 10;
integral=integral+proporcional_pasado;
derivativo=(proporcional-proporcional_pasado);
int ITerm=integral*KI;
if(ITerm>=250)ITerm=250;
if(ITerm<=-250)ITerm=-250;

salida_control=(proporcional*KP)+(derivativo*KD)+ITerm;

if(salida_control>velocidad) salida_control=velocidad;
if(salida_control<-velocidad) salida_control=-velocidad;

proporcional_pasado=proporcional;

if(salida_control<0)//si se salio por la izquierda
{
motores(-salida_control,salida_control);
}
if(salida_control>0)//si se salio por la derecha
{
motores(-salida_control,salida_control);
}
if(salida_control==0) //si esta en punto de consigna
{
proporcional=(posicion) - 10;
motores(100,100);
}
// Serial.println(salida_control);
}

void tracking2()
{
posicion=ponderacion();
integral=integral+proporcional_pasado;
derivativo=(proporcional-proporcional_pasado);
int ITerm=integral*KI;
if(ITerm>=100)ITerm=100;
if(ITerm<=-100)ITerm=-100;

salida_control=(proporcional*KP)+(derivativo*KD)+ITerm;

if(salida_control>velocidad) salida_control=velocidad;
if(salida_control<-velocidad) salida_control=-velocidad;

proporcional_pasado=proporcional;

if(salida_control<0)//si se salio por la izquierda
{
motores(-salida_control,0);
}
if(salida_control>0)//si se salio por la derecha
{
motores(0,salida_control);
}
Serial.println(salida_control);
}

void tracking3() // envestida
{
posicion=ponderacion();
proporcional=(posicion) - 10;
integral=integral+proporcional_pasado;
derivativo=(proporcional-proporcional_pasado);
int ITerm=integral*KI;
if(ITerm>=250)ITerm=250;
if(ITerm<=-250)ITerm=-250;

salida_control=(proporcional*KP)+(derivativo*KD)+ITerm;

if(salida_control>velocidad) salida_control=velocidad;
if(salida_control<-velocidad) salida_control=-velocidad;

if(salida_control<0)//si se salio por la izquierda
{

motores(velocidad,velocidad+salida_control);
}
if(salida_control>0)//si se salio por la derecha
{
motores(velocidad-salida_control , velocidad);
}
if(salida_control==0)//si se salio por la derecha
{
motores(velocidad , velocidad);
}
// Serial.println(salida_control);
proporcional_pasado=proporcional;

}

int ponderacion()
{
int i=0;
long avg=0;
long sum=0;
int on_line=0;
sensores();
sensors_values[0]=s1;sensors_values[1]=s2;sensors_values[1]=s2;sensors_values[2]=s3;
// for(int i=0;i<=3;i++){Serial.print(sensors_values[i]);}
//Serial.println("");
for(i=0;i<_numSensors;i++)
{
int value=sensors_values[i];
if(value>0)
{
on_line=1;
}
avg+=(long)(value)*(i*10);
sum+=value;
}
if(!on_line)
{
if(last_value<1)
{
return 0;
}
else
{
return(_numSensors-1)*10;
}
}
last_value = avg/sum;
return last_value;
}

Aquí pueden observar una deliciosa demostracion de este enfoque de robot sumo PID

https://www.youtube.com/watch?v=lIbFQubTADw

https://www.youtube.com/watch?v=K_XCGDzmQNA

https://www.youtube.com/watch?v=wOoIbljZTU0