void *thread_distance (void * arg)
{
int* socketClient=(int *)arg; // On precise la nature de la variable arg
int ecrits = 0;
int distance;
char id = 'd';
int buffer[2];
buffer[0] = htonl(0);
printf("creation du thread1\n");
for(;;)
{
//printf("Distance: %d \n", ultrason());
distance = ultrason();
if(*socketClient != -1)
{
buffer[1] = htonl(distance);
pthread_mutex_lock(&lock);
//ecrits = write(*socketClient, &id, sizeof(id));
ecrits = write(*socketClient, buffer, sizeof(buffer));
pthread_mutex_unlock(&lock);
}
delay(50);
}
return 0;
}
void *deplacement(void *arg){
//deplacementArg *Arg = (deplacementArg *) arg;
int distanceObjet[NB_ULTRASON] = {0};
int *ptabDistance;
ptabDistance = distanceObjet;
// attente de la synchro synchronisation initiale entre le robot et la personne (positionnement)
while (flagInit == false){
if (finProgramme == true){
pthread_exit(NULL);
}
}
while(1){
if (ultrason(&ptabDistance) != 0){
puts("Ultrason failed");
finProgramme = true;
pthread_exit(NULL);
}
if (ptabDistance[ULTRASON_3] < DISTANCE_TROP_PROCHE)
{
//Arret immédiat du robot
robot.stop();
robotAvance = false;
}
// Il detecte un objet tout droit
else if (ptabDistance[ULTRASON_3] < DISTANCE_OBJET_FACE_MIN)
{
// Il detecte un objet proche a 45° a gauche du robot, et on a un minimum de place sur la droite
if (ptabDistance[ULTRASON_2] < DISTANCE_OBJET_TRANSVERSE_MIN && ptabDistance[ULTRASON_4] > DISTANCE_TROP_PROCHE)
{
// Check pour savoir si on voit le meme mur que le 1er ultrason a detecter, si oui calcul precis de repositionnement
if (ptabDistance[ULTRASON_1] < DISTANCE_OBJET_COTE_MIN)
{
//Faire tourner le robot de (atan((ptabDistance[ULTRASON_1]+DISTANCE_CENTRE_ULTRASON)/(ptabDistance[ULTRASON_3]+DISTANCE_CENTRE_ULTRASON))*180/PI
//Sur la droite
robotTourne = true;
flagVirage = int(abs(atan((ptabDistance[ULTRASON_1]+DISTANCE_CENTRE_ULTRASON)/(ptabDistance[ULTRASON_3]+DISTANCE_CENTRE_ULTRASON))*180/PI));
robot.turnRight(60,flagVirage,1);
robotTourne = false;
flagVirage = 0;
robot.goForward(90);
}
// Approximation de l'angle pour un cas ou on ira de maniere transversale, mais sans acces au mur du capteur gauche perpendiculaire
else
{
//Faire tourner le robot de 45° sur la droite
robotTourne = true;
flagVirage = 45;
robot.turnRight(60,45,1);
robotTourne = false;
flagVirage = 0;
robot.goForward(90);
}
}
// Il detecte un objet proche a 45° a droite du robot, et on a un minimum de place sur la gauche
else if (ptabDistance[ULTRASON_4] < DISTANCE_OBJET_TRANSVERSE_MIN && ptabDistance[ULTRASON_2] > DISTANCE_TROP_PROCHE)
{
// Check pour savoir si on voit le meme mur que le 1er ultrason a detecter, si oui calcul precis de repositionnement
if (ptabDistance[ULTRASON_5] < DISTANCE_OBJET_COTE_MIN)
{
//Faire tourner le robot de (atan((ptabDistance[ULTRASON_5]+DISTANCE_CENTRE_ULTRASON)/(ptabDistance[ULTRASON_3]+DISTANCE_CENTRE_ULTRASON))*180/PI
//Sur la gauche
robotTourne = true;
flagVirage = -1*int(abs(atan((ptabDistance[ULTRASON_5]+DISTANCE_CENTRE_ULTRASON)/(ptabDistance[ULTRASON_3]+DISTANCE_CENTRE_ULTRASON))*180/PI));
robot.turnLeft(60,flagVirage,1);
robotTourne = false;
flagVirage = 0;
robot.goForward(90);
}
// Approximation de l'angle pour un cas ou on ira de maniere transversale, mais sans acces au mur du capteur droite perpendiculaire
else
{
//Faire tourner le robot de 45° sur la gauche
robotTourne = true;
flagVirage = -45;
robot.turnLeft(60,45,1);
robotTourne = false;
flagVirage = 0;
robot.goForward(90);
}
}
// Si il n'a pas detecter d'objet trop proche OU si il a detecter des objets trop proche d'un coté ou de l'autre
// Alors on vire de l'autre cote
else if (ptabDistance[ULTRASON_2] < ptabDistance[ULTRASON_4])
{
//tourner à droite de 80°
robotTourne = true;
flagVirage = 80;
robot.turnRight(75,80,1);
robotTourne = false;
flagVirage = 0;
robot.goForward(90);
}
else if (ptabDistance[ULTRASON_4] < ptabDistance[ULTRASON_2])
{
//tourner à gauche de 80°
robotTourne = true;
flagVirage = 80;
robot.turnLeft(75,80,1);
robotTourne = false;
flagVirage = 0;
robot.goForward(90);
}
// Pied de table ?
else if (ptabDistance[ULTRASON_1] < ptabDistance[ULTRASON_5])
{
//tourne à droite de 45°
//wait 1sec
//tourne à gauche de 45°
robotTourne = true;
robot.turnRight(90,45,1);
robot.goForward(90);
sleep(1);
robot.turnLeft(90,45,1);
robotTourne = false;
flagVirage = 0;
robot.goForward(90);
}
else if (ptabDistance[ULTRASON_5] < ptabDistance[ULTRASON_1])
{
//tourne à gauche de 45°
//wait 1sec
//tourne à droite de 45°
robotTourne = true;
robot.turnLeft(90,45,1);
robot.goForward(90);
sleep(1);
robot.turnRight(90,45,1);
robotTourne = false;
flagVirage = 0;
robot.goForward(90);
}
}
//On se dirige doucement sur le mur à gauche
else if (ptabDistance[ULTRASON_2] < (DISTANCE_OBJET_TRANSVERSE_MIN*2) && ptabDistance[ULTRASON_2] < ptabDistance[ULTRASON_1])
{
// tourne a droite de 10°
robotTourne = true;
flagVirage = 10;
robot.turnRight(75,10,1);
robotTourne = false;
flagVirage = 0;
robot.goForward(90);
}
//On se dirige doucement sur le mur à droite
else if (ptabDistance[ULTRASON_4] < (DISTANCE_OBJET_TRANSVERSE_MIN*2) && ptabDistance[ULTRASON_4] < ptabDistance[ULTRASON_5])
{
// tourne a gauche de 10°
robotTourne = true;
flagVirage = 10;
robot.turnLeft(75,10,1);
robotTourne = false;
flagVirage = 0;
robot.goForward(90);
}
//On est parallèle au mur, mais proche du mur de gauche
else if (ptabDistance[ULTRASON_1] < DISTANCE_MUR_PROCHE)
{
// droite a droite de 45° puis instantanement à gauche de 45°
robotTourne = true;
flagVirage = 0;
robot.turnRight(95,45,1);
robot.turnLeft(95,45,1);
robotTourne = false;
robot.goForward(90);
}
//On est parallèle au mur, mais proche du mur de droide
else if (ptabDistance[ULTRASON_5] < DISTANCE_MUR_PROCHE)
{
// droite a gauche de 45° puis instantanement à droite de 45°
robotTourne = true;
flagVirage = 0; //Afin d'éviter d'embeter le programme de positionnement
robot.turnLeft(95,45,1);
robot.turnRight(95,45,1);
robotTourne = false;
robot.goForward(90);
}
//Aucun obstacle génant, on va tout droit
else
{
robotTourne = false;
flagVirage = 0;
}
if (finProgramme == true){
// Fin du thread
pthread_exit(NULL);
}
}
}
