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);
}
}
}
//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;
}
}
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;
}
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;
}
/*
* 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;
}
}
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());
}
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);
}
int main (void){
//number of disks
int n = 4;
towers(n,first,last,mid);
drive_goto(52,0);
return 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);
}
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);
}
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));
}
int turn_180(){
drive_speed(0,0);
pause(100);
drive_goto(-50,50);
pause(200);
return 1;
}
void moveDistance(double mm)
{
double ticks;
ticks = mm/3.25;
ticks = mm;
drive_goto(ticks, ticks);
}
/*
* 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;
}
//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);
}
}
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
}
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);
}
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);
}
}
}
void girarD(){
drive_goto(26,-25);
pause(500);
}
void moveBot(float distance){
int ticks = distance / tick_distance;
drive_goto(ticks,ticks);
}
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);
}
}
}
void derecha(){
drive_goto(26,-25);
}
//Funciones con las que gira el robot
void izquierda(){
drive_goto(-25,26);
}
void girarI(){
drive_goto(-26,25);
pause(500);
}
int main()
{
drive_goto(256, 256);
pause(200);
drive_goto(26, -25);
}
void izquierda(){
drive_speed(0,0);
pause(500);
drive_goto(-25,25); //giro a la derecha
pause(500);
}
