/* dumb0.4.c*/
/* de moins en moins dumb au fur et a mesure....*/
/* mise a jour 07/01
* decoupage du code, multitache, semaphores
*
* mise a jour 08/01
* procedure de calibration
*
* stand up behavior ! qui le propulse en version 0.3 ! d'un coup !
*
* mise a jour 10/01
* correction de quelques bugs dans le multitache
* moind de taches mais plus efficaces...
*/
#include "dumb04.h"
//gestion des evenements / interruptions
wakeup_t emitNoir(wakeup_t data)
{
return ULTRA_NOIR(LIGHT_3);
}
int catchNoir()
{
while(1)
{
dir=1;
wait_event(&emitNoir,0);
// dsound_play(lowbeep);
// wait_event(dsound_finished,0);
}
return 0;
}
wakeup_t touch2(wakeup_t data)
{
return (SENSOR_2<0xf000);
}
//module reading
int watchDir()
{
while(1){
if (ULTRA_BRIGHT(LIGHT_3)) dir= 0;
if (ULTRA_BLANC(LIGHT_3)) dir=-1;
if (ULTRA_NOIR(LIGHT_3)) dir= 1;
}
return 0;
}
int standup(){
//leve toi et marche !
motor_a_dir(fwd);
motor_b_dir(fwd);
motor_c_dir(fwd);
motor_a_speed(255);
motor_b_speed(255);
motor_c_speed(255);
msleep(900);
motor_a_dir(brake);
motor_b_dir(brake);
motor_c_dir(brake);
//msleep(100);
wait_event(&emitNoir,0);
return 0;
}
int calibrate()
{
int tempmin,tempmax;
wait_event(&touch2,0);
tempmin=LIGHT_1;
dsound_play(lowbeep);
wait_event(dsound_finished,0);
wait_event(&touch2,0);
tempmax=LIGHT_3;
dsound_play(hibeep);
wait_event(dsound_finished,0);
lambda=(MAX_SPEED-MIN_SPEED)/(tempmax-tempmin);
muhhhh=(lambda*tempmin-MIN_SPEED);
return 0;
}
//##############----------gestion des sorties------------
//gestion "continue" de l'accélération
/*
entrees : current1, dir
sortie : speed
semaphores: semlight1, semdir, semspeed
*/
int vitesseGrad(){
int dirtemp=0;
int tempspeed=0,current1_temp;
while(1)
{
//sem_wait(&semlight1);
current1_temp=LIGHT_1;
//sem_post(&semlight1);
//sem_wait(&semdir);
//dirtemp=dir;
//sem_post(&semdir);
switch (dir)
{
case 1: //blanc
tempspeed=lambda*current1_temp - muhhhh;
/*tempspeed=GRADFACT * current1_temp -30;*/
if (tempspeed > 255) tempspeed=255;
//tempspeed=255;
break;
case -1:
tempspeed=lambda*current1_temp - muhhhh;
/*tempspeed=GRADFACT * current1_temp -30;*/
if (tempspeed > 255) tempspeed=255;
//tempspeed=255;
break;
case 0:
tempspeed=0;
break;
}
if (tempspeed<0) tempspeed=0;
sem_wait(&semspeed);
speed=tempspeed;
sem_post(&semspeed);
}
return 0;
}
/* ----------------------------commande moteurs
entrees: dir, speed
sortie: action moteurs */
int tuning(){
int speedtemp,dirtemp;
while(1){
sem_wait(&semspeed);
//recupere speed et dir
speedtemp=speed;
dirtemp=dir;
sem_post(&semspeed);
//display
lcd_int(speedtemp);
//react
switch(dirtemp){
case 0 :
//dsound_play(melody);
//wait_event(dsound_finished,0);
//motor_a_dir(brake);
//motor_b_dir(brake);
//motor_c_dir(brake);
//msleep(2);
//motor_a_dir(off);
//motor_b_dir(off);
//motor_c_dir(off);
//sem_wait(&semtime);
//msleep(sleeptime);
//sem_post(&semtime);
if (speed-50>=0)
speedtemp=speed-50;
else
speedtemp=0;
motor_a_speed(speedtemp);
motor_b_speed(speedtemp);
motor_c_speed(speedtemp);
msleep(2);
break;
case -1 : //noir
//dsound_play(lowbeep);
//wait_event(dsound_finished,0);
motor_a_dir(rev);
motor_b_dir(rev);
motor_c_dir(rev);
motor_a_speed(speedtemp);
motor_b_speed(speedtemp);
motor_c_speed(speedtemp);
break;
case 1:
//dsound_play(hibeep);
//wait_event(dsound_finished,0);
motor_a_dir(fwd);
motor_b_dir(fwd);
motor_c_dir(fwd);
motor_a_speed(speedtemp);
motor_b_speed(speedtemp);
motor_c_speed(speedtemp);
break;
}
}
return 0;
}
//-------------------surveillance du comportement du robot
int bigBrother(){
while(1){
wait_event(&touch2,0);
//pendule tombe
sem_wait(&semspeed);
speed=0;
//kill(pid6);
//kill(pid7);
//kill(pid3);
//kill(pid4);
//kill(pid5);
//kill(pid9);
motor_a_dir(off);
motor_b_dir(off);
motor_c_dir(off);
dsound_play(lost);
//wait_event(dsound_finished,0);
sleep(2);
standup();
wait_event(&emitNoir,0);
sem_post(&semspeed);
//rend la main
//pid9=execi(&task_manager,0,NULL,1,DEFAULT_STACK_SIZE);
dsound_play(lowbeep);
dsound_play(hibeep);
}
return 0;
}
int task_manager(){
pid3=execi(&watchDir,0,NULL,1,DEFAULT_STACK_SIZE);
pid5=execi(&catchNoir,0,NULL,1,DEFAULT_STACK_SIZE);
//wait_event(&emitNoir,0);
pid7=execi(&vitesseGrad,0,NULL,1,DEFAULT_STACK_SIZE);
return 0;
}
//############################ MAIN ####################
int main()
{
//pour la gestion des timer
// bonus stuff
//int goodwork=0;
//init sensors
ds_active(&SENSOR_3);
ds_active(&SENSOR_1);
sem_init(&semlight1,0,1);
sem_init(&semlight3,0,1);
sem_init(&semspeed,0,1);
sem_init(&semdir,0,1);
sem_init(&semtime,0,1);
//wait for physical initialisation
sleep(3);
if (touch2(0))
{
//procedure de calibration
wait_event(&touch2,0);
dsound_play(lowbeep);
wait_event(dsound_finished, 0);
dsound_play(melody);
wait_event(dsound_finished, 0);
msleep(100);
dsound_play(hibeep);
wait_event(dsound_finished, 0);
msleep(100);
calibrate();
sleep(1);
}
//Go !
dsound_play(hibeep);
wait_event(dsound_finished, 0);
dsound_play(hibeep);
wait_event(dsound_finished, 0);
dsound_play(hibeep);
wait_event(dsound_finished, 0);
lcd_int(lambda);
sleep(1);
lcd_int(muhhhh);
sleep(2);
pid1=execi(&watchDir,0,NULL,1,DEFAULT_STACK_SIZE);
//apparemment si on les met pas tous en priorite 1, ca bloque...
dsound_play(lost);
pid9=execi(&task_manager,0,NULL,1,DEFAULT_STACK_SIZE);
//permet de tout (re)lancer en bloc
standup();
msleep(200); //temps de decollage avant lequel on libere la commande moteur
pid6=execi(&tuning,0,NULL,1,DEFAULT_STACK_SIZE);
sleep(3);
pid8=execi(&bigBrother,0,NULL,2,DEFAULT_STACK_SIZE);
return 0;
}