#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;
}
}
}