Exemplo n.º 1
0
int main()                                  
{
  while(1)                                    // se crea un ciclo infinito
  {
    distance1 = ping_cm(8);                    //el sensor frontal va conectado al P8 de la placa
    distance2 = ping_cm(4);                   //el sensor lateral va conectado al P4 de la placa
    
    if(distance2<15){                       //declara que la distancia, reconocida por el sensor lateral, sera de 15 cm
      if(distance1<10){                     //declara que la distancia, reconocida por el sensor frontal, sera de 10 cm
        drive_speed(0, 0);                  //si se encuentra un obstaculo, el robot para
        drive_goto(-25, 26);                //luego girara hacia la izquierda de acuerdo al algoritmo wall follower
        pause(200);
      }
      else{
        drive_speed(64, 64);                //de no encontrar obstaculo, el robot sigue avanzando
        pause(100);
      }                
    }
    
    else{
      drive_speed(0, 0);        
      drive_goto(18, 18);
      pause(200);
      drive_goto(26, -25);          //en caso que exista un camino sin salida, gira hacia la derecha. 
      pause(200);
      drive_goto(30, 30);
      pause(200);
    }                      
  } 
}  
Exemplo n.º 2
0
//Función que revisa hacia el frente, derecha e izquierda, para saber si hay obstáculos en ese paso
void revisar(){
  drive_goto(-25,26);
  pause(10);
  if(ping_cm(8)>15){
    izq = 1;           
  }
  else
  {
    izq=0;
  } 
  drive_goto(52,-50);
  pause(10);
  if(ping_cm(8)>15){
    der = 1;
  }
  else
  {
    der=0;
  } 
  pause(10);
  drive_goto(-25,26);
  if(ping_cm(8)>10){
    ade = 1;
  }
  else
  {
    ade=0;
  }    
}
Exemplo n.º 3
0
int turn_left(){
    drive_speed(0,0);
    	pause(100);
   drive_goto(25,25);
	drive_goto(-26,26);
	pause(100);
  drive_goto(75,75);
	//drive_goto(50,50);
	return 1;
}
Exemplo n.º 4
0
int turn_rigt(){
  drive_speed(0,0);
  	pause(100);
  drive_goto(25,25);
	drive_goto(26,-26);
	pause(100);
  drive_goto(75,75);
	//drive_goto(50,50);
	return 0;
}
Exemplo n.º 5
0
/*
 * Turn clockwise or anti-clockwise
 */
void turn(char dir)
{

    switch (dir) {

        case 'a' :
            drive_goto(-26, 25);

            switch (direction) {
                case 'n' :
                    direction = 'w';
                    break;

                case 'w' :
                    direction = 's';
                    break;

                case 's' :
                    direction = 'e';
                    break;

                case 'e' :
                    direction = 'n';
                    break;
            }


            break;

        case 'c' :
            drive_goto(25, -26);
            switch (direction) {
                case 'n' :
                    direction = 'e';
                    break;

                case 'w' :
                    direction = 'n';
                    break;

                case 's' :
                    direction = 'w';
                    break;

                case 'e' :
                    direction = 's';
                    break;
            }

            break;
    }
}
Exemplo n.º 6
0
void turn(int direction)
{
  if (direction == -1)
  {
    drive_goto(-25,26);
  }
  else if (direction == 1)
  {
    drive_goto(26,-25);
  }
  else if (direction == 2)
  {
    drive_goto(51,-51);
  }
}
void gobkwd(void *par)
{
  running = 1;
  drive_goto(-256, -256);
  running = 0;
  cogstop(cogid());
}
void gofwd(void *par)
{
  running = 1;
  drive_goto(256, 256);
  running = 0;
  cogstop(cogid());
}
Exemplo n.º 9
0
void turnInPlaceNoOvershoot(double angle){
  double dist = wheelDistance/2*angle;
  int ticks = (int)(dist);
  static double overshoot = 0;
  
  overshoot += dist-(double)ticks;

  if(overshoot>1) {overshoot-=1; ticks++;}
  
  /*int left,right; drive_getTicks(&left, &right);
  int leftStart = left, rightStart = right;
  int lowestDist = 0xffff;
  while(1){
    int dist = ping_cm(8);
    drive_getTicks(&left, &right);
    if(dist<lowestDist&&
    ((double)abs(right-rightStart)+abs(left-leftStart))/ticks>1.4) lowestDist=dist;
    if(dist>lowestDist&&
    ((double)abs(right-rightStart)+abs(left-leftStart))/ticks>1.5) break;
    if(abs(left-leftStart)>=ticks && abs(right-rightStart)>=ticks) break;
    drive_rampStep(angle>0?32:-32,angle>0?-32:32);
  }
  drive_speed(0,0);*/
  drive_goto(ticks, -ticks);
}
Exemplo n.º 10
0
int main (void){
  //number of disks
  int n = 4;
  towers(n,first,last,mid);
  drive_goto(52,0);
  return 0;
}
Exemplo n.º 11
0
//Función que revisa el stack de camino para regresar al robot hasta a la última bifurcación que encontró
void backtracking(Stack *stder,Stack *stizq,Stack *stcamino)
{
  int valor=0;
  while(top(stcamino)!=0)
  {
    if(top(stcamino)>0)
    {
      valor=top(stcamino);
      pop(stcamino);
      valor=valor*(-1);
      drive_goto(valor,valor);
    }  
    if(top(stcamino)==-1)
    {
      pop(stcamino);
      derecha();
    }
    if(top(stcamino)==-2)
    {
      pop(stcamino);
      izquierda();
    }    
  }
  revisarStacks(stder,stizq,stcamino);   
}
Exemplo n.º 12
0
int main()                     
{
  //drive_pins(14, 15, 12, 13); 

  drive_goto(100, 100);

  drive_speed(0, 0);
  #ifdef interactive_development_mode
  drive_record(1);
  #endif
  pause(300);
  drive_speed(20, 20);
  pause(1600);
  #ifdef interactive_development_mode
  drive_record(0);
  #endif
  pause(6400);
  #ifdef interactive_development_mode
  drive_record(1);
  #endif
  pause(1600);
  drive_speed(0, 0);
  #ifdef interactive_development_mode
  drive_record(0);
  #endif

  #ifdef interactive_development_mode
  drive_displayControlSystem(0, 2500/11);
  #endif

  while(1);
}
Exemplo n.º 13
0
void turnAround(double angle, double radius){
  if(angle>PI) { turnAround(angle-2*PI, radius); return; }
  if(angle<-PI) { turnAround(angle+2*PI, radius); return; }
  double dist = (wheelDistance+radius)*angle;
  int ticks = (int)(dist);
  drive_goto(ticks, (int)(radius*angle));
}
Exemplo n.º 14
0
int turn_180(){
  drive_speed(0,0);
  pause(100);
  drive_goto(-50,50);
  pause(200);
  return 1;
}  
Exemplo n.º 15
0
void moveDistance(double mm)
{
  double ticks;
  ticks = mm/3.25;
  ticks = mm;
  drive_goto(ticks, ticks);
}
Exemplo n.º 16
0
/*
 * Adjust the distance to default distance using the wall the robot is currently pointing to
 */
void adjust_one_wall(int *temp_weight) {
    ping_wall(find_current_cell());
    int adj_weight = default_adjustment;
    int difference = *temp_weight - adj_weight;
    int ticks = (int) (difference * 10.0 / 3.25);
    drive_goto(ticks, ticks);
    *temp_weight = adj_weight;
}
Exemplo n.º 17
0
//Función con la que el robot da una serie de minipasos, verificando en cada uno que no haya obstáculo. Al completar 8, termina el paso completo
void paso(){
  int paso = 0;
  while(paso<40){
    paso = paso + 5;
    drive_goto(5,5);
    contador=contador+5;
    if(ping_cm(8)<7){
      break;
    }
  } 
}  
int main()                                    // Main function
{
  int a;
  sirc_setTimeout(1000);
  while(1)
  {
    a=sirc_button(3);
    if(a==16)
    drive_speed(128,128);		//Move forward
    else if(a==17)
    drive_speed(-128,-128);		//Move backward
    else if(a==18)
    drive_goto(50,0);		//Move Right
    else if(a==19)
    drive_goto(0,50);		//Move left
    else if(a==20)
    drive_speed(0,0);		//Stop moving
    pause(100);
  }  
}
Exemplo n.º 19
0
int main()                                   // Main function
{
    int DO = 22, CLK = 23;                     // SD I/O pins
    int DI = 24, CS = 25;
    sd_mount(DO, CLK, DI, CS);                 // Mount SD file system
    fp = fopen("navset.txt", "r");             // Open navset.txt

    fread(str, 1, 512, fp);                    // navset.txt -> str
    int strLength = strlen(str);               // Count chars in str
    int i = 0;                                 // Declare index variable

    drive_speed(0, 0);                         // Speed starts at 0

    while(1)                                   // Loop through commands
    {
        // Parse command
        while(!isalpha(str[i])) i++;             // Find 1st command char
        sscan(&str[i], "%s", cmdbuf);            // Command -> buffer
        i += strlen(cmdbuf);                     // Idx up by command char count
        if(!strcmp(cmdbuf, "end")) break;        // If command is end, break

        // Parse distance argument
        while(!isdigit(str[i])) i++;             // Find 1st digit after command
        sscan(&str[i], "%s", valbuf);            // Value -> buffer
        i += strlen(valbuf);                     // Idx up by value char count
        val = atoi(valbuf);                      // Convert string to value

        // Execute command
        if(strcmp(cmdbuf, "forward") == 0)       // If forward
            drive_goto(val, val);                  // ...go forward by val
        else if(strcmp(cmdbuf, "backward") == 0) // If backward
            drive_goto(-val, -val);                // ... go backward by val
        else if(strcmp(cmdbuf, "left") == 0)     // If left
            drive_goto(-val, val);                 // ...go left by val
        else if(strcmp(cmdbuf, "right") == 0)    // If right
            drive_goto(val, -val);                 // ... go right by val
    }

    fclose(fp);                                // Close SD file
}
Exemplo n.º 20
0
void turnInPlace(double angle){
  if(angle>PI) { turnInPlace(angle-2*PI); return; }
  if(angle<-PI) { turnInPlace(angle+2*PI); return; }
  double dist = wheelDistance/2*angle;
  int ticks = (int)(dist);
  static double overshoot = 0;
  
  overshoot += dist-(double)ticks;

  /*if(overshoot>1) {overshoot-=1; ticks++;}
  if(overshoot<-1) {overshoot+=1; ticks--;}*/

  //ticks+=rand()%(ticks>>4)-(ticks>>5);

  drive_goto(ticks, -ticks);
}
Exemplo n.º 21
0
 void derecha(){
   for(int n = 1; n <= 65; n++){                // Count to hundred
    drive_rampStep(35,35);                   // move not too fast
    pause(10);                               // 50 ms between reps
    } 
		drive_speed(0,0);
     pause(500);
     drive_goto(25,-25); //giro a la derecha
     pause(500);
     
     for(int n = 1; n <= 65; n++){                // Count to hundred
    drive_rampStep(35,35);                   // move not too fast
    pause(10);                               // 50 ms between reps
    } 
	
	}   
int main()                                    // main function
{
  int DO = 22,CLK = 23, DI = 24, CS = 25;
  sd_mount(DO,CLK,DI,CS);
  wav_volume(10);
  freqout(4, 2000, 3000);                     // Speaker tone: P4, 2 s, 3 kHz
  while(1)                                    // Endless loop
  {
    int wL = input(7);                        // Left whisker -> wL variable
    int wR = input(8);                        // Right whisker -> wR variable
    //print("%c", HOME);                        // Terminal cursor home (top-left)
    //print("wL = %d  wR = %d", wL, wR);        // Display whisker variables
    drive_speed(128,128);
    if(input(7)==0 || input(8) == 0)
    {
    drive_speed(0,0);
    pause(500);
    wav_play("ouch.wav");
    drive_goto(-64,64);
    }
  }
}
Exemplo n.º 23
0
void girarD(){
    drive_goto(26,-25);
    pause(500);
  }
Exemplo n.º 24
0
void moveBot(float distance){
	int ticks = distance / tick_distance;
	drive_goto(ticks,ticks);
}
Exemplo n.º 25
0
int main()
{
  s.top = 0;
  pause(500);
  
  /*Declaracion de Variables*/

  /*Las siguientes 4 variables sirven para tomar las distancias*/
  int adelante;
  int derecha;
  int izquierda;
  int atras;
  
  /*Movimiento que se va a realizar*/
  int actual;
  
  /*Bandera para saber si se puede mover o no el robot*/
  int bandera;
  int mover;
  
  mover     = 0;
  bandera   = 0;
  actual    = 0;
  adelante  = 0;
  derecha   = 0;
  izquierda = 0;
  atras     = 0;
  
  while (1){
    
    
    /*Se revisan todas las posibilidades*/
    /*En caso de usar solo 1 sensor, se gira el robot para tomar distancias*/
    /*De lo contrario, solo te toman en diferentes puertos*/

      adelante = ping_cm(8);
      
      //girarD();
      
      derecha = ping_cm(3);
      
      // girarD();
      //girarD();
      
      izquierda = ping_cm(10);
      
      //girarD();
      
      /* Jerarquia de movimiento: derecha, adelante, izquierda, atras */
      bandera = 0;
      
        if (izquierda>=15){
          push(2);
          bandera = 1;
        }
        
        if (adelante>=15){
          push(1);
          bandera = 1;
        }
        
        if (derecha>=15){
          push(3);
          bandera = 1;
        }
        
        if (bandera!=1){
            push(4);
          }

        adelante = (adelante*10)/3.25;
        derecha  = (derecha *10)/3.25;
        izquierda= (izquierda*10)/3.25;
        actual = pop();
        
        
        /*Se revisa el movimiento que se va a hacer */
        
        if (actual == 3){
            girarD();
            drive_goto(46,46);
          }
        if (actual == 1){
            drive_goto(46,46);
          }
        if (actual == 2){
            girarI();
            drive_goto(46,46);
          }
          
        if (actual == 4){
            girarI();
            girarI();
            drive_goto(48,49);
          }
    
        
   }        
}
Exemplo n.º 26
0
void derecha(){
  drive_goto(26,-25); 
}
Exemplo n.º 27
0
//Funciones con las que gira el robot
void izquierda(){
  drive_goto(-25,26);     
}
Exemplo n.º 28
0
void girarI(){  
    drive_goto(-26,25);
    pause(500);
  }
int main()                    
{
  drive_goto(256, 256);
  pause(200);
  drive_goto(26, -25);
}
Exemplo n.º 30
0
	void izquierda(){
	  drive_speed(0,0);
     pause(500);
     drive_goto(-25,25); //giro a la derecha
     pause(500);
	}