Go4Expert

Go4Expert (http://www.go4expert.com/)
-   C (http://www.go4expert.com/forums/c/)
-   -   Bad robot code (http://www.go4expert.com/forums/bad-robot-code-t3894/)

ahayes 16Apr2007 17:06

Bad robot code
 
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];
        }       
}


Peter_APIIT 20Apr2007 14:42

Re: Bad robot code
 
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.

shabbir 20Apr2007 15:17

Re: Bad robot code
 
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.

Peter_APIIT 21Apr2007 08:50

Re: Bad robot code
 
You are right because he also not concern too much about the project. We cannot help you too much.


All times are GMT +5.5. The time now is 04:04.