suivi moteurs

Code/KirbyTrack.c

@@ -19,8 +19,9 @@
 //ATTENTION AFFICHAGE OPENCV INCOMPATIBLE AVEC AFFICHAGE SFML
 
 /*Headers*/
-void controle_moteur(int vecX, int vecY, int rayon);
+void maj_angle(int vecX, int vecY, int rayon, int* angle); //Met à jour l'angle selon la distance CentreCamera - Cible
-int limite_moteur(int val_pwm);
+void controle_moteur(int* angle);//Envoie les angles au moteur
+int limite_moteur(int val_pwm);//Verifie que les valeurs envoyees aux moteurs sont correctes
 
 void config(int* LowH, int* HighH, int* LowS, int* HighS, int* LowV, int* HighV); //Affiche le panneau de configuration de tracking avec les arguments comme valeur de base
 void affichage_config(IplImage* frame, IplImage* HSV, IplImage* Binaire); //Affiche le flux vidéos et ses différent traitements
@@ -31,13 +32,16 @@ void traitement(IplImage* frame, IplImage* HSV, IplImage* Binaire, int LowH, int
 
 int image_CV2SFML(IplImage* imcv, sf::Image imsf); //Construction de imsf (RGBA) à partir de imcv (BGR), avec alpha constant (=1)
 
+
 int main(int argc, char* argv[])
 {
+	//Initialisations 
 	int height,width,step,channels;  //parameters of the image we are working on
 	int posX, posY; //Position objet
 	int boucle;
-#ifdef SFML
+	int angle[2] = {100,100};
 
+#ifdef SFML
 	//Initialisation SFML
 	
 	sf::Texture txFlux;
@@ -49,12 +53,12 @@ int main(int argc, char* argv[])
 	sf::RenderWindow window(sf::VideoMode(800, 600), "KirbyTrack");
 #endif
 
-    // Open capture device. 0 is /dev/video0, 1 is /dev/video1, etc.
+  //Ouverture flux camera
   CvCapture* capture = cvCaptureFromCAM( 0 );
     
   if( !capture ){
   	printf("ERROR: capture is NULL \n" );
-            return EXIT_FAILURE;
+  	exit(EXIT_FAILURE);
   }
     
 
@@ -112,6 +116,7 @@ int main(int argc, char* argv[])
 	boucle = 1;
 #endif
 	 
+	//BOUCLE PRINCIPALE
   while(boucle)//while(window.isOpen())
   {  
 
@@ -131,7 +136,7 @@ int main(int argc, char* argv[])
 		frame = cvQueryFrame( capture );
 		      
 		if( !frame ){
-                printf("ERROR: frame is null...\n" );
+			perror("ERROR: frame is null...");
 			break;
 		}
 
@@ -166,13 +171,18 @@ int main(int argc, char* argv[])
 		//sfTexture_destroy(texture);
 #endif
 
-	//controle_moteur(posX-width/2, posY-height/2, height/6); //Envoie commande moteur
+		//Mouvements moteurs
+		printf("-PREMAJ_ANGLE...: %d %d\n",width,height);
+		maj_angle(posX-width/2, posY-height/2, height/6, angle);
+		controle_moteur(angle); 
+		cvWaitKey(50);
 
 #ifdef CONFIG
 		affichage_config(frame, hsv_frame, threshold); //Affichage du flux vidéo et de ses traitements
 	
 		if( (cvWaitKey(10) ) >= 0 ) break; //Arret capture
 #endif
+
 	}
     
 	//cvWaitKey(0); //Fin programme
@@ -185,56 +195,64 @@ int main(int argc, char* argv[])
 	cvReleaseImage(&frame);
      
 	return EXIT_SUCCESS;
+}
+
+
+void maj_angle(int vecX, int vecY, int rayon, int* angle){
+	//On ajustera coeff fonction du rayon. Si la cible est à une distance 5*r, il sera 5x plus rapide que s'il était à 1*r
+//double norme = 1.0*vecX*vecX + 1.0*vecY*vecY;
+	int coeffx = vecX/rayon, coeffy = vecY/rayon, l0, l1; 
+	printf("-MAJ_ANGLE...Valeur maj_angle arguments : %d %d %d\n\tAnciens angles : %d %d\n\t",vecX,vecY,rayon,angle[0],angle[1]);
+//	if (norme > rayon*rayon){ //Si la cible est en dehors d'un certain rayon
+
+		//Ajout d'un angle moteur pondéré par la distance
+		angle[0] += coeffx;
+		angle[1] += coeffy;
+
+		//Majoration - minoration des angles moteurs
+		l0 = limite_moteur(angle[0]);
+		l1 = limite_moteur(angle[1]);
+		if (l0 != 0) angle[0] = l0;
+		if (l1 != 0) angle[1] = l1;
+	//}
+
+		printf("Nouveaux angles : %d %d\n",angle[0],angle[1]);
+}
+
+int limite_moteur(int val_pwm){
+	int MAX_PWM = 130, MIN_PWM = 30;
+	if (val_pwm > MAX_PWM){ 
+		return MAX_PWM;
 	}
+	else if (val_pwm < MIN_PWM){
+		return MIN_PWM;
+	}
+	else{
+		return 0;
+	}
+}
 
-/*On se rapproche de (vecX, vecY) si la position se situe en dehors d'un cercle centre sur la camera*/
+void controle_moteur(int* angle){
-void controle_moteur(int vecX, int vecY, int rayon){
 
-	int val_pwm[2];
+	//Ouverture port serie
-
-	/*Lecture valeur*/
 	FILE* fichier = NULL;
-	fichier = fopen("/dev/ttyACM0","r");
-	if(fichier==NULL){
-		printf("Erreur ouverture fichier\n");
-		return ;
-	}
-	
-	fscanf(fichier,"%d,%d",&val_pwm[0],&val_pwm[1]);
-	
-	fclose(fichier);
-
-	/*Ecriture nouvelle valeur*/
 	fichier = fopen("/dev/ttyACM0","w");
 	if(fichier==NULL){
 		printf("Erreur ouverture fichier\n");
-		return ;
+		perror("fopen failed for /dev/ttyACM0" );
-	}
+		exit( EXIT_FAILURE );
-	double norme = 1.0*vecX*vecX + 1.0*vecY*vecY;
-	
-	if (norme > rayon*rayon){
-		if(vecX >= vecY && limite_moteur(val_pwm[0])){ /*Ecart sur x plus important*/
-			fprintf(fichier,"%d,%d",val_pwm[0]++,val_pwm[1]);
-		}
-		else if(vecX <= vecY && limite_moteur(val_pwm[1])){	/*Ecart sur y plus important*/
-			fprintf(fichier,"%d,%d",val_pwm[0],val_pwm[1]++);
-		}
 	}
 
+	//Ecriture angles
+	fprintf(fichier,"%d\n",angle[0]);
+	fprintf(fichier,"%d\n",angle[1]);
+
+	//Fermeture
 	fclose(fichier);
 	return;
 }
 
-/*Verifie que les valeurs envoyees aux moteurs sont correctes*/
+
-int limite_moteur(int val_pwm){
-	int MAX_PWM = 255;
-	if (val_pwm > MAX_PWM || val_pwm < 0){
-		return 0;
-	}
-	else{
-		return 1;
-	}
-}
 /*
 int image_CV2SFML(IplImage* imcv, sf::Image imFlux){
 	
@@ -251,11 +269,11 @@ void traitement(IplImage* frame, IplImage* HSV, IplImage* Binaire, int LowH, int
 
 	//Binarisation
 
-	//CvScalar valinf={LowH,LowS,LowV};
+	CvScalar valinf={LowH,LowS,LowV};
-	//CvScalar valsup={HighH,HighS,HighV};
+	CvScalar valsup={HighH,HighS,HighV};
 
-        //cvInRangeS(HSV, valinf,valsup, Binaire);
+  cvInRangeS(HSV, valinf,valsup, Binaire);
-	cvInRangeS(HSV, CvScalar(LowH,LowS,LowV),CvScalar(HighH,HighS,HighV), Binaire);
+	//cvInRangeS(HSV, CvScalar(LowH,LowS,LowV),CvScalar(HighH,HighS,HighV), Binaire);
       
         //cvSmooth( Binaire, Binaire, CV_GAUSSIAN, 9, 9 ); //Legère suppression des parasites
 }

Test moteurs/moteur.c

@@ -13,6 +13,8 @@ int main(){
 	int angle[2] = {0,0};
 	int coeff = 1;
 
+	printf("COUCOCUOCUCOUC : %d %d %d\n",1/2, 2/3,5/2);
+
 	while(getchar() != 'q'){
 
 		//Ouverture tty