Jonas DOREL / Mbed 2 deprecated Moteurs

Moteurs

Dependencies:   Encoder_Nucleo_16_bits PwmIn mbed

Fork of Automate by Jonas DOREL

function_library.h@9:168226ff8f76, 2017-06-09 (annotated)

Committer:
DOREL
Date:
Fri Jun 09 19:38:56 2017 +0000
Revision:
9:168226ff8f76
Parent:
8:ad8b64ca548d 
Moteurs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DOREL 8:ad8b64ca548d 1 //Variables
DOREL 8:ad8b64ca548d 2
DOREL 8:ad8b64ca548d 3 Serial Pixy (PA_0, PA_1, 230400);
DOREL 8:ad8b64ca548d 4 Serial Pc (PA_2, PA_3, 921600);
DOREL 8:ad8b64ca548d 5
DOREL 8:ad8b64ca548d 6 AnalogIn CNY1 (PC_4);
DOREL 8:ad8b64ca548d 7 AnalogIn CNY2 (PA_7);
DOREL 8:ad8b64ca548d 8 AnalogIn CNY3 (PA_6);
DOREL 8:ad8b64ca548d 9 AnalogIn LD1 (PC_0);
DOREL 8:ad8b64ca548d 10 AnalogIn LD2 (PC_1);
DOREL 8:ad8b64ca548d 11 AnalogIn SD1 (PB_0);
DOREL 8:ad8b64ca548d 12 AnalogIn SD2 (PA_4);
DOREL 8:ad8b64ca548d 13 AnalogIn Vbat (PC_5);
DOREL 8:ad8b64ca548d 14
DOREL 8:ad8b64ca548d 15 DigitalOut Led1 (PA_5);
DOREL 8:ad8b64ca548d 16 DigitalOut Led2 (PD_2);
DOREL 8:ad8b64ca548d 17 DigitalOut Trig1 (PC_3);
DOREL 8:ad8b64ca548d 18 DigitalOut Trig2 (PC_2);
DOREL 8:ad8b64ca548d 19 DigitalOut Trig3 (PA_9);
DOREL 8:ad8b64ca548d 20 DigitalOut En (PC_9);
DOREL 8:ad8b64ca548d 21 DigitalOut SensG (PC_8);
DOREL 8:ad8b64ca548d 22 DigitalOut SensD (PC_6);
DOREL 8:ad8b64ca548d 23 DigitalOut SS (PA_12);
DOREL 8:ad8b64ca548d 24
DOREL 8:ad8b64ca548d 25 InterruptIn Echo1 (PA_10);
DOREL 8:ad8b64ca548d 26 InterruptIn Echo2 (PA_11);
DOREL 8:ad8b64ca548d 27 InterruptIn Echo3 (PB_12);
DOREL 8:ad8b64ca548d 28 InterruptIn BP (PC_13);
DOREL 8:ad8b64ca548d 29 InterruptIn IG (PC_7);
DOREL 8:ad8b64ca548d 30 InterruptIn PWMG (PB_2);
DOREL 8:ad8b64ca548d 31 InterruptIn PWMD (PB_1);
DOREL 8:ad8b64ca548d 32
DOREL 8:ad8b64ca548d 33 PwmIn PWMB (PA_15);
DOREL 8:ad8b64ca548d 34
DOREL 8:ad8b64ca548d 35 PwmOut Pwm_MG (PB_10);
DOREL 8:ad8b64ca548d 36 PwmOut Pwm_MD (PB_3);
DOREL 8:ad8b64ca548d 37 PwmOut Servo (PA_8);
DOREL 8:ad8b64ca548d 38
DOREL 8:ad8b64ca548d 39 I2C Bus_I2C (PB_9, PB_8);
DOREL 8:ad8b64ca548d 40
DOREL 8:ad8b64ca548d 41 SPI SPIG (PC_12, PC_11, PC_10);
DOREL 8:ad8b64ca548d 42 SPI SPID (PB_15, PB_14, PB_13);
DOREL 8:ad8b64ca548d 43
DOREL 8:ad8b64ca548d 44 Nucleo_Encoder_16_bits Gauche (TIM4); // A = PB_6, B = PB_7
DOREL 8:ad8b64ca548d 45 Nucleo_Encoder_16_bits Droite (TIM3); // A = PB_4, B = PB_5
DOREL 8:ad8b64ca548d 46
DOREL 8:ad8b64ca548d 47 Ticker tick;
DOREL 8:ad8b64ca548d 48 Timer times;
DOREL 8:ad8b64ca548d 49
DOREL 8:ad8b64ca548d 50 /** Liste des variables globales
DOREL 8:ad8b64ca548d 51 *
DOREL 8:ad8b64ca548d 52 * Tick -> (long) Compte le nombre de MS écoulé et déclenche l'exécution de la boucle en fonction du temps écoulé.
DOREL 8:ad8b64ca548d 53 * FlagIG -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue gauche
DOREL 8:ad8b64ca548d 54 * FlagID -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue droite
DOREL 8:ad8b64ca548d 55 * EchoXStart -> (long) Donne le temps en µs de début de l'impulsion d'écho de l'US n°X
DOREL 8:ad8b64ca548d 56 * DistUSX -> (float) Donne la distance en mm mesurée par l'US n°X
DOREL 8:ad8b64ca548d 57 */
DOREL 8:ad8b64ca548d 58
DOREL 8:ad8b64ca548d 59 // Structure de temps
DOREL 8:ad8b64ca548d 60 lWord Tick = 0, Gperiod, Dperiod;
DOREL 8:ad8b64ca548d 61
DOREL 8:ad8b64ca548d 62 // Sémaphore d'interruption
DOREL 8:ad8b64ca548d 63 int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0;
DOREL 8:ad8b64ca548d 64 int FlagTick = 0, FlagTickLed = 0, BPPressed = 0, nbTurnG = 0, nbTurnD = 0;
DOREL 8:ad8b64ca548d 65 int Pixy_check = -1;
DOREL 8:ad8b64ca548d 66
DOREL 8:ad8b64ca548d 67 // Dialogue avec la Pixy
DOREL 8:ad8b64ca548d 68 T_pixyCCData Pixy_CCFIFO[20];
DOREL 8:ad8b64ca548d 69 T_pixyNMData Pixy_NMFIFO[20];
DOREL 8:ad8b64ca548d 70 Byte Pixy_CCObjet, Pixy_NMObjet;
DOREL 8:ad8b64ca548d 71
DOREL 8:ad8b64ca548d 72 // Gestion des capteurs Ultrason
DOREL 8:ad8b64ca548d 73 long Echo1Start, Echo2Start, Echo3Start;
DOREL 8:ad8b64ca548d 74 double DistUS1, DistUS2, DistUS3;
DOREL 8:ad8b64ca548d 75
DOREL 8:ad8b64ca548d 76 /** Liste des interruptions
DOREL 8:ad8b64ca548d 77 *
DOREL 8:ad8b64ca548d 78 */
DOREL 8:ad8b64ca548d 79
DOREL 8:ad8b64ca548d 80 void tickTime() {
DOREL 8:ad8b64ca548d 81 Tick++;
DOREL 8:ad8b64ca548d 82 FlagTick = 1;
DOREL 8:ad8b64ca548d 83 if ((Tick%100)==0) FlagTickLed = 1;
DOREL 8:ad8b64ca548d 84 }
DOREL 8:ad8b64ca548d 85
DOREL 8:ad8b64ca548d 86 void BPevent () {
DOREL 8:ad8b64ca548d 87 BPPressed = 1;
DOREL 8:ad8b64ca548d 88 }
DOREL 8:ad8b64ca548d 89
DOREL 8:ad8b64ca548d 90 void PWM_motGRise (void) {
DOREL 8:ad8b64ca548d 91 static lWord oldTime;
DOREL 8:ad8b64ca548d 92 lWord now;
DOREL 8:ad8b64ca548d 93 now = times.read_us();
DOREL 8:ad8b64ca548d 94 Gperiod = now-oldTime;
DOREL 8:ad8b64ca548d 95 oldTime = now;
DOREL 8:ad8b64ca548d 96 nbTurnG++;
DOREL 8:ad8b64ca548d 97 }
DOREL 8:ad8b64ca548d 98
DOREL 8:ad8b64ca548d 99 void PWM_motDRise (void) {
DOREL 8:ad8b64ca548d 100 static lWord oldTime;
DOREL 8:ad8b64ca548d 101 lWord now;
DOREL 8:ad8b64ca548d 102 now = times.read_us();
DOREL 8:ad8b64ca548d 103 Dperiod = now-oldTime;
DOREL 8:ad8b64ca548d 104 oldTime = now;
DOREL 8:ad8b64ca548d 105 nbTurnD++;
DOREL 8:ad8b64ca548d 106 }
DOREL 8:ad8b64ca548d 107
DOREL 8:ad8b64ca548d 108 void echo1Rise () {
DOREL 8:ad8b64ca548d 109 Echo1Start = times.read_us();
DOREL 8:ad8b64ca548d 110 }
DOREL 8:ad8b64ca548d 111
DOREL 8:ad8b64ca548d 112 void echo2Rise () {
DOREL 8:ad8b64ca548d 113 Echo2Start = times.read_us();
DOREL 8:ad8b64ca548d 114 }
DOREL 8:ad8b64ca548d 115
DOREL 8:ad8b64ca548d 116 void echo3Rise () {
DOREL 8:ad8b64ca548d 117 Echo3Start = times.read_us();
DOREL 8:ad8b64ca548d 118 }
DOREL 8:ad8b64ca548d 119
DOREL 8:ad8b64ca548d 120 void echo1Fall () {
DOREL 8:ad8b64ca548d 121 DistUS1 = (double)(times.read_us() - Echo1Start)/5.8;
DOREL 8:ad8b64ca548d 122 FlagUS1 = 1;
DOREL 8:ad8b64ca548d 123 }
DOREL 8:ad8b64ca548d 124
DOREL 8:ad8b64ca548d 125 void echo2Fall () {
DOREL 8:ad8b64ca548d 126 DistUS2 = (double)(times.read_us() - Echo2Start)/5.8;
DOREL 8:ad8b64ca548d 127 FlagUS2 = 1;
DOREL 8:ad8b64ca548d 128 }
DOREL 8:ad8b64ca548d 129
DOREL 8:ad8b64ca548d 130 void echo3Fall () {
DOREL 8:ad8b64ca548d 131 DistUS3 = (double)(times.read_us() - Echo3Start)/5.8;
DOREL 8:ad8b64ca548d 132 FlagUS3 = 1;
DOREL 8:ad8b64ca548d 133 }
DOREL 8:ad8b64ca548d 134
DOREL 8:ad8b64ca548d 135 void getPixyByte () {
DOREL 8:ad8b64ca548d 136 static T_tmpBuffer tmpBuffer;
DOREL 8:ad8b64ca548d 137 static T_structBuffer msgBuffer;
DOREL 8:ad8b64ca548d 138 static T_pixyState PIXY_state = none;
DOREL 8:ad8b64ca548d 139 static Byte byteCount = 0;
DOREL 8:ad8b64ca548d 140 static int PIXY_synced = 0, dummy;
DOREL 8:ad8b64ca548d 141 int i, somme;
DOREL 8:ad8b64ca548d 142 static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0;
DOREL 8:ad8b64ca548d 143 static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0;
DOREL 8:ad8b64ca548d 144
DOREL 8:ad8b64ca548d 145 Pixy_check = 0;
DOREL 8:ad8b64ca548d 146
DOREL 8:ad8b64ca548d 147 if (!PIXY_synced) { // On n'a pas trouvé le START (0x55aa0000)
DOREL 8:ad8b64ca548d 148 tmpBuffer.tab[byteCount] = Pixy.getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire
DOREL 8:ad8b64ca548d 149
DOREL 8:ad8b64ca548d 150 if (byteCount < 3) { // Si on n'a pas encore reçu les 4 premier octets
DOREL 8:ad8b64ca548d 151 byteCount++; // On passe à la case suivante du tableau temporaire
DOREL 8:ad8b64ca548d 152 } else { // Lorsqu'on a 4 octets
DOREL 8:ad8b64ca548d 153 if (tmpBuffer.mot != 0xaa550000) { // Si le code n'est pas le code de START
DOREL 8:ad8b64ca548d 154 for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau
DOREL 8:ad8b64ca548d 155 byteCount = 3; // Et on attend le caractère suivant
DOREL 8:ad8b64ca548d 156 } else { // Si on a trouvé le code de START
DOREL 8:ad8b64ca548d 157 PIXY_synced = 1; // On passe en mode synchronisé
DOREL 8:ad8b64ca548d 158 PIXY_state = begin;
DOREL 8:ad8b64ca548d 159 byteCount = 0;
DOREL 8:ad8b64ca548d 160 }
DOREL 8:ad8b64ca548d 161 }
DOREL 8:ad8b64ca548d 162 }
DOREL 8:ad8b64ca548d 163
DOREL 8:ad8b64ca548d 164 if (PIXY_synced) {
DOREL 8:ad8b64ca548d 165
DOREL 8:ad8b64ca548d 166 switch (PIXY_state) {
DOREL 8:ad8b64ca548d 167 case begin : // l'aiguillage est là !
DOREL 8:ad8b64ca548d 168
DOREL 8:ad8b64ca548d 169 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus
DOREL 8:ad8b64ca548d 170 byteCount++;
DOREL 8:ad8b64ca548d 171 if (byteCount == 2) { // Quand on a 2 octets
DOREL 8:ad8b64ca548d 172
DOREL 8:ad8b64ca548d 173 if (msgBuffer.mot == 0xaa55) { // Si c'est un bloc normal (code 0xAA55)
DOREL 8:ad8b64ca548d 174 PIXY_state = normal; // On part vers le traitement spécifique
DOREL 8:ad8b64ca548d 175 }
DOREL 8:ad8b64ca548d 176
DOREL 8:ad8b64ca548d 177 if (msgBuffer.mot == 0xaa56) { // Si c'est un bloc Color Code (code 0xAA56)
DOREL 8:ad8b64ca548d 178 PIXY_state = colorCode; // On part vers le traitement spécifique
DOREL 8:ad8b64ca548d 179 }
DOREL 8:ad8b64ca548d 180
DOREL 8:ad8b64ca548d 181 if (msgBuffer.mot == 0) { // Si on a un debut de trame (code 0000)
DOREL 8:ad8b64ca548d 182 PIXY_state = doubleZero; // On part vers le traitement spécifique
DOREL 8:ad8b64ca548d 183 }
DOREL 8:ad8b64ca548d 184 if ((PIXY_state == begin) || (PIXY_state == none)) { // Si c'est autre chose
DOREL 8:ad8b64ca548d 185 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
DOREL 8:ad8b64ca548d 186 PIXY_state = none; // Ceinture et bretelle
DOREL 8:ad8b64ca548d 187 }
DOREL 8:ad8b64ca548d 188 byteCount = 0;
DOREL 8:ad8b64ca548d 189 }
DOREL 8:ad8b64ca548d 190 break;
DOREL 8:ad8b64ca548d 191
DOREL 8:ad8b64ca548d 192 case normal : // Si on a un bloc normal
DOREL 8:ad8b64ca548d 193
DOREL 8:ad8b64ca548d 194 Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = Pixy.getc(); // On stocke les octets un à un dans la structure Bloc
DOREL 8:ad8b64ca548d 195 if (byteCount < 11) { // Tant que la structure n'est pas pleine
DOREL 8:ad8b64ca548d 196 byteCount++; // On passe à l'octet suivant
DOREL 8:ad8b64ca548d 197 } else { // Quand elle est pleine
DOREL 8:ad8b64ca548d 198 byteCount = 0; // On réinitialise
DOREL 8:ad8b64ca548d 199 PIXY_state = begin; // On retourne à l'aiguillage
DOREL 8:ad8b64ca548d 200 // On calcule la somme de contrôle
DOREL 8:ad8b64ca548d 201 somme = Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.signature + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.x + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.y + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.width + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.height;
DOREL 8:ad8b64ca548d 202
DOREL 8:ad8b64ca548d 203 if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) { // Si le checksum est bon, on valide la réception
DOREL 8:ad8b64ca548d 204 if (PIXY_wNMObjet < 19) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet
DOREL 8:ad8b64ca548d 205 else PIXY_wNMObjet = 0;
DOREL 8:ad8b64ca548d 206 if (PIXY_nbNMObjet < 19) PIXY_nbNMObjet++; // On dit que l'on a un objet CC de plus
DOREL 8:ad8b64ca548d 207 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
DOREL 8:ad8b64ca548d 208 }
DOREL 8:ad8b64ca548d 209 }
DOREL 8:ad8b64ca548d 210 break;
DOREL 8:ad8b64ca548d 211
DOREL 8:ad8b64ca548d 212 case colorCode : // Si on a un bloc colorCode
DOREL 8:ad8b64ca548d 213
DOREL 8:ad8b64ca548d 214 Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc
DOREL 8:ad8b64ca548d 215 if (byteCount < 13) byteCount++; // tant que la structure n'est pas pleine on passe à l'octet suivant
DOREL 8:ad8b64ca548d 216 else { // Quand elle est pleine
DOREL 8:ad8b64ca548d 217 byteCount = 0; // On réinitialise
DOREL 8:ad8b64ca548d 218 PIXY_state = begin; // On retourne à l'aiguillage
DOREL 8:ad8b64ca548d 219 // On calcule la somme de contrôle
DOREL 8:ad8b64ca548d 220 somme = Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.signature + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.x + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.y + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.width + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.height + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.angle;
DOREL 8:ad8b64ca548d 221
DOREL 8:ad8b64ca548d 222 if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) { // Si le checksum est bon
DOREL 8:ad8b64ca548d 223 if (PIXY_wCCObjet < 19) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet
DOREL 8:ad8b64ca548d 224 else PIXY_wCCObjet = 0;
DOREL 8:ad8b64ca548d 225 if (PIXY_nbCCObjet < 19) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter
DOREL 8:ad8b64ca548d 226 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
DOREL 8:ad8b64ca548d 227 }
DOREL 8:ad8b64ca548d 228 }
DOREL 8:ad8b64ca548d 229 break;
DOREL 8:ad8b64ca548d 230
DOREL 8:ad8b64ca548d 231 case doubleZero : // Si on a reçu le code de début d'une nouvelle trame.
DOREL 8:ad8b64ca548d 232
DOREL 8:ad8b64ca548d 233 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus
DOREL 8:ad8b64ca548d 234 byteCount++;
DOREL 8:ad8b64ca548d 235 if (byteCount == 2) { // Quand on a 2 octets
DOREL 8:ad8b64ca548d 236 if (msgBuffer.mot == 0xaa55) { // On doit impérativement trouver le code 0xAA55
DOREL 8:ad8b64ca548d 237 PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage
DOREL 8:ad8b64ca548d 238 Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement
DOREL 8:ad8b64ca548d 239 Pixy_CCObjet = PIXY_nbCCObjet;
DOREL 8:ad8b64ca548d 240 PIXY_nbCCObjet = 0;
DOREL 8:ad8b64ca548d 241 PIXY_nbNMObjet = 0;
DOREL 8:ad8b64ca548d 242 FlagPixy = 1; // On valide le traitement de la trame précédente.
DOREL 8:ad8b64ca548d 243 } else { // Si on trouve autre chose
DOREL 8:ad8b64ca548d 244 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
DOREL 8:ad8b64ca548d 245 PIXY_state = none; // Ceinture et bretelle
DOREL 8:ad8b64ca548d 246 }
DOREL 8:ad8b64ca548d 247 byteCount = 0;
DOREL 8:ad8b64ca548d 248 }
DOREL 8:ad8b64ca548d 249 break;
DOREL 8:ad8b64ca548d 250 }
DOREL 8:ad8b64ca548d 251 }
DOREL 8:ad8b64ca548d 252 }
DOREL 8:ad8b64ca548d 253
DOREL 8:ad8b64ca548d 254 void ir_read(double SD2,double LD2,double *SD2_dist,double *LD2_dist) {
DOREL 8:ad8b64ca548d 255 double SD2_val, LD2_val;
DOREL 8:ad8b64ca548d 256
DOREL 8:ad8b64ca548d 257 SD2_val = SD2;
DOREL 8:ad8b64ca548d 258 LD2_val = LD2;
DOREL 8:ad8b64ca548d 259
DOREL 8:ad8b64ca548d 260 if (SD2_val < 0.06) {
DOREL 8:ad8b64ca548d 261 SD2_val = 0;
DOREL 8:ad8b64ca548d 262 *SD2_dist = 40;
DOREL 8:ad8b64ca548d 263 } else {
DOREL 8:ad8b64ca548d 264 *SD2_dist = 11.611/(SD2_val*3.3-0.0237);
DOREL 8:ad8b64ca548d 265 if (*SD2_dist > 40) *SD2_dist = 40;
DOREL 8:ad8b64ca548d 266 }
DOREL 8:ad8b64ca548d 267
DOREL 8:ad8b64ca548d 268 if (LD2_val < 0.1) {
DOREL 8:ad8b64ca548d 269 LD2_val = 0;
DOREL 8:ad8b64ca548d 270 *LD2_dist = 150;
DOREL 8:ad8b64ca548d 271 } else {
DOREL 8:ad8b64ca548d 272 *LD2_dist = 59.175/(LD2_val*3.3-0.0275);
DOREL 8:ad8b64ca548d 273 if (*LD2_dist > 150) *LD2_dist = 150;
DOREL 8:ad8b64ca548d 274 }
DOREL 9:168226ff8f76 275 }
DOREL 9:168226ff8f76 276
DOREL 9:168226ff8f76 277 void motor_command(int mg_command, int md_command, int* mg_pwm, int* md_pwm, int* mg_sens, int* md_sens) {
DOREL 9:168226ff8f76 278 if(mg_command > 0) {
DOREL 9:168226ff8f76 279 *mg_sens = 1;
DOREL 9:168226ff8f76 280 *mg_pwm = mg_command;
DOREL 9:168226ff8f76 281 } else {
DOREL 9:168226ff8f76 282 *mg_sens = -1;
DOREL 9:168226ff8f76 283 *mg_pwm = -mg_command;
DOREL 9:168226ff8f76 284 }
DOREL 9:168226ff8f76 285 if(md_command > 0) {
DOREL 9:168226ff8f76 286 *mg_sens = 1;
DOREL 9:168226ff8f76 287 *md_pwm = md_command;
DOREL 9:168226ff8f76 288 } else {
DOREL 9:168226ff8f76 289 *md_sens = -1;
DOREL 9:168226ff8f76 290 *md_pwm = md_command;
DOREL 9:168226ff8f76 291 }
DOREL 8:ad8b64ca548d 292 }