Brazo Robotico - Movimiento Fichas

unsigned int i;
unsigned int duty_50;

void printHandlerI(char c)
{
Uart1_Write_char(c);
}

void printHandlerII(char c)
{
Uart2_Write_char(c);
}


void main() {

Uart1_Init(115200);
Uart2_Init(115200);

TRISB = 0;
TRISD=0;
Delay_ms(1000);

PORTDbits.RD1=1;
PORTDbits.RD3=0;
PORTDbits.RD0=1;
PORTDbits.RD2=0;

PORTBbits.RB1=1;

duty_50 = Pwm_Mc_Init(3000,1,0xFF,0);
Pwm_Mc_Set_Duty(i = duty_50,1);
i=i/2;
Pwm_Mc_Set_Duty(i,1);
Pwm_Mc_Start();



PORTDbits.RD1=1;
PORTDbits.RD3=0;
printout(printHandlerI, "#0P1550#1P1500#2P1520#3P1500T1000\r");
printout(printHandlerII, "Standard Pose Iniciado");
delay_ms(1300);

printout(printHandlerI, "#0P1750#1P1280#2P2000#3P1100T1000\r");
printout(printHandlerII, "Se coloca en posicion para recoger ficha");
delay_ms(1300);
PORTDbits.RD2=1;
printout(printHandlerII, "Se activa el electroiman");
delay_ms(500);

printout(printHandlerI, "#0P1550#1P1500#2P1520#3P1500T1000\r");
printout(printHandlerII, "Standard Pose con ficha");
delay_ms(1300);
PORTDbits.RD3=1;
delay_ms(2500);
PORTDbits.RD3=0;

printout(printHandlerI, "#0P1750#1P1280#2P2000#3P1100T1000\r");
printout(printHandlerII, "Se coloca en posicion para dejar la ficha");
delay_ms(1300);

PORTDbits.RD2=0;
printout(printHandlerII, "Se suelta la ficha");
delay_ms(500);

printout(printHandlerI, "#0P1550#1P1500#2P1520#3P1500T1000\r");
printout(printHandlerII, "Standard Pose");
delay_ms(3000);

printout(printHandlerII, "Tarea Finalizada");

}