AntoineH/LivingMachine
1
0
Fork 0
mirror of https://github.com/AntoineHX/LivingMachine.git synced 2025-05-04 05:40:46 +02:00
Code Issues Projects Releases Packages Wiki Activity

suivi moteurs

Browse source
This commit is contained in:
Forest 2017-05-09 16:29:35 +02:00
parent 4bf5661a84
commit 66cca07ea6
6 changed files with 138 additions and 118 deletions
Download patch file Download diff file Expand all files Collapse all files

BIN
Arch Pro bin/mbed-os-motor-control-serial_ARCH_PRO.bin Normal file
View file

Binary file not shown.

BIN
Code/KirbyTrack Executable file
View file

Binary file not shown.

244
Code/KirbyTrack.c
View file

@ -19,8 +19,9 @@
//ATTENTION AFFICHAGE OPENCV INCOMPATIBLE AVEC AFFICHAGE SFML //ATTENTION AFFICHAGE OPENCV INCOMPATIBLE AVEC AFFICHAGE SFML
/*Headers*/ /*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 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 void affichage_config(IplImage* frame, IplImage* HSV, IplImage* Binaire); //Affiche le flux vidéos et ses différent traitements
@ -31,43 +32,46 @@ 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 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[]) int main(int argc, char* argv[])
{ {
//Initialisations
int height,width,step,channels; //parameters of the image we are working on int height,width,step,channels; //parameters of the image we are working on
int posX, posY; //Position objet int posX, posY; //Position objet
int boucle; int boucle;
#ifdef SFML int angle[2] = {100,100};
#ifdef SFML
//Initialisation SFML //Initialisation SFML
sf::Texture txFlux; sf::Texture txFlux;
sf::Sprite spFlux; sf::Sprite spFlux;
sf::Image imFlux; sf::Image imFlux;
sf::Event event; sf::Event event;
//Création de la fenetre principale //Création de la fenetre principale
sf::RenderWindow window(sf::VideoMode(800, 600), "KirbyTrack"); sf::RenderWindow window(sf::VideoMode(800, 600), "KirbyTrack");
#endif #endif
// Open capture device. 0 is /dev/video0, 1 is /dev/video1, etc. //Ouverture flux camera
CvCapture* capture = cvCaptureFromCAM( 0 ); CvCapture* capture = cvCaptureFromCAM( 0 );
if( !capture ){ if( !capture ){
printf("ERROR: capture is NULL \n" ); printf("ERROR: capture is NULL \n" );
return EXIT_FAILURE; exit(EXIT_FAILURE);
} }
// grab an image from the capture // grab an image from the capture
IplImage* frame = cvQueryFrame( capture ); IplImage* frame = cvQueryFrame( capture );
// get the image data // get the image data
height = frame->height; height = frame->height;
width = frame->width; width = frame->width;
step = frame->widthStep; step = frame->widthStep;
// capture size - // capture size -
CvSize size = cvSize(width,height); CvSize size = cvSize(width,height);
#ifdef SFML #ifdef SFML
//Intialisation de la texture //Intialisation de la texture
@ -77,21 +81,21 @@ int main(int argc, char* argv[])
} }
#endif #endif
// Initialize different images that are going to be used in the program // Initialize different images that are going to be used in the program
IplImage* hsv_frame = cvCreateImage(size, IPL_DEPTH_8U, 3); // image converted to HSV plane IplImage* hsv_frame = cvCreateImage(size, IPL_DEPTH_8U, 3); // image converted to HSV plane
IplImage* threshold = cvCreateImage(size, IPL_DEPTH_8U, 1); IplImage* threshold = cvCreateImage(size, IPL_DEPTH_8U, 1);
//Controle couleur //Controle couleur
#ifdef KIRBY #ifdef KIRBY
//Setup Kirby //Setup Kirby
int iLowH = 139; int iLowH = 139;
int iHighH = 179; int iHighH = 179;
int iLowS = 48; int iLowS = 48;
int iHighS = 255; int iHighS = 255;
int iLowV = 101; int iLowV = 101;
int iHighV = 255; int iHighV = 255;
#endif #endif
#ifdef ETOILE #ifdef ETOILE
//Setup Etoile //Setup Etoile
@ -112,129 +116,143 @@ int main(int argc, char* argv[])
boucle = 1; boucle = 1;
#endif #endif
while(boucle)//while(window.isOpen()) //BOUCLE PRINCIPALE
{ while(boucle)//while(window.isOpen())
{
#ifdef SFML #ifdef SFML
boucle = window.isOpen(); boucle = window.isOpen();
// on inspecte tous les évènements de la fenêtre qui ont été émis depuis la précédente itération // on inspecte tous les évènements de la fenêtre qui ont été émis depuis la précédente itération
while (window.pollEvent(event)) while (window.pollEvent(event))
{ {
// évènement "fermeture demandée" : on ferme la fenêtre // évènement "fermeture demandée" : on ferme la fenêtre
if (event.type == sf::Event::Closed) if (event.type == sf::Event::Closed)
window.close(); window.close();
} }
#endif #endif
// Get one frame // Get one frame
frame = cvQueryFrame( capture ); frame = cvQueryFrame( capture );
if( !frame ){ if( !frame ){
printf("ERROR: frame is null...\n" ); perror("ERROR: frame is null...");
break; break;
} }
//Binarisation du flux vidéo //Binarisation du flux vidéo
traitement(frame, hsv_frame, threshold, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV); traitement(frame, hsv_frame, threshold, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
// Calculate the moments to estimate the position of the ball // Calculate the moments to estimate the position of the ball
Position_moy(threshold, &posX, &posY); Position_moy(threshold, &posX, &posY);
//Dessine les informations de tracking sur frame //Dessine les informations de tracking sur frame
Affichage_Tracking(frame, posX, posY, width, height); Affichage_Tracking(frame, posX, posY, width, height);
#ifdef SFML #ifdef SFML
//Affichage SFML //Affichage SFML
/* Clear the screen */ /* Clear the screen */
window.clear(sf::Color::Black); window.clear(sf::Color::Black);
//Conversion de la frame en image smfl //Conversion de la frame en image smfl
if(image_CV2SFML(frame, imFlux)){ if(image_CV2SFML(frame, imFlux)){
printf("Erreur conversion OpenCV-SFML\n"); printf("Erreur conversion OpenCV-SFML\n");
break; break;
} }
spFlux.setTexture(txFlux); spFlux.setTexture(txFlux);
window.draw(spFlux); window.draw(spFlux);
/* Update the window */ /* Update the window */
//window.display(); //window.display();
//sfSprite_destroy(sprite); //sfSprite_destroy(sprite);
//sfTexture_destroy(texture); //sfTexture_destroy(texture);
#endif #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 #ifdef CONFIG
affichage_config(frame, hsv_frame, threshold); //Affichage du flux vidéo et de ses traitements affichage_config(frame, hsv_frame, threshold); //Affichage du flux vidéo et de ses traitements
if( (cvWaitKey(10) ) >= 0 ) break; //Arret capture if( (cvWaitKey(10) ) >= 0 ) break; //Arret capture
#endif #endif
}
}
//cvWaitKey(0); //Fin programme //cvWaitKey(0); //Fin programme
// Release the capture device housekeeping // Release the capture device housekeeping
cvReleaseCapture( &capture ); cvReleaseCapture( &capture );
cvReleaseImage(&threshold); cvReleaseImage(&threshold);
cvReleaseImage(&hsv_frame); cvReleaseImage(&hsv_frame);
cvReleaseImage(&frame); cvReleaseImage(&frame);
return EXIT_SUCCESS; return EXIT_SUCCESS;
} }
/*On se rapproche de (vecX, vecY) si la position se situe en dehors d'un cercle centre sur la camera*/
void controle_moteur(int vecX, int vecY, int rayon){
int val_pwm[2]; 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
/*Lecture valeur*/ //Ajout d'un angle moteur pondéré par la distance
FILE* fichier = NULL; angle[0] += coeffx;
fichier = fopen("/dev/ttyACM0","r"); angle[1] += coeffy;
if(fichier==NULL){
printf("Erreur ouverture fichier\n"); //Majoration - minoration des angles moteurs
return ; 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;
}
}
fscanf(fichier,"%d,%d",&val_pwm[0],&val_pwm[1]); void controle_moteur(int* angle){
fclose(fichier); //Ouverture port serie
FILE* fichier = NULL;
/*Ecriture nouvelle valeur*/
fichier = fopen("/dev/ttyACM0","w"); fichier = fopen("/dev/ttyACM0","w");
if(fichier==NULL){ if(fichier==NULL){
printf("Erreur ouverture fichier\n"); 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); fclose(fichier);
return; 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){ int image_CV2SFML(IplImage* imcv, sf::Image imFlux){
@ -251,11 +269,11 @@ void traitement(IplImage* frame, IplImage* HSV, IplImage* Binaire, int LowH, int
//Binarisation //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 //cvSmooth( Binaire, Binaire, CV_GAUSSIAN, 9, 9 ); //Legère suppression des parasites
} }

BIN
Code/KirbyTrack.o Normal file
View file

Binary file not shown.

BIN
Test moteurs/moteur
View file

Binary file not shown.

2
Test moteurs/moteur.c
View file

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