martes, 17 de marzo de 2009

Primera Fase del Programa para la Planeacion de la Trayectoria

EL siguiente código es la primera etapa del programa base para el control del robot, éste corre en el microcontrolador y está hecho en CodeWarrior para la tarjeta DEMOQE128. En esta primera fase el programa permite grabar una trayectoria predefinida por el usuario para después reproducirla, además en la trayectoria se pueden establecer puntos específicos dónde se desean tomar fotografías.

/*********************************************************************/
/* Project Name: Treyectory */
/* Source fle name: RTC.c */
/*********************************************************************/
/* Robotecnicos */
/*********************************************************************/
/*********************************************************************/
/* Code for QE128 MCU's */
/* Module: Trayectory */
/* The code was developed and tested on CodeWarrior 6.0 version */
/* */
/* Description: To learn the trayecory to be followed */
/* */
/*********************************************************************/
/* */
/* Date: 07/03/2009 */
/* Robotecnicos */
/* ITESM */
/*********************************************************************/

#include /* for EnableInterrupts macro */
#include "derivative.h" /* include peripheral declarations */

/*********************************************************************/
/* Globals */
/*********************************************************************/

#define RECORDING 1
#define LOOP 2
#define RESET 3

#define UP 1
#define DOWN 2
#define RIGHT 4
#define LEFT 8
#define PHOTO 16

typedef unsigned char UINT8;

unsigned char edo_past = 0;

unsigned char begin= 0;
unsigned char finish = 0;

unsigned char in = 0;

unsigned int count = 0;
unsigned int direction = 0;
unsigned int elements = 0;

unsigned int i = 0;

unsigned char state = RECORDING;

unsigned int trayectory[5][2];


/*********************************************************************/
/* Function declarations */
/*********************************************************************/

void MCU_Init(void) {

SOPT1 = 0x23; /* Watchdog disable. Stop Mode Enable. Background Pin enable. RESET pin enable */
SCGC1 = 0x01; /* Bus Clock to the SCI1 module is enable */
SCGC2 = 0x04; /* Bus Clock to the RTC module is enable */
}

void GPIO(void) {


PTAPE = 0x0C;
PTADD = 0x00; // Configure Port A as inputs

PTDPE = 0x0C;
PTDDD = 0x00; // Configure Port D as inputs

PTCDD = (PTCD 0x3F); /* Configure PTC0-PTC5 as outputs */
PTEDD = (PTED 0xC0); /* Configure PTE6 and PTE7 pins as outputs */
PTCD = 0x3F; /* Put 1's in port C in order to turn off the LEDs */
PTED = 0xC0; /* Put 1's in port E port in order to turn off the LEDs */


}

// To print data in the LEDs of DEMOQE

void display(byte x){ /* LEDS low power active */

x = ~x;
PTED = (x & 0xC0); /* Move the adquired ADC value to port E */
PTCD = (x & 0x3F); /* Move the adquired ADC value to port C */
}


void RTC_configuration (void) {

RTCSC = 0x08; /* RTCPS configure prescaler period every 1s */
RTCMOD = 0x00; /* RTCMOD configure to interrupt every 1s */
}

void SCI_configuration (void) {

SCI1C1 = 0x00; /* 8-bit mode. Normal operation */
SCI1C2 = 0x2C; /* Receiver interrupt enable. Transmitter and receiver enable */
SCI1C3 = 0x00; /* Disable all errors interrupts */
SCI1BDL = 0x1A; /* This register and the SCI1BDH are used to configure the SCI baud rate */
SCI1BDH = 0x00; /* BUSCLK 4MHz */
/* Baud rate = -------------------- = ------------ = 9600bps */
} /* [SBR12:SBR0] x 16 26 x 16 */

/*********************************************************************
* Main Function *
*********************************************************************/

void main(void) {


MCU_Init(); /* Function that initializes the MCU */
GPIO(); /* Function that initializes the Ports of the MCU */
RTC_configuration(); /* Function that initializes the RTC module */
SCI_configuration();

EnableInterrupts; /* enable interrupts */

/* Enable RTC interrupt */
for(;;) {

// in = ((~PTAD) & 0x0C);
// in = in >> 2;
// in = in ((~PTDD) & 0x0C);

switch (state){

case RECORDING:



if (finish == 0x01){

finish = 0x00;
begin = 0x00;
trayectory[elements][0] = direction;
trayectory[elements][1] = count;
count = 0;
elements ++;
//if (elements == 5) state = LOOP;
}

if(begin == 0x00) {

edo_past = in;
switch(in){
case 1 : PTCD_PTCD0 =0; RTCSC_RTIE = 1; begin = 0x01; direction = UP; break;
case 2 : PTCD_PTCD1 =0; RTCSC_RTIE = 1; begin = 0x01; direction = DOWN; break;
case 4 : PTCD_PTCD2 =0; RTCSC_RTIE = 1; begin = 0x01; direction = RIGHT; break;
case 8 : PTCD_PTCD3 =0; RTCSC_RTIE = 1; begin = 0x01; direction = LEFT; break;
case 16: PTCD_PTCD4 =1; RTCSC_RTIE = 1; begin = 0x01; direction = PHOTO; break;
default: PTCD = 0xEF; RTCSC_RTIE = 0;

}
}


break;

case LOOP:

PTED_PTED6 = 0;
RTCSC_RTIE = 1;


break;

case RESET:

elements = 0;

PTCD_PTCD0 =1;
PTCD_PTCD1 =1;
PTCD_PTCD2 =1;
PTCD_PTCD3 =1;
PTCD_PTCD4 = 0;


break;


}



} /* loop forever */
/* please make sure that you never leave this function */
}


/*********************************************************************
* Interrupt Service Routines *
*********************************************************************/

void interrupt VectorNumber_Vsci1rx SCI_RX_ISR(void) {

SCI1S1_RDRF = 0; /* Receive interrupt disable */

// display (SCI1D); /* Display on PTE the received data from SCI */

switch (SCI1D){

case '0': in = 0; break;
case '1': state = RECORDING; break;
case '2': state = LOOP; break;
case '3': in = 1; break;
case '4': in = 2; break;
case '5': in = 4; break;
case '6': in = 8; break;
case '7': in = 16; break;
case '8': state = RESET; break;

}



while (SCI1S1_TDRE == 0); /* Wait for the transmitter to be empty */
//SCI1D = '1'; /* Send a character by SCI */
}

void interrupt VectorNumber_Vrtc RTC_ISR(void) {

RTCSC = RTCSC 0x80; /* Clear the RTC flag */
PTED_PTED7 ^= 1; /* Toggles PTE7 pin */


// in = ((~PTAD) & 0x0C);
// in = in >> 2;
// in = in ((~PTDD) & 0x0C);

switch(state){

case RECORDING:
count++;
if(edo_past != in){

finish = 0x01;
RTCSC_RTIE = 0;

}

break;

case LOOP:



if (count == trayectory[i][1]){ PTCD_PTCD0 =1; PTCD_PTCD1 =1; PTCD_PTCD2 =1; PTCD_PTCD3 =1; PTCD_PTCD4 = 0; i++; count = 0;}
count ++;

if(i == elements) i = 0;

direction = trayectory[i][0];

switch(direction) {
case UP : PTCD_PTCD0 =0; break;
case DOWN : PTCD_PTCD1 =0; break;
case RIGHT : PTCD_PTCD2 =0; break;
case LEFT : PTCD_PTCD3 =0; break;
case PHOTO : PTCD_PTCD4 =1; break;
default: PTCD = 0xFF;
}



break;
}

}

No hay comentarios:

Publicar un comentario


R2BEER2 !!!!!!!!!!