Código para el robot.

 #include <16F887.h>

#DEVICE ADC=10

#FUSES HS,NOWDT,NOPROTECT,NOPUT,NOLVP,BROWNOUT

#use delay(CLOCK=20M)

#USE STANDARD_IO(A)

#USE STANDARD_IO(B)

#USE STANDARD_IO(D)

#DEFINE LCD_DB4 PIN_D4

#DEFINE LCD_DB5 PIN_D5

#DEFINE LCD_DB6 PIN_D6

#DEFINE LCD_DB7 PIN_D7

#DEFINE LCD_RS  PIN_D2

#DEFINE LCD_E   PIN_D3

#DEFINE USE_SERVO_1

#DEFINE USE_SERVO_2

#DEFINE USE_SERVO_3

#DEFINE SERVO_1 PIN_B5

#DEFINE SERVO_2 PIN_B6

#DEFINE SERVO_3 PIN_B7

#INCLUDE <servo_st.c>

#INCLUDE <map_function.c>

#INCLUDE <math.h>

#INCLUDE <LCD_16X2.c>

LONG VALOR_ADC=0;

FLOAT POSX=0;

FLOAT POSY=0;

FLOAT POSZ=0;

INT POSX_T=0;

INT POSY_T=0;

INT POSZ_T=0;

FLOAT R=0;

FLOAT H=0;

FLOAT THETA2=0;

FLOAT ALPHA=0;

FLOAT BETA=0;

FLOAT L1=14.0;

FLOAT L2=13.95;

INT ANGULO=0;

INT ANGULO2=0;

INT ANGULO3=0;

INT GRAB1[3]={0,0,0};

INT GRAB2[3]={0,0,0};

INT GRAB3[3]={0,0,0};

INT GRAB4[3]={0,0,0};

INT SELECTOR=0;

INT CONTADOR=0;

VOID MANUAL1(){

   DELAY_US(20);

   SET_ADC_CHANNEL(0);                 //ELEGIR CANAL ANALOGO 1

   DELAY_US(20);

   VALOR_ADC=READ_ADC();               //LEER VALOR ANÁLOGO

   ANGULO=MAP(VALOR_ADC,0,1023,6,178); //MAPEAR VALOR 0V=0° 5V= 180°

   SERVO_1_WRITE(ANGULO);              //ESCRIBIR EL ANGULO

   

   SET_ADC_CHANNEL(1);

   DELAY_US(20);

   VALOR_ADC=READ_ADC();

   ANGULO2=MAP(VALOR_ADC,0,1023,20,160);

   SERVO_2_WRITE(ANGULO2);

   

   SET_ADC_CHANNEL(2);

   DELAY_US(20);

   VALOR_ADC=READ_ADC();

   ANGULO3=MAP(VALOR_ADC,0,1023,25,175);

   SERVO_3_WRITE(ANGULO3);

   

    IF(INPUT(PIN_A4)==1){

      OUTPUT_HIGH(PIN_B4);

   }

   ELSE{

      OUTPUT_LOW(PIN_B4);

   }

   

  

     

   

   lcd_gotoxy(1,1);

   printf(lcd_putc,"bas=%3u",ANGULO);

   

   lcd_gotoxy(9,1);

   printf(lcd_putc,"bra=%3u",ANGULO2);

   

   lcd_gotoxy(1,2);

   printf(lcd_putc,"br2=%3u",ANGULO3);

   

   

}

VOID IK(){

   DELAY_US(20);

   SET_ADC_CHANNEL(0);                 //ELEGIR CANAL ANALOGO 1

   DELAY_US(20);

   VALOR_ADC=READ_ADC();               //LEER VALOR ANÁLOGO

   POSX=MAP(VALOR_ADC,0,1023,-10,10); //MAPEAR VALOR 0V=0° 5V= 180°

                    

   

   SET_ADC_CHANNEL(1);

   DELAY_US(20);

   VALOR_ADC=READ_ADC();

   POSY=MAP(VALOR_ADC,0,1023,10,20);

      

   SET_ADC_CHANNEL(2);

   DELAY_US(20);

   VALOR_ADC=READ_ADC();

   POSZ=MAP(VALOR_ADC,0,1023,0,10);

   

   POSX_T=POSX;

   POSY_T=POSY;

   POSZ_T=POSZ;

   

   R=SQRT(POSX*POSX+POSY*POSY);

   H=SQRT(R*R+POSZ*POSZ);

   THETA2=ATAN(POSZ/R)*180.0/PI;

   ALPHA=ACOS((-L2*L2+L1*L1+H*H)/(2*L1*H))*180.0/PI;

   BETA=ACOS((L2*L2+L1*L1-H*H)/(2*L1*L2))*180.0/PI;

      

   ANGULO=(ATAN2(POSY,POSX))*180.0/PI+8.0;

   ANGULO2=150-ALPHA-THETA2;

   ANGULO3=180-BETA;

      

   SERVO_1_WRITE(ANGULO);

   SERVO_2_WRITE(ANGULO2);

   SERVO_3_WRITE(ANGULO3);

   

   IF(INPUT(PIN_A4)==1){

      OUTPUT_HIGH(PIN_B4);

   }

   ELSE{

      OUTPUT_LOW(PIN_B4);

   }

   

   lcd_gotoxy(1,1);

   printf(lcd_putc,"POX=%3D",POSX_T);

   

   lcd_gotoxy(9,1);

   printf(lcd_putc,"POY=%3D",POSY_T);

   

   lcd_gotoxy(1,2);

   printf(lcd_putc,"POZ=%3D",POSZ_T);

}

VOID GRABAR(){

   DELAY_US(20);

   SET_ADC_CHANNEL(0);                 //ELEGIR CANAL ANALOGO 1

   DELAY_US(20);

   VALOR_ADC=READ_ADC();               //LEER VALOR ANÁLOGO

   ANGULO=MAP(VALOR_ADC,0,1023,0,180); //MAPEAR VALOR 0V=0° 5V= 180°

   SERVO_1_WRITE(ANGULO);              //ESCRIBIR EL ANGULO

   

   SET_ADC_CHANNEL(1);

   DELAY_US(20);

   VALOR_ADC=READ_ADC();

   ANGULO2=MAP(VALOR_ADC,0,1023,0,180);

   SERVO_2_WRITE(ANGULO2);

   

   SET_ADC_CHANNEL(2);

   DELAY_US(20);

   VALOR_ADC=READ_ADC();

   ANGULO3=MAP(VALOR_ADC,0,1023,0,180);

   SERVO_3_WRITE(ANGULO3);

   

    IF(INPUT(PIN_A4)==1){

      OUTPUT_HIGH(PIN_B4);

   }

   ELSE{

      OUTPUT_LOW(PIN_B4);

   }

      

   

   

   IF(INPUT(PIN_A5)==1){

      WHILE(INPUT(PIN_A5)==1){

         DELAY_MS(10);

         GRAB1[CONTADOR]=ANGULO;

         GRAB2[CONTADOR]=ANGULO2;

         GRAB3[CONTADOR]=ANGULO3;

         GRAB4[CONTADOR]=INPUT(PIN_A4);

         LCD_GOTOXY(1,1);

         LCD_PUTC("GRABANDO");

         CONTADOR++;

         DELAY_MS(500);

         IF(CONTADOR>2){

            CONTADOR=0;

         

      }

   }

}

}

VOID REPRODUCIR(){

  

   

   SERVO_1_WRITE(GRAB1[0]); 

   SERVO_2_WRITE(GRAB2[0]);

   SERVO_3_WRITE(GRAB3[0]);

   DELAY_MS(1500);

  /*

   DELAY_MS(1500);

   OUTPUT_BIT(PIN_B4,GRAB4[0]);

   DELAY_MS(500);

   */

   LCD_GOTOXY(1,1);

   LCD_PUTC("1");

 

   

   SERVO_1_WRITE(GRAB1[1]); 

   SERVO_2_WRITE(GRAB2[1]);

   SERVO_3_WRITE(GRAB3[1]);

   DELAY_MS(1500);

   /*

   DELAY_MS(1500);

   OUTPUT_BIT(PIN_B4,GRAB4[1]);

   DELAY_MS(500);

   */

   LCD_GOTOXY(1,1);

   LCD_PUTC("2");

   

   SERVO_1_WRITE(GRAB1[2]); 

   SERVO_2_WRITE(GRAB2[2]);

   SERVO_3_WRITE(GRAB3[2]);

   DELAY_MS(1500);

   /*

   DELAY_MS(1500);

   OUTPUT_BIT(PIN_B4,GRAB4[2]);

   DELAY_MS(500);

   */

   LCD_GOTOXY(1,1);

   LCD_PUTC("3");

   

   

   

}

  

VOID MAIN(){

   

  

   SETUP_ADC_PORTS(sAN0|sAN1|sAN2);

   SETUP_ADC(ADC_CLOCK_INTERNAL); 

   SERVO_INIT();

   LCD_INIT();

   OUTPUT_B(0x00);

   SELECTOR=0;

   

   WHILE(TRUE){

   

   

   

   IF(INPUT(PIN_A3)==1){

      WHILE(INPUT(PIN_A3)==1){

         DELAY_MS(10);

         SELECTOR++;

         DELAY_MS(500);

         IF(SELECTOR>3){

            SELECTOR=0;

         }

      }

   }

   

   SWITCH(SELECTOR){

      CASE 0:

      MANUAL1();

      BREAK;

      

      CASE 1:

      IK();

      BREAK;

      

      CASE 2:

      LCD_CLEAR();

      GRABAR();

      BREAK;

      

      CASE 3:

      LCD_CLEAR();

      REPRODUCIR();

      BREAK;

     

   }

   }

}