void cross_turn_left()
{
	unsigned int left,right;
	while(1)
	{
		left = analog(0);
		right = analog(2);
		if(left>REF && right>REF)
		{
			forward(10);
		}
		else if(left<REF && right>REF)
		{
			turn_left(10);
		}
		else if(left>REF && right<REF)
		{
			turn_right(10);
		}
		else if(left<REF && right<REF)
		{
			forward(300);
			turn_left(800);
			break;
		}
	}
}
예제 #2
0
void main()
{
	char left,right;
	lcd("Press SW1");
	sw1_press();
	while(1)
	{
		left = in_d(0);
		right = in_d(1);
		if(left==1 && right==1)
		{
			forward(10);
		}
		else if(left==0 && right==1)
		{
			backward(1000);
			turn_right(800);
		}
		else if(left==1 && right==0)
		{
			backward(1000);
			turn_left(800);
		}
		else if(left==0 && right==0)
		{
			backward(1000);
			turn_left(1500);
		}
	}

}
void MyRobot::run()
{

    while (step(_time_step) != -1)
    {

        //Get the encoder values
        _left_encoder = getLeftEncoder();
        _right_encoder = getRightEncoder();

        //Converts rad to meters
        if(flag == 2 || flag == 8){
            _distance = _right_encoder/ENCODER_RESOLUTION*WHEEL_RADIO;
        }else{
            _distance = _left_encoder/ENCODER_RESOLUTION*WHEEL_RADIO;
        }

        print_odometry_data();      

        //switch case to make the necesary steps to avoid the obstacle
        switch (flag){
        case 1:
            cout << "linea recta"<< endl;
            goStraight(5);
            break;
        case 2:
            turn_left();
            break;
        case 3:
            goStraight(3.5);
            break;
        case 4:
            turn_right();
            break;
        case 5:
            goStraight( 7);
            break;
        case 6:
            turn_right();
            break;
        case 7:
            goStraight(3.5);
            break;
        case 8:
            turn_left();
            break;
        case 9:
            goStraight(5);
            break;
        default:
            _left_speed = 0;
            _right_speed = 0;
            break;
        }

        setSpeed(_left_speed, _right_speed);
    }
}
예제 #4
0
파일: rbtree.c 프로젝트: yumm007/C
static void insert_balance(struct node_st **tree, struct node_st *node)
{
      while (node != *tree && node->p->color == RED) {
	    if (node->p == node->p->p->l) {
		  /* case 1 */
		  if (node->p->p->r->color == RED) {
			node->p->color = node->p->p->r->color = BLACK;
			node->p->p->color = RED;
			node = node->p->p;
			continue;
		  }

		  /* case 2 */
		  if (node == node->p->r) {
			node = node->p;
			turn_left(node, tree);
		  }

		  /* case 3 */
		  node->p->color = BLACK;
		  node->p->p->color = RED;
		  turn_right(node->p->p, tree);
	    } else {
		  /* case 1 */
		  if (node->p->p->l->color == RED) {
			node->p->color = node->p->p->l->color = BLACK;
			node->p->p->color = RED;
			node = node->p->p;
			continue;
		  }

		  /* case 2 */
		  if (node == node->p->l) {
			node = node->p;
			turn_right(node, tree);
		  }

		  /* case 3 */
		  node->p->color = BLACK;
		  node->p->p->color = RED;
		  turn_left(node->p->p, tree);
	    }

	    node = *tree;
      }

      (*tree)->color = BLACK;
}
예제 #5
0
void red2()
{

      while(true)
      {
        cleargyro();
        if(SensorValue(sonar) <50)		// Loop while robot's Ultrasonic sensor is further than 20 inches away from an object
        {

          while(SensorValue[gyro] < 500){
              turn_left();
            }

          go_straight();

        }
        else
        {
          motor[right] = 127;
          motor[left] = 127;
        }


      }
}
예제 #6
0
파일: Legs.cpp 프로젝트: duducosmos/legs
void Legs::move_according_state(){
    /*
    Given the current state, update the moviment.
    */
    switch (get_current_state()) {
        case FORWARD:
        move_forward();
        break;
        case BACKWARD:
        move_backward();
        break;
        case LEFT:
        turn_left();
        break;
        case RIGHT:
        turn_right();
        break;
        case BYELEFT:
        bye_bye_left();
        break;
        case BYERIGHT:
        bye_bye_right();
        break;
        case STOP:
        zero_pos();
        break;
        default:
        zero_pos();
        break;
    }

}
예제 #7
0
void main()
{
	unsigned int left,mid,right;
	lcd("Press SW1");
	sw1_press();
	while(1)
	{
		left = analog(0);
		mid = analog(1);
		right = analog(2);
		if(left<REF && mid>REF && right<REF)
		{
			forward(10);
		}
		else if(left>REF && mid<REF && right<REF)
		{
			turn_left(10);
		}
		else if(left<REF && mid<REF && right>REF)
		{
			turn_right(10);
		}
		else if(left>REF && mid>REF && right>REF)
		{
			pause();
			sleep(3000);
			forward(500);
		}
	}
}
예제 #8
0
void templateAppToucheBegan( float x, float y, unsigned int tap_count, unsigned int id )
{
	    openBrowser=false;
		if( game_state == 1 && tap_count >= 2 ) game_state = 2;


		if( x < ( screen_width * 0.5f ) )
		{
			if(y>screen_height*0.2)
			{
			if(!turn_left()&&!game_state)
					{
						touch_type[id]=1;
						console_print("move_change");
					}
			}


		}
		else
			{
			if(y>screen_height*0.2)
			{
			if(!turn_right()&&!game_state)
					{
						touch_type[id]=2;
						console_print("view_change");
					}
			}


			}


}
예제 #9
0
int			launcher(t_box *box)
{
	char buf[1];
	int	dir;

	jump;
	ft_putstr("\033[35mWHICH DIRECTION FOR THE NEXT MOVE ? :\033[0m");
	jump;
	while (read(0, buf, sizeof(buf)))
	{
		if (!(dir = check(buf)))
			continue ;
		if (dir == 2)
			turn_right(box);
		if (dir == 1)
			turn_left(box);
		if (dir == 3)
			turn_up(box);
		if (dir == 4)
			turn_down(box);

		ft_putstr("YOUR SCORE IS : ");
		ft_putnbr(box->score);
		jump;
		jump;
		ft_putstr("\033[35mAND NOW ? : \033[0m");
		jump;
		init(box, (ft_random((box->size * box->size), 0)));

		print_tab(box);
	}
	return (0);
}
예제 #10
0
int main(void) {
    const unsigned short MainCycle = 60;
    Init(MainCycle);

    for (;;) {
        step();
        if (is_black() == 0){
        	LED(3);
        }else if((is_black_left() == 1) && (is_black_right() == 0)){
            turn_left();
            LED(2);
        }else if((is_black_left() == 0) && (is_black_right() == 1)){
        	turn_right();
        	LED(1);
        }else if((is_black_left() == 1) && (is_black_right() == 1)){
        	stop();
        	LED(0);
        	break;
        }

    }

    stop();
    return 0;
}
예제 #11
0
void slalom(void)
{
	int x,y;

	while (1) {

		x = ADRead(0);
		y = ADRead(1);

		if (x>THRES&&y>THRES) {        ///黒黒
			stop();Wait(250); break;
		}
		if (x>THRES&&y<THRES) {         //黒白
			turn_left();Wait(50);

		}
		if (x<THRES&&y>THRES) {         //白黒
			turn_right();Wait(50);

		}
		if (x<THRES&&y<THRES) {         //白白
			forward();

		}


	}

}
예제 #12
0
int main( void )
{
    WDTCTL = WDTPW + WDTHOLD; // Disable watchdog timer
      
    P1DIR = 0b01000111; // IN:UltraEcho(P1.6) Colour1(P1.1),Colour2(P1.2),Colour3(P1.3) OUT:UltraTrig(P1.7)
    P2DIR = 0b00110110; // OUT:MotorOppCap(P2.1 and P2.2), MotorCap(P2.3 and P2.4)

    // Configure the Basic Clock Module
    DCOCTL  = CALDCO_1MHZ;
    BCSCTL1 = CALBC1_1MHZ;


    // Main loop repeats forever
    while(1)
    {
		if (detected() == 1)  // If ultrasonic detects within 77cm 
        {
            led(1); // Turn on the LED
            forward();  // Drive both motors forward
        }

        else    // If ultrasonic doesn't detect within 74cm  
        {
            led(0); // Turn off the LED
            turn_left(); // Begin Spinning to the left. 
        }

        if (colourFor() == 1)
        {
            backward(); // Drive both motors forward.
        }

        if (colourBkL() == 1)
        {
            turn_right(); // Spin the robot to the right.
        }

        if (colourBkR() == 1)
        {
            turn_left(); // Spin the robot to the left.
        }

        __delay_cycles(50000); // Delay for 50ms to allow echo pulse to die down
    }
} 
예제 #13
0
int main()
{
  go_straight();
  turn_right();
  go_upstair();
  turn_left();
  enter_restroom();
  return 0;
}
예제 #14
0
void loop() {
    unsigned long distance = sense_distance();
    if (distance >= DISTANCE_THRESHOLD) {
        go_forward();
    } else {
        go_backward();
        turn_left();
    }
}
예제 #15
0
void autonomouswalk_walk(uint8_t sensors[5])
{
	uint8_t leftSideAlgorithm = navigation_left_algorithm();
	if(leftSideAlgorithm)
	{
		if(navigation_check_left_turn(sensors[0], sensors[2]) == 2)
		{
			turn_left();
		}
		else if(sensors[4] > CORRIDOR_WIDTH)
		{
			walk_forward(sensors);
		}
		else if(navigation_check_right_turn(sensors[1], sensors[3]) == 2)
		{
			turn_right();
		}
		else
		{
			turn_around(sensors[4]);
		}
	}
	else
	{
		if(navigation_check_right_turn(sensors[1], sensors[3]) == 2)
		{
			turn_left();
		}
		else if(sensors[4] > CORRIDOR_WIDTH)
		{
			walk_forward(sensors);
		}
		else if(navigation_check_left_turn(sensors[0], sensors[2]) == 2)
		{
			turn_right();
		}
		else
		{
			turn_around(sensors[4]);
		}
	}
}
예제 #16
0
int main()
{
	int i;
	
	setup_movement();
	
	ahead();
	
	turn_right();

	turn_left();
}
예제 #17
0
파일: 118.c 프로젝트: ksyu/uvajudge
void run_instruction(int x, int y, char ort, int row, int col)
{
    int i, tx, ty;
    enum { NIL, T } lost = NIL;

    for(i = 0; instruction[i]; i++)
    {
        if(instruction[i] == 'L')
            ort = turn_left(ort);

        if(instruction[i] == 'R')
            ort = turn_right(ort);

        if(instruction[i] == 'F')
        {
            tx = x, ty = y;

            /* move forward */
            if(ort == 'N') y -= 1;
            else if(ort == 'E') x += 1;
            else if(ort == 'S') y += 1;
            else if(ort == 'W') x -= 1;

            /* out of grid, lost */
            if(x < 0 || x > col)
            {
                x = tx;
                if(grid[x][y] == 0)
                {
                    grid[x][y] = 1;
                    lost = T;
                    break;
                }
            }

            if(y < 0 || y > row)
            {
                y = ty;
                if(grid[x][y] == 0)
                {
                    grid[x][y] = 1;
                    lost = T;
                    break;
                }
            }
        }
    }

    if(lost)
        printf("%d %d %c LOST\n", x, row - y, ort);
    else
        printf("%d %d %c\n", x, row - y, ort);
}
예제 #18
0
/*Line follow: arguments are whatever threshold we are using, 
plus however many iterations we want the function to perform*/
void line_follow(int threshold, int distance) {
  while(get_create_distance() < distance) {
    printf("Value: %d \n" , get_create_distance());
    msleep(100);
    if(analog(0)>=threshold) {
      turn_right(3);
    }
    if(analog(0)<=threshold) {
      turn_left(3);
    }
  }
}
예제 #19
0
void go_straight(void)
{
     // very simple motor control
     
     if ( SeeLine.b.Center ) straight_fwd();
     else if (SeeLine.b.Left) spin_left();
     else if (SeeLine.b.CntLeft) turn_left();
     else if (SeeLine.b.CntRight) turn_right();
     else if (SeeLine.b.Right) spin_right();

     if ( (SeeLine.B ) == 0b00000u) motors_brake_all();
}
예제 #20
0
/*0 - False
1 - True
*/
int decision_ida(int derecha, int izquierda, int recto){  
      //regreso
    if (recto==1 && derecha == 1 && izquierda == 1){
      return  2;        
    }
            
    if(derecha==0  && izquierda == 1 && recto == 1 ){
      return  turn_rigt();

    }    
    
    if(derecha == 1 && izquierda==0  && recto==1 ){
      return  turn_left();
           
    }

    if(derecha == 1 && izquierda==1  && recto==0 ){
      return forward();
           
    }
            
    if (derecha == 0  && izquierda == 0 && recto==0 ){
      return  forward();
              
    }
    
    if (derecha == 0 && izquierda == 0 && recto==1 ){
      return  turn_left();
              
    }
    
    if (derecha == 1 && izquierda == 0  &&   recto==0 ){
      return forward();
              
    }
    
    if (derecha == 0 && izquierda == 1   &&recto==0 ){
      return forward();              
    }
}  
//Main function
int main(void)
{
	//Initialize value array with 0
	int i;
	for(i=0;i<1000;i++){
		val_array[i] = 0;
	}
	
	data_pos = 0;
	bot_pos = 0;
	
	init_devices();

	//LCD_Reset_4bit();
	lcd_cursor(2,1);
	lcd_string("DRAWOID");
	
	while(1){
		if (bot_pos < data_pos){
			lcd_print(1,9,val_array[bot_pos],3);
			lcd_cursor(1,1);
			if (mov_array[bot_pos] == 'F'){
				lcd_string("F");
				move_straight(val_array[bot_pos],1);
			}
			else if (mov_array[bot_pos] == 'B'){
				lcd_string("B");
				move_straight(val_array[bot_pos],0);
			}
			else if (mov_array[bot_pos] == 'R'){
				lcd_string("R");
				turn_right(val_array[bot_pos]);
			}
			else if (mov_array[bot_pos] == 'L'){
				lcd_string("L");
				turn_left(val_array[bot_pos]);
			}
			else if (mov_array[bot_pos] == 'U'){
				lcd_string("U");
				pen_up();
			}
			else if (mov_array[bot_pos] == 'D'){
				lcd_string("D");
				pen_down();
			}
			bot_pos++;
			_delay_ms(1000);
		}
	}

	return 0;
}
예제 #22
0
파일: events.c 프로젝트: pdaumont/my_lab
int			loop_event(t_app *app)
{
	if (app->player.move_forward == 1)
		move_forward(app);
	if (app->player.move_backward == 1)
		move_backward(app);
	if (app->player.turn_left == 1)
		turn_left(app);
	if (app->player.turn_right == 1)
		turn_right(app);
	refresh(app);
	return (0);
}
예제 #23
0
void			move_wall(t_game *p)
{
  if (p->algo == 1)
    front(p);
  else if (p->algo == 2)
    cone(p);
  else if (p->algo == 3)
    turn_left(p);
  else if (p->algo == 4)
    turn_right(p);
  else if (p->algo == 5)
    reverse_cone(p);
}
예제 #24
0
파일: main.cpp 프로젝트: treeffle/sdbot
void perform_action(gamestate_t *gs, int action)
{
	switch(action)
	{
		case 0: noop(gs); qbLog("Noop"); break;
		case 1: turn_left(gs); qbLog("turn left"); break;
		case 2: turn_right(gs); qbLog("turn right"); break;
		case 3: move_forward(gs); qbLog("move forward"); break;
		case 4: move_backward(gs); qbLog("move backward"); break;
		case 5: jump(gs); qbLog("jump"); break;
		case 6: shoot(gs); qbLog("shoot"); break;
	}
}
예제 #25
0
void main()
{
   setup_timer_2(T2_DIV_BY_16,255,1);      //4.0 ms overflow, 4.0 ms interrupt

   setup_ccp1(CCP_PWM);
   setup_ccp2(CCP_PWM);
   set_pwm1_duty((int16)200);
   set_pwm2_duty((int16)200);

   enable_interrupts(INT_SSP);
   enable_interrupts(GLOBAL);
   
   SET_TRIS_B(0);
   stop();
   //delay_ms(500); 
   
   while(TRUE)
   {
      
      if(hasCommand)
      {
         hasCommand= FALSE;
         if(buffer[0] == 's'){
            show();
         }
         
         else
            switch(buffer[1]) 
            { 
               case 1: 
                 go_forward();
                 break;
               case 2:
                 go_backward();
                 break;
               case 3:
                 turn_right();
                 break;
               case 4:
                 turn_left();
                 break;
               case 5:
                 stop();                            
                 break;
               default : 
                  break;
            }
      }
   }

}
예제 #26
0
void loop()
{
    if (Serial.available() > 0)
    {
        int command = Serial.read();
        Serial.print(command, DEC);
        Serial.print(" ");
        switch (command)
        {
            case 1:
            move_stop();
            delay(500);
            move_forward();
            break;
            case 2:
            move_stop();
            delay(500);
            turn_left();
            break;
            case 3:
            move_stop();
            delay(500);
            turn_right();
            break;
            case 4:
            move_stop();
            delay(500);
            move_backward();
            break;
            case 5:
            move_stop();
            break;
            case 6:
            front_led_control(true);
            break;
            case 7:
            front_led_control(false);
            break;
            case 8:
            rear_led_control(true);
            break;
            case 9:
            rear_led_control(false);
            break;
            default:
            move_stop();
            front_led_control(false);
            rear_led_control(false);
        }
    }
}
예제 #27
0
void main()
{
	unsigned int dist,dist_l,dist_r;
	servo(0,GET_MID);
	lcd("Press SW1");
	sw1_press();
	
	while(1)
	{
		servo(0,GET_MID);
		dist = srf05_dist(ECHO_PB4,PULSE_PB1);
		if(dist>=3 && dist<=12)
		{
			pause();
			
			servo(0,GET_LEFT);
			sleep(2000);
			dist_l = srf05_dist(ECHO_PB4,PULSE_PB1);
			
			servo(0,GET_RIGHT);
			sleep(2000);
			dist_r = srf05_dist(ECHO_PB4,PULSE_PB1);
			servo(0,GET_MID);
			
			if(dist_l>dist_r && dist_l>DIST_MIN)
			{
				turn_left(200);	
			}
			else if(dist_r>dist_l && dist_r>DIST_MIN)
			{
				turn_right(200);	
			}
			else if(dist_r==dist_l && dist_r>DIST_MIN)
			{
				turn_right(200);	
			}
			else
			{
				backward(1000);
				turn_right(500);
			}
			
		}
		else
		{
			forward(10);
		}
		
	}

}
예제 #28
0
void put_poms_out(){
	printf("put poms out \n");
	turn_left();
	double sec= seconds();
	while(analog10(LEFT_SENSOR)<left_blk-150&&analog10(RIGHT_SENSOR)<right_blk-150&&(sec+1)>seconds()){ }
	forward();
	while(analog10(LEFT_SENSOR)<left_blk-150&&analog10(RIGHT_SENSOR)<right_blk-150){}
	if(analog10(LEFT_SENSOR)>=left_blk-150&&analog10(RIGHT_SENSOR)>=right_blk-150){
			put_out();
	}
	else if(analog10(LEFT_SENSOR)>=left_blk-150){
			turn_left();
			while(analog10(RIGHT_SENSOR)>=right_blk-150){}
			stop();
			msleep(1000);
			forward();
			while(analog10(LEFT_SENSOR)<left_blk-150){}
			stop();
			msleep(1000);
			turn_left();
			while(analog10(RIGHT_SENSOR)<right_blk-150){}
			put_out();
	}
	else if(analog10(RIGHT_SENSOR)>=right_blk-150){
			turn_right();
			while(analog10(LEFT_SENSOR)>=left_blk-150){}
			stop();
			msleep(1000);
			forward();
			while(analog10(RIGHT_SENSOR)<right_blk-150){}
			stop();
			msleep(1000);
			turn_right();
			while(analog10(LEFT_SENSOR)<left_blk-150){}
			put_out();
	}
}
예제 #29
0
void segue_coordenadas(int8 dados)
{
    //o objetivo é centralizar a bolinha antes de seguir em frente    

    int16 obstaculo = 0;

    switch (dados) {

    case CEL_DIREITA:
        turn_right();
#ifdef DEBUG
        printf("cellphone turning right...\n");
#endif
        break;

    case CEL_ESQUERDA:
        turn_left();
#ifdef DEBUG
        printf("cellphone turning left...\n");
#endif
        break;

    case CEL_FRENTE:
#ifdef sensorir    
        obstaculo = verificaObstaculo();
        if (obstaculo == 0) {
#endif   
            go_forward();
#ifdef DEBUG

            printf("no obstacle, go ahead...\n");
#endif
#ifdef sensorir    

        } else if (obstaculo == 1) {
            go_idle(); 
            obstaculo = 0;
#ifdef DEBUG
            printf("obstacle, going idle to not crash...\n");
#endif
        }
#endif       

    default:
        break;

    }

}
예제 #30
0
int main()
{
    int speed = 10;
    
    serializer_connect();
    turn_left(90.,speed);
    block_digo_done();
    sleep(1.0);
    print_encoders();
    turn_left(90.,speed);
    block_digo_done();
    sleep(1.0);
    print_encoders();
    turn_left(90.,speed);
    block_digo_done();
    sleep(1.0);
    print_encoders();
    turn_left(90.,speed);
    block_digo_done();
    sleep(1.0); 
    print_encoders();
	sleep(1.0);
    serializer_disconnect();
}