Brazo con dos motores DC

ah! trucho ejemplo...


#include "16f84a.h"
#fuses HS,NOWDT,PUT,PROTECT
#use delay(clock=4000000)
#use fast_io(A)
#use fast_io(B)

#define UP input(PIN_A0)
#define DOWN input(PIN_A1)
#define RIGHT input(PIN_A2)
#define LEFT input(PIN_A3)
#define INICIO input(PIN_A4)

void recoger(void)
{
output_b(0b10000);
delay_ms(2000);
}

void subir(short relay)
{
output_b(0b0001 | (relay<<4));
while(UP) delay_ms(5);
delay_ms(100);
}

void bajar(short relay)
{
output_b(0b0010 | (relay<<4));
while(DOWN) delay_ms(5);
delay_ms(100);
}

void soltar(void)
{
output_b(0);
delay_ms(2000);
}

void derecha(short relay)
{
output_b(0b0100 | (relay<<4));
while(RIGHT) delay_ms(5);
delay_ms(100);
}
void izquierda(short relay)
{
output_b(0b1000 | (relay<<4));
while(LEFT) delay_ms(5);
delay_ms(100);
}
void regresar(void)
{
subir(0);
izquierda(0);
output_b(0);
}


void main(void)
{
output_b(0);
set_tris_a(0x1F);
set_tris_b(0);
regresar();
while(1)
{
if (!INICIO)
{
bajar(0);
recoger();
subir(1);
derecha(1);
bajar(1);
soltar();
regresar();

}
}
}