Bé, un cop construit el nostre robot, el qual creiem que esta format dels millors elements i materials possibles, el que ens toca és començar a iniciar el moviment del robot.
De moment només hem aconseguit que el robot fagi algunes petites ordres, però cal comentar que acabem d'iniciar la programació i que d'aqui uns dies ja la dominarem i aconseguirem que el robot segeixi la línia blanca amb la màxima velocitat possible, el qual es l'objectiu principal.
El microcontrolador que porta la placa Baby Orangutan del robot és un Micro Atmega 328P. Per a programar-lo utilitzarém el connector AVRISP, la funció d'aquest és portar les ordres del ordinador al microcontrolador. Per a programar-lo utilizem un programa anomenat AVR estudio 4.
Per a començar em iniciat tres moviments fàcils:
#include
int main()
{
set_digital_input(IO_D4,PULL_UP_ENABLED);
while(is_digital_input_high(IO_D4))
{
}
while(1)
{
red_led(1);
delay_ms(100);
red_led(0);
delay_ms(100);
}
}
Amb aquest programa aconseguim que en cas de que no es pitgi el polsador el led es queda apagat, però quan el pitgem aquest comença a fer pampallugues amb un interval de 100 milisegons .
#include
int main()
{
set_digital_input(IO_D4,PULL_UP_ENABLED);
while(is_digital_input_high(IO_D4))
{
}
while(1)
{
delay_ms(500);
set_motors(20,-20);
delay_ms(500);
set_motors(-20,20);
delay_ms(1000);
set_motors(20,-20);
}
}
Per a fer aquest programa hem utilitzat la mateixa metodologia que per a fer les pampallugues del led, però en aquest cas el que fem es moure els dos motors del robot.
#include
int main()
{
set_digital_input(IO_D4,PULL_UP_ENABLED);
while(is_digital_input_high(IO_D4))
{
}
delay_ms(500);
for (char i = 0; i < 80; i++)
{
if ((i<21) (i>59))
{
set_motors(20,-20);
}
else
{
set_motors(-20,20);
}
delay_ms(20);
}
set_motors(0,0);
}
Aquest programa funciona de manera diferent que els dos anteriors. El que fem ara és que el microcontrolador conti el temps, i segons en quin instant es troba ha de fer un moviment o un altre. Aquest programa ens anirà bé per a què en iniciar les curses amb el robot rastrejador, aquest faci el moviment abans de començar i així els sensors reculleixin la informació dels canvis de llum que troba al terra.
·Lectura sensors:
Aquesta part del programa serveix per a detectar el color sobre el qual es despaça el robot, cada
sensor detecta un color, i això serveix per detectar on esta la linia que ha de seguir el robot.
Aquestes dades senvien al programa Putty que enregistra les dades captades pel sensors.
#include
#include
unsigned int sensors[8]; // array amb les lectures dels sensors
unsigned char qtr_analog_pins[] = {7, 6, 0, 1, 2, 3, 4, 5}; // com estan col·locats els sensors
char buffer[4]; // buffer per realitzar la conversio enter - ascii
int main()
{s
erial_set_baud_rate(9600);// velocitat comunicacio serie
qtr_analog_init(qtr_analog_pins, 8, 3, IO_D7); // 8 sensors, 3 mostres per sensor. emissor D7
set_digital_input(IO_D4,PULL_UP_ENABLED);
while(is_digital_input_high(IO_D4))
{
}
while(1)
{
delay_ms(500);
qtr_read(sensors, QTR_EMITTERS_ON); // lectura analogica dels sensors
qtr_emitters_off(); //apaguem els sensors
for (char i = 0; i < 8; i++)
{
sensors[i]=1024-sensors[i];
itoa(sensors[i],buffer,10); // convertim a ascii la lectura del sensor i
serial_send(buffer, 4); // ho enviem pel port serie
delay_ms(10); // esperem
serial_send(" ", 1); // espai
delay_ms(10); // esperem
}s
erial_send("\r\n",2); // canvi de linia
}
}
·Detecció de la posició del robot.
Una vegada tenim els valors obtinguts pels sensors hem de determinar la posició en la qual es troba el robot respecta la línia blanca. D'aquesta manera el podrem corregir per tal de que sempre tingui els dos sensors centrals sobre la línia.
#include
#include
unsigned int sensors[8]; // array amb les lectures dels sensors
unsigned char qtr_analog_pins[] = {7, 6, 0, 1, 2, 3, 4, 5}; // com estan col·locats els sensors
char buffer[4]; // buffer per realitzar la conversio enter - ascii
int main()
{s
erial_set_baud_rate(9600);// velocitat comunicacio serie
qtr_analog_init(qtr_analog_pins, 8, 3, IO_D7); // 8 sensors, 3 mostres per sensor. emissor D7
set_digital_input(IO_D4,PULL_UP_ENABLED);
while(is_digital_input_high(IO_D4))
{
}
while(1)
{
delay_ms(500);
qtr_read(sensors, QTR_EMITTERS_ON); // lectura analogica dels sensors
qtr_emitters_off(); //apaguem els sensors
unsigned int suma=0;
for (unsigned char i = 0; i < 8; i++)
{
sensors[i]=(1024-sensors[i])/10;
suma= suma+sensors[i];
itoa(sensors[i],buffer,10); // convertim a ascii la lectura del sensor i
serial_send(buffer, 4); // ho enviem pel port serie
delay_ms(10); // esperem
serial_send(" ", 1); // espai
delay_ms(10); // esperem
}s
erial_send("\r\n",2); // canvi de linia
itoa(suma,buffer,10);
serial_send(buffer, 4);
delay_ms(10); // esperem
serial_send("\r\n",2);
unsigned int posicio= (10*sensors[0] + 20*sensors[1] + 30*sensors[2] + 40*sensors[3] +
50*sensors[4] + 60*sensors[5] + 70*sensors[6] + 80*sensors[7])/suma;
itoa(posicio,buffer,10);
serial_send(buffer, 4);
delay_ms(10); // esperem
serial_send("\r\n",2);
}
}
·Funcionament robot amb control proporcional
Quan ja tenim la posició per a poder corregir la posició del robot fem servir l'error proporcional per a variar les velocitats de cadascuna de les rodes, depenent del sentit de la corba. D'aquesta manera el robot sempre seguira la línia blanca.
#include
/*
* motors1: for for the Orangutan LV, SV, SVP, X2, Baby-O and 3pi robot.
*
* This example uses the OrangutanMotors functions to drive
* motors in response to the position of user trimmer potentiometer
* and blinks the red user LED at a rate determined by the trimmer
* potentiometer position. It uses the OrangutanAnalog library to measure
* the trimpot position, and it uses the OrangutanLEDs library to provide
* limited feedback with the red user LED.
*
* http://www.pololu.com/docs/0J20
* http://www.pololu.com
* http://forum.pololu.com
*/
int main()
{
unsigned char velocitat=115;
int cp=0;
int kp=30;
int posicio=50;
int posicio_ant=50;
unsigned int sensors[8]; // array amb les lectures dels sensors
int sensors_max[]={86, 97, 100, 100, 100, 100, 100, 100};
int sensors_min[]={14, 14, 25, 27, 28, 33, 29, 31};
unsigned char qtr_analog_pins[] = {7, 6, 0, 1, 2, 3, 4, 5}; // com estan col·locats els sensors
qtr_analog_init(qtr_analog_pins, 8, 3, IO_D7); // 8 sensors, 3 mostres per sensor. emissor D7
set_digital_input(IO_D4,PULL_UP_ENABLED);
while(is_digital_input_high(IO_D4));
{}
delay_ms(500);
// aqui caldria posar la sequencia del polsador per iniciar.
while(1)
{
posicio_ant=posicio;
qtr_read(sensors, QTR_EMITTERS_ON); // lectura analogica dels sensors
qtr_emitters_off(); //apaguem els sensors
// delay_ms(500); // esperem
unsigned int suma=0;
for (unsigned char i=0;i<=7;i++) // sumem valors i escalem perque no surti d'escala. Linia
blanca, fons negre
{
// sensors[i]=(sensors[i])/10; //fons blanc
sensors[i]=(1024-sensors[i])/10; //fons negre
if (sensors[i]>=sensors_max[i])
{
sensors[i]=100;
}else if (
sensors[i]<=sensors_min[i])
{
sensors[i]=0;
}else
{
sensors[i]=100*(sensors[i]-sensors_min[i])/(sensors_max[i]-sensors_min[i]);
}suma=suma +
sensors[i];
}
if (suma>100)
{
posicio=15*sensors[0]+25*sensors[1]+35*sensors[2]+45*sensors[3]+55*sensors[4]+65*sensors[5]
+75*sensors[6]+85*sensors[7];
posicio=posicio/suma;
}
else
{
if (posicio_ant<50)
{
posicio=15;
}else
{
posicio=85;
}
}
// ara que tenim la posicio, calcularem els errors
cp= kp*(posicio-50)/10;
if (cp>0)
{
if (cp>velocitat)
{
cp=velocitat;
}s
et_motors(-velocitat,-(velocitat-cp));
}else
{
if (cp<-velocitat)
{
cp=velocitat;
}s
et_motors(-(velocitat+cp),-velocitat);
}
}
}
A partir de canviar les variables de velocitat i de la constant, l'error proporcional canvia i podem fer que augmenti la rapidesa.
