Ultimo Codigo Robot Bakumatsu 2009

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);

}