#define TIMER_VAMOS 0
#define TIMER_NADA 1
#define TIMER_NOMBRE 2
#define TIMER_TONE 4 /* es usado siempre!!! */
#define PIEDROS 20
#define DOS_PIEDROS_Y_UNA 41
#define ARREDADO 0.001
#define STOP 0
#define SLOW 50
#define FAST 100
#define DISTANCIA 0.137
#define MP_REVOLUCION 0.195
#define MAX_VEL 0.47
#define CLIQUOS_REV_DEL_TORO 40
#define MODOS 3
#define NO_VEL 0.20
#define MIN_VEL 0.43
#define MIT_VEL 0.46
#define ACC_VEL 0.48
#define BIEN_VEL 0.53
#define ALTO_PARA_DOSA 1
#define ALTO_PARA_LUZ 2
int corrador;
int uc_izquierdos, uc_derechos;
/*
int lc_izquierda[]={-35,-35,-34,-34,-31, -30,-29,-26,-23,-21, -18,-14,-12,-8,-3, -2,-2,-1,0,0, 0,0,0,1,2, 2,3,4,9,11, 14,16,19,22,23, 26,27,29,30,31, 32};
int lc_derecha[] = {-36,-36,-35,-34,-32, -31,-30,-28,-24,-22, -19,-15,-12,-8,-3, -2,-1,-1,0,0, 0,0,0,1,1, 2,3,6,12,16, 20,22,25,26,27, 30,33,34,35,36, 36};
int lc_izquierda[DOS_PIEDROS_Y_UNA];
int lc_derecha[DOS_PIEDROS_Y_UNA];
*/
int vel_pwm_izquierda[DOS_PIEDROS_Y_UNA]={-88,-84,-80,-70,-69, -65,-62,-58,-55,-52, -49,-47,-45,-40,-38, -35,-35,-33,-30,-15, 0,15,24,35,36, 38,40,45,46,50, 53,56,60,61,65, 71,73,80,82,90, 94};
int vel_pwm_derecha[DOS_PIEDROS_Y_UNA] ={-85,-81,-73,-65,-63, -62,-59,-54,-53,-49, -46,-43,-41,-38,-36, -35,-34,-32,-30,-19, 0,14,23,30,33, 34,37,40,41,45, 47,48,52,53,59, 60,63,71,72,77, 80};
float antes_multiplicador, detras_multiplicador;
long DELTA_t;
int invertador;
int invertador_para_el_invertador;
float max_mps_izq_antes, max_mps_izq_detras, max_mps_der_antes, max_mps_der_detras;
float v;
int w;
int luzi, luzd;
int luz_es_encontrada;
float v_errar, v_avert, v_seguir, v_buscar, v_ole, v_vamosluz, v_dosa;
int w_errar, w_avert, w_seguir, w_buscar, w_ole, w_vamosluz, w_dosa;
int ind_errar, ind_avert, ind_seguir, ind_buscar, ind_ole, ind_vamosluz, ind_dosa;
int bloccada;
/* int lc_psd[101];
int lc_psd[] = {0, 121, 92, 89, 99, 77, 59, 55, 56, 64, 73, 84, 92, 101, 108, 115, 119, 125, 129, 134, 138, 141, 144, 148, 150, 153, 155, 157, 160, 163, 165, 166, 168, 170, 170, 172, 174, 175, 176, 177, 178, 179, 179, 180, 180, 181, 181, 182, 182, 182, 183, 183, 183, 183, 184, 184, 184, 184, 184, 185, 185, 185, 185, 185, 185, 185, 185, 185, 185, 185, 185, 185, 185, 185, 186, 186, 186, 186, 186, 186, 187, 187, 187, 187, 187, 187, 187, 187, 187, 187, 187, 187, 187, 188, 188, 188, 188, 188, 188, 188, 188};
int cm_psd[128]; */
int cm_psd[] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 1, 2, 3, 4, 5, 6, 7, 8, 8, 8, 8, 9, 9, 9, 9, 9, 10, 10, 10, 10, 10, 11, 11, 11, 11, 12, 12, 12, 12, 12, 13, 13, 13, 14, 14, 14, 14, 15, 15, 16, 16, 16, 17, 17, 18, 18, 19, 19, 20, 20, 21, 22, 22, 23, 24, 24, 25, 26, 27, 28, 28, 29, 31, 32, 34, 35, 36, 38, 40, 44, 49, 58, 79, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100};
int cm_secscan[CLIQUOS_REV_DEL_TORO];
int prob_secscan[CLIQUOS_REV_DEL_TORO];
void main( void )
{
DELTA_t = 25L;
mode = 0;
v = 0.30;
w = 0;
luz_es_encontrada = 0;
ind_errar = ind_avert = ind_seguir = ind_buscar = ind_ole = ind_vamosluz = 0;
max_mps_izq_antes = max_mps_izq_detras = max_mps_der_antes = max_mps_der_detras = MIN_VEL;
start_process(son());
init_motors();
init_velocity();
while(1)
{
if (mode == 0)
{
printf("\nAppuyas el bumper!");
while (bumper() == 0);
printf("\nModo Zero: Medido los extremos.");
motor_y_son(LEFT, SLOW);
motor_y_son(RIGHT, SLOW);
timer_start(TIMER_VAMOS, 500L);
while(timer_time(TIMER_VAMOS) > 0L)
defer();
motor_y_son(LEFT, FAST);
motor_y_son(RIGHT, FAST);
get_left_clicks();
get_right_clicks();
timer_start(TIMER_VAMOS, 2000L);
while(timer_time(TIMER_VAMOS) > 0L)
defer();
max_mps_izq_antes = ((float)get_left_clicks()/2.0)/16.0 * MP_REVOLUCION;
max_mps_der_antes = ((float)get_right_clicks()/2.0)/16.0 * MP_REVOLUCION;
printf ("\niz_an:%f/de_an:%f",max_mps_izq_antes, max_mps_der_antes);
motor_y_son(LEFT, STOP);
motor_y_son(RIGHT, STOP);
timer_start(TIMER_VAMOS, 333L);
while(timer_time(TIMER_VAMOS) > 0L)
defer();
motor_y_son(LEFT, -SLOW);
motor_y_son(RIGHT, -SLOW);
timer_start(TIMER_VAMOS, 500L);
while(timer_time(TIMER_VAMOS) > 0L)
defer();
motor_y_son(LEFT, -FAST);
motor_y_son(RIGHT, -FAST);
get_left_clicks();
get_right_clicks();
timer_start(TIMER_VAMOS, 2000L);
while(timer_time(TIMER_VAMOS) > 0L)
defer();
max_mps_izq_detras = ((float)get_left_clicks()/2.0)/16.0 * MP_REVOLUCION;
max_mps_der_detras = ((float)get_right_clicks()/2.0)/16.0 * MP_REVOLUCION;
motor_y_son(LEFT, STOP);
motor_y_son(RIGHT, STOP);
printf ("\niz_de:%f/de_de:%f",max_mps_izq_detras, max_mps_der_detras);
sleep(2.0);
printf("\nEstoy un hombre muy confusado.");
if (max_mps_izq_antes+max_mps_der_antes+max_mps_izq_detras+max_mps_der_detras < NO_VEL*4.0)
{
printf("\nERROR: no hay un accum.! %f", (max_mps_izq_antes+max_mps_der_antes+max_mps_izq_detras+max_mps_der_detras)/0.04);
while(bumper()==0)
{
tone (50.0,200);
tone (60.0,200);
tone (70.0,200);
}
mode = MODOS ;
}
else if (max_mps_izq_antes+max_mps_der_antes+max_mps_izq_detras+max_mps_der_detras < MIN_VEL*4.0)
{
printf("\nERROR: cambiar el accum. %f", (max_mps_izq_antes+max_mps_der_antes+max_mps_izq_detras+max_mps_der_detras)/0.04);
while (bumper() == 0)
{
tone (75.0,200);
tone (85.0,200);
tone (95.0,200);
}
mode = MODOS ;
}
else if (max_mps_izq_antes+max_mps_der_antes+max_mps_izq_detras+max_mps_der_detras < MIT_VEL*4.0)
{
printf("\nAV:accumul. es MUY pobre %f", (max_mps_izq_antes+max_mps_der_antes+max_mps_izq_detras+max_mps_der_detras)/0.04);
tone(3000.0, 1500);
tone(2000.0, 750);
msleep (1000L);
}
else if (max_mps_izq_antes+max_mps_der_antes+max_mps_izq_detras+max_mps_der_detras < ACC_VEL*4.0)
{
printf("\nAVERTIDO: accumul. pobre %f", (max_mps_izq_antes+max_mps_der_antes+max_mps_izq_detras+max_mps_der_detras)/0.04);
tone(2000.0, 1500);
tone(1000.0, 750);
msleep (1000L);
}
else if (max_mps_izq_antes+max_mps_der_antes+max_mps_izq_detras+max_mps_der_detras < BIEN_VEL*4.0)
{
printf("\nINFO: accumulador es oke %f", (max_mps_izq_antes+max_mps_der_antes+max_mps_izq_detras+max_mps_der_detras)/0.04);
tone(1000.0, 1500);
tone(2000.0, 750);
msleep (1000L);
}
else
{
printf("\nERR: accu demasiado LLENO %f", (max_mps_izq_antes+max_mps_der_antes+max_mps_izq_detras+max_mps_der_detras)/0.04);
while (bumper() == 0)
{
tone(500.0, 200);
tone(550.0, 200);
tone(600.0, 200);
}
mode = MODOS ;
}
if (mode != MODOS)
mode = comunicar(MODOS+1);
}
else if (mode == 1)
{
float v_nueva;
int w_nueva;
int med, ole, avt, luz, seg, err, vam, dos;
med = start_process(medidas_luz());
ole = start_process(ole_la_luz_voy_la_luz());
avt = start_process(avertosotidar());
vam = start_process(vamos_a_la_luz());
luz = start_process(buscar_la_luz());
seg = start_process(seguir_pared());
err = start_process(errar());
dos = start_process(buscar_la_dosa());
printf ("\nModo *uno*: Los Behaviores");
luz_es_encontrada = 0;
while (1)
{
if (ind_errar == 1 && bloccada == 0)
{
v_nueva = v_errar;
w_nueva = w_errar;
ind_errar = 0;
}
if (ind_seguir == 1 && bloccada == 0)
{
v_nueva = v_seguir;
w_nueva = w_seguir;
ind_seguir = 0;
}
if (ind_buscar == 1 && bloccada != ALTO_PARA_DOSA)
{
v_nueva = v_buscar;
w_nueva = w_buscar;
ind_buscar = 0;
}
if (ind_vamosluz == 1 && bloccada == 0)
{
v_nueva = v_vamosluz;
w_nueva = w_vamosluz;
ind_vamosluz = 0;
}
if (ind_dosa == 1 && bloccada != ALTO_PARA_LUZ)
{
v_nueva = v_dosa;
w_nueva = w_dosa;
ind_dosa = 0;
}
if (ind_avert == 1)
{
v_nueva = v_avert;
w_nueva = w_avert;
ind_avert = 0;
}
if (ind_ole == 1 && bloccada == 0)
{
v_nueva = v_ole;
w_nueva = w_ole;
ind_ole = 0;
}
vamos_vwirregulada(v_nueva, w_nueva);
defer();
}
kill_process(med);
kill_process(luz);
kill_process(vam);
kill_process(seg);
kill_process(avt);
kill_process(ole);
kill_process(err);
kill_process(dos);
mode = 0;
}
else if (mode == 2)
{
int act_psd;
int corr_med;
int corr_sec, cliquos, act_cli;
int sec_con_distancia_min=0;
int la_maxima_dosa, larg, sec;
long tiempo;
printf ("\nModo dos: sectorisacion");
cliquos = 0;
act_cli = 2; /* para comencar con 2-2=0 */
get_left_clicks(); get_right_clicks();
set_vw(0.0, 30);
while (cliquos < CLIQUOS_REV_DEL_TORO)
{
StartPSD(0);
tiempo = mseconds() + 55L;
while(tiempo > mseconds())
defer();
act_psd = GetPSD(0);
printf("\nact_psd= %d", act_psd);
if (act_psd == 0)
act_psd = -1;
cm_secscan[cliquos] = cm_psd[act_psd/2];
tone(1500.0-10.0*(float)cm_secscan[cliquos], 15);
printf("\ncm_secscan[%d]= %d", cliquos, cm_secscan[cliquos]);
act_cli -= 2;
while (act_cli < 2)
{
act_cli+=get_left_clicks();
act_cli+=get_right_clicks();
defer();
}
cliquos++;
}
set_vw(0.0, 0);
tone(100.0, 100);
tone(150.0, 100);
tone(200.0, 100);
la_maxima_dosa = -1;
for (larg=1; larg <= 4; larg++)
{
printf("\ncalculando prob'dades %d", larg);
for (sec = 0; sec < CLIQUOS_REV_DEL_TORO; sec++)
{
hog_processor();
prob_secscan[sec] = (int)(prob(sec, larg)*100.0);
}
tone(1000.0 + (float)larg*200.0, 30);
printf("\ncalculando maxima", larg);
hog_processor();
for (sec = 0; sec < CLIQUOS_REV_DEL_TORO; sec++)
{
if (((la_maxima_dosa != -1) && (prob_secscan[sec] > prob_secscan[la_maxima_dosa]))
|| ((la_maxima_dosa == -1) && (prob_secscan[sec] > 0)))
la_maxima_dosa = sec;
}
tone(2000.0 + (float)larg*200.0, 50);
}
if (la_maxima_dosa != -1)
{
corr_sec = 0;
get_left_clicks(); get_right_clicks();
set_vw(0.0, -30);
while(corr_sec < (CLIQUOS_REV_DEL_TORO-la_maxima_dosa) * 2)
{
corr_sec+=get_left_clicks();
corr_sec+=get_right_clicks();
defer();
}
set_vw(0.0, 0);
tone(200.0, 100);
tone(250.0, 100);
tiempo = mseconds() + (long)((float)cm_secscan[la_maxima_dosa]/25.0*1000.0);
set_vw(0.25,0);
while(mseconds()0)
angulo =(int)((float)angulo/((( (float)angulo/360.0)*0.7+0.01)^0.25666));/*antes 26*/
else /*menos = menos correccion*/
/*keine correccion: An-Stelle-Rotation bei niedrigen Geschwindigkeiten betraegt nur 180 Grad statt 360*/
angulo = (int)( 0.97 * (float)angulo/((( (float)angulo*(-1.0)/360.0)*0.7+0.01)^0.24));/*detras 24*/
vel_izq_mps = vel_mps - ((float)angulo/180.0*3.141592653589*DISTANCIA/2.0);
vel_der_mps = vel_mps + ((float)angulo/180.0*3.141592653589*DISTANCIA/2.0);
if (vel_izq_mps < 0.0)
vel_izq_por = (int)(100.0 * vel_izq_mps / max_mps_izq_detras);
else
vel_izq_por = (int)(100.0 * vel_izq_mps / max_mps_izq_antes);
if (vel_der_mps < 0.0)
vel_der_por = (int)(100.0 * vel_der_mps / max_mps_der_detras);
else
vel_der_por = (int)(100.0 * vel_der_mps / max_mps_der_antes);
if (vel_izq_por < -100)
{
tone(2000.0, 150);
tone(2500.0, 300);
vel_izq_por = -100;
}
if (vel_izq_por > 100)
{
tone(2000.0, 150);
tone(2500.0, 150);
vel_izq_por = 100;
}
if (vel_der_por < -100)
{
tone(2000.0, 300);
tone(2500.0, 300);
vel_der_por = -100;
}
if (vel_der_por > 100)
{
tone(2000.0, 300);
tone(2500.0, 150);
vel_der_por = 100;
}
motor_y_son(LEFT, vel_pwm_izquierda [PIEDROS+vel_izq_por*PIEDROS/100]);
motor_y_son(RIGHT, vel_pwm_derecha [PIEDROS+vel_der_por*PIEDROS/100]);
}
void buscar_la_dosa()
{
long tiempo;
int act_psd;
int corr_med;
int corr_sec, cliquos, act_cli;
int la_maxima_dosa, larg, sec;
long ultima_reconquista;
while(1)
{
ultima_reconquista = mseconds();
while ((mseconds() < ultima_reconquista + 10000L + (long)randomint(10000)
|| RLS() != 0)
&& (bloccada != ALTO_PARA_DOSA))
{
ind_dosa = 0;
defer();
}
/* !!!!! */
bloccada = ALTO_PARA_DOSA;
/* !!!!! */
printf("\nbuscando una dosa");
cliquos = 0;
act_cli = 2; /* para comencar con 2-2=0 */
get_left_clicks(); get_right_clicks();
v_dosa = 0.0;
w_dosa = 30;
ind_dosa = 1;
defer();
while (cliquos < CLIQUOS_REV_DEL_TORO)
{
if (RLS() == 2)
{
bloccada = ALTO_PARA_LUZ;
break;
}
StartPSD(0);
tiempo = mseconds() + 55L;
while(mseconds() < tiempo)
{
ind_dosa = 1;
defer();
}
act_psd = GetPSD(0);
if (act_psd == 0)
act_psd = -1;
cm_secscan[cliquos] = cm_psd[act_psd/2];
tone(1500.0-10.0*(float)cm_secscan[cliquos], 15);
printf("\ncm_secscan[%d]= %d", cliquos, cm_secscan[cliquos]);
act_cli -= 2;
while (act_cli < 2)
{
act_cli+=get_left_clicks();
act_cli+=get_right_clicks();
}
cliquos++;
}
tone(100.0, 100);
tone(150.0, 100);
tone(200.0, 100);
if (RLS() == 2)
{
bloccada = ALTO_PARA_LUZ;
break;
}
v_dosa = 0.0;
w_dosa = 0;
defer();
/* tiempo = mseconds() + 260L;
while(mseconds() prob_secscan[la_maxima_dosa]))
|| ((la_maxima_dosa == -1) && (prob_secscan[sec] > 0)))
la_maxima_dosa = sec;
}
tone(2000.0 + (float)larg*200.0, 50);
}
if (la_maxima_dosa != -1)
{
printf("\nvoy a la dosa...");
corr_sec = 0;
get_left_clicks(); get_right_clicks();
v_dosa = 0.0;
w_dosa = -30;
ind_dosa = 1;
defer();
while(corr_sec < (CLIQUOS_REV_DEL_TORO-la_maxima_dosa) * 2)
{
corr_sec+=get_left_clicks();
corr_sec+=get_right_clicks();
ind_dosa = 1;
}
v_dosa = 0.0;
w_dosa = 0;
ind_dosa = 1;
defer();
tone(200.0, 100);
tone(250.0, 100);
tiempo = mseconds() + (long)((float)cm_secscan[la_maxima_dosa]/25.0*1000.0) + 250L; /* HACK!!! */
v_dosa = 0.25;
w_dosa = 0;
ind_dosa = 1;
defer();
while(mseconds() una vez mas*/
{
printf("\nno tengo una dosa!");
bloccada = ALTO_PARA_DOSA;
}
else
{
printf("\ntengo una dosa!");
bloccada=0;
}
}
else
{
tone(100.0, 100);
tone(75.0, 100);
printf("\nno hay una dosa...");
/* !!!!! */
bloccada = 0;
/* !!!!! */
}
}
}
void errar()
{
long tiempo;
while(1)
{
v_errar = 0.3;
w_errar = w + randomint(51) - 25;
if (w_errar < -60)
w_errar = 30;
if (w_errar > 60)
w_errar = -30;
ind_errar = 1;
tiempo = mseconds();
while(mseconds() < tiempo + 10L*DELTA_t)
defer();
}
}
void vamos_a_la_luz()
{
long tiempo;
while(1)
{
while(luz_es_encontrada && RLS() != 0)
{
v_vamosluz = 0.3;
w_vamosluz = (luzi - luzd)/abs(luzi-luzd) * 35;
tone (2000.0+(float)w_vamosluz*40.0,50);
ind_vamosluz = 1;
defer();
}
ind_vamosluz = 0;
defer();
}
}
void ole_la_luz_voy_la_luz()
{
int corr;
long tiempo;
while(1)
{
if((luzi+luzd)/2 < 40 && RLS()!=0)
{
luz_es_encontrada = 0;
v_ole = 0.0;
w_ole = 0;
ind_ole = 1;
defer();
for(corr=0; corr<=5 && ((luzi+luzd)/2 < 40); corr++)
{
ind_ole = 1;
tone((float)(6-corr)*500.0, 50);
defer();
}
v_ole=0.2;
w_ole=0;
tiempo=mseconds();
while ((mseconds() = CLIQUOS_REV_DEL_TORO/2)
{
v_buscar = 0.0;
w_buscar = -90;
ind_buscar = 1;
defer();
cliquos_tornados = 0;
get_left_clicks(); get_right_clicks();
while(cliquos_tornados < (CLIQUOS_REV_DEL_TORO-angulo_bien)*2)
{
cliquos_tornados += get_left_clicks();
cliquos_tornados += get_right_clicks();
ind_buscar = 1;
}
}
luz_es_encontrada = 1;
tone(2000.0, 300);
tone(3000.0, 300);
tone(4000.0, 300);
ind_buscar = 0;
/* !!!!! */
bloccada = 0;
/* !!!!! */
defer();
}
}
void avertosotidar()
{
long tiempo;
int corr;
int bumpo;
while(1)
{
bumpo = bumper();
if (bumpo > 0)
{
luz_es_encontrada = 0;
v_avert = -0.3;
if (bumpo&0b001 != 0)
w_avert = -90;
else if (bumpo&0b010 != 0)
w_avert = +90;
else if (bumpo&0b011 != 0)
while ((w_avert = 90 * (1-randomint(3))) == 0);
if (bumpo&0b100 != 0)
{
v_avert = 0.3;
while ((w_avert = 90 * (1-randomint(3))) == 0);
}
tiempo = mseconds();
while(mseconds() < tiempo + 500L)
{
ind_avert = 1;
defer();
}
v_avert = 0.3;
w_avert = 0;
ind_avert = 1;
}
defer();
}
}
void seguir_pared()
{
long tiempo;
int corr;
int w_dir = 0;
int w_dir_vieja;
int izq_infrarojo, der_infrarojo, infrarojo;
while(1)
{
infrarojo = ir_detect();
izq_infrarojo = infrarojo&0b001;
der_infrarojo = infrarojo&0b010;
if (izq_infrarojo + der_infrarojo != 0)
{
if (izq_infrarojo != 0 && der_infrarojo == 0)
{
v_seguir = 0.1;
w_seguir = -35;
w_dir = w_dir_vieja = -1;
}
else if (izq_infrarojo == 0 && der_infrarojo != 0)
{
v_seguir = 0.1;
w_seguir = +35;
w_dir = w_dir_vieja = +1;
}
else
{
v_seguir = 0.1;
w_seguir = 135 * w_dir_vieja;
}
ind_seguir = 1;
}
defer();
if (izq_infrarojo + der_infrarojo== 0 && w_dir != 0)
{
int perdido = 1;
tiempo = mseconds();
while (mseconds() < tiempo + 3000L)
{
v_seguir = 0.3;
w_seguir = 30 * -w_dir;
ind_seguir = 1;
infrarojo = ir_detect();
izq_infrarojo = infrarojo&0b001;
der_infrarojo = infrarojo&0b010;
if (izq_infrarojo + der_infrarojo != 0)
{
perdido = 0;
break;
}
defer();
}
if (perdido == 1)
{
tone(200.0, 100);
w_dir = 0;
ind_seguir = 0;
}
}
defer();
}
}
void medidas_luz()
{
float peso = 0.33333;
long tiempo;
luzi = luzd = 127;
while (1)
{
if (RLS()!=0) /* si tenemos una dosa al ano*/
{
luzi = (int)((1.0-peso)*(float)luzi + peso*(float)analog(photo_left));
luzd = (int)((1.0-peso)*(float)luzd + peso*(float)analog(photo_right));
tiempo = mseconds();
while (mseconds() < tiempo + DELTA_t)
defer();
}
}
}
int comunicar(int modulo)
{
int contador = modulo-1, cuento = 0;
printf("\nAppuyas para comencar un nombre:");
tone(1000.0, 300);
tone(500.0, 150);
while (bumper() == 0);
tone(500.0, 150);
timer_start(TIMER_NOMBRE, 1000L);
while (timer_time(TIMER_NOMBRE) > 0L)
{
if (bumper())
{
contador += 1;
contador = contador%modulo;
printf("\nEl nombre actual es %d.", contador);
tone(600.0, 150);
tone(900.0, 100);
for (cuento = 0; cuento < contador; cuento++)
tone(2500.0, 75);
timer_start(TIMER_NOMBRE, 1000L);
}
}
tone(100.0, 300);
tone(50.0, 500);
return (contador);
}
void motor_y_son(int index, int vel)
{
motor(index, vel);
}
void son(void)
{
while (1)
while (v < 0.0)
{
timer_start(TIMER_TONE, 800L);
while (timer_time(TIMER_TONE) > 0L)
defer();
tone(750.0, 500);
}
}
/**********************************************************************************/
/**********************************************************************************/
float cm_ix(int rel)
{
if (rel<0)
return (float)(cm_secscan[rel%CLIQUOS_REV_DEL_TORO+CLIQUOS_REV_DEL_TORO]);
if (rel>CLIQUOS_REV_DEL_TORO-1)
return (float)(cm_secscan[rel%CLIQUOS_REV_DEL_TORO]);
return (float)(cm_secscan[rel]);
}
float prob(int sec_c, int largoza_testada)
{
float nombre;
int i, min;
if (largoza_testada==1)
{
nombre=cm_ix(sec_c-1)/cm_ix(sec_c)*cm_ix(sec_c+1)/cm_ix(sec_c);
min = 100;
for (i=0; i<=0; i++) /* habia estada: -1...1 */
if ((int)cm_ix(sec_c+i) < min)
min = (int)cm_ix(sec_c+i);
if ((min<25)||(min>100))
nombre = 0.0;
nombre-=1.2;
}
else if (largoza_testada==2)
{
nombre=cm_ix(sec_c-2)/cm_ix(sec_c-1)*cm_ix(sec_c+1)/cm_ix(sec_c);
min = 100;
for (i=-1; i<=0; i++) /* habia estada: -2...1 */
if ((int)cm_ix(sec_c+i) < min)
min = (int)cm_ix(sec_c+i);
if ((min<11)||(min>45))
nombre = 0.0;
nombre-=6.0;
}
else if (largoza_testada==3)
{
nombre=cm_ix(sec_c-2)/cm_ix(sec_c);
nombre*=cm_ix(sec_c+2)/cm_ix(sec_c);
/* nombre=cm_ix(sec_c-2)/cm_ix(sec_c-1)*cm_ix(sec_c-1)/cm_ix(sec_c);
nombre*=cm_ix(sec_c+1)/cm_ix(sec_c)*cm_ix(sec_c+2)/cm_ix(sec_c+1); */
min = 100;
for (i=-1; i<=1; i++) /* habia estada: -2...2 */
if ((int)cm_ix(sec_c+i) < min)
min = (int)cm_ix(sec_c+i);
if ((min<5)||(min>20))
nombre = 0.0;
nombre-=15.0;
}
else if (largoza_testada==4)
{
nombre=cm_ix(sec_c-3)/cm_ix(sec_c-1);
nombre*=cm_ix(sec_c+2)/cm_ix(sec_c);
/* nombre=cm_ix(sec_c-3)/cm_ix(sec_c-2)*cm_ix(sec_c-2)/cm_ix(sec_c-1);
nombre*=cm_ix(sec_c+1)/cm_ix(sec_c)*cm_ix(sec_c+2)/cm_ix(sec_c+1); */
min = 100;
for (i=-2; i<=1; i++) /* habia estada: -3...2 */
if ((int)cm_ix(sec_c+i) < min)
min = (int)cm_ix(sec_c+i);
if ((min<0)||(min>10))
nombre = 0.0;
nombre-=15.0;
}
if (nombre < 0.0)
nombre = 0.0;
return (nombre);
}