Hello:
This is a 3pi with 2 sensors Sharp 2Y0A21 and your code.
/* Programa: paseo.c Jaume Brosa 11/2009
* Descripción:
* Programa para el 3pi con dos sensores Sharp 2YOA21 conectados
* en ADC7 izq y ADC5 der y que le permite esquivar obstáculos
* Quitar jumpers de PC5 y ADC7.
*/
// Includes
#include <pololu/3pi.h> // especifico 3pi
#include <avr/pgmspace.h> // variables en memoria de programa
// Mensajes de introducción guardados en memoria de programa
const char linea1[] PROGMEM = " Pololu";
const char linea2[] PROGMEM = "3\xf7 Robot";
// Tonos musicales guardados en memoria de programa
const char hola[] PROGMEM = ">g32>>c32";
const char ir[] PROGMEM = "L16 cdegreg4";
// Variables generales
unsigned int sens_der; // sensor right
unsigned int sens_izq; // sensor left
int motor_der; // motor right
int motor_izq; // motor left
void initialize(){
// Set PC5 as an input with internal pull-up disabled
DDRC &= ~(1<< PORTC5);
PORTC &= ~(1<< PORTC5);
emitters_off();
// Toca music y muestra mensaje de hola
print_from_program_space(linea1);
lcd_goto_xy(0,1);
print_from_program_space(linea2);
play_from_program_space(hola);
delay_ms(1000);
//Muestra el voltaje de la batería y espera botón
while(!button_is_pressed(BUTTON_B))
{
clear();
print_long(read_battery_millivolts()); // usa ADC6 para batería
print("mV");
lcd_goto_xy(0,1);
if (read_battery_millivolts()<4800){
print (" !Ahhh¡");
red_led(1);
}
else
print("Pulsa B");
delay_ms(100);
}
// Espera botón B para empezar a moverse
wait_for_button_release(BUTTON_B);
while(!button_is_pressed(BUTTON_B))
{
clear();
lcd_goto_xy(0,0);
print ("I ");
print_long(analog_read(7)); // valor ADC7 sensor <
lcd_goto_xy(0,1);
print ("D ");
print_long(analog_read(5)); // valor ADC5 sensor >
delay_ms(200);
}
wait_for_button_release(BUTTON_B);
clear();
// Toca música y espera a que termine para empezar.
play_from_program_space(ir);
while(is_playing());
}
void lee_sensores(){
// Mira derecha si hay obstáculos
if (!analog_is_converting()) sens_der = analog_read(5);
// mira izquierda si hay obstáculos
if (!analog_is_converting()) sens_izq = analog_read(7);
}
void busca_exit(){
clear();
lcd_goto_xy(0,0);
print("Buscando"); // Busca salida
red_led(1); // Encienda leds
green_led(1);
while (sens_der>400 || sens_izq>400){
play ("c32");
set_motors(40,-40); // gira or -40,40
delay_ms(200);
lee_sensores();
play ("g32");
}
red_led(0); // OK salida encontrada
green_led(0); // apaga leds
}
int main(){
// inicializa 3pi
initialize();
// Bucle principal.
while(1){
lee_sensores();
if (sens_der>400 || sens_izq>400){
set_motors(0,0); // stop motors
delay_ms(200);
busca_exit();
}
if (sens_izq > 200 && sens_der < 200){
// obstaculo izq gira a derecha ------>
motor_izq=100-sens_der/10; // acelera
motor_der=100-sens_izq/10; // reduce
}
if (sens_izq < 200 && sens_der > 200){
// obstaculo a der gira izquierda <-----
motor_izq=100-sens_der/10; // reduce -
motor_der=100-sens_izq/10; // acelera +
}
if (sens_izq < 200 && sens_der < 200){
motor_izq=127;
motor_der=127;
}
if (sens_izq < 100 && sens_der < 100){
motor_izq=150;
motor_der=150;
}
set_motors(motor_izq, motor_der); //izq^.^.^.^.^der
delay_ms(100);
clear(); // Mostrar valores
lcd_goto_xy(0,0);
print_long(sens_izq);
lcd_goto_xy(5,0); // valor sensor
print_long(sens_der);
lcd_goto_xy(0,1);
print_long(motor_izq);
lcd_goto_xy(5,1); // valor motores
print_long(motor_der);
}
}
// end
