Estudi, disseny i implementació d’un humanoide pel seguiment de trajectòries
Abstract
Els humanoides són robots que posseeixen una configuració similar a la morfologia humana, és a dir, que consten d’un cap, un tronc, dos braços i dues cames (en certs casos els humanoides poden simular-ne sols una part, anomenant-se llavors humanoides parcials). La forma més habitual de locomoció dels humanoides és caminant, i és per això que presenten especial dificultat en el món de la robòtica. La intenció d’aquest estudi és la de dissenyar, construir i programar un humanoide capaç de seguir trajectòries. Per a tals efectes, el robot (batejat com a Tamtam) haurà de ser capaç de desplaçar-se de forma estable seguint la ruta que ell mateix haurà disposat. Així doncs es plantegen tres objectius: el primer és el de ser estable, el segon implica que ha de capaç de caminar seguint un rumb definit i el tercer és el de calcular el traçat que s’ha de seguir per arribar al punt desitjat sortejant, si s’escau, els obstacles que ho impedeixin. Per a assolir l’objectiu de la locomoció es proposa l’estudi de l’estabilitat de l’humanoide i el control de la mateixa utilitzant la tècnica de control PID. Després es pretén aplicar zero moment point per aconseguir el desplaçament. Pel que fa al traçat de trajectòries, s’usa les llibreries d’OpenCV i ZBar amb una càmera per trobar els codis QR que identifiquen els obstacles i punt d’arribada i la seva ubicació per definir el trajecte a seguir. Un cop aconseguit resta la implementació del seguiment de la trajectòria trobada. Amb la tecnologia actual i les tècniques de control que existeixen, és possible la materialització d’un humanoide com el descrit en aquest projecte. S’ha aconseguit la seva estabilització, cosa que fa que sigui més fàcil el desplaçament caminant. També s’ha aconseguit la detecció dels marcadors. Tanmateix és una feina laboriosa. Cada vegada hi ha més informació sobre el tema, i també molts més projectes que apliquen tècniques noves. El problema ara es troba en la implementació, ja que la informació disponible està encarada a projectes amb característiques molt diferents a la del projecte, per tant l’adaptació s’ha hagut de cuidar molt. De cara a la continuació d’aquest projecte es podria buscar un entorn més semblant al de la competició CEABOT i buscar que el robot aprofiti parts d’aquest projecte per assolir-ne més proves. Aquest concurs és un concurs organitzat per al grup CEA (Comitè Espanyol d’Automàtica) que es realitza cada any des del 2006 entre universitaris i grups de recerca de tota Espanya. En la prova hi ha un camp de 2x2.5m amb les parets marcades amb QR i una sèrie d’obstacles també marcats (el competidor en desconeix la quantitat i la disposició). L’objectiu de la prova és que l’humanoide travessi el camp i després torni al punt de partida en el menor temps possible (penalitzant caigudes i xocs amb els obstacles). Fins ara ningú no ha realitzat aquesta prova utilitzant visió, sinó que ho han fet amb sensors òptics. És per això que aquest problema és interessant, i fins i tot té una aplicació directe en el món de la competició.
Full text
Appendices 1
Apèndix A Disseny Tamtam 2
Figura A.1: Dibuix d’en Tamtam vista anterior Figura A.2: Dibuix d’en Tamtam vista posterior 3
Apèndix B Fotos Tamtam 4
Figura B.1: Tamtam vista anterior Figura B.2: Tamtam vista anterior 2 Figura B.3: Tamtam vista lateral Figura B.4: Tamtam vista posterior 5
Apèndix C Esquema de la placa 6
Apèndix D Codi Arduino tamtam_arduino_slave.ino 1#include <Wire . h> 2 3#define RED_LED 5 4#define GREEN_LED 9 5#define BLUE_LED 6 6#define BATTERY_PIN A0 7#define ARDUINO_I2C_ADDRESS 0 x0f 8 9#define SET_RED_LED 0x01 10 #define SET_GREEN_LED 0x02 11 #define SET_BLUE_LED 0x03 12 #define READ_BATTERY 0x04 13 14 15 void setup () 16 { 17 Wire . begin (ARDUINO_I2C_ADDRESS) ; 18 pinMode(RED_LED,OUTPUT) ; 19 pinMode(GREEN_LED,OUTPUT) ; 20 pinMode(BLUE_LED,OUTPUT) ; 21 pinMode(BATTERY_PIN,INPUT) ; 22 Wire . onReceive ( receiveEvent ) ; 23 Wire . onRequest ( requestEvent ) ; 24 } 25 26 27 void loop () 28 { 29 30 } 31 8
32 void receiveEvent() 33 { 34 char cmd = Wire . read () ; 35 i f (cmd==SET_RED_LED) 36 { 37 char colour = Wire . read () ; 38 set_red ( colour ) ; 39 } 40 els e i f (cmd==SET_GREEN_LED) 41 { 42 char colour = Wire . read () ; 43 set_green ( colour ) ; 44 } 45 els e i f (cmd==SET_BLUE_LED) 46 { 47 char colour = Wire . read () ; 48 set_blue ( colour ) ; 49 } 50 } 51 52 void requestEvent() 53 { 54 int c= analogRead (BATTERY_PIN) ; 55 unsigned char bat = ( c /4) ; 56 Wire . write ( bat ) ; 57 } 58 59 void set_red ( char c ) 60 { 61 analogWrite (RED_LED, c ) ; 62 } 63 64 void set_green(char c ) 65 { 66 analogWrite (GREEN_LED, c ) ; 67 } 68 69 void set_blue(char c ) 70 { 71 analogWrite (BLUE_LED, c ) ; 72 } 9
42 tr ansl ate ( [ body_width−R,R] ) cyli nder ( r=R, h=12) ; 43 } 44 tr ansl ate ( [ body_width/2,33+body_height +0.25 ,2.5]) rotate ([0 , −90 ,90]) mg90s (0 , true ) ; 45 tr ansl ate ( [ body_width /2 ,2 ,2]) rotate ( [ 9 0 , 0 ] ) t ra nsl ate ([ −3 , −1.5 , −1.25]) cube ( [ 6 , 4 , 5 . 5 ] ) ; 46 tr ansl ate ( [ body_width/2−20,body_height −2 ,2]) rotate ([90 ,0 ,180]) translate ([ −3 , −1.5 , −1.25]) cube ( [ 6 , 4 , 5 . 5 ] ) ; 47 tr ansl ate ( [ body_width/2+20,body_height −2 ,2]) rotate ([90 ,0 ,180]) translate ([ −3 , −1.5 , −1]) cube ( [ 6 , 4 , 5 . 5 ] ) ; 48 } 49 translate ([3 ,4 , −10]) cube ( [ battery_length , battery_heigth , battery_width ] ) ; 50 tr ansl ate ( [ body_width−3−battery_length ,4 , −10]) cube ( [ battery_length , battery_heigth , battery_width ] ) ; 51 tr an sl at e ( [ body_width /2 , body_height /2+4]) hull () 52 { 53 translate ([6 −2.5 ,20 −2.5 , −10]) cylinder ( r =2.5 ,h= battery_width) ; 54 translate ([6 −2.5 , −20+2.5,−10]) cylinder ( r =2.5 ,h= battery_width) ; 55 translate([−6+2.5 , −20+2.5 , −10]) cylinder ( r =2.5 ,h= battery_width) ; 56 translate([−6+2.5,20−2.5 , −10]) cylinder ( r =2.5 ,h= battery_width) ; 57 } 58 tr ansl ate ( [R_S+5,R_S+5,back_cover_height −2]) write ("TamTam" , t=4,h=8) ; 59 } 16
head_lower_part.scad 1/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 2∗head_lower_part 3∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 4∗@author FioPio ( Ferr iol Pey Comas) ferriol73pey@gmail . com 5∗@version v2 .3 20/01/2018 6∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 7∗This f i l e contains the 3D model of the lower part of the 8∗head. 9∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/ 10 use<mg90s . scad> 11 12 $fn =200; 13 R=3; 14 R_cam_optic=1.5; 15 head_lenght=45; 16 head_widht=42; 17 head_hight=35; 18 margin=4; 19 di f f er e nc e () 20 { 21 intersection() 22 { 23 di f f er e nc e () 24 { 25 //head 26 hull () 27 { 28 tr ansl ate ( [ head_widht/2−R,head_lenght/2−R] ) cyl in de r ( r =R, h=R) ; 29 tr ansl ate ( [R−head_widht /2 , head_lenght/2−R] ) cyl in de r ( r =R, h=R) ; 30 tr ansl ate ( [ head_widht/2−R,R−head_lenght /2]) cylinder ( r =R, h=R) ; 31 tr ansl ate ( [R−head_widht /2 ,R−head_lenght /2]) cylinder ( r =R, h=R) ; 32 tr ansl ate ( [ head_widht/2−R,head_lenght/2−R,head_hight− R] ) cylinde r ( r=R, h=R) ; 33 tr ansl ate ( [R−head_widht /2 , head_lenght/2−R,head_hight− R] ) cylinde r ( r=R, h=R) ; 34 tr ansl ate ( [ head_widht/2−R,R−head_lenght /2 , head_hight− R] ) cylinde r ( r=R, h=R) ; 35 tr ansl ate ( [R−head_widht /2 ,R−head_lenght /2 , head_hight− R] ) cylinde r ( r=R, h=R) ; 36 } 37 union () 38 { 39 // t ranslate ([6+3 , −6 ,28]) rotate ([0 ,18 0 ,0]) mg90s (0) ; 17
40 //translate([−19,−6 , −18]) cube ([33 ,12 ,25]) ; 41 //Camera 42 tr ansl ate ( [ head_widht/2−5.5−1 ,−6,4]) union() 43 { 44 translate([−0.5 ,0]) cube ([6 ,12 , 1 2]) ; 45 tr an sl ate ( [ 5 . 5 , 6 , 6 ] ) rotate ( [ 0 , 90 , 0] ) c yl inder ( r1= R_cam_optic , r2=R_cam_optic+17,h=5) ; 46 } 47 //Camera wires 48 tr ansl ate ( [ head_widht /2 −5.5−1.5 , −6 ,2]) cube ([5 . 5 , 12 ,2 6 ] ) ; //∗/ 49 //upper cover empty operation 50 di f f er e nc e () 51 { 52 hull () 53 { 54 tr ansl ate ( [ head_widht/2−R−margin , head_lenght/2− R−margin , head_hight−R−margin ] ) sphere ( r=R) ; 55 tr ansl ate ( [R−head_widht/2+margin , head_lenght/2− R−margin , head_hight−R−margin ] ) sphere ( r=R) ; 56 tr ansl ate ( [ head_widht/2−R−margin ,R−head_lenght /2+margin , head_hight−R−margin ] ) sphere ( r=R) ; 57 tr ansl ate ( [R−head_widht/2+margin ,R−head_lenght /2+margin , head_hight−R−margin ] ) sphere ( r=R) ; 58 59 tr ansl ate ( [ head_widht/2−R−margin , head_lenght/2− R−margin , head_hight/2 −1]) cylinder ( r=R, h=2) ; 60 tr ansl ate ( [R−head_widht/2+margin , head_lenght/2− R−margin , head_hight/2 −1]) cylinder ( r=R, h=2) ; 61 tr ansl ate ( [ head_widht/2−R−margin ,R−head_lenght /2+margin , head_hight/2 −1]) cylinder ( r=R, h=2) ; 62 tr ansl ate ( [R−head_widht/2+margin ,R−head_lenght /2+margin , head_hight/2 −1]) cylinder ( r=R, h=2) ; 63 } 64 tr ansl ate ( [ head_widht/2−margin −2 ,10 ,25]) rotate ([ 0 ,9 0 ]) cylinder ( r=5,h=10) ; 65 tr ansl ate ( [ head_widht/2−margin −2 ,10 −5 ,25]) cube ([10,10,10]); 66 tr ansl ate ( [ head_widht/2−margin −2 , −10 ,25]) rotate ([ 0 ,9 0 ]) cylinder ( r=5,h=10) ; 67 tr ansl ate ( [ head_widht/2−margin −2 , −15 ,25]) cube ([10,10,10]); 68 } 69 // eyes 70 tr ansl ate ( [ head_widht/2−margin −2 ,10 ,25]) rotate ([ 0 ,9 0 ]) cylinder ( r1 =2.75 , r2=5,h=10) ; 71 tr ansl ate ( [ head_widht/2−margin −2 , −10 ,25]) rotate ([ 0 ,9 0 ]) cylinder ( r1 =2.75 , r2=5,h=10) ; 72 // cable exit 18
73 translate([−head_widht/2−5,−15,head_hight /2]) cube ([15,30 ,4]) ; 74 translate([−head_widht/2−5,−5,head_hight /2]) cube ([15,10 ,6]) ; 75 } 76 // translate ([−35 ,−35 , head_hight /2]) cube ([70 ,70 , head_hight /2+5]) ; 77 } 78 union () 79 { 80 translate([−head_widht/2+1.8,−head_lenght /2+1.8]) cube ( [ head_widht −3.6 , head_lenght −3.6 , head_hight /2+3]) ; 81 translate([−35 , −35]) cube ([70 ,70 , head_hight /2]) ; 82 } 83 } 84 translate ([2 , 0 ,28]) rotate ([0 ,180 ,0 ]) mg90s (0) ; 85 translate([−19.5,−6.5 , −18]) cube ( [33 ,13 ,25]) ; 86 } 19
head_upper_part.scad 1/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 2∗head_upper_part 3∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 4∗@author FioPio ( Ferr iol Pey Comas) ferriol73pey@gmail . com 5∗@version v2 .1 15/12/2018 6∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 7∗This f i l e contains the 3D model of the upper part of the 8∗head. 9∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/ 10 use<mg90s . scad> 11 12 $fn =120; 13 R=3; 14 R_cam_optic=2; 15 head_lenght=45; 16 head_widht=42; 17 head_hight=35; 18 margin=4; 19 20 di f f er e nc e () 21 { 22 //head 23 hull () 24 { 25 tr ansl ate ( [ head_widht/2−R,head_lenght/2−R] ) cyl in der ( r=R, h=R ) ; 26 tr ansl ate ( [R−head_widht /2 , head_lenght/2−R] ) cylind er ( r=R, h=R ) ; 27 tr ansl ate ( [ head_widht/2−R,R−head_lenght /2]) cylinder ( r=R, h=R ) ; 28 tr ansl ate ( [R−head_widht /2 ,R−head_lenght /2]) cylinder ( r=R, h=R ) ; 29 tr ansl ate ( [ head_widht/2−R,head_lenght/2−R,head_hight−R] ) cylinder ( r=R, h=R) ; 30 tr ansl ate ( [R−head_widht /2 , head_lenght/2−R,head_hight−R] ) cylinder ( r=R, h=R) ; 31 tr ansl ate ( [ head_widht/2−R,R−head_lenght /2 , head_hight−R] ) cylinder ( r=R, h=R) ; 32 tr ansl ate ( [R−head_widht /2 ,R−head_lenght /2 , head_hight−R] ) cylinder ( r=R, h=R) ; 33 } 34 union () 35 { 36 //Camera wires 37 tr ansl ate ( [ head_widht /2 −5.5−1.5 , −6 ,2]) cube ([ 5. 5 ,1 2 ,2 6] ) ; // ∗/ 38 //upper cover empty operation 20
39 di f f er e nc e () 40 { 41 hull () 42 { 43 tr ansl ate ( [ head_widht/2−R−margin , head_lenght/2−R− margin , head_hight−R−margin ] ) sphere ( r=R) ; 44 tr ansl ate ( [R−head_widht/2+margin , head_lenght/2−R− margin , head_hight−R−margin ] ) sphere ( r=R) ; 45 tr ansl ate ( [ head_widht/2−R−margin ,R−head_lenght/2+ margin , head_hight−R−margin ] ) sphere ( r=R) ; 46 tr ansl ate ( [R−head_widht/2+margin ,R−head_lenght/2+ margin , head_hight−R−margin ] ) sphere ( r=R) ; 47 48 tr ansl ate ( [ head_widht/2−R−margin , head_lenght/2−R− margin , head_hight/2 −1]) cylinder ( r=R, h=2) ; 49 tr ansl ate ( [R−head_widht/2+margin , head_lenght/2−R− margin , head_hight/2 −1]) cylinder ( r=R, h=2) ; 50 tr ansl ate ( [ head_widht/2−R−margin ,R−head_lenght/2+ margin , head_hight/2 −1]) cylinder ( r=R, h=2) ; 51 tr ansl ate ( [R−head_widht/2+margin ,R−head_lenght/2+ margin , head_hight/2 −1]) cylinder ( r=R, h=2) ; 52 } 53 tr ansl ate ( [ head_widht/2−margin −2 ,10 ,25]) rotate ([ 0 ,9 0 ]) cylinder ( r=5,h=10) ; 54 tr ansl ate ( [ head_widht/2−margin −2 ,10 −5 ,25]) cube ([10,10,10]); 55 tr ansl ate ( [ head_widht/2−margin −2 , −10 ,25]) rotate ( [ 0 , 9 0 ]) cylinder ( r=5,h=10) ; 56 tr ansl ate ( [ head_widht/2−margin −2 , −15 ,25]) cube ([10,10,10]); 57 } 58 // eyes 59 tr ansl ate ( [ head_widht/2−margin −2 ,10 ,25]) rotate ([ 0 ,9 0 ]) cylinder ( r1 =2.75 , r2=5,h=10) ; 60 tr ansl ate ( [ head_widht/2−margin −2 , −10 ,25]) rotate ( [ 0 , 9 0 ]) cylinder ( r1 =2.75 , r2=5,h=10) ; 61 // cable exit 62 translate([−head_widht/2−5,−15,head_hight /2]) cube ([15,30 ,2]) ; 63 translate([−head_widht/2−5,−5,head_hight /2]) cube ([15 ,10 , 6 ]) ; 64 } 65 union () 66 { 67 translate([−head_widht/2+1.5,−head_lenght /2+1.5]) cube ( [ head_widht −3,head_lenght−3,head_hight /2+4]) ; 68 translate([−35 , −35, −2]) cube ([70 ,70 , head_hight /2+2]) ; 69 } 70 // translate ([−35 ,−35 , head_hight /2]) cube ([70 ,70 , head_hight 21
/2+5]) ; 71 } 72 73 // ears 74 translate ([0 ,0 , head_hight /2]) d i ff e re n ce () 75 { 76 hull () 77 { 78 translate ( [ 3 , head_lenght /2 ,3]) sphere (2) ; 79 translate([−3 , head_lenght /2 ,3]) sphere (2) ; 80 translate([−3,head_lenght/2,−3]) sphere (2) ; 81 tr anslat e ([3 , head_lenght /2 , −3]) sphere (2) ; 82 translate ([3 , −head_lenght /2 ,3]) sphere (2) ; 83 translate([−3,−head_lenght /2 ,3]) sphere (2) ; 84 translate([−3,−head_lenght/2,−3]) sphere (2) ; 85 translate ([3 , −head_lenght/2,−3]) sphere (2) ; 86 } 87 cube ( head_lenght , center=true ) ; 88 } 22
lower_leg_right.scad 1/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 2∗lower_leg 3∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 4∗@author FioPio ( Ferr iol Pey Comas) ferriol73pey@gmail . com 5∗@version v1 .1 04/12/2018 6∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 7∗This f i l e contains the lower part of the leg . 8∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/ 9 10 use <mg90s . scad> 11 $fn =120; 12 R=6; 13 14 di f f er e nc e () 15 { 16 hull () 17 { 18 rotate ([ −90 ,0]) cylinder ( r=R, h=33+8) ; 19 translate ([ 0 , 0 , 3 0 ] ) rotate ([ −90 ,0]) cylinder ( r=R, h=33+8) ; 20 translate([−5,0,7.5])rotate([−90 ,0]) cylinder ( r=8,h=33+8) ; 21 } 22 translate([−36 ,4 , −10]) cube ([30 ,33 ,60]) ; 23 translate([−7 ,4 , −10]) cube ([ 3 0 , 3 3 , 2 3 . 5 ] ) ; 24 translate([−7 ,4 ,16.5]) cube ([30 ,33 ,22]) ; 25 tr an sla te ( [ 0 , 8 ] ) rotate ([ −90 ,180])mg90s(90 , false , true ) ; 26 translate ([ 0 , 8 , 3 0 ] ) rotate ([ −90 ,0]) mg90s (90 , true , true ) ; 27 } 23
mg90s.scad 1/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 2∗mg90s 3∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 4∗@author FioPio ( Ferr iol Pey Comas) ferriol73pey@gmail . com 5∗@version v1 .3 20/01/2018 6∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 7∗This f i l e contains the 3D model of microservos mg90s , this 8∗has been done tinking on tamtam , a minihumanoid low cost 9∗designed by me, FioPio , but i t may be used on other 10 ∗projects due the l i c e n se of this . Please check the LICENSE 11 ∗f i l e to know the de ta i l s . 12 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/ 13 $fn =120; 14 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 15 ∗mg90s 16 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 17 ∗This module creates a mg90s sol i d . 18 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 19 ∗@param alpha ( optional ) i t ’ s the rotation angle of the 20 ∗holder of the servo . I f you don ’ t use t his i t w il l not 21 ∗apear . 22 ∗@param is_cutted ( optional ) i f i t i s set to true , the 23 ∗the servo holders w i l l desapear , in order to have a 24 ∗module without i t i f needed . 25 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/ 26 module mg90s( alpha=−1,is_cutted= f als e , two_axies=f a l s e ) 27 { 28 translate([−6.5 , −6.5]) 29 { 30 cube ([23 ,13 ,23]) ; 31 i f ( ! is_cutted ) 32 { 33 translate([−5 ,0 ,18.75]) union() 34 { 35 cube ([ 33 ,13 ,3]) ; 36 translate ([2.5 ,6.5 , −(10 −2.5) /2 −4]) cylinder ( r =.75 ,h =15) ; 37 translate ([2.5 ,6.5 ,10 −(10 −2.5) /2]) cylinder ( r =2.5 ,h=5) ; 38 translate ([33 −2.5 ,6.5 , −(10 −2.5) /2−4]) cylinder ( r =.75 ,h =15) ; 39 translate ([33 −2.5 ,6.5 ,10 −(10 −2.5) /2]) cylinder ( r =2.5 ,h =5) ; 40 } 41 } 42 i f ( two_axies ) 43 { 24
44 translate ([6.5 ,6.5 , −1.2]) //Axis continuation 45 { 46 cylinder ( r1=1,r2 =1.9 ,h=1.2) ; 47 translate ( [ 0 , 0 , 0.0 1] ) cylinder ( r1=1,r2 =1.9 ,h =1.2) ; 48 translate ([0 ,0 , −8]) cylinder ( r=1,h=9) ; 49 } 50 } 51 translate ( [ 6 . 5 , 6 . 5 ] ) cylinder ( r =6.5 ,h=29) ; 52 tr ans late ( [ 1 2 . 5 , 6 . 5 ] ) cylinder ( r =3.5 ,h=29) ; 53 translate ( [ 6 . 5 , 6 . 5 ] ) cylinder ( r =3.75 ,h=35.1) ; 54 translate([−4 ,3.5 ,4.5]) cube ( [ 5 , 5 , 2 ] ) ; 55 i f ( alpha!=−1) 56 { 57 translate ([6.5 ,6.5 ,23+6+6.1 −2]) rotate ([0 ,0 , alpha ] ) union () 58 { 59 hull () 60 { 61 cylinder ( r =3.75 ,h=2) ; 62 translate ( [ 16 ,0 ]) cylinder ( r =2.25 ,h=2) ; 63 } 64 // holes f or the screws ( there i s one every 2 mm) 65 translate ([4.5 ,0 , −5]) cylinder ( r =.5 ,h=12) ; 66 translate ([12.5 ,0 , −5]) cylinder ( r =.5 ,h=12) ; 67 } 68 } 69 } 70 } 25
two_servo_holder.scad 1/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 2∗two_servo_holder 3∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 4∗@author FioPio ( Fe rriol Pey Comas) ferriol73pey@gmail . com 5∗@version v1 .0 04/12/2018 6∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 7∗This f i l e contains the 3D model of the piece that holds two 8∗servos to have two degres of freedom "in the same jo i n t " used 9∗in the ankle and in the upper part of the leg . 10 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/ 11 12 13 use <mg90s . scad> //To use the module created before 14 $fn =120; 15 di f f er e nc e () 16 { 17 translate ([0.05 , −33.05+29 ,0.05]) cube ([ 3 2. 9 , 32 . 9 ,2 5 . 8 4 ] ) ; //The box that contains everything 18 translate ([6.5+5 ,0 ,6.5]) rotate ([ −90 ,0]) 19 { 20 mg90s(−1, false , true ) ; //The lower servo 21 translate([−15 , −3 ,4.5]) cube ([10 ,10 ,2]) ; //The hole to pass the cable 22 translate([−6.5 ,0 ,22]) cube (16) ; //The hole to introduce the servo 23 } 24 translate ([29 ,17.4 ,32.4 −13]) rotate ([90 ,0 , −90]) 25 { 26 mg90s(−1, false , true ) ; //The upper one 27 translate([−15 , −3 ,4.5]) cube ([10 ,10 ,2]) ; //The hole to pass the cable 28 translate([−6.5 ,0 ,22]) cube (16) ; //The hole to introduce the servo 29 } 30 } 32
upper_leg_right.scad 1/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 2∗upper_leg_right 3∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 4∗@author FioPio ( Ferr iol Pey Comas) ferriol73pey@gmail . com 5∗@version v1 .0 05/12/2018 6∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 7∗This f i l e contains the 3D model of the upper part of the 8∗right leg . 9∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/ 10 11 use <mg90s . scad> 12 13 $fn =120; 14 di f f er e nc e () 15 { 16 union () 17 { 18 hull () 19 { 20 cube([12+5,32.9,6]); 21 translate ( [11 ,0 ,36] ) rotate ([ −90 ,0]) cylinder ( r=6,h=32.9) ; 22 } 23 translate ([ 4 ,0 , 0 ]) cube ([12+5 ,32.9 ,16]) ; 24 hull () 25 { 26 translate ([1.64 , −4 ,14.5]) cube ([12+5 −1.64 ,32.9+8 ,1.5]) ; 27 translate ([11 , −4.05 ,36]) rotate ([ −90 ,0]) cylinder ( r=6,h =33+8); 28 } 29 } 30 translate ([5+6.5 ,4 ,6]) rotate ([ −90 ,0]) 31 { 32 mg90s(−1, false , true ) ; 33 translate([−13 , −3 ,4.5]) cube ([10 ,10 ,2]) ; 34 } 35 translate ([5 ,26 , −1]) cube ([20 ,7 , 7 ] ) ; 36 translate([−7 ,0 ,30 −13.5+6]) cube ([ 3 0 , 3 3 , 2 3 . 5 ] ) ; 37 translate([−10 , −4 ,14.5]) rotate ( [ 0 , 9 0 ,0]) cylinder ( r=4,h=50) ; 38 translate([−10 ,36.9 ,14.5]) rotate ( [0 , 9 0 ,0]) cylinder ( r=4,h=50) ; 39 hull () 40 { 41 translate([−10,2,30+6−13.5]) rotate ([0 , 9 0 ,0]) cylinder ( r=2, h=50) ; 42 translate([−10 ,33 −2 ,30+6 −13.5]) rotate ( [0 , 9 0 ,0]) cylinder ( r =2,h=50) ; 33
43 } 44 translate ([5+6.5 ,4 ,30+6.5]) rotate ([ −90 ,0]) mg90s (90 , true , true ) ; 45 translate ([11 , −4 ,6]) rotate ( [0 ,130 ,0] ) translate ([0 ,0 , −30]) di f f er e nc e () 46 { 47 hull () 48 { 49 rotate ([ −90 ,0]) cylinder ( r=6,h=33+8) ; 50 translate ([ 0 , 0 , 3 0 ] ) rotate ([ −90 ,0]) cylinder ( r=6,h=33+8) ; 51 translate([−5,0,7.5])rotate([−90 ,0]) cylinder ( r=8,h=33+8) ; 52 } 53 translate([−36 ,4 , −10]) cube ([3 0 ,33 ,60] ) ; 54 translate([−7 ,4 , −10]) cube ([30 ,33 ,23.5 ] ) ; 55 translate([−7 ,4 ,16.5]) cube ([30 ,33 ,22]) ; 56 translate ([6 ,8 , −6]) rotate ([ −90 ,180]) mg90s (90 , false , true ) ; 57 translate([−6 ,8 ,36]) rotate ([ −90 ,0])mg90s(90 , true , true ) ; 58 } 59 } 34
Apèndix F CalibraciÃş camera compute_intrinsecs.py 1# ####################################################################### 2# code extracted from the opecv t u t o r i a l s : 3# https :// opencv−python−t utr oa ls . readthedocs . io /en/ l a t e s t / py_tutorials /py_calib3d/ py_calibration / py_calibration . html 4# 30/05/19 5 6import numpy as np 7import cv2 8import glob 9import time 10 start = time . time () 11 12 # termination c r i t e r i a 13 c r i t e r i a = ( cv2 .TERM_CRITERIA_EPS + cv2 .TERM_CRITERIA_MAX_ITER, 30 , 0.001) 14 15 # prepare object points , l i k e (0 ,0 ,0) , (1 ,0 ,0) , (2 ,0 ,0) .... ,(6 ,5 ,0) 16 objp = np . zeros ((6∗7 ,3) , np . float32 ) 17 objp [ : , : 2 ] = np . mgrid [ 0 : 7 , 0 : 6 ] .T. reshape (−1 ,2) 18 19 # Arrays to store object points and image points from a l l the images. 20 objpoints = [ ] # 3d point in real world space 21 imgpoints = [ ] # 2d points in image plane . 22 23 counter=0 24 #obtenint imatge del video 25 images = glob . glob ( ’∗. jpg ’ ) 35
26 for fname in images: 27 img = cv2 . imread (fname) 28 #girant la imatge 29 gray = cv2 . cvtColor (img , cv2 .COLOR_BGR2GRAY) 30 31 # Find the chess board corners 32 ret , corners = cv2 . findChessboardCorners ( gray , (7 ,6) ,None) 33 34 # I f found , add object points , image points ( a ft er r ef i ni n g them) 35 i f ret == True : 36 objpoints . append( objp ) 37 38 corners2 = cv2 . cornerSubPix ( gray , corners ,(11 ,11) ,(−1,−1) , criteria) 39 imgpoints . append ( corners2 ) 40 # Draw and display the corners 41 img = cv2 . drawChessboardCorners (img , (7 ,6) , corners2 , ret ) 42 # Draw and display the corners 43 #cv2 . imshow ( ’ img ’ , img ) 44 #cv2 . waitKey (250) 45 counter+=1 46 #obtenint imatge del video 47 48 #cv2.destroyAllWindows() 49 50 end = time . time () 51 print ("ellapsed ", end −start , " seconds and "+str (counter)+" photos found") 52 53 ret , mtx , dist , rvecs , tvecs = cv2 . calibrateCamera ( objpoints , imgpoints , gray . shape [:: −1 ] , None , None) 54 55 print ("matrix : " ) 56 print (mtx) 57 print ("_______________") 58 print (" dist : ") 59 print ( dist ) 60 print ("_______________") 61 print (" rvecs : ") 62 print ( rvecs ) 63 print ("_______________") 64 print (" tvecs : ") 65 print ( tvecs ) 66 print ("_______________") 67 68 img = cv2 . imread ( ’ teta . jpg ’ ) 69 h , w = img . shape [ : 2 ] 70 newcameramtx , r oi=cv2 . getOptimalNewCameraMatrix (mtx , dist , (w, h) 36
,1 ,(w, h) ) 71 72 # undistort 73 dst = cv2 . undistort (img , mtx , dist , None , newcameramtx) 74 75 # crop the image 76 #x , y ,w, h = r oi 77 #dst = dst [ y : y+h , x : x+w] 78 cv2 . imwrite ( ’ c a l i b r es ul t 1 . png ’ , dst ) 79 80 # undistort 81 mapx,mapy = cv2 . initUndistortRectifyMap (mtx , dist , None , newcameramtx , (w, h) ,5) 82 dst = cv2 . remap(img ,mapx,mapy, cv2 .INTER_LINEAR) 83 84 # crop the image 85 #x , y ,w, h = r oi 86 #dst = dst [ y : y+h , x : x+w] 87 cv2 . imwrite ( ’ c a l i b r es ul t 2 . png ’ , dst ) 88 89 mean_error = 0 90 tot_error=0 91 for iin range (len ( objpoints ) ) : 92 imgpoints2 , _ = cv2 . projectPoints ( objpoints [ i ] , rvecs [ i ] , tvecs [ i ] , mtx , dist ) 93 error = cv2 . norm( imgpoints [ i ] , imgpoints2 , cv2 .NORM_L2)/ len ( imgpoints2 ) 94 tot_error += error 95 96 print (" total error : " , mean_error/len ( objpoints ) ) 37
Apèndix G TraÃğat de trajectÚries creamapa.cpp 1/∗ 2−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− 3|+−−−+−−−++−−−+−−−+−−−+−−−+−−−+−−−++−−−+−−−+| 4| | | | | | | | | | | | | | | 5|+−−−+−−−++−−−+−−−+−−−+−−−+−−−+−−−++−−−+−−−+| 6| | | | | | | | | | | | | | | 7|+−−−+−−−++−−−+−−−+−−−+−−−+−−−+−−−++−−−+−−−+| 8| | | | | | | | | | | | | | | 9|+−−−+−−−++−−−+−−−+−−−+−−−+−−−+−−−++−−−+−−−+| 10 | | | | | | | | | | | | | | | 11 |+−−−+−−−++−−−+−−−+−−−+−−−+−−−+−−−++−−−+−−−+| 12 | | | | | | | | | | | | | | | 13 |+−−−+−−−++−−−+−−−+−−−+−−−+−−−+−−−++−−−+−−−+| 14 | | | | | | | | | | | | | | | 15 |+−−−+−−−++−−−+−−−+−−−+−−−+−−−+−−−++−−−+−−−+| 16 | | | | | | | | | | | | | | | 17 |+−−−+−−−++−−−+−−−+−−−+−−−+−−−+−−−++−−−+−−−+| 18 | | | | | | | | | | | | | | | 19 |+−−−+−−−++−−−+−−−+−−−+−−−+−−−+−−−++−−−+−−−+| 20 −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− 21 ∗/ 22 #define IMPRIMEIX_ITERACIONS 23 #define SOLVER 24 #include <stdio . h> 25 #include <iostream> 26 #include <s t dl ib . h> // per e l system 27 28 #define PARET "+−−−+−−−\033[1;37m++\033[m −−−+−−−+−−−+−−−+−−−+−−−\033[1;37m++\033[m−−−+−−−+\n" 29 #define ESPAI " | | \033[1;37m|| \ 0 3 3 [m | | | | | \033[1;37m|| \ 0 3 3 [m | |\ n" 38
30 #define X_MAX 9 31 #define Y_MAX 7 32 33 //#define ESPAI "| | | | | | | | | | | | |\n" 34 35 using namespace std ; 36 typedef enum {r=1,g=2, bf=4,bc=6}Color ; 37 typedef struct 38 { 39 int x ; 40 int y ; 41 } Punt ; 42 43 typedef struct 44 { 45 int x ; 46 int y ; 47 int ant ; 48 } Cela ; 49 50 51 52 void imprimeix_mapa() ; 53 void imprimeix_objecte (Punt p , Color c=r , int i =0) ; 54 Cela ∗solver (Punt r , Punt∗obstacles , int n_obstacles); 55 Punt ∗troba_obstacles(int &n) ; 56 57 int main () 58 { 59 Punt∗p , q ; 60 Punt robot ; 61 Cela ∗cami; 62 int n , camins=0; 63 p=troba_obstacles(n); 64 robot . x=1; 65 robot . y=3; 66 system(" clear ") ; // Neteja pantalla 67 imprimeix_mapa() ; 68 for (int i =0;i<n ; i++) imprimeix_objecte (p [ i ] , r , i +1) ; //Mostra el s obtacles al mapa 69 imprimeix_objecte ( robot , g) ; //Mostra la posiciÃş del robot al mapa 70 #i f d e f SOLVER 71 cami=so lve r ( robot , p , n) ; 72 i f ( cami!=NULL) 73 while( cami [ camins ] . ant!=−1) 74 { 75 q . x=cami [ camins ] . x ; 76 q . y=cami [ camins ] . y ; 39
77 imprimeix_objecte (q , bf ) ; 78 camins++; 79 } 80 #endif 81 } 82 83 Punt ∗troba_obstacles(int &n) 84 { 85 Punt ∗obstacles; 86 obstacles=(Punt ∗) malloc (15∗sizeof(Punt) ) ; 87 obstacles [ 0 ] . x=5; 88 obstacles [ 0 ] . y=5; 89 obstacles [ 1 ] . x=3; 90 obstacles [ 1 ] . y=2; 91 obstacles [ 2 ] . x=3; 92 obstacles [ 2 ] . y=3; 93 obstacles [ 3 ] . x=2; 94 obstacles [ 3 ] . y=4; 95 obstacles [ 4 ] . x=5; 96 obstacles [ 4 ] . y=1; 97 obstacles [ 5 ] . x=4; 98 obstacles [ 5 ] . y=0; 99 obstacles [ 6 ] . x=5; 100 obstacles [ 6 ] . y=2; 101 obstacles [ 7 ] . x=7; 102 obstacles [ 7 ] . y=4; 103 obstacles [ 8 ] . x=7; 104 obstacles [ 8 ] . y=3; 105 obstacles [ 9 ] . x=5; 106 obstacles [ 9 ] . y=6; 107 obstacles [ 1 0 ] . x=5; 108 obstacles [ 1 0 ] . y=7; 109 obstacles [ 1 1 ] . x=5; 110 obstacles [ 1 1 ] . y=4; 111 obstacles [ 1 2 ] . x=6; 112 obstacles [ 1 2 ] . y=2; 113 n=13; 114 return obstacles ; 115 } 116 117 void imprimeix_mapa() 118 { 119 string paret=PARET, espai=ESPAI; 120 for (int i =0;i <8; i++) 121 { 122 //Mira s i hi ha obst acle s : 123 // printf ("0x1B[%37m") ; 124 // pr in tf ("%c [m" , 0x1B) ; 125 //Imprimeix el mapa 40
126 cout<<paret ; 127 cout<<espai ; 128 espai=ESPAI; 129 } 130 cout<<paret ; 131 } 132 133 void imprimeix_objecte (Punt p , Color c , int i ) 134 { 135 i f (p . x<2) //Abans de la lÃŋnia de sortida 136 { 137 cout<<" \033[ "<<2∗p . y+1<<";"<<4∗p . x+1<<" f " ;//Posiciona el cursor a la f i l a 2∗p . y+1 i a la columna 4∗p . x+1 138 cout<<"\033[1;3"<<c<<"m+−−−+\033[m" ;//Imprimeix l ’ obstacle ( part superior ) 139 cout<<" \033[ "<<2∗p . y+2<<";"<<4∗p . x+1<<" f " ;//Posiciona el cursor 140 i f ( c==g ) cout<<"\033[1;3"<<c<<"m|\033[m\033[1;30;4 "<<c<<"m X \033[m\033[1;3 "<<c<<"m|\033[m" ;//Imprimeix l ’ obstacle ( l a t e r a l esquerre i dret ) 141 el s e i f ( i==0) cout<<"\033[1;3"<<c<<"m|\033[m\033[4 "<<c<<"m \033[m\033[1;3 "<<c<<"m|\033[m" ;//Imprimeix l ’ obstacle ( l a t e r a l esquerre i dret ) 142 el s e cout<<"\033[1;3"<<c<<"m|\033[m\033[1;30;41m "<<i<<" \033[m\033[1;3 "<<c<<"m|\033[m" ;//Imprimeix l ’ obstacle ( l a t e r a l esquerre i dret ) 143 cout<<" \033[ "<<2∗p . y+3<<";"<<4∗p . x+1<<" f " ;//Posiciona el cursor 144 cout<<"\033[1;3"<<c<<"m+−−−+\033[m" ;//Imprimeix l ’ obstacle ( part i n f e r i o r ) 145 } 146 el s e i f (p . x<8) //Abans de la lÃŋnia d ’ arribada 147 { 148 cout<<" \033[ "<<2∗p . y+1<<";"<<4∗p . x+2<<" f " ;//Posiciona el cursor a la f i l a 2∗p . y+1 i a la columna 4∗p . x+2 149 cout<<"\033[1;3"<<c<<"m+−−−+\033[m" ;//Imprimeix l ’ obstacle ( part superior ) 150 cout<<" \033[ "<<2∗p . y+2<<";"<<4∗p . x+2<<" f " ;//Posiciona el cursor 151 i f ( c==g ) cout<<"\033[1;3"<<c<<"m|\033[m\033[1;30;4 "<<c<<"m X \033[m\033[1;3 "<<c<<"m|\033[m" ;//Imprimeix l ’ obstacle ( l a t e r a l esquerre i dret ) 152 el s e i f ( i==0) cout<<"\033[1;3"<<c<<"m|\033[m\033[4 "<<c<<"m \033[m\033[1;3 "<<c<<"m|\033[m" ;//Imprimeix l ’ obstacle ( l a t e r a l esquerre i dret ) 153 el s e cout<<"\033[1;3"<<c<<"m|\033[m\033[1;30;4 "<<c<<"m "<<i<< " \033[m\033[1;3 "<<c<<"m|\033[m" ;//Imprimeix l ’ obstacle ( l a t e r a l esquerre i dret ) 154 cout<<" \033[ "<<2∗p . y+3<<";"<<4∗p . x+2<<" f " ;//Posiciona el 41
68 # FILES 69 # Action1 . action 70 # Action2 . action 71 # ) 72 73 ## Generate added messages and se r v ice s with any dependencies l i s t e d here 74 generate_messages( 75 DEPENDENCIES 76 std_msgs # Or other packages containing msgs 77 ) 78 79 ################################################ 80 ## Declare ROS dynamic reconfigure parameters ## 81 ################################################ 82 83 ## To declare and build dynamic reconfigure parameters within this 84 ## package , follow these steps : 85 ## ∗In the f i l e package . xml : 86 ## ∗add a build_depend and a exec_depend tag for " dynamic_reconfigure" 87 ## ∗In this f i l e (CMakeLists . txt ) : 88 ## ∗add "dynamic_reconfigure" to 89 ## find_package ( catkin REQUIRED COMPONENTS . . . ) 90 ## ∗uncomment the "generate_dynamic_reconfigure_options" section below 91 ## and l i s t every . cfg f i l e to be processed 92 93 ## Generate dynamic reconfigure parameters in the ’ cfg ’ folder 94 # generate_dynamic_reconfigure_options( 95 # cfg /DynReconf1 . cfg 96 # cfg /DynReconf2 . cfg 97 # ) 98 99 ################################### 100 ## catkin s p e c i f i c configuration ## 101 ################################### 102 ## The catkin_package macro generates cmake config f i l e s for your package 103 ## Declare things to be passed to dependent projects 104 ## INCLUDE_DIRS: uncomment this i f your package contains header f i l e s 105 ## LIBRARIES: l i b r a r i e s you create in this project that dependent projects also need 106 ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 ## DEPENDS: system dependencies of this project that dependent projects also need 108 catkin_package( 48
109 # INCLUDE_DIRS include 110 # LIBRARIES orientation 111 CATKIN_DEPENDS roscpp rospy message_runtime 112 # DEPENDS system_lib 113 ) 114 115 ########### 116 ## Build ## 117 ########### 118 119 ## Specify additional locations of header f i l e s 120 ## Your package loc atio ns should be l i s t e d before other l ocat ion s 121 include_directories( 122 # include 123 ${catkin_INCLUDE_DIRS} 124 ) 125 126 ## Declare a C++ library 127 # add_library (${PROJECT_NAME} 128 # src /${PROJECT_NAME}/ orientation . cpp 129 # ) 130 131 ## Add cmake target dependencies of the l i brary 132 ## as an example , code may need to be generated before l i b r a r i e s 133 ## eith er from message generation or dynamic reconfigure 134 # add_dependencies (${PROJECT_NAME} ${${PROJECT_NAME} _EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 136 ## Declare a C++ executable 137 ## With catkin_make a l l packages are buil t within a s in gle CMake context 138 ## The recommended prefix ensures that target names across packages don ’ t c o l l i d e 139 # add_executable (${PROJECT_NAME}_node src /orientation_node . cpp) 140 141 ## Rename C++ executable without prefi x 142 ## The above recommended prefix causes long target names , the following renames the 143 ## target back to the shorter version for ease of user use 144 ## e . g . "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 # set_target_properties (${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 147 ## Add cmake target dependencies of the executable 148 ## same as for the l i b r a ry above 149 # add_dependencies (${PROJECT_NAME}_node ${${PROJECT_NAME} _EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 49
151 ## Specify l i b r a r i e s to link a li brary or executable target against 152 # target_link_libraries (${PROJECT_NAME}_node 153 # ${catkin_LIBRARIES} 154 # ) 155 156 ############# 157 ## I n s t a l l ## 158 ############# 159 160 # a l l i n s t a l l targets should use catkin DESTINATION variables 161 # See http :// ros . org/doc/ api / catkin /html/adv_user_guide/ v ariab les .html 162 163 ## Mark executable s c r i pt s ( Python etc . ) for i n s t a l l a t i o n 164 ## in contrast to setup . py , you can choose the destination 165 i n s t a l l (PROGRAMS 166 sc r ip t s / orientation_node . py 167 DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 ) 169 170 ## Mark executables and/or l i b r a r i e s fo r i n s t a l l a t i o n 171 # i n s t a l l (TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 172 # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 # ) 176 177 ## Mark cpp header f i l e s for i n s t a l l a t i o n 178 # i n s t a l l (DIRECTORY include /${PROJECT_NAME}/ 179 # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 # FILES_MATCHING PATTERN "∗.h" 181 # PATTERN ". svn" EXCLUDE 182 # ) 183 184 ## Mark other f i l e s for i n s t a l l a t i o n ( e . g . launch and bag f i l e s , etc . ) 185 # i n s t a l l (FILES 186 # # myfile1 187 # # myfile2 188 # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 # ) 190 191 ############# 192 ## Testing ## 193 ############# 194 195 ## Add gtest based cpp test target and link l i b r a r i e s 196 # catkin_add_gtest (${PROJECT_NAME}−test test / test_orientation . cpp 50
) 197 # i f (TARGET ${PROJECT_NAME}−test ) 198 # target_link_libraries (${PROJECT_NAME}−t est ${PROJECT_NAME}) 199 # endif () 200 201 ## Add fo l de r s to be run by python nosetests 202 # catkin_add_nosetests ( test ) 51
package.xml 1<?xml version=" 1.0 "?> 2<package format="2"> 3<name>orientation</name> 4<version>0.0. 0</ version> 5<description>The orientation package</ description> 6 7<!−− One maintainer tag required , multiple allowed , one person per tag −−> 8<!−− Example: −−> 9<!−− <maintainer email=" jane . doe@example . com">Jane Doe</ maintainer> −−> 10 <maintainer email=" ferriol73pey@gmail .com">FioPio</maintainer> 11 12 13 <!−− One l i c e n s e tag required , multiple allowed , one l i c e n s e per tag −−> 14 <!−− Commonly used l i ce n s e s t r i n g s : −−> 15 <!−− BSD, MIT, Boost Software License , GPLv2, GPLv3, LGPLv2 .1 , LGPLv3 −−> 16 <l i c e n s e>TODO</ li ce n s e> 17 18 19 <!−− Url tags are optional , but multiple are allowed , one per tag −−> 20 <!−− Optional attribute type can be: website , bugtracker , or repository −−> 21 <!−− Example: −−> 22 <!−− <url type="website ">http: // wiki . ros . org/ orientation</ url> −−> 23 24 25 <!−− Author tags are optional , multiple are allowed , one per tag −−> 26 <!−− Authors do not have to be maintainers , but could be −−> 27 <!−− Example: −−> 28 <!−− <author email="jane . doe@example . com">Jane Doe</author> −−> 29 30 31 <!−− The ∗depend tags are used to specify dependencies −−> 32 <!−− Dependencies can be catkin packages or system dependencies −−> 33 <!−− Examples: −−> 34 <!−− Use depend as a shortcut for packages that are both build and exec dependencies −−> 35 <!−− <depend>roscpp</depend> −−> 36 <!−− Note that t hi s i s equivalent to the f o ll o wi n g: −−> 37 <!−− <build_depend>roscpp</build_depend> −−> 52
38 <!−− <exec_depend>roscpp</exec_depend> −−> 39 <!−− Use build_depend for packages you need at compile time: −− > 40 <!−− <build_depend>message_generation</build_depend> −−> 41 <!−− Use build_export_depend for packages you need in order to build against this package: −−> 42 <!−− <build_export_depend>message_generation</ build_export_depend> −−> 43 <!−− Use buildtool_depend for build tool packages: −−> 44 <!−− <buildtool_depend>catkin</buildtool_depend> −−> 45 <!−− Use exec_depend for packages you need at runtime: −−> 46 <!−− <exec_depend>message_runtime</exec_depend> −−> 47 <!−− Use test_depend fo r packages you need only f or t e s t i n g : −− > 48 <!−− <test_depend>gtest</test_depend> −−> 49 <!−− Use doc_depend for packages you need only for building documentation: −−> 50 <!−− <doc_depend>doxygen</doc_depend> −−> 51 <buildtool_depend>catkin</buildtool_depend> 52 <build_depend>roscpp</build_depend> 53 <build_depend>rospy</build_depend> 54 <build_depend>message_generation</build_depend> 55 <build_export_depend>roscpp</build_export_depend> 56 <build_export_depend>rospy</build_export_depend> 57 <exec_depend>roscpp</exec_depend> 58 <exec_depend>rospy</exec_depend> 59 <exec_depend>message_runtime</exec_depend> 60 61 62 <!−− The export tag contains other , unspecified , tags −−> 63 <export> 64 <!−− Other to ols can request additional information be placed here −−> 65 66 </export> 67 </package> 53
scripts/orientation_node.py 1#! / usr /bin/env python 2 3import rospy 4from orientation . msg import Orientation 5from Adafruit_BNO055 import BNO055 6 7#ROS NODE START 8rospy . init_node ( ’orientation_node ’ , anonymous=True ) 9#Setting up the publisher 10 pub = rospy . Publisher ( ’ orientation ’ , Orientation , queue_size=10) 11 #printing i n i t message 12 rospy . l o g i n f o (" Starting orientation node") 13 #set ting the maximum desired rate 14 rate = rospy . Rate (50) 15 #set ting BNO055 communication 16 bno = BNO055.BNO055( serial_port=’ /dev/ s e r i a l 0 ’ ) 17 # I n i t i a l i z e the BNO055 and stop i f something went wrong . 18 i f not bno . begin () : 19 raise RuntimeError( ’ Failed to i n i t i a l i z e BNO055! Is the sensor connected ? ’ ) 20 #Ros code 21 while not rospy . is_shutdown () : 22 #get pitch r o l l and yaw 23 heading , roll , pitch = bno . read_euler () 24 orient=Orientation () 25 orient . pitch=pitch 26 orient . r o l l=r o l l 27 ori ent . yaw=heading 28 #publish i t 29 pub . publish ( orient ) 30 rate . sleep () 54
msg/Orientation.msg 1float32 pitch 2float32 yaw 3float32 r o l l 55
H.2 Tamtam control node CMakeLists.txt 1cmake_minimum_required(VERSION 2.8 . 3) 2project ( tamtam_control ) 3 4## Compile as C++11, supported in ROS Kinetic and newer 5# add_compile_options(−std=c++11) 6 7find_library ( wiringPi_LIB wiringPi ) 8## Find catkin macros and l i b r a r i e s 9## i f COMPONENTS l i s t l i k e find_package ( catkin REQUIRED COMPONENTS xyz ) 10 ## i s used , also find other catkin packages 11 find_package ( catkin REQUIRED COMPONENTS 12 orientation 13 roscpp 14 rospy 15 ) 16 17 ## System dependencies are found with CMake’ s conventions 18 # find_package ( Boost REQUIRED COMPONENTS system ) 19 20 21 ## Uncomment this i f the package has a setup . py . This macro ensures 22 ## modules and global s c r i p t s declared therein get i n s t a l l e d 23 ## See http :// ros . org/doc/ api / catkin /html/ user_guide/setup_dot_py .html 24 # catkin_python_setup() 25 26 ################################################ 27 ## Declare ROS messages , s erv i c es and actions ## 28 ################################################ 29 30 ## To declare and build messages , se r vic e s or actions from within this 31 ## package , follow these steps : 32 ## ∗Let MSG_DEP_SET be the set of packages whose message types you use in 33 ## your messages/ s erv i c es / actions ( e . g . std_msgs , actionlib_msgs , . . . ) . 34 ## ∗In the f i l e package . xml : 35 ## ∗add a build_depend tag for "message_generation" 36 ## ∗add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 37 ## ∗I f MSG_DEP_SET isn ’ t empty the f ol lo wing dependency has been pulled in 56
38 ## but can be declared for certainty nonetheless : 39 ## ∗add a exec_depend tag for "message_runtime" 40 ## ∗In this f i l e (CMakeLists . txt ) : 41 ## ∗add "message_generation" and every package in MSG_DEP_SET to 42 ## find_package ( catkin REQUIRED COMPONENTS . . . ) 43 ## ∗add "message_runtime" and every package in MSG_DEP_SET to 44 ## catkin_package (CATKIN_DEPENDS . . . ) 45 ## ∗uncomment the add_∗_files section s below as needed 46 ## and l i s t every . msg /. srv /. action f i l e to be processed 47 ## ∗uncomment the generate_messages entry below 48 ## ∗add every package in MSG_DEP_SET to generate_messages ( DEPENDENCIES . . . ) 49 50 ## Generate messages in the ’msg ’ f older 51 # add_message_files( 52 # FILES 53 # Message1 . msg 54 # Message2 . msg 55 # ) 56 57 ## Generate serv i ces in the ’ srv ’ fo l d e r 58 # add_service_files( 59 # FILES 60 # Service1 . srv 61 # Service2 . srv 62 # ) 63 64 ## Generate actions in the ’ action ’ folder 65 # add_action_files ( 66 # FILES 67 # Action1 . action 68 # Action2 . action 69 # ) 70 71 ## Generate added messages and se r v ice s with any dependencies l i s t e d here 72 # generate_messages( 73 # DEPENDENCIES 74 # std_msgs # Or other packages containing msgs 75 # ) 76 77 ################################################ 78 ## Declare ROS dynamic reconfigure parameters ## 79 ################################################ 80 81 ## To declare and build dynamic reconfigure parameters within this 82 ## package , follow these steps : 57
48 #define BE 15 49 #define BD 16 50 #define ZE 17 51 #define ZD 18 52 #define CAP 19 53 54 //Home p ositions 55 #define BPE 105.0 56 #define BPD 116.0 57 #define BTE 99.0 58 #define BTD 85.0 59 #define BGE 119.0 60 #define BGD 51.0 61 #define BFE 27.0 // 24.0// replace servo 62 #define BFD 136.0 // 138.0 63 #define BAE 55.0 64 #define BAD 67.0 65 #define BCE 70.0 66 #define BCD 84.0 67 #define BEE 80.0 68 #define BED 60.0 69 #define BBE 142.0 70 #define BBD 15.0 71 #define BZE 68.0 72 #define BZD 70.0 73 #define BCA 90.0 74 75 // Parameters for the system 76 #define ANGLE_EQUIVALENCE 256.00/180.0 77 #define PI 3.14159265359 78 79 80 typedef enum { OFF=1, RED=2, GREEN=3, BLUE=4, YELLOW=5, CYAN=6, PURPLE=7, WHITE=8, ORANGE=9 } colour ; 81 82 struct LEG_SETPOINT_T 83 { 84 float x ; // distance in mm 85 float y ; // distance in mm 86 float z ; // distance in mm 87 float zeta ; // angle in radians 88 }; 89 90 struct LEG_CONFIGURATION_T 91 { 92 float alpha ; 93 float beta ; 94 float gamma; 95 float delta ; 64
96 float epsilon ; 97 float zeta ; 98 }; 99 100 class tamtam 101 { 102 public : 103 void set_eyes_colour ( unsigned char r , unsigned char g , unsigned char b) ; 104 void set_eyes_red_led(unsigned char c ) ; 105 void set_eyes_green_led ( unsigned char c ) ; 106 void set_eyes_blue_led(unsigned char c ) ; 107 void set_eyes_colour ( colour c ) ; 108 void disable_motors() ; 109 void disable_motor(int s ) ; 110 void spin () ; 111 unsigned char read_serial(); 112 void set_servo ( int s , float a) ; 113 void setup () ; 114 void to_home() ; 115 float get_battery_level() ; 116 void set_legs (LEG_CONFIGURATION_T right_leg , LEG_CONFIGURATION_T left_leg ) ; 117 LEG_CONFIGURATION_T compute_angles (LEG_SETPOINT_T setpoint ) ; 118 119 private: 120 int SD20 ; 121 int ARDUINO; 122 float to_set_angle [19]; 123 void setExpand () ; 124 }; 125 126 127 128 #endif 65
src/tamtam.cpp 1/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 2∗TAMTAM 3∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 4∗@author FioPio ( F erriol Pey Comas) @version v1 .2 15/05/2019 5∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 6∗This f i l e provides the p rinc ipal functions and c l ases 7∗to make the Tamtam robot work with raspberry pi zero W. 8∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/ 9#include <tamtam . h> 10 #include <math . h> 11 12 void tamtam : : setup () 13 { 14 printf ("%c[%dmSetting up TamTam%c [m\n" ,0x1B,34 ,0x1B) ; 15 wiringPiSetupSys() ; 16 SD20=wiringPiI2CSetup (0 x61 ) ; 17 i f (SD20==−1) printf ("%c[%dmError : No conection with SD20 established%c [m\n" ,0x1B ,31 ,0x1B) ; 18 el s e printf ("%c[%dmConection with SD20 established%c [m\n" ,0x1B ,34 ,0x1B) ; 19 ARDUINO=wiringPiI2CSetup (ARDUINO_I2C_ADDRESS) ; 20 i f (SD20==−1) printf ("%c[%dmError : No conection with ARDUINO established%c [m\n" ,0x1B ,31 ,0x1B) ; 21 el s e printf ("%c[%dmConection with ARDUINO established%c [m\n" ,0 x1B,34 ,0x1B) ; 22 set_eyes_colour (GREEN) ; 23 disable_motors() ; 24 // Set expanded mode 25 setExpand () ; 26 } 27 28 void tamtam : : set_eyes_colour ( unsigned char r , unsigned char g , unsigned char b) 29 { 30 set_eyes_red_led ( r ) ; 31 set_eyes_green_led (g) ; 32 set_eyes_blue_led (b) ; 33 } 34 35 36 void tamtam : : set_eyes_colour ( colour c ) 37 { 38 switch (c) 39 { 40 case OFF: 41 set_eyes_colour (0 ,0 ,0) ; 42 break; 66
43 case RED: 44 set_eyes_colour (255 ,0 ,0) ; 45 break; 46 case GREEN: 47 set_eyes_colour (0 ,255 ,0) ; 48 break; 49 case BLUE: 50 set_eyes_colour (0 ,0 ,255) ; 51 break; 52 case YELLOW: 53 set_eyes_colour (255 ,255 ,0) ; 54 break; 55 case CYAN: 56 set_eyes_colour (0 ,255 ,255) ; 57 break; 58 case PURPLE: 59 set_eyes_colour (255 ,0 ,255) ; 60 break; 61 case WHITE: 62 set_eyes_colour (255 ,255 ,255) ; 63 break; 64 case ORANGE: 65 set_eyes_colour (255 ,15 ,0) ; 66 break; 67 } 68 } 69 70 void tamtam : : set_eyes_red_led ( unsigned char c ) 71 { 72 wiringPiI2CWriteReg8 (ARDUINO, SET_RED_LED, c ) ; 73 delayMicroseconds(70) ; 74 } 75 76 void tamtam : : set_eyes_green_led ( unsigned char c ) 77 { 78 wiringPiI2CWriteReg8 (ARDUINO, SET_GREEN_LED, c ) ; 79 delayMicroseconds(70) ; 80 } 81 82 void tamtam : : set_eyes_blue_led ( unsigned char c ) 83 { 84 wiringPiI2CWriteReg8 (ARDUINO, SET_BLUE_LED, c ) ; 85 delayMicroseconds(70) ; 86 } 87 88 void tamtam : : disable_motors () 89 { 90 for (int i =1;i <20; i++) disable_motor ( i ) ; 91 } 67
92 93 void tamtam : : disable_motor ( int s ) 94 { 95 i f ( s>0 and s <20) 96 { 97 wiringPiI2CWriteReg8 (SD20 , s , 0) ; 98 delayMicroseconds(70) ; 99 } 100 } 101 102 void tamtam : : set_servo ( int s , float a) 103 { 104 i f ( s>0 and s <20) 105 { 106 int angle = a ∗ANGLE_EQUIVALENCE; 107 i f ( angle <1) angle = 1; 108 el s e i f ( angle >255) angle = 255; 109 wiringPiI2CWriteReg8 (SD20 , s , angle ) ; 110 delayMicroseconds(70) ; 111 } 112 } 113 114 void tamtam : : to_home () 115 { 116 set_servo (PE,BPE) ; 117 set_servo (PD,BPD) ; 118 set_servo (TE,BTE) ; 119 set_servo (TD,BTD) ; 120 set_servo (GE,BGE) ; 121 set_servo (GD,BGD) ; 122 set_servo (FE,BFE) ; 123 set_servo (FD,BFD) ; 124 set_servo (AE,BAE) ; 125 set_servo (AD,BAD) ; 126 set_servo (CE,BCE) ; 127 set_servo (CD,BCD) ; 128 set_servo (EE,BEE) ; 129 set_servo (ED,BED) ; 130 set_servo (BE,BBE) ; 131 set_servo (BD,BBD) ; 132 set_servo (ZE,BZE) ; 133 set_servo (ZD,BZD) ; 134 set_servo (CAP,BCA) ; 135 // Disables de servos that are not in load 136 delay (250) ; 137 disable_motor(EE); 138 disable_motor(ED); 139 disable_motor(BE) ; 140 disable_motor(BD) ; 68
141 disable_motor(ZE); 142 disable_motor(ZD) ; 143 disable_motor(CAP); 144 } 145 146 147 void tamtam : : set_legs (LEG_CONFIGURATION_T right_leg , LEG_CONFIGURATION_T left_leg ) 148 { 149 set_servo (PE, BPE −((180.0/ PI ) ∗left_leg . alpha ) ) ; 150 set_servo (PD, BPD −((180.0/ PI)∗right_leg . alpha ) ) ; 151 set_servo (TE, BTE −((180.0/ PI )∗left_leg . beta ) ) ; 152 set_servo (TD, BTD + ((180.0/ PI)∗right_leg . beta ) ) ; 153 set_servo (GE, BGE −((180.0/ PI)∗left_leg .gamma) ) ; 154 set_servo (GD, BGD + ((180.0/ PI)∗right_leg .gamma) ) ; 155 set_servo (FE, BFE + ((180.0/ PI )∗left_leg . delta ) ) ; 156 set_servo (FD, BFD −((180.0/ PI)∗right_leg . delta ) ) ; 157 set_servo (AE, BAE −((180.0/ PI)∗left_leg . epsilon ) ) ; 158 set_servo (AD, BAD −((180.0/ PI)∗right_leg . epsilon ) ) ; 159 set_servo (CE, BCE + ((180.0/ PI)∗left_leg . zeta ) ) ; 160 set_servo (CD, BCD −((180.0/ PI)∗right_leg . zeta ) ) ; 161 } 162 163 164 float tamtam:: get_battery_level() 165 { 166 return ( wiringPiI2CReadReg8 (ARDUINO,READ_BATTERY) ∗(10.0 /255.0)); 167 } 168 169 LEG_CONFIGURATION_T tamtam : : compute_angles (LEG_SETPOINT_T setpoint) 170 { 171 LEG_CONFIGURATION_T leg_config ; 172 float BD_DIS, x , y , z ; 173 //computing the new frame values 174 x=(setpoint . x∗cos ( setpoint . zeta ) )+(setpoint . y∗sin ( setpoint . zeta ) ) ; 175 y=(setpoint . y∗cos ( setpoint . zeta ) )−(setpoint.x∗sin ( setpoint . zeta ) ) ; 176 z=setpoint . z ; 177 //computing the jo int angles 178 leg_config . alpha=atan ( y / ( z−TA_DIS−EF) ) ; 179 float a=y−(AB∗sin ( leg_config . alpha ) )−(DE∗sin ( leg_config . alpha ) ) ; 180 float b=z−(AB∗cos ( leg_config . alpha ) )−(DE∗cos ( leg_config . alpha ) ) −TA_DIS −EF; 181 BD_DIS=sqrt (( x∗x)+(a∗a)+(b∗b) ) ; 182 leg_config .gamma=PI−acos ( ((BC∗BC)+(CD_DIS∗CD_DIS)−(BD_DIS∗ 69
BD_DIS) ) /(2∗BC∗CD_DIS) ) ; 183 leg_config . delta= asin (x/BD_DIS)+acos ( ((CD_DIS∗CD_DIS)+(BD_DIS∗ BD_DIS)−(BC∗BC) ) /(2∗CD_DIS∗BD_DIS) ) ; 184 leg_config . beta=leg_config .gamma−leg_config . delta ; 185 leg_config . epsilon=leg_config . alpha ; 186 leg_config . zeta=setpoint . zeta ; 187 return leg_config ; 188 } 189 190 191 void tamtam : : setExpand () 192 { 193 wiringPiI2CWriteReg8 (SD20 , 21 , 32) ; 194 delayMicroseconds(70) ; 195 wiringPiI2CWriteReg8 (SD20 , 22 , 1) ; 196 delayMicroseconds(70) ; 197 wiringPiI2CWriteReg8 (SD20 , 23 , 224) ; 198 delayMicroseconds(70) ; 199 } 70
src/tamtam.cpp 1/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 2∗tamtam_control 3∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 4∗@author FioPio ( F erriol Pey Comas) @version v3 .2 07/06/2019 5∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ 6∗This f i l e provides the code for the ros node tamtam_control 7∗and i t c ontrols the tamtam movement and l og ic . 8∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/ 9 10 11 #include <tamtam . h> 12 #include <ros / ros . h> 13 #include <ori ent atio n / Orientation . h> 14 #include <fstream> 15 #include <stdio . h> 16 #include <math . h> 17 18 #define T_SAMPLE 20000 19 #define FILENAME "/home/ pi /data/data_D_PID . txt " 20 21 //Walking and base parameters 22 #define T_WALKING 2.0 23 #define STAND_HEIGH 121.0 24 #define BASE_HEIGH 107.5 25 #define STEP_HEIGH 15 26 #define STEP_LATERAL 35 27 #define STEP_FORWARD 22.5 28 #define W (2.0∗PI)/T_WALKING 29 30 31 #define KP_a 0.0 32 #define KP_p 0.14625 33 34 #define KI_a 0.0 35 #define KI_p 1.76 36 37 #define KD_a 0.0 38 #define KD_p 0.1942 39 #define KD_e 0.0 40 41 #define MAX_PITCH_CORRECTION 40.0 42 #define MAX_ALPHA_CORRECTION 20.0 43 #define PITCH_0 −85.1875 44 45 #define T_CORRECCIO 50 //ms −> 20Hz 46 #define FILTRE_DERIV 0.1 47 71
48 class TamTam_Control 49 { 50 public : 51 TamTam_Control() 52 { 53 Tam. setup () ; 54 readed=f a l s e ; 55 write=true; 56 myfile . open (FILENAME) ; 57 this −>n = ros : : NodeHandle ("~") ; 58 this −>orient_sub = n . subscribe ("/ orientation " ,1 , & TamTam_Control : : orientation_callback , this ) ; 59 t_ant=m i l l i s () ; 60 t_init_file=t_ant ; 61 step =0; 62 Tam. to_home () ; 63 left _leg . zeta =0; 64 right_leg . zeta =0; 65 left_leg . x=0; 66 right_leg . x=0; 67 left_leg . y=0; 68 right_leg . y=0; 69 left_leg . z=BASE_HEIGH; 70 right_leg . z=BASE_HEIGH; 71 correction_p =0; 72 err_p_f_a=0; 73 step =0; 74 t=0; 75 } 76 77 void orientation_callback(const orientation : : Orientation orient_msg) 78 { 79 yaw=orient_msg . yaw ; 80 r o l l=orient_msg . r o l l ; 81 pitch=orient_msg . pitch ; 82 readed=true ; 83 } 84 85 //////////////////////////////////////////////////////////// 86 void loop () 87 { 88 switch ( step ) 89 { 90 case 3: 91 i f ( t >3) 92 { 93 t=0; 94 step =0; 72
95 } 96 break; 97 case 0: 98 right_leg . y= STEP_LATERAL ∗sin (W∗t ) ; 99 left_leg . y= STEP_LATERAL ∗sin (W∗t ) ; 100 i f ( t > T_WALKING/4.0 ) 101 { 102 t = 0; 103 right_leg . y = STEP_LATERAL ; 104 left_leg . y = STEP_LATERAL ; 105 step++; 106 } 107 break; 108 } 109 110 i f ( m i l l i s ()−t_ant > T_CORRECCIO) 111 { 112 float y_pitch=pitch−PITCH_0; 113 float dt=( m i l l i s ()−t_ant ) /1000.0; 114 t_ant=m i l l i s () ; 115 err_p= −y_pitch; 116 float err_p_f=FILTRE_DERIV∗err_p +(1−FILTRE_DERIV) ∗ err_p_f_a; 117 err_p_f_a=err_p_f ; 118 de_p= ( err_p_f ) −err_ant_p ; 119 err_ant_p=err_p_f ; 120 err_acu_p+=err_p ∗dt ; 121 122 correction_p=−KP_p∗y_pitch + KD_p ∗(de_p /dt ) + KI_p ∗ err_acu_p; 123 i f ( correction_p > MAX_PITCH_CORRECTION) 124 { 125 correction_p = MAX_PITCH_CORRECTION; 126 err_acu_p−=err_p∗dt ; 127 } 128 i f ( correction_p < −MAX_PITCH_CORRECTION) 129 { 130 correction_p = −MAX_PITCH_CORRECTION; 131 err_acu_p−=err_p∗dt ; 132 } 133 134 i f (( m i l l i s ()−t_init_file )>T_SAMPLE && write ) 135 { 136 write=f a l s e ; 137 myfile . close () ; 138 ROS_INFO("DADES ESCRITES") ; 139 Tam. set_eyes_colour (OFF) ; 140 Tam. disable_motors () ; 141 } 73