Esempio n. 1
0
void koch (int len, int level)
{
  int newlen;
  if (0 == level) {
    forwd (len);
    return;
  }
  else {
    newlen = (int) ceil (len / 3.0);
    koch (newlen, level - 1);
    turnleft (60);
    koch (newlen, level - 1);
    turnright (120);
    koch (newlen, level - 1);
    turnleft (60);
    koch (newlen, level - 1);
  }
}
Esempio n. 2
0
static
void
approachintersection(void * unusedpointer,
                     unsigned long carnumber)
{
    int car_direction;
    int turn_direction;
        /*
         * Avoid unused variable and function warnings.
         */

    (void) unusedpointer;
    (void) carnumber;
	(void) gostraight;
	(void) turnleft;
	(void) turnright;

        /*
         * cardirection is set randomly.
         */

    car_direction = random() % 4;
    turn_direction = random() % 3;


    // we enforce the FIFO ordering for the cars approaching
    // the intersection from the same direction
    if (car_direction == 0) {// North
        // north
        P(from_north);
    } else if (car_direction == 1) { //East
        P(from_east);
    } else if (car_direction == 2) {//South
        P(from_south);
    } else {
        assert(car_direction == 3); //West
        P(from_west);
    }

    // Step 1: approaching an intersection
    // message(APPROACHING, carnumber, car_direction, turn_direction);

    // kprintf("car: %d, from: %c, turn_direction: %d\n", carnumber, directions[car_direction], turn_direction);
    // after we approached the intersection, we enter it 
    if (turn_direction == TURN_LEFT) { //0
        turnleft(car_direction, carnumber);
    } else if (turn_direction == TURN_RIGHT) { //1
        turnright(car_direction, carnumber);
    } else {
        assert(turn_direction == GO_STRAIGHT); //2
        gostraight(car_direction, carnumber);
    }


    V(join_sem);
}
Esempio n. 3
0
void star_6 (int len)
{
  int i;
  for (i = 0; i < 6; i++) {
    forwd (len);
    turnright (120);
    forwd (len);
    turnleft (60);
  }
}
Esempio n. 4
0
/*The integer for choice is based on the index of items in the keywords array in the main() function*/
void chooseMove(int choice, int duration)
{
    if (choice == 0)
        forward(duration);
    else if (choice == 1)
        reverse(duration);
    else if (choice == 2)
        turnright(duration);
    else if (choice == 3)
        turnleft(duration);
    else if (choice == 4)
        pause(duration);
}
Esempio n. 5
0
task main()
{
//power,rotations
//1200 is around 1 rotation
//1200 rotations turn is 270 degrees
forward(50,1200);
wait1Msec(2000);
turnright(50,1200);
wait1Msec(2000);
turnleft(50,1200);
wait1Msec(2000);
backward(50,3600);
}
Esempio n. 6
0
void sq_koch (int len, int level)
{
  if (0 == level) {
    forwd (len);
    return;
  }
  else {
    sq_koch (len / 4, level - 1);
    turnleft (90);
    sq_koch (len / 4, level - 1);
    turnright (90);
    sq_koch (len / 4, level - 1);
    turnright (90);
    sq_koch (len / 4, level - 1);
    sq_koch (len / 4, level - 1);
    turnleft (90);
    sq_koch (len / 4, level - 1);
    turnleft (90);
    sq_koch (len / 4, level - 1);
    turnright (90);
    sq_koch (len / 4, level - 1);
  }
}
Esempio n. 7
0
void getinput()
{
//hit ESC to quit
if (key[KEY_ESC]) gameover = 1;
//WASD - SPACE keys control tank 1
if (key[KEY_W]) forward(0);
if (key[KEY_D]) turnright(0);
if (key[KEY_A]) turnleft(0);
if (key[KEY_S]) backward(0);
if (key[KEY_SPACE]&& loopcnt==LOOPDELAY) {fireweapon(0);
loopcnt=0;
}
//arrow - ENTER keys control tank 2
if (key[KEY_UP]) forward(1);
if (key[KEY_RIGHT]) turnright(1);
if (key[KEY_DOWN]) backward(1);
if (key[KEY_LEFT]) turnleft(1);
if (key[KEY_ENTER]&& loopcnt==LOOPDELAY) {fireweapon(1);
loopcnt=0;
}
//short delay after keypress
rest(20);
}
Esempio n. 8
0
void _3DTree::rule1(int i)
{
	if (i == 0)
		drawline();
	else
	{
		rule1(i - 1);
		glPushMatrix();
		turnright();
		pitchdown();
		rollleft();
		rule1(i - 1);
		glPopMatrix();
		glPushMatrix();
		rollleft();
		turnleft();
		turnleft();
		pitchdown();
		rule1(i - 1);
		glPopMatrix();
		turnaround();
		turnaround();
		rule1(i - 1);
		glPushMatrix();
		turnright();
		turnright();
		pitchdown();
		rollright();
		rule1(i - 1);
		glPopMatrix();
		glPushMatrix();
		turnleft();
		pitchdown();
		rule1(i - 1);
		glPopMatrix();
	}
}
Esempio n. 9
0
void hilbert_left (int len, int level)
{
  int bearing;
  
  if (level > 0) {
    
    bearing = heading ();
    turnleft (90);
    hilbert_right (len, level - 1);
    
    setheading (bearing);
    forwd (len);
    
    bearing = heading ();
    hilbert_left (len, level - 1);
    
    setheading (bearing);
    turnleft (90);
    forwd (len);
    
    bearing = heading ();
    turnright (90);
    hilbert_left (len, level - 1);
    
    setheading (bearing);
    turnleft (90);
    forwd (len);
    
    bearing = heading ();
    turnleft (90);
    hilbert_right (len, level - 1);
    setheading (bearing);
    
  }
  
} /* hilbert_left () */
Esempio n. 10
0
void Simple_Tree::rule1(int i)
{
	if (i == 0)
		drawline();
	else
	{
		rule2(i - 1);
		glPushMatrix();
		turnleft();
		rule1(i - 1);
		glPopMatrix();
		turnright();
		rule1(i - 1);
	}
}
Esempio n. 11
0
float Walk::turnleft(float theta)
{
	if(theta==0)
	{
		legRotfi=0;
		supLegRotfi=0;
		dribble();
		return 0;
	}
	if(theta>15)
		legRotfi=((int)leg)*15;
	else
		legRotfi=((int)leg)*theta;
	supLegRotfi=0;
	dribble();	
	theta=theta-legRotfi;
	turnleft(theta);	
}
Esempio n. 12
0
static
void
approachintersection(void * unusedpointer,
                     unsigned long carnumber)
{
        int cardirection;

        /*
         * Avoid unused variable and function warnings.
         */

        (void) unusedpointer;
        (void) carnumber;
	(void) gostraight;
	(void) turnleft;
	(void) turnright;

        /*
         * cardirection is set randomly.
         */

	//cardirection = 0 ---> from North
	//cardirection = 1 ---> from East
	//cardirection = 2 ---> from South
	//cardirection = 3 ---> from West

        cardirection = random() % 4;

	//move = 0 ---> go straight
	//move = 1 ---> go right
	//move = 2 ---> go left

	int move = random() % 3;

	//lock the intersections if 3 cars are already inside
	P(Slock);
	if(move == 0)
		gostraight(cardirection, carnumber);
	else if(move == 1)
		turnright(cardirection, carnumber);
	else if(move == 2)
		turnleft(cardirection, carnumber);
	V(Slock);
}
Esempio n. 13
0
void drawScene()
{
    GLfloat x,y,z;
        
    glColor3f(1.0f,1.0f,1.0f);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
    glPushMatrix();
    keySpecialOperations();/*
    if(keyStates['w'])
    {
         fprintf(stdout,"* cameraz=%f\nreferencez=%f\n",cameraz,referencez);
    }*/
    //gluLookAt(-5*TRACK_WIDTH-13.5,12,18,-TRACK_WIDTH-13.5,2,-1,0,1,0);
    gluLookAt(camerax,cameray,cameraz,referencex,referencey,referencez,upx,upy,upz);
    // tack draw
    trackStraight(-(TRACK_WIDTH/2.0f),(TRACK_WIDTH/2.0f),-(6*TorusRadius),0);
    trackStraight(-(TRACK_WIDTH/2.0f),(TRACK_WIDTH/2.0f),0,3*CAR_LENGTH);
    bumpdraw(-(TRACK_WIDTH/2.0f)-1.5,CAR_LENGTH,1);
    BumpyTrack(-(TRACK_WIDTH/2.0f),(TRACK_WIDTH/2.0f),3*CAR_LENGTH,6*CAR_LENGTH,(TorusRadius/2.0f));
    trackStraight(-(TRACK_WIDTH/2.0f),(TRACK_WIDTH/2.0f),6*CAR_LENGTH,9*CAR_LENGTH);
    bumpdraw((TRACK_WIDTH/2.0f),6*CAR_LENGTH,0);
    for(GLint i=0;i<=3*GRASS_WIDTH;i+=GRASS_WIDTH)
    {
          grass(-(TRACK_WIDTH/2.0f)-i,(TRACK_WIDTH/2.0f)-i,-(6*TorusRadius),0);
          grass(-(TRACK_WIDTH/2.0f)-i,(TRACK_WIDTH/2.0f)-i,1,3*CAR_LENGTH);
          grass(-(TRACK_WIDTH/2.0f)-i,(TRACK_WIDTH/2.0f)-i,3*CAR_LENGTH+1,6*CAR_LENGTH);
          grass(-(TRACK_WIDTH/2.0f)-i,(TRACK_WIDTH/2.0f)-i,6*CAR_LENGTH+1,9*CAR_LENGTH-1);
    }
    turnleft(-(TRACK_WIDTH/2.0f)-1,6*CAR_LENGTH,1);
    trackTurnLeft(-(TRACK_WIDTH/2.0f),(TRACK_WIDTH/2.0f),9*CAR_LENGTH,13*CAR_LENGTH,0,180.0f);
    trackStraight(-5*TRACK_WIDTH-2.25,-4*TRACK_WIDTH-2.25,6*CAR_LENGTH,9*CAR_LENGTH);
    BumpyTrack(-5*TRACK_WIDTH-2.25,-4*TRACK_WIDTH-2.25,3*CAR_LENGTH,6*CAR_LENGTH,(TorusRadius/2.0f));
    trackStraight(-5*TRACK_WIDTH-2.25,-4*TRACK_WIDTH-2.25,0,3*CAR_LENGTH);
    trackStraight(-5*TRACK_WIDTH-2.25,-4*TRACK_WIDTH-2.25,-(6*TorusRadius),0);
    trackTurnLeft(-(TRACK_WIDTH/2.0f),(TRACK_WIDTH/2.0f),-1.2*CAR_LENGTH,2.8*CAR_LENGTH,180.0f,355.5f);
    //trackTurnRight(-(int)(TRACK_WIDTH/2.0f)-1,(int)(TRACK_WIDTH/2.0f),8*CAR_LENGTH,12*CAR_LENGTH,180.0f);
    glTranslatef(0.0f,TorusRadius-(TorusRadius/2.75),0.0f);
    glScalef(0.5f,0.5f,0.5f);
    assembly();
    glScalef(2.0f,2.0f,2.0f);
    glTranslatef(0.0f,-TorusRadius+(TorusRadius/2.75),0.0f);
    glFlush();
    glPopMatrix();   
}
Esempio n. 14
0
void _3DTree3::rule1(int i)
{
	if (i == 0)
		drawellipse(2, 0,2,1);
	else
	{
		glScaled(.6, .6, .6);
		rule2(i - 1);
		glPushMatrix();
		turnleft();
		rule1(i - 1);
		glPopMatrix();
		rollleft();
		rollleft();
		glPushMatrix();
		turnleft();
		rule1(i - 1);
		glPopMatrix();
		rollleft();
		rollleft();
		glPushMatrix();
		turnleft();
		rule1(i - 1);
		glPopMatrix();
		rollleft();
		rollleft();
		glPushMatrix();
		turnleft();
		rule1(i - 1);
		glPopMatrix();
		rollleft();
		rollleft();
		glPushMatrix();
		turnleft();
		rule1(i - 1);
		glPopMatrix();
		rollleft();
		rollleft();
		glPushMatrix();
		turnleft();
		rule1(i - 1);
		glPopMatrix();

	}
}
Esempio n. 15
0
void turnleft(void)
{

    motorALL_ON();
   motor2_Forward(); 
motor1_OFF();
if ((checkbit(PINF,6)!=0 && checkbit(PINF,7)!=0 && checkbit(PINF,5)!=0)||(checkbit(PINF,6)!=0 && checkbit(PINF,7)==0 && checkbit(PINF,5)!=0))  //THE CROSSOVER DETECTED
                {
					
				               
                              
								j++;
							  
								/*lcd_gotoxy2(5);
								lcd_showvalue(j);*/
								PORTC=~j;
								_delay_ms(500);	
								
				}
//PORTC=0x02;
if((checkbit(PINF,6)==0) && (checkbit(PINF,7)==0) && (checkbit(PINF,5)==0)||(checkbit(PINF,6)!=0) && (checkbit(PINF,7)!=0) && (checkbit(PINF,5)!=0)) 
turnleft();
}
Esempio n. 16
0
task main()
{
	initializeRobot();

	ereset();

	waitForStart();

	ch = 1;

	brake();
	wait1Msec(300);
	{
		motor[motorH] = 50;
		motor[motorI] = 50;
		wait1Msec(300);
		motor[motorH] = 0;
		motor[motorI] = 0;

		movebackward();
		wait1Msec(2000);

		servo[servo3] = 247;

		brake(); //temporary stop
		wait1Msec(300);

		turnleft(); //turn  left to face the center goal structure
		wait1Msec(1100);

		brake(); //temporary stop
		wait1Msec(100);

		turnright();
		wait1Msec(1100);

		brake();
		wait1Msec(100);

		movebackward();// move forward to the center goal structure
		wait1Msec(2525);

		brake();
		wait1Msec(1000);

		if (SensorRaw(irs) > 3 && SensorRaw(irs) < 7)//checks for the ir value to know if the center goal structure is at postion 1
		{ //center goal structure is in the 1

			moveforward(); // move forward to be adjacent to the kickstand
			wait1Msec(650);

			brake(); //temporary stop
			wait1Msec(50);

			turnright(); //turn right to face the kickstand
			wait1Msec(1550);

			brake(); //temporary stop
			wait1Msec(100);

			moveforward(); //moveforward to knock down the kickstand and to be adjacent to the ir beacon and center goal
			wait1Msec(1250);

			brake();
			wait10Msec(3000);

			motor[motorH] = -50;//resets the initialize
			motor[motorI] = -50;
			wait1Msec(300);
			motor[motorH] = 0;//stops the 80/20 so that it does not move
			motor[motorI] = 0;
		}

		else if(SensorRaw(irs) <= 3 || SensorRaw(irs) >= 7)//if the ir sensor senses a value between 3 and 7
		{ //center goal is in position 2/3
			movebackward();//move the robot back to be adjacent to the ir beacon
			wait1Msec(900);

			turnleft();//turns left to check for position 2.
			wait1Msec(900);

			movebackward();//moves abckwards to be next to the ir beacon
			wait1Msec(1250);

			ch = 0;//gives the integer chicks the value of of 0
		}

		if (SensorRaw(irs) >= 7 && SensorRaw(irs) <= 8)//if ir value is between 7 and 8
		{ //center goal structure is in the 3 position

			while(SensorRaw(irs) < 5)//if it senses a value less than 5 until it senses at 5
			{
				movebackward();
			}
			moveforward();//moves the robot forward to be adjacent to the
			wait1Msec(500);

			turnright();//turn right to face the kickstand
			wait1Msec(900);

			moveforward();//knocks down the kickstand
			wait1Msec(1000);
		}

		else
		{ //center goal structure is in the 2 position
			moveforward();//move forward to be adjacent to the kickstand
			wait1Msec(1259);

			turnright();//turn right to face the kickstand
			wait1Msec(1600);

			moveforward();//move forward to knock down the kickstand
			wait1Msec(1500);
		}
	}
}
Esempio n. 17
0
void decidemove(player& pl)
{
    if (pl.wrongsteps <= 3 &&
	pl.steps <= 3)
    {
	return;
    }

    int r = rand()%30;
    switch(r)
    {
    case 0:
	    if (isrightempty(pl))
	    {
		turnright(pl);
		pl.steps = 0;
	    }
	    else if (isleftempty(pl))
	    {
		turnleft(pl);
	    pl.steps = 0;
	    }
	    break;
    case 1:
	if (isleftempty(pl))
	{
		turnleft(pl);
		pl.steps = 0;
	    }
	    else if (isrightempty(pl))
	    {
		turnright(pl);
		pl.steps = 0;
	}
	break;
    }

    if (isaheadempty(pl))
    {
	return;
    }
    else if (r%2 == 0)
    {
	if (isleftempty(pl))
	{
	    turnleft(pl);
	    pl.steps = 0;
	}
	else if (isrightempty(pl))
	{
	    turnright(pl);
	    pl.steps = 0;
	}
    }
    else
    {
	if (isrightempty(pl))
	{
	    turnright(pl);
	    pl.steps = 0;
	}
	else if (isleftempty(pl))
	{
	    turnleft(pl);
	    pl.steps = 0;
	}
    }

    if (r==0)
    {
	turnleft(pl);
	pl.steps = 0;
    }
    else if (r==1)
    {
	turnright(pl);
	pl.steps = 0;
    }
}
Esempio n. 18
0
void main()
{

DDRA=0XFF;
DDRC=0xFF;
//PORTC=0XFF;           //led 
                    // PORT F Initialization
DDRF=0x00;
PORTF=0xFF;            //SENSOR


DDRB=0XF0;


DDRE=0x0F;  //motor enable
int i=PINF;
    
while(1)
   { 
 
 if(checkbit(i,4)==0)
  {
  PORTA=0X00;
   
   if(( checkbit(i,6)==0 && checkbit(i,7)!=0 && checkbit(i,5)!=0) || (checkbit(i,6)==0 && checkbit(i,7)==0 && checkbit(i,5)!=0))
		turnright();
  
	else if(( checkbit(i,6)!=0 && checkbit(i,7)!=0 && checkbit(i,5)==0) || (checkbit(i,6)!=0 && checkbit(i,7)==0 && checkbit(i,5)==0))

          turnleft();
		  
  else if(checkbit(i,5)==0 && checkbit(i,7)!=0 && checkbit(i,6)==0)                               
	{
		motorALL_ON();
		motor2_Forward();   
	   motor1_Forward();
		//PORTC=0x00;
		
			 if ((checkbit(i,6)!=0 && checkbit(i,7)!=0 && checkbit(i,5)!=0)||(checkbit(i,6)!=0 && checkbit(i,7)==0 && checkbit(i,5)!=0))  //THE CROSSOVER DETECTED
                {
					
				               
                              motorALL_ON();
                               motor2_Forward();   
							   motor1_Forward();
								j++;
							  
								/*lcd_gotoxy2(5);
								lcd_showvalue(j);*/
								PORTC=~j;
								_delay_ms(500);	
								
				}
	}
}						
	else
	 {
	    PORTA=0X0A;
        //_delay_ms(500);
        //PORTA=0X00; 
        motor2_OFF();
		motor1_OFF();
		motorALL_OFF();
		
		}

     }
  
}
Esempio n. 19
0
     static INLINE void tracepoint (int xp, int yp, int dir, unsigned int color, int xstart, int xend, int ystart, int yend)
{
    unsigned char *calc;
    cpixeldata_t mycolor;
    int i, lookdir;
    unsigned int c;
    int x, y;
    int periodicity = (dir & 8) != 0;
    dir &= ~8;
    calc = calculated + xp + yp * CALCWIDTH;

    if (!(*calc & (CALCULATED | CALCULATING))) {
        *calc |= CALCULATING;
        mycolor = (cpixeldata_t) calculatepixel (xp, yp, periodicity);
        putpixel (xp, yp, mycolor);
        *calc |= CALCULATED;
        *calc &= ~CALCULATING;
    } else {
        if (*calc & CALCULATING) {
            /*Bad luck..some other procesor is working with out pixel :) try
             *later.*/
            addstack (xp, yp, dir, color, periodicity);
            return;
        }
        mycolor = getpixel (xp, yp);
    }

    while (1) {
        periodicity = (mycolor == inset || color == inset);
        lookdir = turnright (dir);
        for (i = 0; i < 3; i++) {
            x = xp + dirrections[lookdir][0];
            y = yp + dirrections[lookdir][1];
            if (x >= xstart && x <= xend && y >= ystart && y <= yend) {
                calc = calculated + x + y * CALCWIDTH;
                if (!(*calc & (CALCULATED | CALCULATING))) {
                    *calc |= CALCULATING;
                    c = calculatepixel (x, y, periodicity);
                    putpixel (x, y, c);
                    *calc |= CALCULATED;
                    *calc &= ~CALCULATING;
                } else {
                    if (*calc & CALCULATING) {
                        /*Bad luck..some other procesor is working with out pixel :) try
                         *later.*/
                        addstack (xp, yp, dir, color, periodicity);
                        return;
                    }
                    c = getpixel (x, y);
                }
                if (c == mycolor)
                    break;
                if (c != color) {
                    int dir2 = turnright (lookdir);
                    int mask = (1 << dir2) + (1 << turnright (dir2));
                    if (!(*calc & mask)) {
                        addstack (x, y, dir2, mycolor, periodicity);
                    }
                    color = c;
                }
            }
            lookdir = turnleft (lookdir);
        }
        x = xp + dirrections[lookdir][0];
        y = yp + dirrections[lookdir][1];
        if (x >= xstart && x <= xend && y >= ystart && y <= yend) {
            calc = calculated + x + y * CALCWIDTH;
            if (!(*calc & (1 << lookdir))) {
                *calc |= (1 << lookdir);
                if (size < 10) {
                    addstack (x, y, lookdir, color, periodicity);
                    return;
                } else {
                    xp = x;
                    yp = y;
                    dir = lookdir;
                    calc = calculated + xp + yp * CALCWIDTH;
                }
            } else
                return;
        } else
            return;
    }
}
Esempio n. 20
0
//returns true for limit pass, pass or put, false for movement or 
// .. inp: a,b,c,d,e,f,g :
// a: forward, b: back, c: left, d: right,
// e: put, f: pass, g: pause
bool GTPWrapper::doThis(bool first, vector<double> inp)
{
  //  cout << "dothis inp: " << printvector(inp);
  //check for limits if within inc counter..
  if(first){
    if(fcount>=thinksteps){
      setPass(first);
      return true;
    }else 
      fcount++;
  }
  if(!first){
    if(scount>=thinksteps){
      setPass(first);
      return true;
    }else
      scount++;
  }

  //needs conflict resolution
  //first check for movement(default in kens code)
  //default in con  flicts; back & forward => forward, left & righ ->left
  bool forward,back,west,east;
  forward=back=west=east=false;
  bool moving = false;
  int *heading = getHeading(first);
  if(inp.at(0)>0.5){//forward..
    moving = true;
    if(*heading == 0)
      forward = true;
    else if(*heading == 1)
      east = true;
    else if(*heading == 2)
      back = true;
    else if(*heading == 3)
      west = true;
  }
  bool turned = false;
  if(inp.at(1)>0.5){//turn left
    turnleft(first);
    turned = true;
  }else if(inp.at(2)>0.5){//right..
    turnright(first);
    turned = true;
  }
  
  if(!moving&&!turned){
    int ind = 5;
    int n = 5;
//     if(headingsupport)
//       n++;
    double max=0.5;
    for(int i=3;i<n;i++){
      if(inp.at(i)>max){
	ind = i;
	max = inp.at(i);
      }
    }
    if(ind==5){//default
      //      cout << "doing nothing.." << endl;
      return false;
    }
    if(ind==3){
//       if(!justput)
      //cout << "putting " << getColoredCoord(first) << endl; 

//       if(justput)
// 	secondput = true;
//       justput = true;
//       if(justmoved && secondput)
// 	cout << "!!!! second put " << getColoredCoord(first) << endl;
      if(put(first)){//regardless of illegal move?? heh
	moves++;
// 	if(moves>4)
// 	cout<<"four moves!!!: " << endl << getLocalBoardAscii();
	unsetPass(first);
	unsetCount(first);
      }else{
	setPass(first);
	unsetCount(first);
      }
      return true;
    }else if(ind==4){
      //      cout << "passing" << endl;
      setPass(first);
      unsetCount(first);
      return true;
    }
//     else if(headingsupport&&ind==7){
//       //      cout << "turning around!!" << endl;
      
//       unsetPass(first);
//       return false;
//     }
  }else{
//     if(!justmoved)
    //cout << "fpass:"******" spass:"******" moving(fc:"<<fcount<<" sc:"<<scount<<")" << "forward: " << forward << " back: "<<back<<" west: "<<west<<"  east:" << east<< endl;
    unsetPass(first);
//     justmoved = true;
    //    unsetCount(first);
    if(forward)
      up(first);
    if(back)
      down(first);
    if(west)
      left(first);
    if(east)
      right(first);
    return false;
  }
  return true;
}
Esempio n. 21
0
void main(void)
{
	int front, left, right;
	int totalerror = 0;

	// Set inputs and outputs
	
	TRISA = 0x00;		// left motor
	TRISB = 0b11111111;	// Analog inputs (set all as input)
	TRISC = 0x00;		// right motor
	TRISD = 0x00;

	//TRISE = 0x00;
	PORTD = 0b00000001;
	

	front = FetchSensor(FRONTSENSOR);
	
	FetchAllSensors(&left, &front, &right);

	/* Wait for sign to start */
	while(1)
	{
		if(front > 300)
		{
			Delay10KTCYx(1000);
			break;
		}
		front = FetchSensor(FRONTSENSOR);

	}


	/* Main Program loop */
	while(1) 
	{	

		FetchAllSensors(&left, &front, &right);
	
		/*	if the front is clear and sides are blocked */
		if( (front <= 600)  && (right > 100) && (left > 100) ){
			
			/* Just keep tracking forward */
			totalerror+=track(20);
			correct(&totalerror);
		}
			
		/*	If at a dead end */
		if( (front > 400) && (right > 100) && (left > 100) )
		{
			/* Turn Around */
			track(40);
			turnright(150);
		}
		


		/*	If there is an opening on the left */
		if( (front < 700) && (right > 90) && (left < 90) )
		{	
			int i;
			Delay10KTCYx(100);
			right = FetchSensor(RIGHTSENSOR);
			/* Track off right wall for a bit */
			for(i = 0; i < 100 ; i ++){
				totalerror=trackleft(1, right-25);
				correct(&totalerror);
			}

			/* NO Tracking */
			while(front < 700)
			{
				front = FetchSensor(FRONTSENSOR);
				forward(1,1,1);
			}

			turnleft(80);
		}
		
		/* 	If there is an opening on the right */	
		if( (front < 700) && (right < 150) && (left > 150) )
		{	
			int i;
			left = FetchSensor(LEFTSENSOR);

			for(i = 0; i< 50 ; i++){
				totalerror=trackleft(1, left);
				correct(&totalerror);
			}

			while(front < 700)
			{
				front = FetchSensor(FRONTSENSOR);
				forward(1,1,1);
			}

			turnright(80);
		}

		FetchAllSensors(&left, &front, &right);


	}


}