-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathTask_control.c
195 lines (151 loc) · 6.76 KB
/
Task_control.c
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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
/*********************************************************************
* FileName: Task_control.c
* Dependencies: main.h
* Processor: PIC32MX795F512L
* Compiler: Microchip C32 v1.11A or higher
*
* Overview: Tâche responsable des différents lois de commande
********************************************************************/
#include "main.h"
/********************************************************************
* Tache: void TaskControl(void *pvParameters)
*
* Overview: Tâche responsable des différents lois de commande.
* Asservissement de position pour la direction, asservissement de
* courant (couple) pour la propulsion et supervision du niveau de
* batterie. Une fois les traitement fais, elle est responsable du
* post des grandeurs intermédaire pour le debug à la tâche d'affichage
*
* Auteur Date Commentaire
*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
* Théou Jean-Baptiste 7 avril 2011 Première version
* Descoubes hugo 16 mai 2011 vs 1.1
*******************************************************************/
void TaskControl(void *pvParameters){
/* asservissement de position */
short sDirMeasure; // Mesure de position (servomoteur). Unsigned integer sur 10 bits
short sDirConsigne; // Consigne de position (servomoteur). Unsigned integer sur 10 bits
short sDirCommand; // Commande de position (servomoteur). entier compris entre 0 et 100, rapport cyclique pour PWM
/* asservissement de courant */
float fPropMeasure; // Mesure image du courant absorbé par la MCC (propulsion).
short sPropConsigne; // Consigne de courant(propulsion). Unsigned integer sur 10 bits
short sPropCommand; // Commande de courant (propulsion). entier compris entre 0 et 100, rapport cyclique pour PWM
short sPowerMeasure; // Mesure du niveau de batterie. Unsigned integer sur 10 bits
char stateControl = PROPULSION_CONTROL;// machnie d'état pour la commande
struct_DebugPrint printData; // Données à afficher (debug)
struct_mesures measuresReceive; // Mesures reçues depuis l'ISR du timer 3
for( ;; ){
/* Récupération des grandeurs de mesures pour les lois de commande */
xQueueReceive( xQueueAcquiData, &measuresReceive, portMAX_DELAY); // fonction bloquante
/* Mise à jour mesures */
sDirMeasure = measuresReceive.sDirMeasure;
fPropMeasure = (float) measuresReceive.sCurrentMeasure / 35.0; // 35 = facteur d'échelle (capteur + conditionneur + ADC)
sPowerMeasure = measuresReceive.sBattMeasure;
/* Mise à jour mesures - Affichage par le réseau */
resultat = sDirMeasure; // Mise à jour var. globale réseau
resultat_courant = measuresReceive.sCurrentMeasure; // Mise à jour var. globale réseau
/* Mise à jour consignes */
/* Protection Vitesse*/
xSemaphoreTake(xSemaphoreVitesse,portMAX_DELAY);
{
sPropConsigne = Vitesse; // récupération var. globale réseau
}
xSemaphoreGive(xSemaphoreVitesse);
/* Protection ConsDir */
xSemaphoreTake(xSemaphoreConsDir,portMAX_DELAY);
{
sDirConsigne = ConsDir; // récupération var. globale réseau
}
xSemaphoreGive(xSemaphoreConsDir);
/* Machine d'état - tâche contrôle/commande/supervision */
switch(stateControl){
case PROPULSION_CONTROL :
/* Protection Consigne courant */
if(sPropConsigne > 100){
sPropConsigne = 100;
}
else if(sPropConsigne < 0){
sPropConsigne = 0;
}
/* Asservissement de courant (couple). Régulation PI (modèle non-linéaire) */
/* Calcul fais avec des flottant - temps de traitement long */
sPropCommand = propulsionCommand (sPropConsigne , fPropMeasure);
case PROPULSION_PWM :
if((fPropMeasure < 0.0) || (fPropMeasure > MAX_CURRENT) ){
sPropCommand = 0;
}
/* Mise à jour rapport cyclique pour module PWM propulsion */
/* Protection au niveau des erreurs de calculs */
if(sPropCommand < 10)
{
sPropCommand = 0;
}
SetDCOC4PWM(ReadPeriod3() * sPropCommand / 100 );
case DIRECTION_CONTROL :
/* Protection Consigne direction */
if(sDirConsigne > HAUT_BUTE){
sDirConsigne = HAUT_BUTE;
}
else if(sDirConsigne < BAS_BUTE){
sDirConsigne = BAS_BUTE;
}
/* Limitation sur la var globale ConsDir fait localement sur le MCU ... pour le moment ! */
/* Protection ConsDir */
xSemaphoreTake(xSemaphoreConsDir,portMAX_DELAY);
{
ConsDir = sDirConsigne;
}
xSemaphoreGive(xSemaphoreConsDir);
/* Asservissement de position. Régulation proporionnelle (modèle grandement non-linéaire) */
sDirCommand = directionCommand(sDirConsigne, sDirMeasure);
case DIRECTION_PWM :
/* Vérification butées direction */
if( sDirMeasure < HAUT_BUTE && sDirMeasure > BAS_BUTE){
if(init_ok == 0){
/* Attendre initialisation utilisateur via réseau */
sDirCommand = 0;
}
else{
/* valeur absolue et sens de rotation - PWM comprise entre 0 et 100 */
if ( sDirCommand < 0 ){
/* Sens de rotation pour le driver de bras de pont */
PORTSetBits(BROCHE_DIR_DIRECTION);
sDirCommand = -sDirCommand;
}
else{
/* Sens de rotation pour le driver de bras de pont */
PORTClearBits(BROCHE_DIR_DIRECTION);
}
/* Protection et limitation Commande */
if(sDirCommand < 20){
sDirCommand = 0; // Pas de commande si dans dead zone
}
else if(sDirCommand < 30){
sDirCommand = 40; // compensation dead zone
}
else if (sDirCommand > 100){
sDirCommand = 100; // limitation PWM
}
}
}
else{
sDirCommand = 0;
}
/* Mise à jour rapport cyclique pour module PWM direction */
SetDCOC2PWM(ReadPeriod3() * sDirCommand / 100 );
case POWER_SUPERVIOR :
break;
default:
break;
}
/* Post les valeurs à afficher pour le debug vers la tâche d'affichage */
printData.commandeDir = sDirCommand;
printData.mesureDir = sDirMeasure;
printData.consigneDir = sDirConsigne;
printData.consigneMove = (float) sPropConsigne;
printData.mesureMove = fPropMeasure;
printData.commandeMove = sPropCommand;
printData.mesurePower = sPowerMeasure;
xQueueSend(xQueueDebugPrint, &printData, 0);
}
}