Bad robot code

ahayes's Avatar, Join Date: Mar 2007
Light Poster
I have this robot program for one of my classes, it's supposed to perform two ATD conversion sequences to scan for walls and openings and turn to the first opening available, however, it instead goes through one section, turns until it is facing east (relative east, with north being the original direction), and plows into the wall there. I can't figure out why the hell this is happening.

It still reacts to sensors meaning that it's still executing meaningful after it does this so I don't think I'm blowing the top off my stack with the linked list or program code.

Code:
#include <stdio.h> 
#define _SCI 
#include <hcs12dp256.h>
#include <stdlib.h>

struct sect
{
int x;
int y;
int open[4];
struct sect *next;
};

struct sect* lastSect(struct sect *first);
struct sect* matchSect(struct sect *first, int x, int y);
struct sect* newSect(struct sect *first, int x, int y, int open[4]);

//extern int _textmode;

void delay(void);
void delayt2(int t); //1/10th of delayt
void pwm(unsigned char freq, unsigned char duty1, unsigned char duty2);
void Forward(void);
void Backward(void);
void Left(void);
void LeftPivot(void);
void RightPivot(void);
void Right(void);
void delayt(int timel);
void RunATD(void);
void SpitResults(void);
void assignOpen(int open[4]);

#define NORTH 0
#define EAST 1
#define SOUTH 2
#define WEST 3

int mine;  //set to 1 if there is a mine
int direction;
int posx;
int posy;
struct sect *locations;

unsigned int result[4];

int putchar(char c) 
        {
        if (c == '\n')
                putchar('\r');
        while ((SC0SR1 & TDRE) == 0)
                ;
        SC0DRL = c;
        return c;
        }

int main(){
	int open[4];
	posx=0; //start at 0,0
	posy=0;
	mine = 0; //no mine
	direction = NORTH;  //initiate relative direction
	locations = 0; //initialize list of locations
	PORTAD1=0x00; //set AD1 as inputs
	ATD1CTL2 = 0x80; //power up ATD converter
	delay(); //let it warm up
	while(1)
	{
	 		RunATD();
			#define ENABLESPIT
			SpitResults();
			printf(" %u\n", (result[1]+result[2])/2);			
    //*/
	//observations:
	//14*delayt == one space
	//3*delayt+8*delayt2 == 90 degree turn
	/*DDRA = 0xFE;
	while(!(PORTA & 0x01))
	delayt(10);
	while(1){
		int checkspot;
		checkspot = 0;
    	Forward();
		switch (direction)
    	{
    	case NORTH:
    		 posy++;
    	case EAST:
    		 posx++;
    	case WEST:
    		 posx--;
    	case SOUTH:
    		 posy--;
    	}
		if(result[0] < 30)
    		open[WEST] = 1;
		else
    		open[WEST] = 0;
		if(result[3] < 30)
    		open[EAST] = 1;
		else
    		open[EAST] = 0;
		if(1)
		{
		 	struct sect *again;
   			assignOpen(open);
			again = matchSect(locations, posx, posy);
			if(again)
			{
    			int i;
    			for(i = 0; i < 4; i++)
    			{
				open[i] = open[i] & again->open[i];
				again->open[i] = open[i];    			
    			}
			}
			else
			{
			 	newSect(locations, posx, posy, open);
			}
			if(open[NORTH])
			{
        		while(direction != NORTH)
            		RightPivot();
			}
			else if(open[EAST])
			{
    			while(direction != EAST)
        			RightPivot();
			}
			else if(open[WEST])
			{
    			while(direction != EAST)
        			RightPivot();
			}
			else if(open[SOUTH])
			{
			 	while(direction != SOUTH)
    				RightPivot();
			}										
		}
	//*/
	/*Forward();
	delayt(20);
	//*/
	}//*/
	return 0;
}

void delay(void)
{
 	 int i;
	 int j;
 	 for(i=0;i<65535; i++)
	 {
 	  		for(j=0; j<2; j++)
					 asm("nop");
	 }
}

void delayt2(int t)
{
 	 int i;
	 int j;
	 int k;
	 for(k = 0; k < t; k++)
	 {
    	 for(i = 0; i < 6554; i++)
    	 {
    	  	   for(j = 0; j < 2; j++)
    		   		 asm("nop");
    	 }
	 }
}

void pwm(unsigned char freq, unsigned char duty1, unsigned char duty2)
{
	PWMCLK = 0xFF; //use scale clock A and B for all channels
	PWMPRCLK = 0x77; //use e/128 prescaler
	PWMSCLA = 0x10; //scale clocks by 32.
	PWMSCLB = 0x80; //our PWM frequency is now 5859hz one tick = .17ms
	PWMPER0 = (char)(5859/(int)freq); //our frequency in tics
	PWMPER1 = (char)(5859/(int)freq); //our frequency in tics
	PWMDTY0 = duty1; 
	PWMDTY1 = duty2;
	PWME = PWME | 0x03; //enable PWM
}

void Forward(void)
{
	pwm(50, 116, 100);
	delayt(7);
	delayt2(5);
	pwm(50,0,0);
	delayt(10);
	RunATD();
	pwm(50, 116, 100);
	delayt(7);
	delayt2(5);
	pwm(50,0,0);
	delayt(10);
}
void Backward(void)
{
	//pwm(50, 100, 116);
	RightPivot();
}
void Left(void)
{
	pwm(50, 0, 100);
}
void LeftPivot(void)
{
	pwm(50, 100, 100);
	direction++;
	direction = direction%4;
	delayt(3);
	delayt2(12);
	pwm(50,0,0);
}
void RightPivot(void)
{
	int i;
	pwm(50, 116, 116);
	if(direction > 0)
	direction--;
	else
	direction = 3;
	delayt(3);
	delayt2(18);
	pwm(50,0,0);
}
void Right(void)
{
	pwm(50, 116, 0);
}

void delayt(int timel)
{
int i;
for(i=0; i<timel; i++)
{
delay();
}
}

struct sect* lastSect(struct sect *first)
{
	struct sect *current;
	current = first;
	while(current !=0 && current->next != 0)
	{
		current = current->next;
	}
	return current;
}



struct sect* matchSect(struct sect *first, int x, int y)
{
	struct sect *current;
	struct sect *match;
	match = 0;
	current = first;
	while (current != 0 && current->next != 0)
	{
		int cx;
		int cy;
		cx = current->x;
		cy = current->y;
		if(x==cx && y ==cy)
			match = current;
		current = current->next;
	}
	return match;
}

struct sect* newSect(struct sect *first, int x, int y, int open[4])
{
	int i;
	struct sect *current;
	current = lastSect(first);
	if(current!=0){
	current->next = (struct sect*)malloc(sizeof(struct sect));
	current = current->next;
	}
	else
	{
		current = (struct sect*)malloc(sizeof(struct sect));
	}
	current->next = 0;
	current->x = x;
	current->y = y;
	for(i = 0; i< 4; i++)
	{
		current->open[i] = open[i];
	}
	return current;
}

void RunATD(void)
{
    ATD1CTL3 = 0x20; //four conversions per sequence
    ATD1CTL4 = 0xE0; //8 bit resolution conversion time 32 clock periods
    ATD1CTL5 = 0x90;
    while(!(ATD1STAT0 & 0x80)); //wait for conversion to finish
    result[0]=ATD1DR0;
    result[1]=ATD1DR1;
    result[2]=ATD1DR2;
    result[3]=ATD1DR3;
}

void SpitResults(void)
{
    #ifdef ENABLESPIT
	int i;
    for(i=0; i <4; i++)
    {
        printf("Result %u: %u\t", i, result[i]);
    }
	#else
	asm("nop");
	#endif
}

void assignOpen(int open[4])
{
	int temp[4];
	int i;
	RunATD();
	if((result[1]+result[2])/2 < 70)
    	open[NORTH] = 1;
	else
    	open[NORTH] = 0;
	for(i = 0; i < 4; i++)
	{
    	temp[i]=open[i];
	}
	switch (direction)
	{
    case NORTH:
        open[NORTH]=temp[NORTH];
        open[EAST]=temp[EAST];
        open[WEST]=temp[WEST];
        open[SOUTH]=0;
	case EAST:
        open[NORTH]=temp[WEST];
        open[EAST]=temp[NORTH];
        open[WEST]=0;
        open[SOUTH]=temp[EAST];
	case SOUTH:
        open[NORTH]=0;
        open[EAST]=temp[WEST];
        open[WEST]=temp[EAST];
        open[SOUTH]=temp[NORTH];
	case WEST:
        open[NORTH]=temp[EAST];
        open[EAST]=0;
        open[WEST]=temp[NORTH];
        open[SOUTH]=temp[WEST];
	}	
}
0
Peter_APIIT's Avatar, Join Date: Apr 2007
Contributor
Could you please submit a zip file because its really difficult for us to see the file at a little box there ?

After you have submited the file, we all can help you out.
0
shabbir's Avatar, Join Date: Jul 2004
Go4Expert Founder
Quote:
Originally Posted by Peter_APIIT
Could you please submit a zip file because its really difficult for us to see the file at a little box there ?

After you have submited the file, we all can help you out.
No one will download and look at the source code if he is not worried of submitting where he finds the problem.
0
Peter_APIIT's Avatar, Join Date: Apr 2007
Contributor
You are right because he also not concern too much about the project. We cannot help you too much.