int main() // main function
{
// Proportional control
servo_angle(16, 650); // 7 ticks/sec / 20 ms
while(1) // main loop
{
//pause(200);
distance = ping_cm(17);
//print("distance= %d\n", distance);
pause(200);
if (distance >25)
{
drive_speed(75,75);
}
else
{
drive_speed(0,0);
servo_angle(16, 10);
pause(1000);
int distanceleft = ping_cm(17);
pause(1000);
//print("distance left= %d\n", distanceleft);
servo_angle(16, 1600);
pause(1000);
//print("distance= %d\n", distanceleft);
int distanceright = ping_cm(17);
pause(1000);
//print("distance right= %d\n", distanceright);
servo_angle(16, 650);
pause(1000);
//print("distance= %d\n", distanceleft);
//servo_angle(16, 750);
int a = distanceleft - distanceright;
//pause(200);
//print("a= %d\n", a);
pause(200);
if (a == 0)
{
drive_speed(64,-64);
pause(300);
print("1\n");
}
else if (a > 0)
{
drive_speed(-64,64);
pause(150);
print("2\n");
}
else if(a < 0)
{
drive_speed(64,-64);
pause(150);
print("3\n");
}
}
// Calculate correction speed
}
}
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);
}
}
}
int main() // main function
{
drive_setRampStep(10); // 10 ticks/sec / 20 ms
drive_ramp(128, 128); // Forward 2 RPS
// While disatance greater than or equal
// to 20 cm, wait 5 ms & recheck.
while(ping_cm(8) >= 20) pause(5); // Wait until object in range
drive_ramp(0, 0); // Then stop
// Turn in a random direction
turn = rand() % 2; // Random val, odd = 1, even = 0
if(turn == 1) // If turn is odd
drive_speed(64, -64); // rotate right
else // else (if turn is even)
drive_speed(-64, 64); // rotate left
// Keep turning while object is in view
while(ping_cm(8) < 20); // Turn till object leaves view
drive_ramp(0, 0); // Stop & let program end
}
//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 main() // Main function
{
low(26);
low(27);
int irLeft, irRight;
// Add startup code here.
int leftDist, rightDist;
while(1)
{
drive_setRampStep(10);
drive_ramp(64, 64);
while(1)
{
irLeft = checkLeft();
irRight = checkRight();
irRight = ping_cm(8);
print ("left = %d, right = %d \n", irLeft, irRight);
if(irLeft == 1)
{
high(26);
}
else
{
low(26);
}
if(ping_cm(8)<=10)
{
high(27);
}
else
{
low(27);
}
if(irLeft == 1)
{
drive_ramp(0, 0);
turnLeftH();
print (" Left empty hall detected");
break;
}
if (ping_cm(8)<=9)
{
drive_ramp(0, 0);
turnBack();
print (" End of the line");
break;
}
}
}
}
void correct() {
if(ping_cm(8) < 14) { // if too close, move back
while(ping_cm(8) < 14) {
drive_speed(-10,-10);
pause(10);
}
} else {
while(ping_cm(8) > 14) { // if too far, move forward
drive_speed(10,10);
pause(10);
}
}
drive_speed(0,0);
}
void turnRightH()
{
moveDistance(80);
turnRight();
if(ping_cm(8)>= 32)
moveDistance(80);
}
void navigate()
{
extern enum driveState tess_drive_state;
tess_ping = ping_cm(ULTRASONIC_PIN);
debug("Distance = %i\n", tess_ping);
if (tess_ping == 0 && tess_drive_state != IDLE)
{
tess_drive_state = IDLE;
debug("Ultrasonic Error!\n", 0);
setServo(0,0,"Error: in void navigate()\n");
}
else
{
if (tess_ping < OBJECT_DISTANCE && tess_drive_state != TURNING)
{
//TODO stop and turn
stop();
debug("Turning on LED!\n", 0);
high(LED_1);
debug("About to turn!\n", 0);
turn(LEFT); //enum from drivetrain
}
else if(tess_ping >= OBJECT_DISTANCE && tess_drive_state != MOVING)
{
driveForward(-1);
low(LED_1);
}
}
}
void turnLeftH()
{
moveDistance(80);
turnLeft();
if(ping_cm(8)>= 32)
moveDistance(80);
}
int main() // Main function
{
// Add startup code here.
//dac_ctr(26, 0, 0);
//dac_ctr(27, 1, 0);
low(26);
low(27);
while(1)
{
/*
freqout(10, 1, 38000);
irLeft = input(11);
pause(10);
low(10);
freqout(1, 1, 38000);
irRight = input(2);
pause(10);
low(1);
*/
distance = ping_cm(ULTRASONIC);
if ((distance > 15)) {
print("%c Distancia = %d .. Seguir adelante",
HOME, distance);
drive_speed(60, 60);
} else if (irLeft == 1){
//print("%c irLeft = %d, irRight = %d .. Rotar izquierda",
//HOME, irLeft, irRight);
//drive_speed(0, 26);
//pause(1000);
//drive_speed(0, 0);
} else if (distance < 15) {
print("%c Distancia = %d .. Girar izquierda",
HOME, distance);
drive_speed(0, 51);
pause(1000);
drive_speed(0, 0);
pause(500);
} else {
//print("%c irLeft = %d, irRight = %d .. Ni idea",
//HOME, irLeft, irRight);
// drive_speed(52, 0);
//pause(2000);
//drive_speed(0, 0);
}
/*
print("%c distance = %d%c cm",
HOME, distance, CLREOL);
*/
//print("%c irLeft = %d, irRight = %d", HOME, irLeft, irRight);
pause(50);
}
}
int main() // Main function
{
// Add startup code here.
while(1)
{
freqout(1, 1, 33000); // Repeat for right detector
irRight = input(2);
if(ping_cm(8)>50){ //Salida del laberinto
drive_speed(64,64);
pause (3000);
}
if(irRight == 1){ // Si no hay pared a la derecha
drive_speed(42,42); //Avanza
pause(600);
high(26);
high(27);
//drive_goto(25,-26); //Gira derecha
drive_speed(32,-32);
pause(750);
//drive_speed(0,0);
low(26);
low(27);
drive_speed(42,42); //Avanza
pause(1200);
//drive_speed(0,0);
}
if(irRight == 0 && ping_cm(8)<10){ //Si hay pared a la derecha y enfrente
drive_speed(0,0);
//Gira izquierda
drive_speed(-32,32);
pause(800);
drive_speed(0,0);
}
else{ //Si no hay pared enfrente
drive_speed(42,42);
pause(50);
}
}
}
//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 lecturad (){
int medicion;
int distancia= ping_cm(9);
//pause(200);
if (distancia<25){
return medicion= 1;
}
else {
return medicion=0;
}
}
int frontChecker()
{
int result;
int distance = ping_cm(8);
if (distance < 35)
{
correct();
return 1;
}
else
{
return 0;
}
}
int main() // Main function
{
while(1) // Main loop
{
distance = ping_cm(8); // Get cm distance from Ping)))
if(distance>30){
int qtis = getQTIs(7, 4); // Check stripe position
if(qtis == 0b1000) drive_speed(-64, 64); // Far left, rotate left
if(qtis == 0b1100) drive_speed(0, 64); // Left, pivot left
if(qtis == 0b0100) drive_speed(32, 64); // A little left, curve left
if(qtis == 0b0110) drive_speed(64, 64); // Stripe centered, forward
if(qtis == 0b0010) drive_speed(64, 32); // A little right, curve right
if(qtis == 0b0011) drive_speed(64, 0); // Right, pivot right
if(qtis == 0b0001) drive_speed(64, -64); // Far right, rotate right
if(qtis == 0b1111) drive_speed(0, 0);
lcd_print_string("station reached");
}
}
}
void pollPingSensors(void *par) {
// The last IR sensor will be retagged with this position number,
// in case there are more PINGs than IRs.
const int lastIRposition = 7;
propterm = fdserial_open(QUICKSTART_RX_PIN, QUICKSTART_TX_PIN, 0, 115200);
//term = fdserial_open(31, 30, 0, 115200); // for Debugging
// Initialize variables outside of loop
// This may or may not improve performance.
int ping = 0, ir = 0;
char receivedChar;
while(1) // Repeat indefinitely
{
/* We wait for input from the other side,
Which lets us not activate the sensors
if the other end is not working,
and also lets the other end rate limit the input. */
receivedChar = fdserial_rxChar(propterm);
//char receivedChar = 'i'; // for Debugging - Cause it to always run instead of waiting for a signal
// Only send data when we get the expected "init" character, avoiding running on random garbage from an open connection
if (receivedChar == 'i') {
high(16); // LEDs for debugging
isActive = 1;
for(int i=0; i < NUMBER_OF_PING_SENSORS; i++ ) {
ping = ping_cm(FIRST_PING_SENSOR_PIN + i);
dprint(propterm, "p,%d,%d.", i, ping);
receivedChar = fdserial_rxChar(propterm); // Should get a character after each output for rate limiting
//dprint(term, "p,%d,%d\n", i, ping); // For Debugging
if(i < NUMBER_OF_IR_SENSORS) { // If there is also an IR sensor at this number check it too
ir = mcp3208_IR_cm(i);
dprint(propterm, "i,%d,%d.", i, ir);
receivedChar = fdserial_rxChar(propterm); // Should get a character after each output for rate limiting
}
}
//dprint(term, "\n"); // For Debugging - add a line break here and pull the above two
}
low(16); // LEDs for debugging
}
/* TODO: Be sure to test that the data coming in is REAL TIME! */
}
int checkClose(){
irLeft = 0;
irRight = 0;
distance = ping_cm(8);
for(int dacVal = 0; dacVal < 160; dacVal += 8){
dac_ctr(26, 0, dacVal);
freqout(11, 1, 38000);
irLeft += input(10);
dac_ctr(27, 1, dacVal);
freqout(1, 1, 38000);
irRight += input(2);
}
if (irLeft<7) {
drive_speed(25,0);
} else if (irLeft > 9 || irLeft == 0){
drive_speed(0,25);
} else {
drive_speed(40,40);
}
if (distance<=5){
fullTurn(90);
degrees+=90;
end=1;
}
drive_getTicks(&ticksLeft, &ticksRight);
leftDeg = ((ticksLeft * 360) / 332.38128) * 3.25;
rightDeg = ((ticksRight * 360) / 332.38128) * 3.25;
degrees = degrees + (rightDeg - leftDeg);
moved = ticksLeft * 3.25;
}
int distanciaAdelante() {
return ping_cm(8);
}
int main() // Main function
{
stack_1.top=-1;
stack_2.top=-1;
i = 0;
while (i < 100){
stack_1.s[i]=0;
stack_2.s[i]=0;
i = i+1;
}
enfrente = 0;
derecha = 0;
izquierda = 0;
paso = 10;
num = 0;
pasosalir = 12;
while(1)
{
sensor=ping_cm(8);
if(sensor>15){
enfrente = 1;
}
else enfrente = 0;
drive_goto(-25,26);
sensor=ping_cm(8);
if(sensor>15){
izquierda = 1;
}
else izquierda = 0;
drive_goto(51,-50);
sensor=ping_cm(8);
if(sensor>15){
derecha = 1;
}
else derecha = 0;
drive_goto(-25,26);
//Parte de procesamiento de la informacion
if((enfrente == 1)&&(derecha==0)&&(izquierda==0)){
push(&stack_1,1);
drive_goto(paso,paso);
}
if((enfrente == 0)&&(derecha == 1)&&(izquierda==0)){
push(&stack_1,3);
push(&stack_1,1);
drive_goto(pasosalir,pasosalir);
drive_goto(26,-25);
drive_goto(41,41);
}
if((enfrente == 0)&&(derecha == 0)&&(izquierda == 1)){
push(&stack_1,2);
push(&stack_1,1);
drive_goto(pasosalir,pasosalir);
drive_goto(-25,26);
drive_goto(41,41);
}
if((enfrente == 1)&&(derecha==1)&&(izquierda==0)){
push(&stack_2,3);
push(&stack_1,5);
push(&stack_1,1);
drive_goto(paso,paso);
}
if((enfrente == 1)&&(derecha==0)&&(izquierda==1)){
push(&stack_2,1);
push(&stack_1,5);
push(&stack_1,1);
drive_goto(paso,paso);
}
if((enfrente == 0)&&(derecha==1)&&(izquierda==1)){
push(&stack_2,1);
push(&stack_1,5);
push(&stack_1,3);
push(&stack_1,1);
drive_goto(pasosalir,pasosalir);
drive_goto(26,-25);
drive_goto(38,38);
}
if((enfrente == 0)&&(derecha == 0)&&(izquierda == 0)){
num = pop(&stack_1);
while (num != 5){
if(num == 1){
drive_goto(-paso,-paso);
}
if(num == 2){
drive_goto(-38,-38);
drive_goto(26,-25);
drive_goto(-pasosalir,-pasosalir);
}
if(num == 3){
drive_goto(-38,-38);
drive_goto(-25,26);
drive_goto(-pasosalir,-pasosalir);
}
num = pop(&stack_1);
}
num = pop(&stack_2);
if(num == 1){
drive_goto(pasosalir,pasosalir);
drive_goto(-25,26);
drive_goto(38,38);
}
if(num == 2){
drive_goto(38,38);
}
if(num == 3){
drive_goto(pasosalir,pasosalir);
drive_goto(26,-25);
drive_goto(38,38);
}
}
}
}
int main() // main function
{
pause(1000);
//low(27);
while(1)
{ // Realiza el ping frontal y del lado para chequear que haya espacio disponible
sFront = ping_cm(8);
sRight = ping_cm(10);
while(sFront >= 10 && sRight < 19)
{
sFront = ping_cm(8);
sRight = ping_cm(10);
drive_ramp(50,50);
pause(5); // Wait until object in range
}
/*drive_ramp(20,20);
cont = 0;
while(cont < 20)
{
cont++;
pause(5);
}*/
// Si el sensor lateral tiene suficiente espacio gira a la derecha
if(sRight < 19)
{
drive_speed(-30,30);
cont = 0;
while(cont < 97)
{
cont++;
pause(5);
}
}else{
cont = 0;
while(cont < 160)
{
drive_ramp(30,30);
cont++;
pause(5);
}
// Si el frente y la derecha estan cubiertos gira a la izquierda
drive_speed(30,-30);
cont = 0;
while(cont < 148)
{
cont++;
pause(5);
}
drive_speed(50,50);
cont = 0;
while(cont < 180)
{
cont++;
pause(5);
}
}
}
}
/*
* Measure distance from a wall in current direction
*/
int pingDistance() { return ping_cm(8); }
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);
}
}
}
int main(void)
{
//drive_pins(12, 13);
sirc_setTimeout(60);
simpleterm_close();
pause(100);
term = fdserial_open(31, 30, 0, 115200);
pause(100);
drive_open();
while(1)
{
dprint(term, "%c", CLS);
button = sirc_button(10);
if(button == PWR)
{
set_mode();
}
switch(mode)
{
case 0:
switch(button)
{
case CH_UP:
//drive_speed(100, 100);
drive_speed(smul, smul);
break;
case CH_DN:
//drive_speed(-100, -100);
drive_speed(-smul, -smul);
break;
case VOL_DN:
//drive_speed(-100, 100);
drive_speed(-smul, smul);
break;
case VOL_UP:
//drive_speed(100, -100);
drive_speed(smul, -smul);
break;
default:
//drive_speed(0, 0);
drive_speed(0, 0);
break;
}
break;
case 1:
for(int i = 0; i < 4; i++)
{
dist[i] = ping_cm(14+i);
//dprint(term, "%d=%03d\n", i, dist[i]);
pause(10);
}
speedL = 0;
speedR = 0;
if(dist[BACK] < 30)
{
speedL += 100;
speedR += 100;
}
if(dist[FRONT] < 30)
{
speedL -= 100;
speedR -= 100;
}
if(dist[LEFT] < 30)
{
if(speedL == 0)
speedL -= 100;
else if(speedL < 0)
//speedR += 200;
speedR += 50;
else if(speedL > 0)
//speedR -= 200;
speedR -= 50;
}
if(dist[RIGHT] < 30)
{
if(speedR == 0)
speedR -= 100;
else if(speedR < 0)
//speedL += 200;
speedL += 50;
else if(speedR > 0)
//speedL -= 200;
speedL -= 50;
}
drive_speed(speedL, speedR);
pause(100);
break;
case 2:
smul = 50;
mode = 0;
break;
case 3:
smul = 100;
mode = 0;
break;
/*
case 4:
val = get_val();
dprint(term, "val = %d\n", val);
pause(3000);
break;
*/
}
}
}
int distanciaDerecha() {
return ping_cm(9);
}