FRC - Hackathon / Mbed 2 deprecated TestBoard_2018

Programme de test à priori fonctionnel avec la version 2018 de la carte

Dependencies:   PwmIn mbed Encoder_Nucleo_16_bits

main.cpp@5:4955cb4b3646, 2017-06-04 (annotated)

Committer:
haarkon
Date:
Sun Jun 04 15:15:19 2017 +0000
Revision:
5:4955cb4b3646
Parent:
4:f65b0905e4ce
Child:
6:88b4805d33e1
Near Fully functionnal (remaining problem with PWMIN with AS5047D).

Who changed what in which revision?

UserRevisionLine numberNew contents of line
haarkon 0:f00e68bef80c 1 /** Main Test Board
haarkon 0:f00e68bef80c 2 *
haarkon 0:f00e68bef80c 3 * \brief Programme de tests pour le robot NCR 2017
haarkon 0:f00e68bef80c 4 * \author H. Angelis
haarkon 0:f00e68bef80c 5 * \version alpha_1
haarkon 0:f00e68bef80c 6 * \date 15/05/17
haarkon 0:f00e68bef80c 7 *
haarkon 0:f00e68bef80c 8 */
haarkon 0:f00e68bef80c 9
haarkon 0:f00e68bef80c 10 #include "mbed.h"
haarkon 0:f00e68bef80c 11 #include "PwmIn.h"
haarkon 0:f00e68bef80c 12 #include "Nucleo_Encoder_16_bits.h"
haarkon 0:f00e68bef80c 13
haarkon 2:1d440e938c44 14 #define BOUSSOLE_adress 0xC0
haarkon 2:1d440e938c44 15 #define PIXY_adress 0x54
haarkon 2:1d440e938c44 16
haarkon 2:1d440e938c44 17 #define CC_BLOCSIZE 14
haarkon 2:1d440e938c44 18 #define N_BLOCSIZE 12
haarkon 2:1d440e938c44 19
haarkon 2:1d440e938c44 20 #define N_BLOCCODE 0xAA55
haarkon 2:1d440e938c44 21 #define CC_BLOCCODE 0xAA56
haarkon 0:f00e68bef80c 22
haarkon 0:f00e68bef80c 23 typedef unsigned char Byte;
haarkon 0:f00e68bef80c 24 typedef unsigned short Word;
haarkon 0:f00e68bef80c 25 typedef unsigned long lWord;
haarkon 0:f00e68bef80c 26
haarkon 0:f00e68bef80c 27 typedef enum {S_monte = 1, S_descente = 0} T_SERVODIR;
haarkon 0:f00e68bef80c 28
haarkon 2:1d440e938c44 29 typedef enum {none, begin, normal, colorCode, doubleZero} T_pixyState;
haarkon 2:1d440e938c44 30
haarkon 0:f00e68bef80c 31 typedef union {
haarkon 2:1d440e938c44 32 lWord mot;
haarkon 0:f00e68bef80c 33 Byte tab[4];
haarkon 2:1d440e938c44 34 } T_tmpBuffer;
haarkon 2:1d440e938c44 35
haarkon 2:1d440e938c44 36 typedef union {
haarkon 2:1d440e938c44 37 Word mot;
haarkon 2:1d440e938c44 38 Byte tab[2];
haarkon 2:1d440e938c44 39 } T_structBuffer;
haarkon 0:f00e68bef80c 40
haarkon 0:f00e68bef80c 41 typedef struct {
haarkon 0:f00e68bef80c 42 Word checksum;
haarkon 0:f00e68bef80c 43 Word signature;
haarkon 0:f00e68bef80c 44 Word x;
haarkon 0:f00e68bef80c 45 Word y;
haarkon 0:f00e68bef80c 46 Word width;
haarkon 0:f00e68bef80c 47 Word height;
haarkon 2:1d440e938c44 48 Word angle;
haarkon 0:f00e68bef80c 49 } T_pixyCCBloc;
haarkon 0:f00e68bef80c 50
haarkon 2:1d440e938c44 51 typedef struct {
haarkon 2:1d440e938c44 52 Word checksum;
haarkon 2:1d440e938c44 53 Word signature;
haarkon 2:1d440e938c44 54 Word x;
haarkon 2:1d440e938c44 55 Word y;
haarkon 2:1d440e938c44 56 Word width;
haarkon 2:1d440e938c44 57 Word height;
haarkon 3:3dfe26a10796 58 } T_pixyNMBloc;
haarkon 0:f00e68bef80c 59
haarkon 0:f00e68bef80c 60 typedef union {
haarkon 0:f00e68bef80c 61 Byte tab[14];
haarkon 0:f00e68bef80c 62 T_pixyCCBloc CCbloc;
haarkon 2:1d440e938c44 63 } T_pixyCCData;
haarkon 2:1d440e938c44 64
haarkon 2:1d440e938c44 65 typedef union {
haarkon 2:1d440e938c44 66 Byte tab[12];
haarkon 3:3dfe26a10796 67 T_pixyNMBloc NMbloc;
haarkon 3:3dfe26a10796 68 } T_pixyNMData;
haarkon 0:f00e68bef80c 69
haarkon 0:f00e68bef80c 70
haarkon 2:1d440e938c44 71
haarkon 0:f00e68bef80c 72 /** Liste des objets
haarkon 0:f00e68bef80c 73 *
haarkon 0:f00e68bef80c 74 * Serial #4 Pixy
haarkon 0:f00e68bef80c 75 * Serial #2 Pc
haarkon 0:f00e68bef80c 76 *
haarkon 0:f00e68bef80c 77 * AnalogIn C1, C2, C3, LD1, LD2, SD1, SD2, Vbat
haarkon 0:f00e68bef80c 78 *
haarkon 0:f00e68bef80c 79 * DigitalOut Led1, Led2, Trig1, Trig2, Trig3, En, SensG, SensD
haarkon 0:f00e68bef80c 80 *
haarkon 0:f00e68bef80c 81 * InterruptIn IndexG, IndexD, Echo1, Echo2, Echo3, BP
haarkon 0:f00e68bef80c 82 *
haarkon 0:f00e68bef80c 83 * PwmOut Pwm_MG, Pwm_MD, Servo
haarkon 0:f00e68bef80c 84 *
haarkon 0:f00e68bef80c 85 * PwmIn PWMG, PWMD, PWMB
haarkon 0:f00e68bef80c 86 *
haarkon 0:f00e68bef80c 87 * I2C Bus_I2C
haarkon 0:f00e68bef80c 88 *
haarkon 0:f00e68bef80c 89 * SPI MotG, MotD
haarkon 0:f00e68bef80c 90 *
haarkon 0:f00e68bef80c 91 * Nucleo_Encoder_16_bits Gauche, Droite
haarkon 0:f00e68bef80c 92 *
haarkon 0:f00e68bef80c 93 * Ticker timer
haarkon 0:f00e68bef80c 94 */
haarkon 0:f00e68bef80c 95
haarkon 0:f00e68bef80c 96 /** Liste des PINs
haarkon 0:f00e68bef80c 97 *
haarkon 0:f00e68bef80c 98 * PIN MAP (ordre alphabetique) des PINs de la Nucléo 64 utilisée
haarkon 0:f00e68bef80c 99 * PA_0 -> Pixy RX (Serial)
haarkon 0:f00e68bef80c 100 * PA_1 -> Pixy TX (Serial)
haarkon 0:f00e68bef80c 101 * PA_2 -> PC TX (Serial)
haarkon 0:f00e68bef80c 102 * PA_3 -> PX RX (Serial)
haarkon 0:f00e68bef80c 103 * PA_4 -> GP2 SD #2 (Analog In)
haarkon 0:f00e68bef80c 104 * PA_5 -> LED1 (Digital Out)
haarkon 0:f00e68bef80c 105 * PA_6 -> CNY3 (Analog In)
haarkon 0:f00e68bef80c 106 * PA_7 -> CNY2 (Analog In)
haarkon 0:f00e68bef80c 107 * PA_8 -> Servomoteur (PWM Out)
haarkon 0:f00e68bef80c 108 * PA_9 -> US Trigger #3 (Digital Out)
haarkon 1:d95546f84105 109 * PA_10 -> US Echo #1 (Pwm In)
haarkon 0:f00e68bef80c 110 * PA_11 -> US Echo #2 (Pwm In)
haarkon 1:d95546f84105 111 * PA_12 -> SS (SPI Slave Select) (Digital Out)
haarkon 0:f00e68bef80c 112 * PA_13
haarkon 0:f00e68bef80c 113 * PA_14
haarkon 0:f00e68bef80c 114 * PA_15 -> Boussole (Pwm In)
haarkon 0:f00e68bef80c 115 *
haarkon 0:f00e68bef80c 116 * PB_0 -> GP2 SD #1 (Analog In)
haarkon 0:f00e68bef80c 117 * PB_1 -> Position D (Pwm In)
haarkon 0:f00e68bef80c 118 * PB_2 -> Position G (Pwm In)
haarkon 0:f00e68bef80c 119 * PB_3 -> PWM Mot D (PWM Out)
haarkon 0:f00e68bef80c 120 * PB_4 -> Enocdeur Droit A (QE)
haarkon 0:f00e68bef80c 121 * PB_5 -> Enocdeur Droit B (QE)
haarkon 0:f00e68bef80c 122 * PB_6 -> Enocdeur Gauche A (QE)
haarkon 0:f00e68bef80c 123 * PB_7 -> Enocdeur Gauche B (QE)
haarkon 0:f00e68bef80c 124 * PB_8 -> SCL (I2C)
haarkon 0:f00e68bef80c 125 * PB_9 -> SDA (I2C)
haarkon 0:f00e68bef80c 126 * PB_10 -> PWM Mot G (PWM Out)
haarkon 0:f00e68bef80c 127 * PB_11
haarkon 0:f00e68bef80c 128 * PB_12 -> US Echo #3 (Pwm In)
haarkon 0:f00e68bef80c 129 * PB_13 -> SCK Encodeur D (SPI)
haarkon 0:f00e68bef80c 130 * PB_14 -> MISO Encodeur D (SPI)
haarkon 0:f00e68bef80c 131 * PB_15 -> MOSI Encodeur D (SPI)
haarkon 0:f00e68bef80c 132 *
haarkon 0:f00e68bef80c 133 * PC_0 -> GP2 LD #1 (Analog In)
haarkon 0:f00e68bef80c 134 * PC_1 -> GP2 LD #2 (Analog In)
haarkon 0:f00e68bef80c 135 * PC_2 -> US Trigger #2 (Digital Out)
haarkon 0:f00e68bef80c 136 * PC_3 -> US Trigger #1 (Digital Out)
haarkon 0:f00e68bef80c 137 * PC_4 -> CNY1 (Analog In)
haarkon 0:f00e68bef80c 138 * PC_5 -> Vbat (Analog In)
haarkon 0:f00e68bef80c 139 * PC_6 -> Dir Mot Droit (Digital Out)
haarkon 0:f00e68bef80c 140 * PC_7 -> I (Encodeur Gauche) (IRQ In)
haarkon 0:f00e68bef80c 141 * PC_8 -> Dir Mot Gauche (Digital Out)
haarkon 0:f00e68bef80c 142 * PC_9 -> Enable Moteurs (Digital Out)
haarkon 0:f00e68bef80c 143 * PC_10 -> SCK Encodeur G (SPI)
haarkon 0:f00e68bef80c 144 * PC_11 -> MISO Encodeur G (SPI)
haarkon 0:f00e68bef80c 145 * PC_12 -> MOSI Encodeur G (SPI)
haarkon 0:f00e68bef80c 146 * PC_13 -> User BP (IRQ In)
haarkon 0:f00e68bef80c 147 * PC_14
haarkon 0:f00e68bef80c 148 * PC_15
haarkon 0:f00e68bef80c 149 *
haarkon 0:f00e68bef80c 150 * PD_1
haarkon 0:f00e68bef80c 151 * PD_2 -> Led2 (Digital Out)
haarkon 0:f00e68bef80c 152 */
haarkon 0:f00e68bef80c 153
haarkon 1:d95546f84105 154 Serial Pixy (PA_0, PA_1, 230400);
haarkon 5:4955cb4b3646 155 Serial Pc (PA_2, PA_3, 921600);
haarkon 0:f00e68bef80c 156
haarkon 0:f00e68bef80c 157 AnalogIn CNY1 (PC_4);
haarkon 0:f00e68bef80c 158 AnalogIn CNY2 (PA_7);
haarkon 0:f00e68bef80c 159 AnalogIn CNY3 (PA_6);
haarkon 0:f00e68bef80c 160 AnalogIn LD1 (PC_0);
haarkon 0:f00e68bef80c 161 AnalogIn LD2 (PC_1);
haarkon 0:f00e68bef80c 162 AnalogIn SD1 (PB_0);
haarkon 0:f00e68bef80c 163 AnalogIn SD2 (PA_4);
haarkon 0:f00e68bef80c 164 AnalogIn Vbat (PC_5);
haarkon 0:f00e68bef80c 165
haarkon 0:f00e68bef80c 166 DigitalOut Led1 (PA_5);
haarkon 0:f00e68bef80c 167 DigitalOut Led2 (PD_2);
haarkon 0:f00e68bef80c 168 DigitalOut Trig1 (PC_3);
haarkon 0:f00e68bef80c 169 DigitalOut Trig2 (PC_2);
haarkon 0:f00e68bef80c 170 DigitalOut Trig3 (PA_9);
haarkon 0:f00e68bef80c 171 DigitalOut En (PC_9);
haarkon 0:f00e68bef80c 172 DigitalOut SensG (PC_8);
haarkon 0:f00e68bef80c 173 DigitalOut SensD (PC_6);
haarkon 1:d95546f84105 174 DigitalOut SS (PA_12);
haarkon 0:f00e68bef80c 175
haarkon 1:d95546f84105 176 InterruptIn Echo1 (PA_10);
haarkon 0:f00e68bef80c 177 InterruptIn Echo2 (PA_11);
haarkon 0:f00e68bef80c 178 InterruptIn Echo3 (PB_12);
haarkon 0:f00e68bef80c 179 InterruptIn BP (PC_13);
haarkon 0:f00e68bef80c 180 InterruptIn IG (PC_7);
haarkon 1:d95546f84105 181
haarkon 1:d95546f84105 182 PwmIn PWMG (PB_2);
haarkon 1:d95546f84105 183 PwmIn PWMD (PB_1);
haarkon 1:d95546f84105 184 PwmIn PWMB (PA_15);
haarkon 0:f00e68bef80c 185
haarkon 0:f00e68bef80c 186 PwmOut Pwm_MG (PB_10);
haarkon 0:f00e68bef80c 187 PwmOut Pwm_MD (PB_3);
haarkon 0:f00e68bef80c 188 PwmOut Servo (PA_8);
haarkon 0:f00e68bef80c 189
haarkon 0:f00e68bef80c 190 I2C Bus_I2C (PB_9, PB_8);
haarkon 0:f00e68bef80c 191
haarkon 0:f00e68bef80c 192 SPI MotG (PC_12, PC_11, PC_10);
haarkon 0:f00e68bef80c 193 SPI MotD (PB_15, PB_14, PB_13);
haarkon 0:f00e68bef80c 194
haarkon 0:f00e68bef80c 195 Nucleo_Encoder_16_bits Gauche (TIM4); // A = PB_6, B = PB_7
haarkon 0:f00e68bef80c 196 Nucleo_Encoder_16_bits Droite (TIM3); // A = PB_4, B = PB_5
haarkon 0:f00e68bef80c 197
haarkon 2:1d440e938c44 198 Ticker tick;
haarkon 2:1d440e938c44 199 Timer times;
haarkon 0:f00e68bef80c 200
haarkon 0:f00e68bef80c 201 /** Liste des variables globales
haarkon 0:f00e68bef80c 202 *
haarkon 0:f00e68bef80c 203 * Tick -> (long) Compte le nombre de MS écoulé et déclenche l'exécution de la boucle en fonction du temps écoulé.
haarkon 0:f00e68bef80c 204 * FlagIG -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue gauche
haarkon 0:f00e68bef80c 205 * FlagID -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue droite
haarkon 0:f00e68bef80c 206 * EchoXStart -> (long) Donne le temps en µs de début de l'impulsion d'écho de l'US n°X
haarkon 0:f00e68bef80c 207 * DistUSX -> (float) Donne la distance en mm mesurée par l'US n°X
haarkon 0:f00e68bef80c 208 */
haarkon 0:f00e68bef80c 209
haarkon 0:f00e68bef80c 210 // Structure de temps
haarkon 3:3dfe26a10796 211 lWord Tick = 0;
haarkon 0:f00e68bef80c 212
haarkon 0:f00e68bef80c 213 // Sémaphore d'interruption
haarkon 3:3dfe26a10796 214 int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0;
haarkon 3:3dfe26a10796 215 int FlagTick = 0, FlagTickLed = 0, BPPressed = 0;
haarkon 3:3dfe26a10796 216 int Pixy_check = -1;
haarkon 0:f00e68bef80c 217
haarkon 0:f00e68bef80c 218 // Dialogue avec la Pixy
haarkon 3:3dfe26a10796 219 T_pixyCCData Pixy_CCFIFO[20];
haarkon 3:3dfe26a10796 220 T_pixyNMData Pixy_NMFIFO[20];
haarkon 3:3dfe26a10796 221 Byte Pixy_CCObjet, Pixy_NMObjet;
haarkon 0:f00e68bef80c 222
haarkon 0:f00e68bef80c 223 // Gestion des capteurs Ultrason
haarkon 3:3dfe26a10796 224 long Echo1Start, Echo2Start, Echo3Start;
haarkon 3:3dfe26a10796 225 double DistUS1, DistUS2, DistUS3;
haarkon 0:f00e68bef80c 226
haarkon 0:f00e68bef80c 227 /** Liste des interruptions
haarkon 0:f00e68bef80c 228 *
haarkon 0:f00e68bef80c 229 */
haarkon 0:f00e68bef80c 230
haarkon 0:f00e68bef80c 231 void tickTime()
haarkon 0:f00e68bef80c 232 {
haarkon 0:f00e68bef80c 233 Tick++;
haarkon 0:f00e68bef80c 234 FlagTick = 1;
haarkon 0:f00e68bef80c 235 if ((Tick%100)==0) FlagTickLed = 1;
haarkon 0:f00e68bef80c 236 }
haarkon 0:f00e68bef80c 237
haarkon 0:f00e68bef80c 238 void BPevent ()
haarkon 0:f00e68bef80c 239 {
haarkon 0:f00e68bef80c 240 BPPressed = 1;
haarkon 0:f00e68bef80c 241 }
haarkon 0:f00e68bef80c 242
haarkon 0:f00e68bef80c 243 void echo1Rise ()
haarkon 0:f00e68bef80c 244 {
haarkon 2:1d440e938c44 245 Echo1Start = times.read_us();
haarkon 0:f00e68bef80c 246 }
haarkon 0:f00e68bef80c 247
haarkon 0:f00e68bef80c 248 void echo2Rise ()
haarkon 0:f00e68bef80c 249 {
haarkon 2:1d440e938c44 250 Echo2Start = times.read_us();
haarkon 0:f00e68bef80c 251 }
haarkon 0:f00e68bef80c 252
haarkon 0:f00e68bef80c 253 void echo3Rise ()
haarkon 0:f00e68bef80c 254 {
haarkon 2:1d440e938c44 255 Echo3Start = times.read_us();
haarkon 0:f00e68bef80c 256 }
haarkon 0:f00e68bef80c 257
haarkon 0:f00e68bef80c 258 void echo1Fall ()
haarkon 0:f00e68bef80c 259 {
haarkon 2:1d440e938c44 260 DistUS1 = (double)(times.read_us() - Echo1Start)/5.8;
haarkon 0:f00e68bef80c 261 FlagUS1 = 1;
haarkon 0:f00e68bef80c 262 }
haarkon 0:f00e68bef80c 263
haarkon 0:f00e68bef80c 264 void echo2Fall ()
haarkon 0:f00e68bef80c 265 {
haarkon 2:1d440e938c44 266 DistUS2 = (double)(times.read_us() - Echo2Start)/5.8;
haarkon 0:f00e68bef80c 267 FlagUS2 = 1;
haarkon 0:f00e68bef80c 268 }
haarkon 0:f00e68bef80c 269
haarkon 0:f00e68bef80c 270 void echo3Fall ()
haarkon 0:f00e68bef80c 271 {
haarkon 2:1d440e938c44 272 DistUS3 = (double)(times.read_us() - Echo3Start)/5.8;
haarkon 0:f00e68bef80c 273 FlagUS3 = 1;
haarkon 0:f00e68bef80c 274 }
haarkon 0:f00e68bef80c 275
haarkon 0:f00e68bef80c 276 void getPixyByte ()
haarkon 0:f00e68bef80c 277 {
haarkon 2:1d440e938c44 278 static T_tmpBuffer tmpBuffer;
haarkon 2:1d440e938c44 279 static T_structBuffer msgBuffer;
haarkon 3:3dfe26a10796 280 static T_pixyState PIXY_state = none;
haarkon 2:1d440e938c44 281 static Byte byteCount = 0;
haarkon 3:3dfe26a10796 282 static int PIXY_synced = 0, dummy;
haarkon 2:1d440e938c44 283 int i, somme;
haarkon 3:3dfe26a10796 284 static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0;
haarkon 3:3dfe26a10796 285 static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0;
haarkon 0:f00e68bef80c 286
haarkon 2:1d440e938c44 287 Pixy_check = 0;
haarkon 0:f00e68bef80c 288
haarkon 3:3dfe26a10796 289 if (!PIXY_synced) { // On n'a pas trouvé le START (0x55aa0000)
haarkon 2:1d440e938c44 290 tmpBuffer.tab[byteCount] = Pixy.getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire
haarkon 2:1d440e938c44 291
haarkon 2:1d440e938c44 292 if (byteCount < 3) { // Si on n'a pas encore reçu les 4 premier octets
haarkon 2:1d440e938c44 293 byteCount++; // On passe à la case suivante du tableau temporaire
haarkon 2:1d440e938c44 294 } else { // Lorsqu'on a 4 octets
haarkon 2:1d440e938c44 295 if (tmpBuffer.mot != 0xaa550000) { // Si le code n'est pas le code de START
haarkon 2:1d440e938c44 296 for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau
haarkon 2:1d440e938c44 297 byteCount = 3; // Et on attend le caractère suivant
haarkon 2:1d440e938c44 298 } else { // Si on a trouvé le code de START
haarkon 3:3dfe26a10796 299 PIXY_synced = 1; // On passe en mode synchronisé
haarkon 3:3dfe26a10796 300 PIXY_state = begin;
haarkon 2:1d440e938c44 301 byteCount = 0;
haarkon 0:f00e68bef80c 302 }
haarkon 0:f00e68bef80c 303 }
haarkon 2:1d440e938c44 304 }
haarkon 2:1d440e938c44 305
haarkon 3:3dfe26a10796 306 if (PIXY_synced) {
haarkon 2:1d440e938c44 307
haarkon 3:3dfe26a10796 308 switch (PIXY_state) {
haarkon 2:1d440e938c44 309 case begin : // l'aiguillage est là !
haarkon 2:1d440e938c44 310
haarkon 2:1d440e938c44 311 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus
haarkon 2:1d440e938c44 312 byteCount++;
haarkon 2:1d440e938c44 313 if (byteCount == 2) { // Quand on a 2 octets
haarkon 2:1d440e938c44 314
haarkon 2:1d440e938c44 315 if (msgBuffer.mot == 0xaa55) { // Si c'est un bloc normal (code 0xAA55)
haarkon 3:3dfe26a10796 316 PIXY_state = normal; // On part vers le traitement spécifique
haarkon 2:1d440e938c44 317 }
haarkon 2:1d440e938c44 318
haarkon 2:1d440e938c44 319 if (msgBuffer.mot == 0xaa56) { // Si c'est un bloc Color Code (code 0xAA56)
haarkon 3:3dfe26a10796 320 PIXY_state = colorCode; // On part vers le traitement spécifique
haarkon 2:1d440e938c44 321 }
haarkon 2:1d440e938c44 322
haarkon 2:1d440e938c44 323 if (msgBuffer.mot == 0) { // Si on a un debut de trame (code 0000)
haarkon 3:3dfe26a10796 324 PIXY_state = doubleZero; // On part vers le traitement spécifique
haarkon 2:1d440e938c44 325 }
haarkon 3:3dfe26a10796 326 if ((PIXY_state == begin) || (PIXY_state == none)) { // Si c'est autre chose
haarkon 3:3dfe26a10796 327 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
haarkon 3:3dfe26a10796 328 PIXY_state = none; // Ceinture et bretelle
haarkon 2:1d440e938c44 329 }
haarkon 2:1d440e938c44 330 byteCount = 0;
haarkon 2:1d440e938c44 331 }
haarkon 2:1d440e938c44 332 break;
haarkon 2:1d440e938c44 333
haarkon 2:1d440e938c44 334 case normal : // Si on a un bloc normal
haarkon 2:1d440e938c44 335
haarkon 3:3dfe26a10796 336 Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = Pixy.getc(); // On stocke les octets un à un dans la structure Bloc
haarkon 2:1d440e938c44 337 if (byteCount < 11) { // Tant que la structure n'est pas pleine
haarkon 2:1d440e938c44 338 byteCount++; // On passe à l'octet suivant
haarkon 2:1d440e938c44 339 } else { // Quand elle est pleine
haarkon 2:1d440e938c44 340 byteCount = 0; // On réinitialise
haarkon 3:3dfe26a10796 341 PIXY_state = begin; // On retourne à l'aiguillage
haarkon 2:1d440e938c44 342 // On calcule la somme de contrôle
haarkon 3:3dfe26a10796 343 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;
haarkon 2:1d440e938c44 344
haarkon 3:3dfe26a10796 345 if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) { // Si le checksum est bon, on valide la réception
haarkon 3:3dfe26a10796 346 if (PIXY_wNMObjet < 19) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet
haarkon 3:3dfe26a10796 347 else PIXY_wNMObjet = 0;
haarkon 3:3dfe26a10796 348 if (PIXY_nbNMObjet < 19) PIXY_nbNMObjet++; // On dit que l'on a un objet CC de plus
haarkon 2:1d440e938c44 349 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
haarkon 2:1d440e938c44 350 }
haarkon 2:1d440e938c44 351 }
haarkon 2:1d440e938c44 352 break;
haarkon 2:1d440e938c44 353
haarkon 2:1d440e938c44 354 case colorCode : // Si on a un bloc colorCode
haarkon 2:1d440e938c44 355
haarkon 3:3dfe26a10796 356 Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc
haarkon 2:1d440e938c44 357 if (byteCount < 13) byteCount++; // tant que la structure n'est pas pleine on passe à l'octet suivant
haarkon 2:1d440e938c44 358 else { // Quand elle est pleine
haarkon 2:1d440e938c44 359 byteCount = 0; // On réinitialise
haarkon 3:3dfe26a10796 360 PIXY_state = begin; // On retourne à l'aiguillage
haarkon 2:1d440e938c44 361 // On calcule la somme de contrôle
haarkon 3:3dfe26a10796 362 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;
haarkon 2:1d440e938c44 363
haarkon 3:3dfe26a10796 364 if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) { // Si le checksum est bon
haarkon 3:3dfe26a10796 365 if (PIXY_wCCObjet < 19) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet
haarkon 3:3dfe26a10796 366 else PIXY_wCCObjet = 0;
haarkon 3:3dfe26a10796 367 if (PIXY_nbCCObjet < 19) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter
haarkon 2:1d440e938c44 368 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
haarkon 2:1d440e938c44 369 }
haarkon 2:1d440e938c44 370 }
haarkon 2:1d440e938c44 371 break;
haarkon 2:1d440e938c44 372
haarkon 2:1d440e938c44 373 case doubleZero : // Si on a reçu le code de début d'une nouvelle trame.
haarkon 2:1d440e938c44 374
haarkon 2:1d440e938c44 375 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus
haarkon 2:1d440e938c44 376 byteCount++;
haarkon 2:1d440e938c44 377 if (byteCount == 2) { // Quand on a 2 octets
haarkon 2:1d440e938c44 378 if (msgBuffer.mot == 0xaa55) { // On doit impérativement trouver le code 0xAA55
haarkon 3:3dfe26a10796 379 PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage
haarkon 3:3dfe26a10796 380 Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement
haarkon 3:3dfe26a10796 381 Pixy_CCObjet = PIXY_nbCCObjet;
haarkon 3:3dfe26a10796 382 PIXY_nbCCObjet = 0;
haarkon 3:3dfe26a10796 383 PIXY_nbNMObjet = 0;
haarkon 3:3dfe26a10796 384 FlagPixy = 1; // On valide le traitement de la trame précédente.
haarkon 2:1d440e938c44 385 } else { // Si on trouve autre chose
haarkon 3:3dfe26a10796 386 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
haarkon 3:3dfe26a10796 387 PIXY_state = none; // Ceinture et bretelle
haarkon 2:1d440e938c44 388 }
haarkon 2:1d440e938c44 389 byteCount = 0;
haarkon 2:1d440e938c44 390 }
haarkon 2:1d440e938c44 391 break;
haarkon 0:f00e68bef80c 392 }
haarkon 0:f00e68bef80c 393 }
haarkon 0:f00e68bef80c 394 }
haarkon 0:f00e68bef80c 395
haarkon 2:1d440e938c44 396
haarkon 0:f00e68bef80c 397 int main()
haarkon 0:f00e68bef80c 398 {
haarkon 0:f00e68bef80c 399
haarkon 2:1d440e938c44 400 int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/;
haarkon 5:4955cb4b3646 401
haarkon 3:3dfe26a10796 402 Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0;
haarkon 2:1d440e938c44 403 int PIXY_objet;
haarkon 0:f00e68bef80c 404
haarkon 0:f00e68bef80c 405 double SERVO_angle = 0, SERVO_angleMAX = 180, SERVO_angleMIN = 0;
haarkon 0:f00e68bef80c 406 Word SERVO_pos;
haarkon 0:f00e68bef80c 407 T_SERVODIR SERVO_dir = S_monte;
haarkon 0:f00e68bef80c 408
haarkon 0:f00e68bef80c 409 char MENU_choix = 0;
haarkon 0:f00e68bef80c 410
haarkon 2:1d440e938c44 411 char BOUSSOLE_status[1] = {0};
haarkon 0:f00e68bef80c 412 char I2C_registerValue[4];
haarkon 3:3dfe26a10796 413 double BOUSSOLE_periode;
haarkon 0:f00e68bef80c 414
haarkon 0:f00e68bef80c 415 double CAP_I2C, CAP_PWM;
haarkon 0:f00e68bef80c 416 double SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val;
haarkon 0:f00e68bef80c 417 double SD1_dist, SD2_dist, LD1_dist, LD2_dist;
haarkon 0:f00e68bef80c 418
haarkon 5:4955cb4b3646 419 double MOTG_duty = 0, MOTD_duty = 0;
haarkon 5:4955cb4b3646 420
haarkon 2:1d440e938c44 421 times.reset();
haarkon 2:1d440e938c44 422 times.start();
haarkon 0:f00e68bef80c 423
haarkon 2:1d440e938c44 424 tick.attach(&tickTime, 0.001);
haarkon 0:f00e68bef80c 425
haarkon 0:f00e68bef80c 426 Bus_I2C.frequency (100000);
haarkon 0:f00e68bef80c 427
haarkon 0:f00e68bef80c 428 BP.rise (&BPevent);
haarkon 0:f00e68bef80c 429
haarkon 0:f00e68bef80c 430 Echo1.rise (&echo1Rise);
haarkon 0:f00e68bef80c 431 Echo2.rise (&echo2Rise);
haarkon 0:f00e68bef80c 432 Echo3.rise (&echo3Rise);
haarkon 0:f00e68bef80c 433 Echo1.fall (&echo1Fall);
haarkon 0:f00e68bef80c 434 Echo2.fall (&echo2Fall);
haarkon 0:f00e68bef80c 435 Echo3.fall (&echo3Fall);
haarkon 0:f00e68bef80c 436
haarkon 1:d95546f84105 437 BP.enable_irq();
haarkon 0:f00e68bef80c 438 IG.enable_irq();
haarkon 0:f00e68bef80c 439 Echo1.enable_irq();
haarkon 0:f00e68bef80c 440 Echo2.enable_irq();
haarkon 0:f00e68bef80c 441 Echo3.enable_irq();
haarkon 0:f00e68bef80c 442
haarkon 0:f00e68bef80c 443 Pixy.attach (&getPixyByte);
haarkon 0:f00e68bef80c 444
haarkon 0:f00e68bef80c 445 Pwm_MG.period_us(50);
haarkon 0:f00e68bef80c 446 Pwm_MD.period_us(50);
haarkon 5:4955cb4b3646 447
haarkon 0:f00e68bef80c 448 En = 0;
haarkon 5:4955cb4b3646 449
haarkon 0:f00e68bef80c 450 Led2 = 0;
haarkon 0:f00e68bef80c 451
haarkon 0:f00e68bef80c 452 Servo.period_ms(20);
haarkon 0:f00e68bef80c 453
haarkon 0:f00e68bef80c 454 while(1) {
haarkon 0:f00e68bef80c 455
haarkon 0:f00e68bef80c 456 do {
haarkon 0:f00e68bef80c 457 Led1 = 0;
haarkon 0:f00e68bef80c 458 Pc.printf ("\n\n\n\n\rProgramme de test\n\n\rEntrez le code du test a effectuer :\n\n");
haarkon 0:f00e68bef80c 459 Pc.printf ("\r1- Capteurs Ultra Son (les 3)\n");
haarkon 0:f00e68bef80c 460 Pc.printf ("\r2- Boussole et I2C\n");
haarkon 0:f00e68bef80c 461 Pc.printf ("\r3- Capteurs GP2 (les 4)\n");
haarkon 0:f00e68bef80c 462 Pc.printf ("\r4- Capteurs CNY70 (les 3)\n");
haarkon 0:f00e68bef80c 463 Pc.printf ("\r5- VBAT\n");
haarkon 4:f65b0905e4ce 464 Pc.printf ("\r6- Moteur Gauche -- NON CODE --\n");
haarkon 4:f65b0905e4ce 465 Pc.printf ("\r7- Moteur Droit -- NON CODE --\n");
haarkon 0:f00e68bef80c 466 Pc.printf ("\r8- Servomoteur\n");
haarkon 4:f65b0905e4ce 467 Pc.printf ("\r9- PIXY (CMUCAM5)\n\r");
haarkon 0:f00e68bef80c 468 MENU_choix = Pc.getc ();
haarkon 0:f00e68bef80c 469 } while (((MENU_choix-'0')<1) || ((MENU_choix-'0')>9));
haarkon 0:f00e68bef80c 470
haarkon 0:f00e68bef80c 471 switch (MENU_choix-'0') {
haarkon 0:f00e68bef80c 472
haarkon 0:f00e68bef80c 473 case 1 :
haarkon 0:f00e68bef80c 474 Pc.printf ("\n\n\rTest des captreurs Ultrason\n");
haarkon 0:f00e68bef80c 475 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 476 do {
haarkon 0:f00e68bef80c 477
haarkon 0:f00e68bef80c 478 if (FlagTickLed) {
haarkon 0:f00e68bef80c 479 Led1 = !Led1;
haarkon 0:f00e68bef80c 480 FlagTickLed = 0;
haarkon 0:f00e68bef80c 481 }
haarkon 0:f00e68bef80c 482
haarkon 0:f00e68bef80c 483 // Gestion des US
haarkon 0:f00e68bef80c 484 if (((Tick%150)==0) && FlagTick) {
haarkon 0:f00e68bef80c 485 Trig1 = 1;
haarkon 0:f00e68bef80c 486 wait_us(20);
haarkon 0:f00e68bef80c 487 Trig1 = 0;
haarkon 0:f00e68bef80c 488 FlagTick = 0;
haarkon 0:f00e68bef80c 489 }
haarkon 0:f00e68bef80c 490
haarkon 0:f00e68bef80c 491 if (((Tick%150)==50) && FlagTick) {
haarkon 0:f00e68bef80c 492 Trig2 = 1;
haarkon 0:f00e68bef80c 493 wait_us(20);
haarkon 0:f00e68bef80c 494 Trig2 = 0;
haarkon 0:f00e68bef80c 495 FlagTick = 0;
haarkon 0:f00e68bef80c 496 }
haarkon 0:f00e68bef80c 497
haarkon 0:f00e68bef80c 498 if (((Tick%150)==100) && FlagTick) {
haarkon 0:f00e68bef80c 499 Trig3 = 1;
haarkon 0:f00e68bef80c 500 wait_us(20);
haarkon 0:f00e68bef80c 501 Trig3 = 0;
haarkon 0:f00e68bef80c 502 FlagTick = 0;
haarkon 0:f00e68bef80c 503 }
haarkon 0:f00e68bef80c 504
haarkon 0:f00e68bef80c 505 if (FlagUS1==1) {
haarkon 0:f00e68bef80c 506 Pc.printf ("\rUS 1 = %04d mm", (int)DistUS1);
haarkon 0:f00e68bef80c 507 FlagUS1 = 0;
haarkon 0:f00e68bef80c 508 }
haarkon 0:f00e68bef80c 509
haarkon 0:f00e68bef80c 510 if (FlagUS2==1) {
haarkon 0:f00e68bef80c 511 Pc.printf ("\r\t\t\tUS 2 = %04d mm", (int)DistUS2);
haarkon 0:f00e68bef80c 512 FlagUS2 = 0;
haarkon 0:f00e68bef80c 513 }
haarkon 0:f00e68bef80c 514
haarkon 0:f00e68bef80c 515 if (FlagUS3==1) {
haarkon 0:f00e68bef80c 516 Pc.printf ("\r\t\t\t\t\t\tUS 3 = %04d mm", (int)DistUS3);
haarkon 0:f00e68bef80c 517 FlagUS3 = 0;
haarkon 0:f00e68bef80c 518 }
haarkon 0:f00e68bef80c 519 } while(!Pc.readable());
haarkon 0:f00e68bef80c 520 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 521 break;
haarkon 0:f00e68bef80c 522
haarkon 0:f00e68bef80c 523 case 2 :
haarkon 0:f00e68bef80c 524 Pc.printf ("\n\n\rBoussole\n");
haarkon 0:f00e68bef80c 525 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 526
haarkon 0:f00e68bef80c 527 Pc.printf ("\n\rVerif du bus I2C :");
haarkon 0:f00e68bef80c 528 I2C_check = Bus_I2C.write (BOUSSOLE_adress,BOUSSOLE_status,1,false);
haarkon 0:f00e68bef80c 529 if (I2C_check==0) {
haarkon 0:f00e68bef80c 530 Pc.printf (" OK\n");
haarkon 0:f00e68bef80c 531 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true);
haarkon 0:f00e68bef80c 532 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4);
haarkon 0:f00e68bef80c 533 Pc.printf ("\rVersion Firmware boussole : %03d\n", I2C_registerValue[0]);
haarkon 0:f00e68bef80c 534 } else {
haarkon 0:f00e68bef80c 535 Pc.printf (" FAIL\n");
haarkon 0:f00e68bef80c 536 }
haarkon 0:f00e68bef80c 537
haarkon 3:3dfe26a10796 538 BOUSSOLE_periode = PWMB.period();
haarkon 0:f00e68bef80c 539 Pc.printf ("\rVerif de la PWM :");
haarkon 3:3dfe26a10796 540 if ((BOUSSOLE_periode > 0.11) || (BOUSSOLE_periode < 0.06)) {
haarkon 0:f00e68bef80c 541 Pc.printf (" FAIL\n\n");
haarkon 0:f00e68bef80c 542 } else {
haarkon 0:f00e68bef80c 543 Pc.printf (" OK\n\n");
haarkon 0:f00e68bef80c 544 BOUSSOLE_check = 0;
haarkon 0:f00e68bef80c 545 }
haarkon 0:f00e68bef80c 546
haarkon 0:f00e68bef80c 547 do {
haarkon 0:f00e68bef80c 548 if (FlagTickLed) {
haarkon 0:f00e68bef80c 549 Led1 = !Led1;
haarkon 0:f00e68bef80c 550 FlagTickLed = 0;
haarkon 0:f00e68bef80c 551 }
haarkon 0:f00e68bef80c 552
haarkon 1:d95546f84105 553 if (((Tick%150)==0) && FlagTick) {
haarkon 0:f00e68bef80c 554 FlagTick = 0;
haarkon 0:f00e68bef80c 555 if (BOUSSOLE_check==0) {
haarkon 0:f00e68bef80c 556 CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10;
haarkon 1:d95546f84105 557 Pc.printf ("\r PWM = %4.1lf", CAP_PWM);
haarkon 0:f00e68bef80c 558 }
haarkon 2:1d440e938c44 559 if (I2C_check==0) {
haarkon 0:f00e68bef80c 560 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true);
haarkon 0:f00e68bef80c 561 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4);
haarkon 0:f00e68bef80c 562 CAP_I2C = (double)(((unsigned short)I2C_registerValue[2]<<8)+(unsigned short)I2C_registerValue[3])/10.0;
haarkon 1:d95546f84105 563 Pc.printf ("\r\t\t I2C = %4.1lf", CAP_I2C);
haarkon 2:1d440e938c44 564 }
haarkon 0:f00e68bef80c 565 }
haarkon 0:f00e68bef80c 566 } while(!Pc.readable());
haarkon 0:f00e68bef80c 567 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 568 break;
haarkon 0:f00e68bef80c 569
haarkon 0:f00e68bef80c 570 case 3 :
haarkon 0:f00e68bef80c 571 Pc.printf ("\n\n\rGP2xx\n");
haarkon 0:f00e68bef80c 572 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 573 do {
haarkon 0:f00e68bef80c 574 if (FlagTickLed) {
haarkon 0:f00e68bef80c 575 Led1 = !Led1;
haarkon 0:f00e68bef80c 576 FlagTickLed = 0;
haarkon 0:f00e68bef80c 577
haarkon 0:f00e68bef80c 578 SD1_val = SD1;
haarkon 0:f00e68bef80c 579 SD2_val = SD2;
haarkon 0:f00e68bef80c 580 LD1_val = LD1;
haarkon 0:f00e68bef80c 581 LD2_val = LD2;
haarkon 0:f00e68bef80c 582
haarkon 0:f00e68bef80c 583 if (SD1_val < 0.06) {
haarkon 0:f00e68bef80c 584 SD1_val = 0;
haarkon 0:f00e68bef80c 585 SD1_dist = 40;
haarkon 0:f00e68bef80c 586 } else {
haarkon 0:f00e68bef80c 587 SD1_dist = 11.611/(SD1_val*3.3-0.0237);
haarkon 0:f00e68bef80c 588 if (SD1_dist > 40) SD1_dist = 40;
haarkon 0:f00e68bef80c 589 }
haarkon 0:f00e68bef80c 590
haarkon 0:f00e68bef80c 591 if (SD2_val < 0.06) {
haarkon 0:f00e68bef80c 592 SD2_val = 0;
haarkon 0:f00e68bef80c 593 SD2_dist = 40;
haarkon 0:f00e68bef80c 594 } else {
haarkon 0:f00e68bef80c 595 SD2_dist = 11.611/(SD2_val*3.3-0.0237);
haarkon 0:f00e68bef80c 596 if (SD2_dist > 40) SD2_dist = 40;
haarkon 0:f00e68bef80c 597 }
haarkon 0:f00e68bef80c 598
haarkon 0:f00e68bef80c 599 if (LD1_val < 0.1) {
haarkon 0:f00e68bef80c 600 LD1_val = 0;
haarkon 0:f00e68bef80c 601 LD1_dist = 150;
haarkon 0:f00e68bef80c 602 } else {
haarkon 0:f00e68bef80c 603 LD1_dist = 59.175/(LD1_val*3.3-0.0275);
haarkon 0:f00e68bef80c 604 if (LD1_dist > 150) LD1_dist = 150;
haarkon 0:f00e68bef80c 605 }
haarkon 0:f00e68bef80c 606
haarkon 0:f00e68bef80c 607 if (LD2_val < 0.1) {
haarkon 0:f00e68bef80c 608 LD2_val = 0;
haarkon 0:f00e68bef80c 609 LD2_dist = 150;
haarkon 0:f00e68bef80c 610 } else {
haarkon 0:f00e68bef80c 611 LD2_dist = 59.175/(LD2_val*3.3-0.0275);
haarkon 0:f00e68bef80c 612 if (LD2_dist > 150) LD2_dist = 150;
haarkon 0:f00e68bef80c 613 }
haarkon 0:f00e68bef80c 614
haarkon 0:f00e68bef80c 615 Pc.printf ("\r SD1 = %3.1lf cm - SD2 = %3.1lf cm - LD1 = %4.1lf cm - LD2 = %4.1lf cm", SD1_dist, SD2_dist, LD1_dist, LD2_dist);
haarkon 0:f00e68bef80c 616 }
haarkon 0:f00e68bef80c 617 } while(!Pc.readable());
haarkon 0:f00e68bef80c 618 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 619 break;
haarkon 0:f00e68bef80c 620
haarkon 0:f00e68bef80c 621 case 4 :
haarkon 0:f00e68bef80c 622 Pc.printf ("\n\n\rCNY70\n");
haarkon 0:f00e68bef80c 623 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 624 do {
haarkon 0:f00e68bef80c 625 if (FlagTickLed) {
haarkon 0:f00e68bef80c 626 Led1 = !Led1;
haarkon 0:f00e68bef80c 627 FlagTickLed = 0;
haarkon 0:f00e68bef80c 628
haarkon 0:f00e68bef80c 629 CNY1_val = CNY1;
haarkon 0:f00e68bef80c 630 CNY2_val = CNY2;
haarkon 0:f00e68bef80c 631 CNY3_val = CNY3;
haarkon 0:f00e68bef80c 632
haarkon 0:f00e68bef80c 633 Pc.printf ("\r CNY1 = %3.2lf V\t CNY2 = %3.2lf V\t CNY3 = %3.2lf V", CNY1_val*3.3, CNY2_val*3.3, CNY3_val*3.3);
haarkon 0:f00e68bef80c 634 }
haarkon 0:f00e68bef80c 635 } while(!Pc.readable());
haarkon 0:f00e68bef80c 636 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 637 break;
haarkon 0:f00e68bef80c 638
haarkon 0:f00e68bef80c 639 case 5 :
haarkon 0:f00e68bef80c 640 Pc.printf ("\n\n\rVbat\n");
haarkon 0:f00e68bef80c 641 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 642 do {
haarkon 0:f00e68bef80c 643 if (FlagTickLed) {
haarkon 0:f00e68bef80c 644 Led1 = !Led1;
haarkon 0:f00e68bef80c 645 FlagTickLed = 0;
haarkon 0:f00e68bef80c 646
haarkon 0:f00e68bef80c 647 Vbat_val = Vbat;
haarkon 0:f00e68bef80c 648 Pc.printf ("\rVbat = %5.3lf V", Vbat_val*3.3*4.3);
haarkon 0:f00e68bef80c 649 }
haarkon 0:f00e68bef80c 650 } while(!Pc.readable());
haarkon 0:f00e68bef80c 651 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 652 break;
haarkon 0:f00e68bef80c 653
haarkon 0:f00e68bef80c 654 case 6 :
haarkon 0:f00e68bef80c 655 Pc.printf ("\n\n\rMoteur Gauche\n");
haarkon 0:f00e68bef80c 656 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 5:4955cb4b3646 657
haarkon 5:4955cb4b3646 658 En = 1;
haarkon 5:4955cb4b3646 659 SensG = 1;
haarkon 5:4955cb4b3646 660 MOTG_duty = 0.5;
haarkon 5:4955cb4b3646 661 Pwm_MG = 0.5;
haarkon 5:4955cb4b3646 662
haarkon 0:f00e68bef80c 663 do {
haarkon 0:f00e68bef80c 664 if (FlagTickLed) {
haarkon 0:f00e68bef80c 665 Led1 = !Led1;
haarkon 0:f00e68bef80c 666 FlagTickLed = 0;
haarkon 0:f00e68bef80c 667 }
haarkon 5:4955cb4b3646 668
haarkon 5:4955cb4b3646 669 if (BPPressed) {
haarkon 5:4955cb4b3646 670 BPPressed = 0;
haarkon 5:4955cb4b3646 671
haarkon 5:4955cb4b3646 672 if (!SensG) {
haarkon 5:4955cb4b3646 673 SensG = 1;
haarkon 5:4955cb4b3646 674 } else {
haarkon 5:4955cb4b3646 675 SensG = 0;
haarkon 5:4955cb4b3646 676 }
haarkon 5:4955cb4b3646 677 Pwm_MG = MOTG_duty;
haarkon 5:4955cb4b3646 678 }
haarkon 5:4955cb4b3646 679
haarkon 5:4955cb4b3646 680 Pc.printf ("\rPos = %ld Tick", Gauche.GetCounter());
haarkon 5:4955cb4b3646 681
haarkon 0:f00e68bef80c 682 } while(!Pc.readable());
haarkon 0:f00e68bef80c 683 MENU_choix = Pc.getc();
haarkon 5:4955cb4b3646 684 En = 0;
haarkon 5:4955cb4b3646 685 if (SensG == 0) Pwm_MG = 0;
haarkon 5:4955cb4b3646 686 else Pwm_MG = 1;
haarkon 0:f00e68bef80c 687 break;
haarkon 0:f00e68bef80c 688
haarkon 0:f00e68bef80c 689 case 7 :
haarkon 0:f00e68bef80c 690 Pc.printf ("\n\n\rMoteur Droit\n");
haarkon 0:f00e68bef80c 691 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 5:4955cb4b3646 692
haarkon 5:4955cb4b3646 693 En = 1;
haarkon 5:4955cb4b3646 694 SensD = 1;
haarkon 5:4955cb4b3646 695 MOTD_duty = 0.5;
haarkon 5:4955cb4b3646 696 Pwm_MD = 0.5;
haarkon 5:4955cb4b3646 697
haarkon 0:f00e68bef80c 698 do {
haarkon 0:f00e68bef80c 699 if (FlagTickLed) {
haarkon 0:f00e68bef80c 700 Led1 = !Led1;
haarkon 0:f00e68bef80c 701 FlagTickLed = 0;
haarkon 0:f00e68bef80c 702 }
haarkon 5:4955cb4b3646 703 if (BPPressed) {
haarkon 5:4955cb4b3646 704 BPPressed = 0;
haarkon 5:4955cb4b3646 705 if (SensD) {
haarkon 5:4955cb4b3646 706 SensD = 0;
haarkon 5:4955cb4b3646 707 } else {
haarkon 5:4955cb4b3646 708 SensD = 1;
haarkon 5:4955cb4b3646 709 }
haarkon 5:4955cb4b3646 710 Pwm_MD = MOTD_duty;
haarkon 5:4955cb4b3646 711 }
haarkon 5:4955cb4b3646 712
haarkon 5:4955cb4b3646 713 Pc.printf ("\rPos = %ld Tick", Droite.GetCounter());
haarkon 5:4955cb4b3646 714
haarkon 0:f00e68bef80c 715 } while(!Pc.readable());
haarkon 0:f00e68bef80c 716 MENU_choix = Pc.getc();
haarkon 5:4955cb4b3646 717 En = 0;
haarkon 5:4955cb4b3646 718 if (SensD == 0) Pwm_MD = 0;
haarkon 5:4955cb4b3646 719 else Pwm_MD = 1;
haarkon 0:f00e68bef80c 720 break;
haarkon 0:f00e68bef80c 721
haarkon 0:f00e68bef80c 722 case 8 :
haarkon 0:f00e68bef80c 723 Pc.printf ("\n\n\rServo Moteur\n");
haarkon 0:f00e68bef80c 724 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 725 do {
haarkon 0:f00e68bef80c 726 if (FlagTickLed) {
haarkon 0:f00e68bef80c 727 Led1 = !Led1;
haarkon 0:f00e68bef80c 728 FlagTickLed = 0;
haarkon 0:f00e68bef80c 729 }
haarkon 0:f00e68bef80c 730
haarkon 0:f00e68bef80c 731 if (((Tick%250)==0) && FlagTick) {
haarkon 0:f00e68bef80c 732 if (SERVO_dir == S_monte) {
haarkon 0:f00e68bef80c 733 if (SERVO_angle >= SERVO_angleMAX) {
haarkon 0:f00e68bef80c 734 SERVO_dir = S_descente;
haarkon 0:f00e68bef80c 735 SERVO_angle = SERVO_angleMAX;
haarkon 0:f00e68bef80c 736 } else {
haarkon 0:f00e68bef80c 737 SERVO_angle += 5;
haarkon 0:f00e68bef80c 738 }
haarkon 0:f00e68bef80c 739 } else {
haarkon 0:f00e68bef80c 740 if (SERVO_angle <= SERVO_angleMIN) {
haarkon 0:f00e68bef80c 741 SERVO_dir = S_monte;
haarkon 0:f00e68bef80c 742 SERVO_angle = SERVO_angleMIN;
haarkon 0:f00e68bef80c 743 } else {
haarkon 0:f00e68bef80c 744 SERVO_angle -= 5;
haarkon 0:f00e68bef80c 745 }
haarkon 0:f00e68bef80c 746 }
haarkon 0:f00e68bef80c 747 SERVO_pos = (lWord)(SERVO_angle*50/9) + 1000;
haarkon 0:f00e68bef80c 748 Servo.pulsewidth_us(SERVO_pos);
haarkon 0:f00e68bef80c 749 Pc.printf ("\rAngle = %4.1lf", SERVO_angle);
haarkon 0:f00e68bef80c 750 }
haarkon 0:f00e68bef80c 751 } while(!Pc.readable());
haarkon 0:f00e68bef80c 752 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 753 break;
haarkon 0:f00e68bef80c 754
haarkon 0:f00e68bef80c 755 case 9 :
haarkon 0:f00e68bef80c 756 Pc.printf ("\n\n\rPixy\n");
haarkon 0:f00e68bef80c 757 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 2:1d440e938c44 758
haarkon 2:1d440e938c44 759 if (Pixy_check == 0) Pc.printf ("\n\rPIXY is ALIVE\n");
haarkon 2:1d440e938c44 760 else Pc.printf ("\n\rPIXY don't talk\n");
haarkon 2:1d440e938c44 761
haarkon 0:f00e68bef80c 762 do {
haarkon 2:1d440e938c44 763 if (FlagPixy) {
haarkon 2:1d440e938c44 764
haarkon 2:1d440e938c44 765 if (FlagPixyOverflow) {
haarkon 2:1d440e938c44 766 Pc.printf ("\rSome Data were lost\n");
haarkon 2:1d440e938c44 767 FlagPixyOverflow = 0;
haarkon 0:f00e68bef80c 768 }
haarkon 3:3dfe26a10796 769 for (PIXY_objet = 0; PIXY_objet < Pixy_NMObjet; PIXY_objet++) {
haarkon 3:3dfe26a10796 770 Pc.printf ("\rNMobj #%hd/%hd : sig = %hd : X=%5hd, Y=%5hd (W=%5hd, H=%5hd)\n", PIXY_objet+1, Pixy_NMObjet, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.signature, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.x, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.y, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.width, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.height);
haarkon 2:1d440e938c44 771
haarkon 3:3dfe26a10796 772 if (PIXY_rNMObjet<19) PIXY_rNMObjet++;
haarkon 3:3dfe26a10796 773 else PIXY_rNMObjet = 0;
haarkon 2:1d440e938c44 774 }
haarkon 3:3dfe26a10796 775 Pixy_NMObjet = 0;
haarkon 0:f00e68bef80c 776
haarkon 3:3dfe26a10796 777 for (PIXY_objet = 0; PIXY_objet < Pixy_CCObjet; PIXY_objet++) {
haarkon 3:3dfe26a10796 778 Pc.printf ("\rCCobj #%hd/%hd : sig = %hd : X=%5hd, Y=%5hd (W=%5hd, H=%5hd)\n", PIXY_objet+1, Pixy_CCObjet, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.signature, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.x, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.y, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.width, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.height);
haarkon 1:d95546f84105 779
haarkon 2:1d440e938c44 780 if (PIXY_rCCObjet<19) PIXY_rCCObjet++;
haarkon 2:1d440e938c44 781 else PIXY_rCCObjet = 0;
haarkon 2:1d440e938c44 782 }
haarkon 3:3dfe26a10796 783 Pixy_CCObjet = 0;
haarkon 3:3dfe26a10796 784 Pc.printf("\n\r");
haarkon 1:d95546f84105 785 FlagPixy = 0;
haarkon 0:f00e68bef80c 786 }
haarkon 1:d95546f84105 787
haarkon 1:d95546f84105 788 if (FlagTickLed) {
haarkon 1:d95546f84105 789 Led1 = !Led1;
haarkon 1:d95546f84105 790 FlagTickLed = 0;
haarkon 1:d95546f84105 791 }
haarkon 0:f00e68bef80c 792 } while(!Pc.readable());
haarkon 0:f00e68bef80c 793 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 794 break;
haarkon 0:f00e68bef80c 795
haarkon 0:f00e68bef80c 796 }
haarkon 0:f00e68bef80c 797 }
haarkon 0:f00e68bef80c 798 }