Robot C-program for Group A-4 (wind), E5 Fall 2002

Jerome Fung

Tanya Gonzales

Richard Metzler

Lisa Nelson

void main(void)
{
    int i, Lwheel = 50;
    int Rwheel = 50; 
    int S[4];
    motor(0, Rwheel);
    motor(1, Lwheel);
    sleep (5.0);
    while (!stop_button())  
    {
        bump;
        for (i=0; i<=3; ++i)  
        {
		S[i] = analog(i+2);
        }
	printf("\n%d %d %d %d   R:%d   L:%d",S[0],S[1],S[2],S[3],Rwheel,Lwheel);
	if (S[0] > S[1])  
          {
            Lwheel = abs(Lwheel) + 5;
            Rwheel = abs(Rwheel) + 5;
            if (Lwheel || Rwheel >= 100)
		{
              Lwheel = Lwheel - 25;
		  Rwheel = Rwheel - 25; 
		}
            motor (0, Rwheel);
		motor (1, Lwheel);
        }  
        if (S[1] > S[0])  
          {
            Lwheel = -1* abs(Lwheel) - 1;
            Rwheel = -1* abs(Rwheel) - 1;
            if (Lwheel || Rwheel <= -100)
		{
              Lwheel = Lwheel + 24;
		  Rwheel = Rwheel + 24;
		}   
            motor (0, Rwheel);
		motor (1, Lwheel);
        }
        if (S[2] > S[3])  
          {
            Rwheel += 5;
            Lwheel -= 5;
            motor (0, Rwheel);
		motor (1, Lwheel);
        }
        if (S[3] > S[1])  
          {
            Lwheel += 5;
            Rwheel -= 5;
		motor (0, Rwheel);
		motor (1, Lwheel);
        }
    }
    ao();
}
int abs (int x)
{
	if (x<=0)
	{
		x = x*-1;
	}
	return x;
}

void bump(void)
{	
	
	int leftbump,rightbump;
	motor(0,100);
	motor(1,100);
	while(!stop_button())
	{
	while(leftbump=digital(7)&& rightbump=digital(8))
	{
	}
	printf("\nbumped");
	motor(0,-100);
	motor(1,-100);
	sleep(1.0);
	motor(1,100);
	sleep(1.0);
	motor(0,100);
	}
	
}