Calcul d'incrémentation pour compteur
Bonjour à tous,
Je galère sur un petit bout de mon programme et là j'avoue que sans aide extérieure, des idées fraîches , je n'y arriverai pas.
Pour le background je peux dire que mon projet est un petit robot qui met en œuvre 2 compteurs (roue droite et gauche) qui servent à compter la distance et le sens de rotation. Ces compteurs comptent entre 0 et 65535. Si ils arrivent à 65535 ils retournent à 0. De la même manière si une roue recule le compteur décompte.
J'ai donc besoin d'une variable qui évolue entre -32767 et 32767 pour l'asservissement en position et, c'est là qu'est mon problème, d'une seconde variable qui évolue toujours positivement (même si le robot recule et que les compteurs décomptent) pour l'asservissement en vitesse.
par exemple si le compteur évolue de la sorte : 1, 2, 4, 3, 1, 65530, 65500
alors la variable pour l'asser en position donne: 1, 2, 4, 3, 1, -5, -35
puis la variable pour l'asserv en vitesse donne: 1, 2, 4, 5, 7, 12, 42
(Légende : Rouge : valeur qui croît; Bleu : valeur qui décroît)
Voici la fonction que j'ai réalisé jusqu'à présent. Pour incrémenter constamment ces variables de vitesse (CmptVD et CmptVG) j'ai tenté une comparaison entre l'itération actuelle et précédente mais la variable décroit tout de même. C'est un échec.
Code:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
|
//---------------------------------------------------------------------------
// FONCTION COMPTAGE -
// En entrée : compteurs hardware ls7366 allant de 0 à 65535 -
// En sortie : CmptD, CmptG allant de -32767 à +32767 -
// CmptVG, CmptVG allant de 0 à 65535 quelque soit le sens -
//
// [0]------------------------[65535]> -
// <[-32767]----------[0]----------[32767]> -
//---------------------------------------------------------------------------
void Comptage(void)
{
CmptD = Qcntr_Read(QcntrD); // charger le compteur Droit
CmptG = Qcntr_Read(QcntrG);
CmptVD = CmptD;
CmptVG = CmptG;
//---------- variable asservissement position
if (Qcntr_Read_Sens(QcntrD) == 0) // AVANCER
{
if (CmptD > 32767){CmptD = 0;} // et dépasser 32767 = retour à 0
}
else // RECULER
{
if (CmptD > 32767){CmptD = CmptD - 65535;} // et dépasser 32767 = compter sous 0
if (CmptD < -32767){CmptD = 0;}
}
if (Qcntr_Read_Sens(QcntrG) == 0) // AVANCER
{
if (CmptG > 32767){CmptG = 0;} // et dépasser 32767
}
else // RECULER
{
if (CmptG > 32767){CmptG = CmptG - 65535;} // et dépasser 32767
if (CmptG < -32767){CmptG = 0;}
}
//---------- variable asservissement vitesse
if (CmptD < OldCmptD ) { CmptVD += CmptVD + (OldCmptD - CmptD) ; } else {CmptVD = CmptD;} // calculer compteur vitesse
OldCmptD = CmptD; // actualiser itération précédente
} |
Merci à vous!
