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

}

Lectura Brujula dsPIC30f4013

unsigned rx1;

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

void main() {
int x=0, grados=0, contador=0;
ADPCFG |= 0xFFFF;
TRISD=0;
Uart2_Init(115200);

while(1){
x=adc_Read(1);
if(x>=500){
while(x>=500){
x=adc_Read(1);
contador++;
delay_us(100);
}
grados = contador;
printout(printHandler, "--%iº", grados);
delay_ms(300);
contador=0;
}
}

}

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

}

Ensamble - Motor DC, Servos y Serial

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

ADPCFG = 0xFFFF;
PORTB = 0xAAAA;
TRISB = 0;
TRISD=0;
Delay_ms(1000);

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

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

while(1){

PORTDbits.RD1=1;
PORTDbits.RD3=1;
printout(printHandlerI, "#0P1750#1P1280#2P2000#3P1100T2000\r");
printout(printHandlerII, "Paso 1");
delay_ms(400);
PORTDbits.RD3=0;
delay_ms(2500);

PORTDbits.RD1=0;
PORTDbits.RD3=1;
printout(printHandlerI, "#0P1550#1P1500#2P1520#3P1500T2000\r");
printout(printHandlerII, "Paso 2");
delay_ms(400);
PORTDbits.RD3=0;
delay_ms(2500);

PORTDbits.RD1=1;
PORTDbits.RD3=1;
printout(printHandlerI, "#0P1200#1P1840#2P1300#3P2000T2000\r");
printout(printHandlerII, "Paso 3");
delay_ms(400);
PORTDbits.RD3=0;
delay_ms(2500);

PORTDbits.RD1=0;
PORTDbits.RD3=1;
printout(printHandlerI, "#0P1550#1P1500#2P1520#3P1500T2000\r");
printout(printHandlerII, "YEAH");
delay_ms(400);
PORTDbits.RD3=0;
delay_ms(2500);
}

}

Serial Espejo

unsigned rx1;

void main() {

Uart2_Init(9600); //Declara el baud rate para la comunicación
Uart2_Write_Char('s'); //Envía un caracter para indicar que el micro está listo para empezar a recibir datos.

while(1)
{
if (Uart2_Data_Ready()) { //Condicional en el cual si el puerto UART recibe algún dato el micro se prepara para leerlo
rx1 = Uart2_Read_Char(); //Lee el dato
Uart2_Write_Char(rx1); //Se envía de vuelta el mismo dato recibido
}
}
}

Serial Pruebas.

Comunicacion con la PC:

void printHandler(char c)
{
uart2_Write_char(c);
}

void main() {

Uart2_Init(9600);
Uart2_Write_Char('s');

while(1)
{
printout(printHandler,"Z");
Delay_ms(1000);
}
}

Comunicacion Serial - Envio de un String

unsigned rx1;

void printHandler(char c) //Esta función ayuda a enviar un string
{ //Caracter por caracter
Uart2_Write_char(c);
}

void main() {

TRISD=0; //Inicializamos el puerto D como info aux.

Uart2_Init(115200);
while(1){
//A continuación se envían los strings para la tarjeta SSC-32
PORTDbits.RD1=1;
printout(printHandler, "#0P1750#1P1280#2P2000#3P1100T2000\r");
delay_ms(3000);
PORTDbits.RD1=0;
printout(printHandler, "#0P1550#1P1500#2P1520#3P1500T2000\r");
delay_ms(3000);
PORTDbits.RD1=1;
printout(printHandler, "#0P1200#1P1840#2P1300#3P2000T2000\r");
delay_ms(3000);
PORTDbits.RD1=0;
printout(printHandler, "#0P1550#1P1500#2P1520#3P1500T2000\r");
delay_ms(3000);
}
}

Comunicacion Serial - Envio de un Caracter

unsigned rx1;

void main() {
TRISB=0;
Uart2_Init(115200);
while(1){
PORTBbits.RB0=1;
delay_ms(100);
Uart2_Write_Char('s');
PORTBbits.RB0=0;
delay_ms(100);
}

}

Envio del PWM

unsigned int i;

unsigned int duty_50;

void main(){

ADPCFG = 0xFFFF;
PORTB = 0xAAAA;
TRISB = 0;
TRISE = 0;

TRISDbits.TRISD3=0;
TRISDbits.TRISD1=0;
PORTDbits.RD3=1;
PORTDbits.RD1=0;

while(1){

duty_50 = Pwm_Mc_Init(3000,1,0x06,0);
Pwm_Mc_Set_Duty(i = duty_50,1);

Pwm_Mc_Set_Duty(i = (duty_50*2),2);

Pwm_Mc_Start();
Delay_ms(1000);
}

}

Lectura LED

void main() {

TRISB=0;

while(1)
{
PORTBbits.RB0=1;
delay_ms(3000);

PORTBbits.RB0=0;
delay_ms(3000);
}

}

Lectura Brujula

unsigned rx1;

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

void main() {
int x=0, grados=0, contador=0;
ADPCFG |= 0xFFFF;
TRISD=0;
Uart2_Init(115200);

while(1){
x=adc_Read(0);
if(x>=500){
while(x>=500){
x=adc_Read(0);
contador++;
delay_ms(1);
}
grados = contador*10;
printout(printHandler, "--%iº", grados);
delay_ms(800);
contador=0;
}
}

}

Lectura del sensor de distancia optico SHARP

void main() { //Este programa lee el valor recibido del lector optico SHARP
int x=0;
TRISE=0; //Se declara el puerto E como salida.
ADPCFG |= 0xFFFF; //Se activan todos los ADC

x=adc_Read(0);

while(1)
{
x=adc_Read(0); //Se lee el puerto ADC 0
if(x<125){ //Si el valor es menos a 125 se enciende el led conectado al
PORTEbits.RE0=1; //puerto E0
delay_ms(500);
}
if(x>=125){ //En caso contrario se apaga
PORTEbits.RE0=0;
//delay_ms(500);
}
}

}

Codigo - Activacion Electroimanes

void main() { //Este programa su unica funcion es la de mandar 5v al puente H
TRISB=0; //Activa puertos B y D como salidas.
TRISD=0;

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

while(1) //Ciclo de activacion desactivacion
{
PORTBbits.RB0=1;
PORTBbits.RB1=1;
delay_ms(2000);

PORTBbits.RB0=0;
PORTBbits.RB1=0;
delay_ms(2000);
}

}