Plateforme Robotique

From Qi-Hardware
Jump to: navigation, search
  1. Ivan Mauricio Rodríguez 1
  2. Andrés Mauricio Rodríguez 1
  3. Gerson Darío Piraquive 1


Contents

[edit] Notes

Pourcentage Présentation 20% Rapport 30% Fonctionnement 50%
1 Remise 10% 1 1 1
2 Remise 30% 2 2 2
3 Remise 30% 2 2 2


4 Remise 30% 5 5 5

[edit] Projet

Pour le projet final du cours de Systèmes Embarqués, on va travailler avec la carte SIE (appellée SAKC précédement). Cette carte compte principalement avec un SoC d'Ingenic Semiconductor Jz4725 et une FPGA Xilinx Spartan 3E XC3S500E, avec plusieurs péripheriques qui la faisent une solution multimédia optimale pour produits de MP3, MP4 et dispositifs mobiles portables. Le processeur du SoC est un XBurst core de 32 bits, qui utilise un ensemble d'instructions SIMD et RISC, en combinant une unité de post-traitement de vidéo.

[edit] Description

Le projet à réaliser est un bras mécanique qui va compter avec servomoteurs afin de controler l'angle de position de chaque articulation. Ce bras aura trois dégrés de liberté qui peuvent s'interpreter comme épaule, coude et pince, en utilisant pour différents fins comme saisir des objets, les lancer, les manipuler, etc. Comme application espécifique le bras prendra des objets d'une bande transporteuse et les placera dans un autre lieu. Pour placer l'objet, on va mettre un senseur de proximité dans un point determiné et quand celui-ci le detectera, la banda stoppera.

Pour son fonctionnement, les moteurs et servomoteurs ont besoin d'une etap de puissance, car ils ont besoin de beaucoup de courant. On utilisera un converteur DC-DC et des optocoupleurs pour mettre attention aux circuits utilisés. Pour le mouvement de la bande transporteuse on va utiliser des moteurs de courant continu controlés par un pont-H.

Afin d'avoir un control plus pratique des servomoteurs, on mettra trois potentiomètres pour augmenter ou réduire la tension qui arrive aux entrées analogiques aux terminals de l'ADC. Cette tension-là va correspondre avec un cycle utile de PWM.

[edit] Fonctions

La plateforme robotique aura les fonctions suivants:

  • Prendre un objet.
  • Lance l'objet.
  • Controler le mouvement de chaque articulation du bras.
  • Prendre des objets d'une bande transporteuse et les deplacer.

  • Apparence précédente de la plateforme.

  • Apparence préliminaire de la plateforme.

  • Gripper utilisé dans la plateforme.

  • Base de la plateforme.

  • Avant-bras de la plateforme.

  • Avant-bras de la plateforme.

  • Bras de la plateforme.

  • Plateforme.

  • Plateforme.

  • Plateforme.

  • Plateforme.

  • Plateforme.

  • Plateforme.

[edit] Outils Hardware

  • Carte SIE
  • Power Driver

Ce driver nous permettra controler les moteurs de deplacement de la plateforme dans les deux directiones et en plus il va nous donner une arret rapide, qui freiner immediatement le moteur.

  • Convertiseurs DC-DC

Les convertiseurs vont nous servir pour alimenter les moteurs et les servomoteurs, en faisant une fonction de step-up de tension de 4.5 à 6V. Dans ce cas le courant de sortie est environ 500 mA. Si on considere que chaque moteur/servomoteur consomme une valeur maximale de 200 mA, c'est à dire, en l'exigeant completement, ces convertiseurs nous servent pour notre but.

  • Servomoteurs (HiTec 311, Futaba S3006, TowerPro SMG995R)

Le range de cycles utiles des servos c'est de 193 à 238 pour la base, 222 à 234 pour le coude et 216 à 225 pour la pince.

  • Optocoupleur (MOCD213)

  • SIE.

  • Full Bridge Power Driver Hip4020.

  • Convertiseur DC-DC MC34063A.

  • Senseur de proximité TCRT1000.

  • Optocoupleurs MOCD213.

[edit] Outils Software

  • Lua

Lua est un langage puissant "embarqué". Cela signifie que Lua n'est pas un langage autonome, sinon une librairie qui peut être enlacée avec d'autres applications pour incorporer des facilités de Lua dans ces applications.

Lua possède aussi un interprte autonome qui execute des instructions (chunks) en temps réel, en agissant avec l'usager. De cette manière on va controler les mouvements du bras mecanique.

  • Xilinx ISE WebPack

Outil pour créer les modules hardware dans la FPGA de la SIE.

  • KiCAD

Avec cet outil on a conçu le circuit imprimé de montage superficiel.

[edit] Hardware

Pour la plateforme robotique le composant hardware va consiter basiquement en:

1. PWM (pulse width modulation). Tecnique pour laquelle on modifie le cycle de travail d'un signal periodique afin de controler la quantité d'énergie qui s'envoie à une charge.

2. Convertiseur ADC (Analog to digital converter). Conversion d'un signal d'entrée analogique en valeur binaire, pour pouvoir la manipuler dans les circuits numériques. Les signaux à convertir sont celles remises pour chacun des senseurs de proximité (TCRT1000) .

3. Controleur du composant HIP4020 (Full Bridge Power Driver). Driver de puissance specialement conçu pour moteurs DC petits.

[edit] Communication Processeur-Peripheriques

Communication

[edit] Peripheriques

On utilisera 5 peripheriques determinés pour le composant hardware décrit précédemment, c'est à dire, 3 modules PWM, 1 controleur d'ADC et 1 controleur du driver des moteurs.

[edit] PWM comme peripherique dans la FPGA

Ce peripherique va se charger de controler les servomoteurs disposés pour le bras.


Diagramme de peripheriques PWM

Comme l'on remarque, celui-ci reçois 8 bits d'information du processeur (sram_data) qui determinent le cycle utile du signal periodique qui sort (pwm), la periode de ce signal est determinée pour un parametre interne du module appelé temp1.

Le diagramme de flux se montre ci-dessous où on explique le comportement du peripherique.

Diagramme de flux PWM

Le module montré ci-dessous est la seconde version du controleur des trois peripheriques utilisés pour le control des servomoteurs. 

  1.   `timescale 1ns / 1ps
  2. module pwm(pwm1, pwm2, pwm3, 
  3. wdBus, we, cs,
  4. reset, clk);
  5.   parameter     B = (7);
  6.  
  7.   input            clk, we, reset;
  8.   input [3:0]      cs;
  9.   input [B:0]      wdBus;
  10.  
  11.   output       pwm1, pwm2, pwm3;
  12.  
  13. //PWM
  14.   parameter temp = 500000;
  15.  
  16.   reg [25:0] temp1, temp2, temp3;
  17.   reg [25:0] counter1, counter2, counter3;
  18.   reg ck1, ck2, ck3;
  19.  
  20. //initialize	
  21. initial
  22. begin
  23. counter1 = 26'b0;
  24. counter2 = 26'b0;
  25. counter3 = 26'b0;
  26. temp1 = 26'b0;
  27. temp2 = 26'b0;
  28. temp3 = 26'b0;
  29. ck1=0;
  30. ck2=0;
  31. ck3=0;
  32. end
  33.  
  34.     //--------------------------------------------------------------------------
  35.  
  36. //PWM peripherial1
  37. always @(negedge clk)
  38. begin
  39. 	if (we & cs[1] & wdBus != temp1)     temp1 = wdBus * temp / 256;
  40. end
  41.  
  42.  
  43. always @(posedge clk)
  44. begin
  45. 	counter1 = counter1 + 1;
  46. 	if (counter1 <= temp1)
  47. 		ck1 = 1'b1;
  48. 	else
  49. 		begin
  50. 			if(counter1 == temp)
  51. 			counter1 = 26'b0;
  52. 			else
  53. 			ck1 = 1'b0;
  54. 		end
  55. end
  56.  
  57. //PWM peripherial2
  58. always @(negedge clk)
  59. begin
  60. 	if (we & cs[2] & wdBus != temp2)     temp2 = wdBus * temp / 256;
  61. end
  62.  
  63.  
  64. always @(posedge clk)
  65. begin
  66. 	counter2 = counter2 + 1;
  67. 	if (counter2 <= temp2)
  68. 		ck2 = 1'b1;
  69. 	else
  70. 		begin
  71. 			if(counter2 == temp)
  72. 			counter2 = 26'b0;
  73. 			else
  74. 			ck2 = 1'b0;
  75. 		end
  76. end
  77.  
  78. //PWM peripherial3
  79. always @(negedge clk)
  80. begin
  81. 	if (we & cs[3] & wdBus != temp3)     temp3 = wdBus * temp / 256;	
  82. end
  83.  
  84.  
  85. always @(posedge clk)
  86. begin
  87. 	counter3 = counter3 + 1;
  88. 	if (counter3 <= temp3)
  89. 		ck3 = 1'b1;
  90. 	else
  91. 		begin
  92. 			if(counter3 == temp)
  93. 			counter3 = 26'b0;
  94. 			else
  95. 			ck3 = 1'b0;
  96. 		end
  97. end
  98.  
  99. assign pwm1 = ck1;
  100. assign pwm2 = ck2;
  101. assign pwm3 = ck3;
  102.  
  103. endmodule

La fréquence du signal PWM c'est de 100 HZ. On a chosit cette fréquence parce que quand on a fait les test avec les servos, ceux-ci ne demandaient pas beaucoup de courant (autour de 400 mA), et si on augmentais la fréquence jusqu'à 150 le courant augmentais quatre fois. Le cycle utile du signal est difinie par le vecteur de 8 bits sram_data.

Simulation du module PWM

[edit] H_BRIDGE

Pour le mouvement de la bande de la plateforme robotique on a disposé de deux moto-reducteurs, chacun est controlé par les drivers de puissance HIP4020.

Le composant reçois 3 signaux d'entrée (Brake, Direction y Enable) avec lesquelles il controle la direction du moteur connecté a celui-ci. Ces trois signaux sont aportées pour le processeur (sram_data).


Diagramme du peripherique H_BRIDGE


Diagrama de flujo H_BRIDGE

La description d'hardware est la suivante: 

  1. `timescale 1ns / 1ps
  2.  
  3. module H_BRIDGE(brake, direction, enableM, wdBus, we, cs,reset, clk
  4.     		);
  5.  
  6. parameter     B = (7);
  7.  
  8. input         clk, we, reset, cs;
  9. input [B:0]   wdBus;
  10.  
  11. output brake, direction, enableM;
  12.  
  13. reg [B:0] temp;
  14. reg brakeB, directionB, enableB;
  15.  
  16. initial
  17. begin
  18. temp = 8'b0;
  19. brakeB = 0;
  20. directionB = 0;
  21. enableB = 0;
  22. end
  23.  
  24. always @(negedge clk)
  25. begin
  26. 	if (we & cs & wdBus != temp)  begin   
  27. 	    temp = wdBus;
  28. 	end			
  29. end
  30.  
  31. always @(posedge clk)
  32. begin
  33. 	brakeB = temp[0];
  34. 	directionB = temp[1];
  35. 	enableB = temp[2];
  36. end
  37.  
  38. assign brake = brakeB;
  39. assign direction = directionB;
  40. assign enableM = enableB;
  41.  
  42. endmodule

[edit] ADC

On a utilisé le controleur de l'ADC de l'example décrit en Scope. Ce controleur-là se base dans le suivant diagramme de temps.

Formas de onda del ADC TLV1548.

[edit] Diagramme de Blocs

[File:BLOQUES.jpg|thumb|thumbtime=27|600px|center|Diagramme de blocs.]]

[edit] Schématique et PCB

  • Schématique.

  • Schématique sous-feuille Moteurs.

  • Schématique sous-feuille Senseurs.

  • Schématique sous-feuille Servos.

  • Schématique sous-hfeuilleja Connecteur.

  • Schématique sous-feuille Convertisseurs DC.

  • PCB.

  • Layer top PCB.

  • Layer bottom PCB.

  • Inter layer PCB.

  • Component Layer PCB.

[edit] Software

[edit] Utilisation de Lua

Comme mentionné dans les sections précédentes, Lua est utilisé pour exécuter l'action en temps réel et d'appeler les API écrites en C. Pour commencer, on a implémenté le PWM driver de périphérique. La référence a été pris comme l'exemple décrit dansLua_Blink_LED. Toutes les fonctions utilisé se trouvent reparties dans les fichiers jz47xx_gpio.c, Control.c, ADCw.c, sram_gpio_wrap.c. jz_adc_peripherial.c est le même fichier utilisé dans l'exemple de Scope/es qui prend la valeur du pointeur quand on fait le mapping des directions de la FPGA. Dans peripherials.lua on trouve les fonctions enlac'e avec les rutines.

Fonctions dans jz47xx_gpio.c 

  1. void
  2. jz_belt (int stop)
  3. {	
  4. 	JZ_REG *virtual4;
  5.  
  6. 	virtual4 = ADCBuffer + 0x700;
  7.  
  8. 	if (stop==1)
  9. 	{
  10. 		virtual4[0]=5;
  11. 		usleep(500000);
  12. 	}
  13. 	else 
  14. 	{
  15. 		virtual4[0]=4;
  16. 		usleep(500000);
  17. 	}
  18. }
  19.  
  20. void
  21. jz_mapping (int duty, int motor)
  22. {
  23.   JZ_REG *virtual2, *virtual3, *virtual1;
  24.  
  25.   switch(motor)
  26. 	{	
  27. 	 case 1:  virtual1 = ADCBuffer + 0x200;
  28. 		  virtual1[0] = duty;
  29. 		  break;
  30. 	 case 2:  virtual2 = ADCBuffer + 0x400;
  31.  	      virtual2[0] = duty;
  32. 		  break;
  33. 	 default: virtual3 = ADCBuffer + 0x600;
  34. 		  virtual3[0]=duty;
  35. 		  break;
  36.  
  37. 	}
  38. }
  • jz_belt() se charge d'écrire dans le péripherique du pont-H une valeur haute ou une valeur base pour que le moteur vire ou stoppe de virer.
  • jz_mmaping() écrit la valeur du cycle utilse dans le servomoteur desiré.

Fonctions dans Control.c 

  1. #define ref 245
  2. #define inc 37
  3. #define inc2 55
  4. #define inc3 70
  5.  
  6.  
  7. int control1(float a)
  8. {
  9. 	int b=215;
  10. 	int a_ant = 0;
  11.  
  12. 	if(a <= a_ant+20) a=a_ant;
  13. 	else if(a<=ref)b=193;
  14. 	else if(a>ref && a<=(ref+inc)) b=195;
  15. 	else if(a>(ref+inc) && a<=(ref+(inc*2))) b=197;
  16. 	else if(a>(ref+inc*2) && a<=(ref+(inc*3))) b=199;
  17. 	else if(a>(ref+inc*3) && a<=(ref+(inc*4))) b=201;
  18. 	else if(a>(ref+inc*4) && a<=(ref+(inc*5))) b=203;
  19. 	else if(a>(ref+inc*5) && a<=(ref+(inc*6))) b=205;
  20. 	else if(a>(ref+inc*6) && a<=(ref+(inc*7))) b=207;
  21. 	else if(a>(ref+inc*7) && a<=(ref+(inc*8))) b=209;
  22. 	else if(a>(ref+inc*8) && a<=(ref+(inc*9))) b=211;
  23. 	else if(a>(ref+inc*9) && a<=(ref+(inc*10))) b=213;
  24. 	else if(a>(ref+inc*10) && a<=(ref+(inc*11))) b=215;
  25. 	else if(a>(ref+inc*11) && a<=(ref+(inc*12))) b=217;
  26. 	else if(a>(ref+inc*12) && a<=(ref+(inc*13))) b=219;
  27. 	else if(a>(ref+inc*13) && a<=(ref+(inc*14))) b=221;
  28. 	else if(a>(ref+inc*14) && a<=(ref+(inc*15))) b=223;
  29. 	else if(a>(ref+inc*15) && a<=(ref+(inc*16))) b=225;
  30. 	else if(a>(ref+inc*16) && a<=(ref+(inc*17))) b=227;
  31. 	else if(a>(ref+inc*17) && a<=(ref+(inc*18))) b=229;
  32. 	else if(a>(ref+inc*18) && a<=(ref+(inc*19))) b=231;
  33. 	else if(a>(ref+inc*19) && a<=(ref+(inc*20))) b=233;
  34. 	else if(a>(ref+inc*20) && a<=(ref+(inc*21))) b=235;
  35. 	else if(a>(ref+inc*21)) b=238;
  36.  
  37. 	a_ant=a;
  38.  
  39. 	return b;
  40. }
  41.  
  42.  
  43. int control2(float a)
  44. {
  45. 	int b=225;
  46. 	int a_ant = 0;
  47.  
  48. 	if(a <= a_ant+inc2) a=a_ant;
  49. 	else if(a<=ref)b=220;
  50. 	else if(a>ref && a<=(ref+inc)) b=221;
  51. 	else if(a>(ref+inc2) && a<=(ref+(inc2*2))) b=222;
  52. 	else if(a>(ref+inc2*2) && a<=(ref+(inc2*3))) b=223;
  53. 	else if(a>(ref+inc2*3) && a<=(ref+(inc2*4))) b=224;
  54. 	else if(a>(ref+inc2*4) && a<=(ref+(inc2*5))) b=225;
  55. 	else if(a>(ref+inc2*5) && a<=(ref+(inc2*6))) b=226;
  56. 	else if(a>(ref+inc2*6) && a<=(ref+(inc2*7))) b=227;
  57. 	else if(a>(ref+inc2*7) && a<=(ref+(inc2*8))) b=228;
  58. 	else if(a>(ref+inc2*8) && a<=(ref+(inc2*9))) b=229;
  59. 	else if(a>(ref+inc2*9) && a<=(ref+(inc2*10))) b=230;
  60. 	else if(a>(ref+inc2*10) && a<=(ref+(inc2*11))) b=231;
  61. 	else if(a>(ref+inc2*11) && a<=(ref+(inc2*12))) b=233;
  62. 	else if(a>(ref+inc2*12) && a<=(ref+(inc2*13))) b=234;
  63. 	else if(a>(ref+inc2*13)) b=235;
  64.  
  65. 	a_ant=a;
  66.  
  67. 	return b;
  68. }
  69.  
  70.  
  71. int control3(float a)
  72. {
  73. 	int b=220;
  74. 	int a_ant = 0;
  75.  
  76. 	if(a <= a_ant+inc3) a=a_ant;
  77. 	else if(a<=ref)b=215;
  78. 	else if(a>ref && a<=(ref+inc)) b=216;
  79. 	else if(a>(ref+inc) && a<=(ref+(inc*2))) b=217;
  80. 	else if(a>(ref+inc*2) && a<=(ref+(inc*3))) b=218;
  81. 	else if(a>(ref+inc*3) && a<=(ref+(inc*4))) b=219;
  82. 	else if(a>(ref+inc*4) && a<=(ref+(inc*5))) b=220;
  83. 	else if(a>(ref+inc*5) && a<=(ref+(inc*6))) b=221;
  84. 	else if(a>(ref+inc*6) && a<=(ref+(inc*7))) b=222;
  85. 	else if(a>(ref+inc*7) && a<=(ref+(inc*8))) b=223;
  86. 	else if(a>(ref+inc*8) && a<=(ref+(inc*9))) b=224;
  87. 	else if(a>(ref+inc*9)) b=225;
  88.  
  89. 	a_ant=a;
  90.  
  91. 	return b;
  92. }
  • control1(), control2() y control3() envoient un cycle en accord avec une tension DC qui est changée pour les potentiometres.

Fonctions dans ADCw.c 

  1. void init()
  2. {
  3. 	BUFFER_OFFSET = 8;		  
  4. 	ADC_SPI_CLKDIV=ADC_SPI_CLKDIV_MAX;
  5. 	BUFFER_LEN=16;
  6. 	MUX_CHANNELS =0;
  7.  
  8. 	ADCBuffer = jz_adc_init();
  9.  
  10. 	for (int i = 0; i < 512; i++)
  11. 	{
  12. 		ADCBuffer[i] = 0x00000000;
  13. 	}
  14.  
  15. 	adcConfig(ADC_CMD_SET_SPI_CLKDIV);
  16. 	adcConfig(ADC_CMD_SET_FAST_CONV);
  17. }
  18.  
  19. float ADC(int servo)
  20. {
  21. 	int a,b,c,d=0;
  22. 	switch (servo)
  23. 	{
  24. 	case 1:		
  25. 		printf("\nTomando muestras potenciometro 1...\n");
  26. 		adcConfig(ADC_CMD_SET_CHANNEL3);
  27. 		adcConfig(ADC_CMD_READ_CHANNEL3);
  28. 		break;
  29. 	case 2:
  30. 		printf("\nTomando muestras potenciometro 2...\n");
  31. 		adcConfig(ADC_CMD_SET_CHANNEL1);
  32. 		adcConfig(ADC_CMD_READ_CHANNEL1);
  33. 		break;
  34. 	case 3:
  35. 		printf("\nTomando muestras potenciometro 3...\n");
  36. 		adcConfig(ADC_CMD_SET_CHANNEL2);
  37. 		adcConfig(ADC_CMD_READ_CHANNEL2);
  38. 		break;
  39. 	case 4:
  40. 		printf("\nTomando muestras del sensor...\n");
  41. 		adcConfig(ADC_CMD_SET_CHANNEL0);
  42. 		adcConfig(ADC_CMD_READ_CHANNEL0);
  43. 		break;
  44. 	default: 
  45. 		printf("\nError...\n");
  46. 	}
  47.  
  48.  
  49. 	for(int i=BUFFER_OFFSET; i< BUFFER_LEN/2+BUFFER_OFFSET; i++)
  50. 	{
  51. 		a=ADCBuffer[i]<<16;
  52. 		b=a>>16;
  53. 		c=ADCBuffer[i]>>16;
  54. 		d=d+b+c;
  55. 	}
  56.  
  57. 	d=d/(BUFFER_LEN/2+BUFFER_OFFSET);
  58.  
  59. 	fflush (stdout);
  60. 	return d;	
  61. }
  62.  
  63.  
  64. void adcConfig(uchar CMD)
  65. {
  66. 	ADCBuffer[0] =  (((MUX_CHANNELS<<6) + CMD)<<24) + \
  67. 					((BUFFER_LEN+BUFFER_OFFSET*2) << 8) + \
  68. 					(ADC_SPI_CLKDIV);
  69. 	while(adcCheckBufferFull()) usleep(10000);
  70. }
  71.  
  72.  
  73.  
  74. int adcCheckBufferFull()
  75. {
  76. 	return ADCBuffer[0]&0x20000000;
  77. }
  • init() initialise l'ADC avec une vitesse d'horloge minimum (97.65 kHz) et le configure en conversion rapide
  • ADC() prend des échantillons d'un determiné canal, qui peut correspondre à quelque potentiometre ou au senseur de proximité et il sort une valeur moyenne de ces echantillons.
  • adcConfig() selectionne un canal d'echantillonage ou une configuration de l'ADC et adcCheckBuffer() revise la capacité du Buffer.

Le fichier sram_gpio_wrap.c est fondamental pour l'usage de Lua parce que il contient le registre des fonctions décrites précédement.

De la même façon, dans le registre de ces fonctions on a ajouté quelque lignes de codage pour agrandir sa fonctionalité. 

  1. static int init_wrap(lua_State *L){
  2. 	init();	
  3. 	return 0;
  4. }
  5.  
  6. static int take_sample_wrap(lua_State *L){
  7. 	double color=ADC(4); 	
  8.         lua_pushnumber(L, color);
  9. 	return 1;
  10. }
  11.  
  12. static int jz_mmap_wrap(lua_State *L){
  13. 	int duty = luaL_checkint(L, 1);
  14. 	int motor= luaL_checkint(L, 2);
  15. 	jz_mapping(duty,motor);
  16. 	return 0;
  17. }
  18.  
  19. static int usleep_wrap(lua_State *L){
  20. 	usleep(500000);
  21. 	return 0;
  22. 	}
  23.  
  24. static int control_wrap(lua_State *L){
  25. 	int duty1_ant=0;
  26. 	int duty2_ant=0;
  27. 	int duty3_ant=0;
  28.  
  29. 	for(;;){
  30. 	int duty1 = control1(ADC(1));
  31. 	printf("Servo 1 --> %d",duty1);
  32. 	int duty2 = control2(ADC(2));
  33. 	printf("Servo 2 --> %d",duty2);
  34. 	int duty3 = control3(ADC(3));
  35. 	printf("Servo 3 --> %d",duty3);
  36.  
  37. 	usleep(10000);
  38.  
  39. 	if(duty1 != duty1_ant && duty1 < duty1_ant+10)
  40. 	jz_mapping(duty1,1);
  41. 	if(duty2 != duty2_ant)
  42. 	jz_mapping(duty2,2);
  43. 	if(duty3 != duty3_ant)
  44. 	jz_mapping(duty3,3);	
  45.  
  46. 	duty1_ant = duty1;
  47. 	duty2_ant = duty2;
  48. 	duty3_ant = duty3;
  49. 	}
  50.  
  51. return 0;
  52. }
  53.  
  54. static int jz_belt_wrap(lua_State *L){
  55. 	int stop = luaL_checkint(L,1);
  56. 	jz_belt(stop);
  57. 	return 0;
  58. }
  59.  
  60. static const struct luaL_reg functions[] = {  
  61.     {"init",init_wrap},
  62.     {"usleep",usleep_wrap},
  63.     {"control",control_wrap},	
  64.     {"sample",take_sample_wrap},	
  65.     {"mmap",jz_mmap_wrap},
  66.     {"belt",jz_belt_wrap},
  67.     { NULL, NULL}  
  68. };  
  69.  
  70. int luaopen_gpio(lua_State *L) {  
  71.     luaL_newmetatable(L, metaname);  
  72.     luaL_register(L, "gpio", functions);  
  73.     return 1;  
  74. }
  • init_wrap() ejecuta init().
  • take_sample_wrap() échantillonne le capteur de proximité et renvoie la moyenne de ces échantillons.
  • jz_mmap_wrap() prend deux arguments de la pile lua et les exécute jz_mmap().
  • usleep_wrap() execute la fonction usleep() de la librairie time.h.
  • control_wrap() se charge d'utiliser control(1), control(2) y control3() dans un cycle infini pour controler les servomoteurs avec les potentiometres.
  • belt_wrap() arrête la bande en fonction de la valeur de l'argument est écrit dans le stack.
  • luaL_reg enregistre les fonctions avec un nom spécifique afin qu'ils puissent travailler.
  • luaL_register () crée un identifiant "gpio" pour appeler des fonctions dans la structure.

peripherials.lua est montré ci-dessous, 

  1. package.cpath = "./?.so"
  2. require "gpio"
  3.  
  4.   function Initial ()
  5.   gpio.mmap(215,1) usleep()
  6.   gpio.mmap(225,2) usleep()
  7.   gpio.mmap(225,3) usleep()
  8.   belt(1)
  9.   end
  10.  
  11.   function init()
  12.   gpio.init()
  13.   end
  14.  
  15.   function PWM (duty,motor)
  16.   gpio.mmap(duty,motor)
  17.   end
  18.  
  19.   function usleep()
  20.   gpio.usleep()
  21.   end
  22.  
  23.   function rutine_white()
  24.   Initial()
  25.   PWM(234,2) usleep() PWM(218,3) usleep()
  26.   PWM(225,2) usleep() PWM(225,1) usleep() PWM(237,1) usleep()
  27.   PWM(234,2) usleep() PWM(223,3) usleep() PWM(225,2) usleep() PWM(225,1)
  28.   end
  29.  
  30.   function rutine_black()
  31.   Initial()
  32.   PWM(234,2) usleep() PWM(218,3) usleep()
  33.   PWM(225,2) usleep() PWM(210,1) usleep() PWM(195,1) usleep()
  34.   PWM(234,2) usleep() PWM(223,3) usleep() PWM(225,2) usleep() PWM(210,1)
  35.   end
  36.  
  37.   function belt(stop)
  38.   gpio.belt(stop)
  39.   end
  40.  
  41.   function rutine()  
  42.   color=gpio.sample()
  43.   print("Taking samples from Lua",color)
  44.   	if color < 7 then 
  45. 	belt(0)
  46. 	elseif color > 70 then
  47. 	belt(1)
  48. 	rutine_white()
  49. 	else
  50. 	belt(1)
  51. 	rutine_black()
  52. 	end
  53.   end
  54.  
  55.   function cycle()
  56. 	for i=1,1000000 do
  57. 	rutine()
  58. 	end
  59.   end
  60.  
  61.   function ctrl()
  62. 	gpio.control()
  63.   end  
  64.  
  65.   init()
  66.   Initial()
  67.   Initial()
  • Initial() place les servos dans une position initiale.
  • PWM() écrit le cycle de travail dans un servo donnée.
  • cycle() fonctionne sur un temps presque indéfinie de la routine de prendre l'objet à partir de la bande transporteuse selon la couleur et le met dans une certaine position.
  • ctrl() exécute le cycle sans fin de la rédaction de position du servo avec les boutons.

D'abord on fait init() après Initial().

Toutes ces fonctions enregistrées et décrites dans peripherials.lua sont appelés en mode interactif avec la fonction lua dofile("peripherials.lua").

[edit] Driver

Le driver est créé pour le contrôle de 3 servos à travers un cycle de service envoyée par le même. Il a le même concept général et les mêmes performances que ce qui est fait dans l'espace utilisateur. Cependant, nous devons garder à l'esprit que dans ce cas, il a été tenu une cartographie virtuelle de la mémoire physique, mais les données sont envoyées directement à l'adresse physique(FPGA_BASE = 0xb5000000).

Dans le premier cas où le driver est chargé CS2 doit être configuré pour un fonctionnement correct 

  1. #define CS2_PIN               JZ_GPIO_PORTB(26)
  1. jz_gpio_set_function(CS2_PIN, JZ_GPIO_FUNC1);

Puis on procède à l'initialisation de chacun des servomoteurs avec des positions définies. 

  1. outb(215,FPGA_BASE + 0x800);
  2. outb(225,FPGA_BASE + 0x1000);
  3. outb(220,FPGA_BASE + 0x1800);

Après avoir terminé ce module est chargé.

L'écriture dans la même entrée a le rapport cyclique suivi d'une virgule et le servomoteur pour envoyer les données (225,1). La fonction d'écriture du driver est indiquée ci-dessous: 

  1. static ssize_t
  2. device_write(struct file *filp, const char *buff, size_t count, loff_t * off)
  3. {
  4.  
  5. 	int cmd = buff[0]-48;
  6. 	int cmda = buff[1]-48;
  7. 	int cmdb = buff[2]-48;
  8. 	int cmdc = buff[3]-48;
  9. 	int cmdd = buff[4]-48;
  10. 	int cmde = 0;
  11. 	int dir;
  12.  
  13. if(cmdc == -4)
  14. {   
  15.     cmde = cmd*100 + cmda*10 + cmdb;
  16. 	printk(KERN_INFO  "Dutyout %d Servo %d\n", cmde, cmdd);
  17. 	dir = cmdd*0x800;
  18. 	outb( cmde , FPGA_BASE + dir);
  19. }
  20.  
  21. return 1;
  22. }

On soustrait le nombre 48, car cette fonction prend une chaîne et comme on envoi un certain numéro, cela est pris en code ASCII. 5 dossiers sont utilisés pour prendre chacun des «numéros» reçu à la fonction, si les données sont correctement utilisées seront l'quatrième album ',' (ASCII 44) entrant dans le conditionnel et l'envoi du rapport cyclique choisi pour le servo .

Un exemple d'utilisation de cette fonction est indiqué ci-dessous (le nœud de communication doit être créé mknod /dev/pwm c 252 0)

$ echo '215,1'>/dev/pwm

Ceci enverra un cycle de 215 devoir de l'une servomoteur. Vous pouvez également envoyer des données à chaque servo simultanément.

$ echo '215,1 230,2 216,3' > /dev/pwm

Dans ce cas, on envoie un rapport cyclique de 215 à 1 servo, 230 à 216 servo servo 2 et 3.

Retrieved from "http://en.qi-hardware.com/wiki/Plateforme_Robotique"
Personal tools
Namespaces
Variants
Actions
Navigation
interactive
Toolbox
Print/export