Este es el ultimo codigo desarrollado para el robot bakumatsu antes de su ultima prueba en el torneo nacional de robotica 2009 - Guadalajara.
unsigned rx1, x1, x2, x3;
void printHandlerI(char c)
{
Uart1_Write_char(c);
}
void printHandlerII(char c)
{
Uart2_Write_char(c);
}
int brujula()
{
int x=0, grados=0, contador=0;
int y=0;
while(1){
x=adc_Read(1);
if(x>=500){
while(x>=500){
x=adc_Read(1);
contador++;
delay_us(100);
y=1;
}
grados = contador;
contador=0;
if(y==1){
return grados; break;
}
}
}
}
void sp(){
printout(printHandlerI, "#0P1550#1P1500#2P1520#3P1500T1500\r");
printout(printHandlerII, "\r-Standard Pose-");
delay_ms(1600);
}
void prepare(){
PORTDbits.RD3=1;
PORTDbits.RD8=1;
PORTDbits.RD9=0;
PORTDbits.RD8=0;
printout(printHandlerII, "\r-Prepare Ejecutado-");
}
void repliegue(){
printout(printHandlerII, "\r-Repliegue del brazo-");
printout(printHandlerI, "#0P1750#1P1280#2P1950#3P1000T500\r");
delay_ms(600);
printout(printHandlerI, "#0P1450#1P1600#2P2190#3P1300T900\r");
delay_ms(1100);
}
int posicionate(int g){
int gmas, gmenos, gmasII, gmenosII, grad, direc, q;
gmas=g+2;
gmenos=g-2;
gmasII=g+3;
gmenosII=g-3;
PORTDbits.RD8=1;
grad=brujula();
printout(printHandlerII, "\r-Primer lectura; grad = %i-", grad);
printout(printHandlerII, "\r-gmas = %i-", gmas);
printout(printHandlerII, "\r-gmenos = %i-", gmenos);
printout(printHandlerII, "\r-gmasII = %i-", gmasII);
printout(printHandlerII, "\r-gmenosII = %i-", gmenosII);
while(1){
grad=brujula();
if(grad>=gmasII){
PORTDbits.RD2=1;
printout(printHandlerII, "\r-Cambio de direccion a la izquierda; grad = %i-", grad);
direc=1;
}
if(grad<=gmenosII){
PORTDbits.RD2=0;
printout(printHandlerII, "\r-Cambio de direccion a la derecha; grad = %i-", grad);
direc=0;
}
if(grad>=gmenos && grad<=gmas){
printout(printHandlerII, "\r-Aceptacion; grad = %i-", grad);
if(direc==1){
PORTDbits.RD2=0;
delay_ms(20);
}
if(direc==0){
PORTDbits.RD2=1;
delay_ms(20);
}
PORTDbits.RD8=0;
grad=brujula();
printout(printHandlerII, "\r-Desactivacion del puente H del motor DC-");
printout(printHandlerII, "\r-Angulo al momento de la desactivacion del puente H %i-", grad);
delay_ms(200);
grad=brujula();
printout(printHandlerII, "\r-Angulo al momento de la desactivacion del puente H + 200ms %i-", grad);
q=grad;
return q;break;
}
}
}
void sensores(){
x1=adc_Read(0);
x2=adc_Read(1);
x3=adc_Read(2);
}
void posicionateII(){
int turno=1;
PORTDbits.RD2=1;
PORTDbits.RD8=1;
printout(printHandlerII, "\r-Posicionate-");
while(1){
sensores();
printout(printHandlerII, "\r-Bx1=%i-", x1);
delay_ms(20);
printout(printHandlerII, "\r-Bx2=%i-", x2);
delay_ms(20);
printout(printHandlerII, "\r-Bx3=%i-", x3);
delay_ms(20);
if(turno==1 && x1>=3500 && x2>=3250 && x3<=3600){
PORTDbits.RD2=0;
printout(printHandlerII, "\r-Posicion 1 x1=%i-", x1);
delay_ms(20);
printout(printHandlerII, "\r-Posicion 1 x2=%i-", x2);
delay_ms(20);
printout(printHandlerII, "\r-Posicion 1 x3=%i-", x3);
delay_ms(20);
PORTDbits.RD8=0;
PORTDbits.RD9=1;
printout(printHandlerI, "#0P1700#1P1340#2P2100#3P1080T1500\r");
delay_ms(1800);
repliegue();
printout(printHandlerI, "#0P1550#1P1500#2P1520#3P1500T1500\r");
delay_ms(2200);
printout(printHandlerI, "#0P1250#1P1790#2P1320#3P2050T1500\r");
delay_ms(2200);
PORTDbits.RD9=0;
PORTDbits.RD8=1;
turno=2;
}
if(turno==2 && x1>=3500 && x2<=3250 && x3<=3600){
printout(printHandlerII, "\r-Posicion 3 x1=%i-", x1);
delay_ms(20);
printout(printHandlerII, "\r-Posicion 3 x2=%i-", x2);
delay_ms(20);
printout(printHandlerII, "\r-Posicion 3 x3=%i-", x3);
delay_ms(20);
PORTDbits.RD8=0;
delay_ms(800);
PORTDbits.RD9=1;
printout(printHandlerI, "#0P1650#1P1390#2P2400#3P1080T1500\r");
delay_ms(1800);
repliegue();
printout(printHandlerI, "#0P1550#1P1500#2P1520#3P1500T1500\r");
delay_ms(2200);
printout(printHandlerI, "#0P1400#1P1640#2P1120#3P2000T1500\r");
delay_ms(2200);
PORTDbits.RD9=0;
PORTDbits.RD8=1;
turno=3;
}
if(turno==3 && x1<=3500 && x2<=3250 && x3>=3600){
PORTDbits.RD2=0;
printout(printHandlerII, "\r-Posicion 5 x1=%i-", x1);
delay_ms(20);
printout(printHandlerII, "\r-Posicion 5 x2=%i-", x2);
delay_ms(20);
printout(printHandlerII, "\r-Posicion 5 x3=%i-", x3);
delay_ms(20);
PORTDbits.RD8=0;
PORTDbits.RD9=1;
printout(printHandlerI, "#0P1700#1P1340#2P2100#3P1080T1500\r");
delay_ms(1800);
repliegue();
printout(printHandlerI, "#0P1550#1P1500#2P1520#3P1500T1500\r");
delay_ms(2200);
printout(printHandlerI, "#0P1250#1P1790#2P1320#3P2050T1500\r");
delay_ms(2200);
PORTDbits.RD9=0;
PORTDbits.RD2=1;
turno=4;break;
}
#line 245 "C:/Documents and Settings/Administrador/Mis documentos/ITESM/Bakumatsu/Programas C/13/Ensambles/Prueba 3/Prueba3.c"
}
}
void main() {
int ang=181, angmas, angmenos, grado, po;
ADPCFG |= 0xFFFF;
TRISD=0;
TrisC=0;
Uart1_Init(115200);
Uart2_Init(115200);
printout(printHandlerII, "\r-Main Start-");
prepare();
sp();
posicionateII();
printout(printHandlerII, "\r-Electroiman activado-");
delay_ms(200);
}
No hay comentarios:
Publicar un comentario