Primer de tot es declaren dos vectors, un anomenat posicio de divuit posicions on s’hi guardarà la posició anterior del moviment que es vagi a fer, inicialment té tots els valors a zero. L’altre, també de divuit posicions, anomenat ID_Motor on s’hi guarda la ID dels motors.
Es crea un objecte de la classe CDS5500, se li pot posar el nom que es vulgui, en aquest cas es SERVO. Serveix perquè cada cop que es vol utilitzar una funció de la llibreria s’ha d’escriure el nom de l’objecte seguit d’un punt i el nom de la funció . Ex: SERVO.Write( ID, posició; velocitat).
Després es crea una funció anomenada presetPose amb dinou paràmetres d’entrada, els primers divuit són les posicions en que es vol que es fiquin els motors, en el mateix ordre en que s’han escrit les ID. L’últim valor és el de la velocitat.
A continuació es declara un altre vector, anomenat ptr_musculs de divuit posicions i en un principi no se li assigna cap valor a cap d’elles. Més endavant es col·locarà cada un dels divuit valors de les posicions introduïts a la funció a una posició a la posició del vector corresponent.
Finalment hi ha un for per escriure cada posició al motor adequat. Aquest for se li declara una variable anomenada i, que cada cop que entri al for li sumarà 1 i que el for es farà fins que el valor de i arribi a disset.
Dins aquest for primer es dona una ordre de posició fent servir la funció Write, com a paràmetres hi ha el valor que hi ha dins el vector ID_Motor en la posició i, el valor que hi ha dins el vector ptr_musculs en la posició i, i la velocitat que li hem introduït inicialment. Com que les posicions estan en el mateix ordre que les ID i estan col·locades en la mateixa posició en cada vector, el for s’executarà les divuit vegades necessàries pels divuit motors amb la seva posició corresponent.
A continuació es calcula el temps necessari per realitzar el moviment a través d’una equació. És calcula el recorregut que fa el motor i transformant-ho en un angle, sabent que el valor 512 que es posa en el vector equival a 150 graus. També es calcula la velocitat en graus per segon, sabent que el valor 0x3FF que es posa al vector equival a 0,372 graus per milisegon. Es divideixen aquestes dues magnituds per obtenir el temps, i es fa el valor absolut per si donés un valor negatiu al restar la posició anterior i l’actual, ja que només interessa veure la diferència entre les dues posicions i no en quin sentit s’ha produït el canvi.
Després se li assigna a la posició del vector posicio el valor que s’ha introduït, ja que passa a ser l’últim moviment fet.
Per últim s’aplica un delay de la quarta part del que triga el motor a fer el moviment, això és perquè després d’haver fet diverses probes s’ha comprovat que si no hi ha un mínim de temps d’espera el robot no segueix les ordres programades.
Hi ha diverses funcions que el que fan es quan se les crida, criden a la funció presetPose i realitzen el moviment que se’ls hi ha introduït.
- La funció Walk_ready és la posició estàndard de repòs, posa el robot amb les cames semi flexionades i els braços a baix.
- La funció Saludar fa que el robot aixequi el braç dret i el mogui de dreta a esquerra.
- La funció Clap fa que el robot aplaudeixi amb les dues mans
- La funció Caminar_endavant fa que el robot camini tot movent les cames, els braços acompanyen el moviment
- La funció Iron_Man_Green_Lantern és la funció dissenyada per quan s’apaguen els llums, fa que el braç dret es posi en un angle de noranta graus amb el cos.
Finalment hi ha una última funció que no forma part del moviment del robot, sinó que serveix per assignar el color dels LED RGB de la boca amb més facilitat. Té tres paràmetres d’entrada, un per a cada intensitat de cada color. Dins de la funció s’escriu aquest valor al pin corresponent. endavant
Codi de la llibreria:
#include “moviments.h”
int posicio[18] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // vector per guardar últim registre de moviment realitzat
int ID_Motor[18] = {13, 23, 12, 22, 11, 21, 14, 24, 16, 26, 15, 25, 17, 27, 18, 28, 19, 29}; // id de motors
CDS5500 SERVO;
void presetPose(int mus1, int mus2, int mus3, int mus4, int mus5, int mus6, int mus7, int mus8, int mus9, int mus10, int mus11, int mus12, int mus13, int mus14, int mus15, int mus16, int mus17, int mus18, int vel)
{
int ptr_musculs[18];
ptr_musculs[0] = mus1; ptr_musculs[1] = mus2; ptr_musculs[2] = mus3; ptr_musculs[3] = mus4; ptr_musculs[4] = mus5; ptr_musculs[5] = mus6; ptr_musculs[6] = mus7; ptr_musculs[7] = mus8; ptr_musculs[8] = mus9; ptr_musculs[9] = mus10; ptr_musculs[10] = mus11; ptr_musculs[11] = mus12; ptr_musculs[12] = mus13; ptr_musculs[13] = mus14; ptr_musculs[14] = mus15; ptr_musculs[15] = mus16; ptr_musculs[16] = mus17; ptr_musculs[17] = mus18;
for (int i = 0; i < 18; i++)
{
SERVO.WritePos(ID_Motor[i], ptr_musculs[i], vel);
int temps = abs((((posicio[i] – ptr_musculs[i]) * 150) / 512) / ((vel * 0.372) / 0x3FF));
posicio[i] = ptr_musculs[i];
delay(temps / 4);
Serial.println(temps);
}
}
void Iron_Man_Green_Lantern()
{
presetPose(512, 788, 204, 744, 512, 561, 358, 666, 507, 516, 341, 682, 240, 783, 647, 376, 507, 516, 0xFA);
}
void Walk_ready()
{
presetPose(235, 788, 279, 744, 462, 561, 358, 666, 507, 516, 341, 682, 240, 783, 647, 376, 507, 516, 0xFA);
}
void Saludar()
{
presetPose(737, 778, 301, 744, 626, 561, 358, 666, 507, 516, 341, 682, 240, 783, 647, 376, 507, 516, 0xFA);
delay(72);
delay(96);
presetPose(737, 778, 208, 744, 367, 561, 358, 666, 507, 516, 341, 682, 240, 783, 647, 376, 507, 516, 0xFA);
delay(299);
presetPose(737, 778, 301, 744, 626, 561, 358, 666, 507, 516, 341, 682, 240, 783, 647, 376, 507, 516, 0xFA);
delay(200);
}
void Clap ()
{
presetPose(512, 512, 301, 722, 426, 567, 358, 666, 507, 516, 341, 682, 240, 783, 647, 376, 507, 516, 0xFA);
delay(72);
delay(96);
presetPose(512, 512, 170, 845, 347, 656, 358, 666, 507, 516, 341, 682, 240, 783, 647, 376, 507, 516, 0xFA);
delay(299);
presetPose(512, 512, 301, 722, 426, 567, 358, 666, 507, 516, 341, 682, 240, 783, 647, 376, 507, 516, 0xFA);
delay(200);
}
void Caminar_endavant ()
{
presetPose(169, 722, 279, 744, 462, 561, 358, 666, 513, 522, 302, 660, 246, 769, 621, 348, 513, 522, 0xFA);
presetPose(166, 719, 279, 744, 462, 561, 358, 666, 518, 527, 308, 651, 248, 761, 625, 348, 518, 527, 0xFA);
presetPose(167, 720, 279, 744, 462, 561, 358, 666, 519, 533, 314, 650, 252, 761, 628, 346, 523, 532, 0xFA);
presetPose(171, 724, 279, 744, 462, 561, 358, 666, 513, 541, 321, 662, 257, 778, 629, 341, 527, 537, 0xFA);
presetPose(179, 732, 279, 744, 462, 561, 358, 666, 504, 550, 327, 682, 262, 801, 630, 338, 530, 541, 0xFA);
presetPose(189, 742, 279, 744, 462, 561, 358, 666, 496, 556, 332, 704, 266, 820, 631, 342, 532, 543, 0xFA);
presetPose(201, 754, 279, 744, 462, 561, 358, 666, 493, 558, 335, 724, 267, 827, 633, 354, 533, 544, 0xFA);
delay(200);
presetPose(215, 768, 279, 744, 462, 561, 358, 666, 496, 556, 337, 736, 266, 820, 636, 374, 532, 543, 0xFA);
presetPose(230, 783, 279, 744, 462, 561, 358, 666, 504, 550, 338, 740, 262, 801, 641, 396, 530, 541, 0xFA);
presetPose(246, 799, 279, 744, 462, 561, 358, 666, 513, 541, 339, 737, 257, 778, 647, 416, 527, 537, 0xFA);
presetPose(260, 813, 279, 744, 462, 561, 358, 666, 519, 533, 340, 732, 252, 761, 654, 428, 523, 532, 0xFA);
presetPose(274, 827, 279, 744, 462, 561, 358, 666, 518, 527, 343, 730, 248, 761, 660, 427, 518, 527, 0xFA);
presetPose(285, 838, 279, 744, 462, 561, 358, 666, 513, 522, 347, 730, 246, 769, 666, 418, 513, 522, 0xFA);
presetPose(294, 847, 279, 744, 462, 561, 358, 666, 507, 516, 354, 726, 248, 775, 671, 409, 507, 516, 0xFA);
delay(200);
presetPose(300, 853, 279, 744, 462, 561, 358, 666, 501, 510, 363, 721, 254, 777, 675, 402, 501, 510, 0xFA);
presetPose(303, 856, 279, 744, 462, 561, 358, 666, 496, 505, 372, 715, 262, 775, 675, 398, 496, 505, 0xFA);
presetPose(302, 855, 279, 744, 462, 561, 358, 666, 490, 504, 373, 709, 262, 771, 677, 395, 491, 500, 0xFA);
presetPose(298, 851, 279, 744, 462, 561, 358, 666, 482, 510, 361, 702, 245, 766, 682, 394, 486, 496, 0xFA);
presetPose(290, 843, 279, 744, 462, 561, 358, 666, 473, 519, 341, 696, 222, 761, 685, 394, 482, 493, 0xFA);
presetPose(280, 833, 279, 744, 462, 561, 358, 666, 467, 527, 319, 691, 203, 757, 681, 392, 480, 491, 0xFA);
presetPose(268, 821, 279, 744, 462, 561, 358, 666, 465, 530, 299, 688, 196, 756, 669, 390, 479, 490, 0xFA);
delay(200);
presetPose(254, 807, 279, 744, 462, 561, 358, 666, 467, 527, 287, 686, 203, 757, 649, 387, 480, 491, 0xFA);
presetPose(239, 792, 279, 744, 462, 561, 358, 666, 473, 519, 283, 685, 222, 761, 627, 382, 482, 493, 0xFA);
presetPose(223, 776, 279, 744, 462, 561, 358, 666, 482, 510, 286, 684, 245, 766, 607, 376, 486, 496, 0xFA);
presetPose(209, 762, 279, 744, 462, 561, 358, 666, 490, 504, 291, 683, 262, 771, 595, 399, 491, 500, 0xFA);
presetPose(195, 748, 279, 744, 462, 561, 358, 666, 496, 505, 293, 680, 262, 775, 596, 363, 496, 505, 0xFA);
presetPose(184, 737, 279, 744, 462, 561, 358, 666, 501, 510, 293, 676, 254, 777, 605, 357, 501, 510, 0xFA);
presetPose(175, 728, 279, 744, 462, 561, 358, 666, 507, 516, 297, 669, 248, 775, 614, 352, 507, 516, 0xFA);
delay(200);
}
void Color(int R, int G, int B)
{
analogWrite(9, G);
analogWrite(11, B);
analogWrite(10, R);
}
A la pàgina de Programa Princial hi ha els links per descarregar-se les llibreries
Treball de Recerca Batxillerat 2016 