Moteurs
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of Automate by
function_library.h@8:ad8b64ca548d, 2017-06-09 (annotated)
- Committer:
- DOREL
- Date:
- Fri Jun 09 17:06:05 2017 +0000
- Revision:
- 8:ad8b64ca548d
- Child:
- 9:168226ff8f76
Automate;
Who changed what in which revision?
User | Revision | Line number | New 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 | 8:ad8b64ca548d | 275 | } |