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@1:d95546f84105, 2017-05-29 (annotated)

Committer:
haarkon
Date:
Mon May 29 16:23:47 2017 +0000
Revision:
1:d95546f84105
Parent:
0:f00e68bef80c
Child:
2:1d440e938c44
Evo pour utilisation de la pixy

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 0:f00e68bef80c 14 #define BOUSSOLE_adress 0xC0
haarkon 0:f00e68bef80c 15
haarkon 0:f00e68bef80c 16 typedef unsigned char Byte;
haarkon 0:f00e68bef80c 17 typedef unsigned short Word;
haarkon 0:f00e68bef80c 18 typedef unsigned long lWord;
haarkon 0:f00e68bef80c 19
haarkon 0:f00e68bef80c 20 typedef enum {S_monte = 1, S_descente = 0} T_SERVODIR;
haarkon 0:f00e68bef80c 21
haarkon 0:f00e68bef80c 22 typedef union {
haarkon 0:f00e68bef80c 23 lWord header;
haarkon 0:f00e68bef80c 24 Byte tab[4];
haarkon 0:f00e68bef80c 25 } T_tmpbuffer;
haarkon 0:f00e68bef80c 26
haarkon 0:f00e68bef80c 27 typedef struct {
haarkon 0:f00e68bef80c 28 Word checksum;
haarkon 0:f00e68bef80c 29 Word signature;
haarkon 0:f00e68bef80c 30 Word x;
haarkon 0:f00e68bef80c 31 Word y;
haarkon 0:f00e68bef80c 32 Word width;
haarkon 0:f00e68bef80c 33 Word height;
haarkon 0:f00e68bef80c 34 Word angle; // angle is only available for color coded blocks
haarkon 0:f00e68bef80c 35 } T_pixyCCBloc;
haarkon 0:f00e68bef80c 36
haarkon 0:f00e68bef80c 37
haarkon 0:f00e68bef80c 38 typedef union {
haarkon 0:f00e68bef80c 39 Byte tab[14];
haarkon 0:f00e68bef80c 40 T_pixyCCBloc CCbloc;
haarkon 0:f00e68bef80c 41 } T_pixyData;
haarkon 0:f00e68bef80c 42
haarkon 0:f00e68bef80c 43
haarkon 0:f00e68bef80c 44 /** Liste des objets
haarkon 0:f00e68bef80c 45 *
haarkon 0:f00e68bef80c 46 * Serial #4 Pixy
haarkon 0:f00e68bef80c 47 * Serial #2 Pc
haarkon 0:f00e68bef80c 48 *
haarkon 0:f00e68bef80c 49 * AnalogIn C1, C2, C3, LD1, LD2, SD1, SD2, Vbat
haarkon 0:f00e68bef80c 50 *
haarkon 0:f00e68bef80c 51 * DigitalOut Led1, Led2, Trig1, Trig2, Trig3, En, SensG, SensD
haarkon 0:f00e68bef80c 52 *
haarkon 0:f00e68bef80c 53 * InterruptIn IndexG, IndexD, Echo1, Echo2, Echo3, BP
haarkon 0:f00e68bef80c 54 *
haarkon 0:f00e68bef80c 55 * PwmOut Pwm_MG, Pwm_MD, Servo
haarkon 0:f00e68bef80c 56 *
haarkon 0:f00e68bef80c 57 * PwmIn PWMG, PWMD, PWMB
haarkon 0:f00e68bef80c 58 *
haarkon 0:f00e68bef80c 59 * I2C Bus_I2C
haarkon 0:f00e68bef80c 60 *
haarkon 0:f00e68bef80c 61 * SPI MotG, MotD
haarkon 0:f00e68bef80c 62 *
haarkon 0:f00e68bef80c 63 * Nucleo_Encoder_16_bits Gauche, Droite
haarkon 0:f00e68bef80c 64 *
haarkon 0:f00e68bef80c 65 * Ticker timer
haarkon 0:f00e68bef80c 66 */
haarkon 0:f00e68bef80c 67
haarkon 0:f00e68bef80c 68 /** Liste des PINs
haarkon 0:f00e68bef80c 69 *
haarkon 0:f00e68bef80c 70 * PIN MAP (ordre alphabetique) des PINs de la Nucléo 64 utilisée
haarkon 0:f00e68bef80c 71 * PA_0 -> Pixy RX (Serial)
haarkon 0:f00e68bef80c 72 * PA_1 -> Pixy TX (Serial)
haarkon 0:f00e68bef80c 73 * PA_2 -> PC TX (Serial)
haarkon 0:f00e68bef80c 74 * PA_3 -> PX RX (Serial)
haarkon 0:f00e68bef80c 75 * PA_4 -> GP2 SD #2 (Analog In)
haarkon 0:f00e68bef80c 76 * PA_5 -> LED1 (Digital Out)
haarkon 0:f00e68bef80c 77 * PA_6 -> CNY3 (Analog In)
haarkon 0:f00e68bef80c 78 * PA_7 -> CNY2 (Analog In)
haarkon 0:f00e68bef80c 79 * PA_8 -> Servomoteur (PWM Out)
haarkon 0:f00e68bef80c 80 * PA_9 -> US Trigger #3 (Digital Out)
haarkon 1:d95546f84105 81 * PA_10 -> US Echo #1 (Pwm In)
haarkon 0:f00e68bef80c 82 * PA_11 -> US Echo #2 (Pwm In)
haarkon 1:d95546f84105 83 * PA_12 -> SS (SPI Slave Select) (Digital Out)
haarkon 0:f00e68bef80c 84 * PA_13
haarkon 0:f00e68bef80c 85 * PA_14
haarkon 0:f00e68bef80c 86 * PA_15 -> Boussole (Pwm In)
haarkon 0:f00e68bef80c 87 *
haarkon 0:f00e68bef80c 88 * PB_0 -> GP2 SD #1 (Analog In)
haarkon 0:f00e68bef80c 89 * PB_1 -> Position D (Pwm In)
haarkon 0:f00e68bef80c 90 * PB_2 -> Position G (Pwm In)
haarkon 0:f00e68bef80c 91 * PB_3 -> PWM Mot D (PWM Out)
haarkon 0:f00e68bef80c 92 * PB_4 -> Enocdeur Droit A (QE)
haarkon 0:f00e68bef80c 93 * PB_5 -> Enocdeur Droit B (QE)
haarkon 0:f00e68bef80c 94 * PB_6 -> Enocdeur Gauche A (QE)
haarkon 0:f00e68bef80c 95 * PB_7 -> Enocdeur Gauche B (QE)
haarkon 0:f00e68bef80c 96 * PB_8 -> SCL (I2C)
haarkon 0:f00e68bef80c 97 * PB_9 -> SDA (I2C)
haarkon 0:f00e68bef80c 98 * PB_10 -> PWM Mot G (PWM Out)
haarkon 0:f00e68bef80c 99 * PB_11
haarkon 0:f00e68bef80c 100 * PB_12 -> US Echo #3 (Pwm In)
haarkon 0:f00e68bef80c 101 * PB_13 -> SCK Encodeur D (SPI)
haarkon 0:f00e68bef80c 102 * PB_14 -> MISO Encodeur D (SPI)
haarkon 0:f00e68bef80c 103 * PB_15 -> MOSI Encodeur D (SPI)
haarkon 0:f00e68bef80c 104 *
haarkon 0:f00e68bef80c 105 * PC_0 -> GP2 LD #1 (Analog In)
haarkon 0:f00e68bef80c 106 * PC_1 -> GP2 LD #2 (Analog In)
haarkon 0:f00e68bef80c 107 * PC_2 -> US Trigger #2 (Digital Out)
haarkon 0:f00e68bef80c 108 * PC_3 -> US Trigger #1 (Digital Out)
haarkon 0:f00e68bef80c 109 * PC_4 -> CNY1 (Analog In)
haarkon 0:f00e68bef80c 110 * PC_5 -> Vbat (Analog In)
haarkon 0:f00e68bef80c 111 * PC_6 -> Dir Mot Droit (Digital Out)
haarkon 0:f00e68bef80c 112 * PC_7 -> I (Encodeur Gauche) (IRQ In)
haarkon 0:f00e68bef80c 113 * PC_8 -> Dir Mot Gauche (Digital Out)
haarkon 0:f00e68bef80c 114 * PC_9 -> Enable Moteurs (Digital Out)
haarkon 0:f00e68bef80c 115 * PC_10 -> SCK Encodeur G (SPI)
haarkon 0:f00e68bef80c 116 * PC_11 -> MISO Encodeur G (SPI)
haarkon 0:f00e68bef80c 117 * PC_12 -> MOSI Encodeur G (SPI)
haarkon 0:f00e68bef80c 118 * PC_13 -> User BP (IRQ In)
haarkon 0:f00e68bef80c 119 * PC_14
haarkon 0:f00e68bef80c 120 * PC_15
haarkon 0:f00e68bef80c 121 *
haarkon 0:f00e68bef80c 122 * PD_1
haarkon 0:f00e68bef80c 123 * PD_2 -> Led2 (Digital Out)
haarkon 0:f00e68bef80c 124 */
haarkon 0:f00e68bef80c 125
haarkon 1:d95546f84105 126 Serial Pixy (PA_0, PA_1, 230400);
haarkon 0:f00e68bef80c 127 Serial Pc (PA_2, PA_3, 460800);
haarkon 0:f00e68bef80c 128
haarkon 0:f00e68bef80c 129 AnalogIn CNY1 (PC_4);
haarkon 0:f00e68bef80c 130 AnalogIn CNY2 (PA_7);
haarkon 0:f00e68bef80c 131 AnalogIn CNY3 (PA_6);
haarkon 0:f00e68bef80c 132 AnalogIn LD1 (PC_0);
haarkon 0:f00e68bef80c 133 AnalogIn LD2 (PC_1);
haarkon 0:f00e68bef80c 134 AnalogIn SD1 (PB_0);
haarkon 0:f00e68bef80c 135 AnalogIn SD2 (PA_4);
haarkon 0:f00e68bef80c 136 AnalogIn Vbat (PC_5);
haarkon 0:f00e68bef80c 137
haarkon 0:f00e68bef80c 138 DigitalOut Led1 (PA_5);
haarkon 0:f00e68bef80c 139 DigitalOut Led2 (PD_2);
haarkon 0:f00e68bef80c 140 DigitalOut Trig1 (PC_3);
haarkon 0:f00e68bef80c 141 DigitalOut Trig2 (PC_2);
haarkon 0:f00e68bef80c 142 DigitalOut Trig3 (PA_9);
haarkon 0:f00e68bef80c 143 DigitalOut En (PC_9);
haarkon 0:f00e68bef80c 144 DigitalOut SensG (PC_8);
haarkon 0:f00e68bef80c 145 DigitalOut SensD (PC_6);
haarkon 1:d95546f84105 146 DigitalOut SS (PA_12);
haarkon 0:f00e68bef80c 147
haarkon 1:d95546f84105 148 InterruptIn Echo1 (PA_10);
haarkon 0:f00e68bef80c 149 InterruptIn Echo2 (PA_11);
haarkon 0:f00e68bef80c 150 InterruptIn Echo3 (PB_12);
haarkon 0:f00e68bef80c 151 InterruptIn BP (PC_13);
haarkon 0:f00e68bef80c 152 InterruptIn IG (PC_7);
haarkon 1:d95546f84105 153
haarkon 1:d95546f84105 154 PwmIn PWMG (PB_2);
haarkon 1:d95546f84105 155 PwmIn PWMD (PB_1);
haarkon 1:d95546f84105 156 PwmIn PWMB (PA_15);
haarkon 0:f00e68bef80c 157
haarkon 0:f00e68bef80c 158 PwmOut Pwm_MG (PB_10);
haarkon 0:f00e68bef80c 159 PwmOut Pwm_MD (PB_3);
haarkon 0:f00e68bef80c 160 PwmOut Servo (PA_8);
haarkon 0:f00e68bef80c 161
haarkon 0:f00e68bef80c 162 I2C Bus_I2C (PB_9, PB_8);
haarkon 0:f00e68bef80c 163
haarkon 0:f00e68bef80c 164 SPI MotG (PC_12, PC_11, PC_10);
haarkon 0:f00e68bef80c 165 SPI MotD (PB_15, PB_14, PB_13);
haarkon 0:f00e68bef80c 166
haarkon 0:f00e68bef80c 167 Nucleo_Encoder_16_bits Gauche (TIM4); // A = PB_6, B = PB_7
haarkon 0:f00e68bef80c 168 Nucleo_Encoder_16_bits Droite (TIM3); // A = PB_4, B = PB_5
haarkon 0:f00e68bef80c 169
haarkon 0:f00e68bef80c 170 Ticker timer;
haarkon 0:f00e68bef80c 171 Timer temps;
haarkon 0:f00e68bef80c 172
haarkon 0:f00e68bef80c 173 /** Liste des variables globales
haarkon 0:f00e68bef80c 174 *
haarkon 0:f00e68bef80c 175 * 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 176 * FlagIG -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue gauche
haarkon 0:f00e68bef80c 177 * FlagID -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue droite
haarkon 0:f00e68bef80c 178 * EchoXStart -> (long) Donne le temps en µs de début de l'impulsion d'écho de l'US n°X
haarkon 0:f00e68bef80c 179 * DistUSX -> (float) Donne la distance en mm mesurée par l'US n°X
haarkon 0:f00e68bef80c 180 */
haarkon 0:f00e68bef80c 181
haarkon 0:f00e68bef80c 182 // Structure de temps
haarkon 0:f00e68bef80c 183 lWord Tick = 0;
haarkon 0:f00e68bef80c 184
haarkon 0:f00e68bef80c 185 // Sémaphore d'interruption
haarkon 0:f00e68bef80c 186 int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0;
haarkon 0:f00e68bef80c 187 int FlagTick = 0, FlagTickLed = 0, BPPressed = 0;
haarkon 0:f00e68bef80c 188
haarkon 0:f00e68bef80c 189 // Dialogue avec la Pixy
haarkon 0:f00e68bef80c 190 T_pixyData Pixy_FIFO[20];
haarkon 1:d95546f84105 191 Byte Pixy_nbObjet = 0, Pixy_wObjet = 0, Pixy_rObjet = 0;
haarkon 0:f00e68bef80c 192
haarkon 0:f00e68bef80c 193 // Gestion des capteurs Ultrason
haarkon 0:f00e68bef80c 194 long Echo1Start, Echo2Start, Echo3Start;
haarkon 0:f00e68bef80c 195 double DistUS1, DistUS2, DistUS3;
haarkon 0:f00e68bef80c 196
haarkon 0:f00e68bef80c 197 /** Liste des interruptions
haarkon 0:f00e68bef80c 198 *
haarkon 0:f00e68bef80c 199 */
haarkon 0:f00e68bef80c 200
haarkon 0:f00e68bef80c 201 void tickTime()
haarkon 0:f00e68bef80c 202 {
haarkon 0:f00e68bef80c 203 Tick++;
haarkon 0:f00e68bef80c 204 FlagTick = 1;
haarkon 0:f00e68bef80c 205 if ((Tick%100)==0) FlagTickLed = 1;
haarkon 0:f00e68bef80c 206 }
haarkon 0:f00e68bef80c 207
haarkon 0:f00e68bef80c 208 void BPevent ()
haarkon 0:f00e68bef80c 209 {
haarkon 0:f00e68bef80c 210 BPPressed = 1;
haarkon 0:f00e68bef80c 211 }
haarkon 0:f00e68bef80c 212
haarkon 0:f00e68bef80c 213 void echo1Rise ()
haarkon 0:f00e68bef80c 214 {
haarkon 0:f00e68bef80c 215 Echo1Start = temps.read_us();
haarkon 0:f00e68bef80c 216 }
haarkon 0:f00e68bef80c 217
haarkon 0:f00e68bef80c 218 void echo2Rise ()
haarkon 0:f00e68bef80c 219 {
haarkon 0:f00e68bef80c 220 Echo2Start = temps.read_us();
haarkon 0:f00e68bef80c 221 }
haarkon 0:f00e68bef80c 222
haarkon 0:f00e68bef80c 223 void echo3Rise ()
haarkon 0:f00e68bef80c 224 {
haarkon 0:f00e68bef80c 225 Echo3Start = temps.read_us();
haarkon 0:f00e68bef80c 226 }
haarkon 0:f00e68bef80c 227
haarkon 0:f00e68bef80c 228 void echo1Fall ()
haarkon 0:f00e68bef80c 229 {
haarkon 0:f00e68bef80c 230 DistUS1 = (double)(temps.read_us() - Echo1Start)/5.8;
haarkon 0:f00e68bef80c 231 FlagUS1 = 1;
haarkon 0:f00e68bef80c 232 }
haarkon 0:f00e68bef80c 233
haarkon 0:f00e68bef80c 234 void echo2Fall ()
haarkon 0:f00e68bef80c 235 {
haarkon 0:f00e68bef80c 236 DistUS2 = (double)(temps.read_us() - Echo2Start)/5.8;
haarkon 0:f00e68bef80c 237 FlagUS2 = 1;
haarkon 0:f00e68bef80c 238 }
haarkon 0:f00e68bef80c 239
haarkon 0:f00e68bef80c 240 void echo3Fall ()
haarkon 0:f00e68bef80c 241 {
haarkon 0:f00e68bef80c 242 DistUS3 = (double)(temps.read_us() - Echo3Start)/5.8;
haarkon 0:f00e68bef80c 243 FlagUS3 = 1;
haarkon 0:f00e68bef80c 244 }
haarkon 0:f00e68bef80c 245
haarkon 0:f00e68bef80c 246 void getPixyByte ()
haarkon 0:f00e68bef80c 247 {
haarkon 0:f00e68bef80c 248
haarkon 0:f00e68bef80c 249 static T_tmpbuffer tmpBuffer;
haarkon 0:f00e68bef80c 250 static Byte bytecount = 0;
haarkon 0:f00e68bef80c 251 static int Pixy_synced = 0;
haarkon 0:f00e68bef80c 252 int i;
haarkon 0:f00e68bef80c 253
haarkon 1:d95546f84105 254 Led2 = ! Led2;
haarkon 0:f00e68bef80c 255 if (!Pixy_synced) { // On n'a pas trouvé le départ (0x55aa55aa ou 0x55aa56aa)
haarkon 0:f00e68bef80c 256 tmpBuffer.tab[bytecount] = Pixy.getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire
haarkon 0:f00e68bef80c 257 if (bytecount < 3) { // Si on n'a pas encore reçu les 4 premier octets
haarkon 0:f00e68bef80c 258 bytecount++; // On passe à la case suivante du tableau temporaire
haarkon 0:f00e68bef80c 259 } else { // Si on a 4 octets de données
haarkon 0:f00e68bef80c 260 if (tmpBuffer.header == 0x55aa56aa) { // Si on a un Color Code Bloc
haarkon 0:f00e68bef80c 261 if (Pixy_wObjet < 19) Pixy_wObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet
haarkon 0:f00e68bef80c 262 else Pixy_wObjet = 0;
haarkon 0:f00e68bef80c 263 Pixy_nbObjet++; // On dit que l'on a un objet de plus
haarkon 0:f00e68bef80c 264 if (Pixy_nbObjet >= 20) FlagPixyOverflow = 1; // Si on a plus de 20 objets (en attente) => Overflow
haarkon 0:f00e68bef80c 265 Pixy_synced = 1; // Et on peut dire que l'on a synchronisé la Pixy
haarkon 0:f00e68bef80c 266 bytecount = 0;
haarkon 0:f00e68bef80c 267 } else { // Si on n'a pas vu le header d'un Color Code Bloc
haarkon 0:f00e68bef80c 268 for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau
haarkon 0:f00e68bef80c 269 }
haarkon 0:f00e68bef80c 270 }
haarkon 0:f00e68bef80c 271 } else { // La Pixy est synchronisée
haarkon 0:f00e68bef80c 272 Pixy_FIFO[Pixy_wObjet].tab[bytecount] = Pixy.getc(); // On stocke les octets un à un dans la structure CCBloc
haarkon 0:f00e68bef80c 273 if (bytecount < 14) bytecount++; // tant que la structure n'est pas pleine
haarkon 0:f00e68bef80c 274 else { // Quand elle est pleine
haarkon 0:f00e68bef80c 275 bytecount = 0; // On réinitialise
haarkon 0:f00e68bef80c 276 Pixy_synced = 0;
haarkon 0:f00e68bef80c 277 FlagPixy = 1; // Et on signale au main qu'un objet est pret à être analysé.
haarkon 0:f00e68bef80c 278 }
haarkon 0:f00e68bef80c 279 }
haarkon 0:f00e68bef80c 280 }
haarkon 0:f00e68bef80c 281
haarkon 0:f00e68bef80c 282 int main()
haarkon 0:f00e68bef80c 283 {
haarkon 0:f00e68bef80c 284
haarkon 0:f00e68bef80c 285 int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, PIXY_check = -1, MOTG_check = -1, MOTD_check = -1*/;
haarkon 0:f00e68bef80c 286 int phase = 0;
haarkon 0:f00e68bef80c 287
haarkon 0:f00e68bef80c 288 double SERVO_angle = 0, SERVO_angleMAX = 180, SERVO_angleMIN = 0;
haarkon 0:f00e68bef80c 289 Word SERVO_pos;
haarkon 0:f00e68bef80c 290 T_SERVODIR SERVO_dir = S_monte;
haarkon 0:f00e68bef80c 291
haarkon 0:f00e68bef80c 292 Byte PIXY_red = 0, PIXY_green = 0, PIXY_blue = 0;
haarkon 0:f00e68bef80c 293 char MENU_choix = 0;
haarkon 0:f00e68bef80c 294
haarkon 0:f00e68bef80c 295 char BOUSSOLE_status[1] = {0}/*, BOUSSOLE_bearingWord[1] = {2}*/;
haarkon 0:f00e68bef80c 296 char I2C_registerValue[4];
haarkon 0:f00e68bef80c 297
haarkon 0:f00e68bef80c 298 double CAP_I2C, CAP_PWM;
haarkon 0:f00e68bef80c 299 double SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val;
haarkon 0:f00e68bef80c 300 double SD1_dist, SD2_dist, LD1_dist, LD2_dist;
haarkon 0:f00e68bef80c 301 double periode;
haarkon 0:f00e68bef80c 302
haarkon 0:f00e68bef80c 303 temps.reset();
haarkon 0:f00e68bef80c 304 temps.start();
haarkon 0:f00e68bef80c 305
haarkon 0:f00e68bef80c 306
haarkon 0:f00e68bef80c 307 timer.attach(&tickTime, 0.001);
haarkon 0:f00e68bef80c 308
haarkon 0:f00e68bef80c 309 Bus_I2C.frequency (100000);
haarkon 0:f00e68bef80c 310
haarkon 0:f00e68bef80c 311 BP.rise (&BPevent);
haarkon 0:f00e68bef80c 312
haarkon 0:f00e68bef80c 313 Echo1.rise (&echo1Rise);
haarkon 0:f00e68bef80c 314 Echo2.rise (&echo2Rise);
haarkon 0:f00e68bef80c 315 Echo3.rise (&echo3Rise);
haarkon 0:f00e68bef80c 316 Echo1.fall (&echo1Fall);
haarkon 0:f00e68bef80c 317 Echo2.fall (&echo2Fall);
haarkon 0:f00e68bef80c 318 Echo3.fall (&echo3Fall);
haarkon 0:f00e68bef80c 319
haarkon 1:d95546f84105 320 BP.enable_irq();
haarkon 0:f00e68bef80c 321 IG.enable_irq();
haarkon 0:f00e68bef80c 322 Echo1.enable_irq();
haarkon 0:f00e68bef80c 323 Echo2.enable_irq();
haarkon 0:f00e68bef80c 324 Echo3.enable_irq();
haarkon 0:f00e68bef80c 325
haarkon 0:f00e68bef80c 326 Pixy.attach (&getPixyByte);
haarkon 0:f00e68bef80c 327
haarkon 0:f00e68bef80c 328 Pwm_MG.period_us(50);
haarkon 0:f00e68bef80c 329 Pwm_MD.period_us(50);
haarkon 0:f00e68bef80c 330 Pwm_MG = 0;
haarkon 0:f00e68bef80c 331 Pwm_MD = 0;
haarkon 0:f00e68bef80c 332 En = 0;
haarkon 0:f00e68bef80c 333 SensG = 0;
haarkon 0:f00e68bef80c 334 SensD = 0;
haarkon 0:f00e68bef80c 335 Led2 = 0;
haarkon 0:f00e68bef80c 336
haarkon 0:f00e68bef80c 337 Servo.period_ms(20);
haarkon 0:f00e68bef80c 338
haarkon 0:f00e68bef80c 339 while(1) {
haarkon 0:f00e68bef80c 340
haarkon 0:f00e68bef80c 341 do {
haarkon 0:f00e68bef80c 342 Led1 = 0;
haarkon 0:f00e68bef80c 343 Pc.printf ("\n\n\n\n\rProgramme de test\n\n\rEntrez le code du test a effectuer :\n\n");
haarkon 0:f00e68bef80c 344 Pc.printf ("\r1- Capteurs Ultra Son (les 3)\n");
haarkon 0:f00e68bef80c 345 Pc.printf ("\r2- Boussole et I2C\n");
haarkon 0:f00e68bef80c 346 Pc.printf ("\r3- Capteurs GP2 (les 4)\n");
haarkon 0:f00e68bef80c 347 Pc.printf ("\r4- Capteurs CNY70 (les 3)\n");
haarkon 0:f00e68bef80c 348 Pc.printf ("\r5- VBAT\n");
haarkon 0:f00e68bef80c 349 Pc.printf ("\r6- Moteur Gauche -- NON CODE\n");
haarkon 0:f00e68bef80c 350 Pc.printf ("\r7- Moteur Droit -- NON CODE\n");
haarkon 0:f00e68bef80c 351 Pc.printf ("\r8- Servomoteur\n");
haarkon 0:f00e68bef80c 352 Pc.printf ("\r9- Test de la PIXY\n\r");
haarkon 0:f00e68bef80c 353 MENU_choix = Pc.getc ();
haarkon 0:f00e68bef80c 354 } while (((MENU_choix-'0')<1) || ((MENU_choix-'0')>9));
haarkon 0:f00e68bef80c 355
haarkon 0:f00e68bef80c 356 switch (MENU_choix-'0') {
haarkon 0:f00e68bef80c 357
haarkon 0:f00e68bef80c 358 case 1 :
haarkon 0:f00e68bef80c 359 Pc.printf ("\n\n\rTest des captreurs Ultrason\n");
haarkon 0:f00e68bef80c 360 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 361 do {
haarkon 0:f00e68bef80c 362
haarkon 0:f00e68bef80c 363 if (FlagTickLed) {
haarkon 0:f00e68bef80c 364 Led1 = !Led1;
haarkon 0:f00e68bef80c 365 FlagTickLed = 0;
haarkon 0:f00e68bef80c 366 }
haarkon 0:f00e68bef80c 367
haarkon 0:f00e68bef80c 368 // Gestion des US
haarkon 0:f00e68bef80c 369 if (((Tick%150)==0) && FlagTick) {
haarkon 0:f00e68bef80c 370 Trig1 = 1;
haarkon 0:f00e68bef80c 371 wait_us(20);
haarkon 0:f00e68bef80c 372 Trig1 = 0;
haarkon 0:f00e68bef80c 373 FlagTick = 0;
haarkon 0:f00e68bef80c 374 }
haarkon 0:f00e68bef80c 375
haarkon 0:f00e68bef80c 376 if (((Tick%150)==50) && FlagTick) {
haarkon 0:f00e68bef80c 377 Trig2 = 1;
haarkon 0:f00e68bef80c 378 wait_us(20);
haarkon 0:f00e68bef80c 379 Trig2 = 0;
haarkon 0:f00e68bef80c 380 FlagTick = 0;
haarkon 0:f00e68bef80c 381 }
haarkon 0:f00e68bef80c 382
haarkon 0:f00e68bef80c 383 if (((Tick%150)==100) && FlagTick) {
haarkon 0:f00e68bef80c 384 Trig3 = 1;
haarkon 0:f00e68bef80c 385 wait_us(20);
haarkon 0:f00e68bef80c 386 Trig3 = 0;
haarkon 0:f00e68bef80c 387 FlagTick = 0;
haarkon 0:f00e68bef80c 388 }
haarkon 0:f00e68bef80c 389
haarkon 0:f00e68bef80c 390 if (FlagUS1==1) {
haarkon 0:f00e68bef80c 391 Pc.printf ("\rUS 1 = %04d mm", (int)DistUS1);
haarkon 0:f00e68bef80c 392 FlagUS1 = 0;
haarkon 0:f00e68bef80c 393 }
haarkon 0:f00e68bef80c 394
haarkon 0:f00e68bef80c 395 if (FlagUS2==1) {
haarkon 0:f00e68bef80c 396 Pc.printf ("\r\t\t\tUS 2 = %04d mm", (int)DistUS2);
haarkon 0:f00e68bef80c 397 FlagUS2 = 0;
haarkon 0:f00e68bef80c 398 }
haarkon 0:f00e68bef80c 399
haarkon 0:f00e68bef80c 400 if (FlagUS3==1) {
haarkon 0:f00e68bef80c 401 Pc.printf ("\r\t\t\t\t\t\tUS 3 = %04d mm", (int)DistUS3);
haarkon 0:f00e68bef80c 402 FlagUS3 = 0;
haarkon 0:f00e68bef80c 403 }
haarkon 0:f00e68bef80c 404 } while(!Pc.readable());
haarkon 0:f00e68bef80c 405 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 406 break;
haarkon 0:f00e68bef80c 407
haarkon 0:f00e68bef80c 408 case 2 :
haarkon 0:f00e68bef80c 409 Pc.printf ("\n\n\rBoussole\n");
haarkon 0:f00e68bef80c 410 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 411
haarkon 0:f00e68bef80c 412 Pc.printf ("\n\rVerif du bus I2C :");
haarkon 0:f00e68bef80c 413 I2C_check = Bus_I2C.write (BOUSSOLE_adress,BOUSSOLE_status,1,false);
haarkon 0:f00e68bef80c 414 if (I2C_check==0) {
haarkon 0:f00e68bef80c 415 Pc.printf (" OK\n");
haarkon 0:f00e68bef80c 416 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true);
haarkon 0:f00e68bef80c 417 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4);
haarkon 0:f00e68bef80c 418 Pc.printf ("\rVersion Firmware boussole : %03d\n", I2C_registerValue[0]);
haarkon 0:f00e68bef80c 419 } else {
haarkon 0:f00e68bef80c 420 Pc.printf (" FAIL\n");
haarkon 0:f00e68bef80c 421 }
haarkon 0:f00e68bef80c 422
haarkon 0:f00e68bef80c 423 periode = PWMB.period();
haarkon 0:f00e68bef80c 424 Pc.printf ("\rVerif de la PWM :");
haarkon 0:f00e68bef80c 425 if ((periode > 0.11) || (periode < 0.06)) {
haarkon 0:f00e68bef80c 426 Pc.printf (" FAIL\n\n");
haarkon 0:f00e68bef80c 427 } else {
haarkon 0:f00e68bef80c 428 Pc.printf (" OK\n\n");
haarkon 0:f00e68bef80c 429 BOUSSOLE_check = 0;
haarkon 0:f00e68bef80c 430 }
haarkon 0:f00e68bef80c 431
haarkon 0:f00e68bef80c 432 do {
haarkon 0:f00e68bef80c 433 if (FlagTickLed) {
haarkon 0:f00e68bef80c 434 Led1 = !Led1;
haarkon 0:f00e68bef80c 435 FlagTickLed = 0;
haarkon 0:f00e68bef80c 436 }
haarkon 0:f00e68bef80c 437
haarkon 1:d95546f84105 438 if (((Tick%150)==0) && FlagTick) {
haarkon 0:f00e68bef80c 439 FlagTick = 0;
haarkon 0:f00e68bef80c 440 if (BOUSSOLE_check==0) {
haarkon 0:f00e68bef80c 441 CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10;
haarkon 1:d95546f84105 442 Pc.printf ("\r PWM = %4.1lf", CAP_PWM);
haarkon 0:f00e68bef80c 443 }
haarkon 1:d95546f84105 444 //if (I2C_check==0) {
haarkon 0:f00e68bef80c 445 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true);
haarkon 0:f00e68bef80c 446 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4);
haarkon 0:f00e68bef80c 447 CAP_I2C = (double)(((unsigned short)I2C_registerValue[2]<<8)+(unsigned short)I2C_registerValue[3])/10.0;
haarkon 1:d95546f84105 448 Pc.printf ("\r\t\t I2C = %4.1lf", CAP_I2C);
haarkon 1:d95546f84105 449 //}
haarkon 0:f00e68bef80c 450 }
haarkon 0:f00e68bef80c 451 } while(!Pc.readable());
haarkon 0:f00e68bef80c 452 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 453 break;
haarkon 0:f00e68bef80c 454
haarkon 0:f00e68bef80c 455 case 3 :
haarkon 0:f00e68bef80c 456 Pc.printf ("\n\n\rGP2xx\n");
haarkon 0:f00e68bef80c 457 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 458 do {
haarkon 0:f00e68bef80c 459 if (FlagTickLed) {
haarkon 0:f00e68bef80c 460 Led1 = !Led1;
haarkon 0:f00e68bef80c 461 FlagTickLed = 0;
haarkon 0:f00e68bef80c 462
haarkon 0:f00e68bef80c 463 SD1_val = SD1;
haarkon 0:f00e68bef80c 464 SD2_val = SD2;
haarkon 0:f00e68bef80c 465 LD1_val = LD1;
haarkon 0:f00e68bef80c 466 LD2_val = LD2;
haarkon 0:f00e68bef80c 467
haarkon 0:f00e68bef80c 468 if (SD1_val < 0.06) {
haarkon 0:f00e68bef80c 469 SD1_val = 0;
haarkon 0:f00e68bef80c 470 SD1_dist = 40;
haarkon 0:f00e68bef80c 471 } else {
haarkon 0:f00e68bef80c 472 SD1_dist = 11.611/(SD1_val*3.3-0.0237);
haarkon 0:f00e68bef80c 473 if (SD1_dist > 40) SD1_dist = 40;
haarkon 0:f00e68bef80c 474 }
haarkon 0:f00e68bef80c 475
haarkon 0:f00e68bef80c 476 if (SD2_val < 0.06) {
haarkon 0:f00e68bef80c 477 SD2_val = 0;
haarkon 0:f00e68bef80c 478 SD2_dist = 40;
haarkon 0:f00e68bef80c 479 } else {
haarkon 0:f00e68bef80c 480 SD2_dist = 11.611/(SD2_val*3.3-0.0237);
haarkon 0:f00e68bef80c 481 if (SD2_dist > 40) SD2_dist = 40;
haarkon 0:f00e68bef80c 482 }
haarkon 0:f00e68bef80c 483
haarkon 0:f00e68bef80c 484 if (LD1_val < 0.1) {
haarkon 0:f00e68bef80c 485 LD1_val = 0;
haarkon 0:f00e68bef80c 486 LD1_dist = 150;
haarkon 0:f00e68bef80c 487 } else {
haarkon 0:f00e68bef80c 488 LD1_dist = 59.175/(LD1_val*3.3-0.0275);
haarkon 0:f00e68bef80c 489 if (LD1_dist > 150) LD1_dist = 150;
haarkon 0:f00e68bef80c 490 }
haarkon 0:f00e68bef80c 491
haarkon 0:f00e68bef80c 492 if (LD2_val < 0.1) {
haarkon 0:f00e68bef80c 493 LD2_val = 0;
haarkon 0:f00e68bef80c 494 LD2_dist = 150;
haarkon 0:f00e68bef80c 495 } else {
haarkon 0:f00e68bef80c 496 LD2_dist = 59.175/(LD2_val*3.3-0.0275);
haarkon 0:f00e68bef80c 497 if (LD2_dist > 150) LD2_dist = 150;
haarkon 0:f00e68bef80c 498 }
haarkon 0:f00e68bef80c 499
haarkon 0:f00e68bef80c 500 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 501 }
haarkon 0:f00e68bef80c 502 } while(!Pc.readable());
haarkon 0:f00e68bef80c 503 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 504 break;
haarkon 0:f00e68bef80c 505
haarkon 0:f00e68bef80c 506 case 4 :
haarkon 0:f00e68bef80c 507 Pc.printf ("\n\n\rCNY70\n");
haarkon 0:f00e68bef80c 508 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 509 do {
haarkon 0:f00e68bef80c 510 if (FlagTickLed) {
haarkon 0:f00e68bef80c 511 Led1 = !Led1;
haarkon 0:f00e68bef80c 512 FlagTickLed = 0;
haarkon 0:f00e68bef80c 513
haarkon 0:f00e68bef80c 514 CNY1_val = CNY1;
haarkon 0:f00e68bef80c 515 CNY2_val = CNY2;
haarkon 0:f00e68bef80c 516 CNY3_val = CNY3;
haarkon 0:f00e68bef80c 517
haarkon 0:f00e68bef80c 518 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 519 }
haarkon 0:f00e68bef80c 520 } while(!Pc.readable());
haarkon 0:f00e68bef80c 521 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 522 break;
haarkon 0:f00e68bef80c 523
haarkon 0:f00e68bef80c 524 case 5 :
haarkon 0:f00e68bef80c 525 Pc.printf ("\n\n\rVbat\n");
haarkon 0:f00e68bef80c 526 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 527 do {
haarkon 0:f00e68bef80c 528 if (FlagTickLed) {
haarkon 0:f00e68bef80c 529 Led1 = !Led1;
haarkon 0:f00e68bef80c 530 FlagTickLed = 0;
haarkon 0:f00e68bef80c 531
haarkon 0:f00e68bef80c 532 Vbat_val = Vbat;
haarkon 0:f00e68bef80c 533 Pc.printf ("\rVbat = %5.3lf V", Vbat_val*3.3*4.3);
haarkon 0:f00e68bef80c 534 }
haarkon 0:f00e68bef80c 535 } while(!Pc.readable());
haarkon 0:f00e68bef80c 536 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 537 break;
haarkon 0:f00e68bef80c 538
haarkon 0:f00e68bef80c 539 case 6 :
haarkon 0:f00e68bef80c 540 Pc.printf ("\n\n\rMoteur Gauche\n");
haarkon 0:f00e68bef80c 541 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 542 do {
haarkon 0:f00e68bef80c 543 if (FlagTickLed) {
haarkon 0:f00e68bef80c 544 Led1 = !Led1;
haarkon 0:f00e68bef80c 545 FlagTickLed = 0;
haarkon 0:f00e68bef80c 546 }
haarkon 0:f00e68bef80c 547
haarkon 0:f00e68bef80c 548
haarkon 0:f00e68bef80c 549
haarkon 0:f00e68bef80c 550 } while(!Pc.readable());
haarkon 0:f00e68bef80c 551 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 552 break;
haarkon 0:f00e68bef80c 553
haarkon 0:f00e68bef80c 554 case 7 :
haarkon 0:f00e68bef80c 555 Pc.printf ("\n\n\rMoteur Droit\n");
haarkon 0:f00e68bef80c 556 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 557 do {
haarkon 0:f00e68bef80c 558 if (FlagTickLed) {
haarkon 0:f00e68bef80c 559 Led1 = !Led1;
haarkon 0:f00e68bef80c 560 FlagTickLed = 0;
haarkon 0:f00e68bef80c 561 wait (0.1);
haarkon 0:f00e68bef80c 562 }
haarkon 0:f00e68bef80c 563 } while(!Pc.readable());
haarkon 0:f00e68bef80c 564 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 565 break;
haarkon 0:f00e68bef80c 566
haarkon 0:f00e68bef80c 567 case 8 :
haarkon 0:f00e68bef80c 568 Pc.printf ("\n\n\rServo Moteur\n");
haarkon 0:f00e68bef80c 569 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 570 do {
haarkon 0:f00e68bef80c 571 if (FlagTickLed) {
haarkon 0:f00e68bef80c 572 Led1 = !Led1;
haarkon 0:f00e68bef80c 573 FlagTickLed = 0;
haarkon 0:f00e68bef80c 574 }
haarkon 0:f00e68bef80c 575
haarkon 0:f00e68bef80c 576 if (((Tick%250)==0) && FlagTick) {
haarkon 0:f00e68bef80c 577 if (SERVO_dir == S_monte) {
haarkon 0:f00e68bef80c 578 if (SERVO_angle >= SERVO_angleMAX) {
haarkon 0:f00e68bef80c 579 SERVO_dir = S_descente;
haarkon 0:f00e68bef80c 580 SERVO_angle = SERVO_angleMAX;
haarkon 0:f00e68bef80c 581 } else {
haarkon 0:f00e68bef80c 582 SERVO_angle += 5;
haarkon 0:f00e68bef80c 583 }
haarkon 0:f00e68bef80c 584 } else {
haarkon 0:f00e68bef80c 585 if (SERVO_angle <= SERVO_angleMIN) {
haarkon 0:f00e68bef80c 586 SERVO_dir = S_monte;
haarkon 0:f00e68bef80c 587 SERVO_angle = SERVO_angleMIN;
haarkon 0:f00e68bef80c 588 } else {
haarkon 0:f00e68bef80c 589 SERVO_angle -= 5;
haarkon 0:f00e68bef80c 590 }
haarkon 0:f00e68bef80c 591 }
haarkon 0:f00e68bef80c 592 SERVO_pos = (lWord)(SERVO_angle*50/9) + 1000;
haarkon 0:f00e68bef80c 593 Servo.pulsewidth_us(SERVO_pos);
haarkon 0:f00e68bef80c 594 Pc.printf ("\rAngle = %4.1lf", SERVO_angle);
haarkon 0:f00e68bef80c 595 }
haarkon 0:f00e68bef80c 596 } while(!Pc.readable());
haarkon 0:f00e68bef80c 597 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 598 break;
haarkon 0:f00e68bef80c 599
haarkon 0:f00e68bef80c 600 case 9 :
haarkon 0:f00e68bef80c 601 Pc.printf ("\n\n\rPixy\n");
haarkon 0:f00e68bef80c 602 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 603 do {
haarkon 1:d95546f84105 604 if (((Tick%50)==0) && FlagTick) {
haarkon 1:d95546f84105 605 FlagTick = 0;
haarkon 0:f00e68bef80c 606 switch (phase) {
haarkon 0:f00e68bef80c 607 case 0 :
haarkon 0:f00e68bef80c 608 PIXY_red += 5;
haarkon 1:d95546f84105 609 if (PIXY_red == 255) {
haarkon 1:d95546f84105 610 phase = 1;
haarkon 1:d95546f84105 611 //Pc.printf ("\rPhase 1");
haarkon 1:d95546f84105 612 }
haarkon 0:f00e68bef80c 613 break;
haarkon 0:f00e68bef80c 614 case 1 :
haarkon 0:f00e68bef80c 615 PIXY_green += 5;
haarkon 1:d95546f84105 616 if (PIXY_green == 255) {
haarkon 1:d95546f84105 617 phase = 2;
haarkon 1:d95546f84105 618 //Pc.printf ("\rPhase 2");
haarkon 1:d95546f84105 619 }
haarkon 0:f00e68bef80c 620 break;
haarkon 0:f00e68bef80c 621 case 2 :
haarkon 0:f00e68bef80c 622 PIXY_red -= 5;
haarkon 1:d95546f84105 623 if (PIXY_red == 0) {
haarkon 1:d95546f84105 624 phase = 3;
haarkon 1:d95546f84105 625 //Pc.printf ("\rPhase 3");
haarkon 1:d95546f84105 626 }
haarkon 0:f00e68bef80c 627 break;
haarkon 0:f00e68bef80c 628 case 3 :
haarkon 0:f00e68bef80c 629 PIXY_blue += 5;
haarkon 1:d95546f84105 630 if (PIXY_blue == 255) {
haarkon 1:d95546f84105 631 phase = 4;
haarkon 1:d95546f84105 632 //Pc.printf ("\rPhase 4");
haarkon 1:d95546f84105 633 }
haarkon 0:f00e68bef80c 634 break;
haarkon 0:f00e68bef80c 635 case 4 :
haarkon 0:f00e68bef80c 636 PIXY_green -= 5;
haarkon 1:d95546f84105 637 if (PIXY_green == 0) {
haarkon 1:d95546f84105 638 phase = 5;
haarkon 1:d95546f84105 639 //Pc.printf ("\rPhase 5");
haarkon 1:d95546f84105 640 }
haarkon 0:f00e68bef80c 641 break;
haarkon 0:f00e68bef80c 642 case 5 :
haarkon 0:f00e68bef80c 643 PIXY_red += 5;
haarkon 1:d95546f84105 644 if (PIXY_red == 255) {
haarkon 1:d95546f84105 645 phase = 6;
haarkon 1:d95546f84105 646 //Pc.printf ("\rPhase 6");
haarkon 1:d95546f84105 647 }
haarkon 0:f00e68bef80c 648 break;
haarkon 0:f00e68bef80c 649 case 6 :
haarkon 0:f00e68bef80c 650 PIXY_green += 5;
haarkon 1:d95546f84105 651 if (PIXY_green == 255) {
haarkon 1:d95546f84105 652 phase = 7;
haarkon 1:d95546f84105 653 //Pc.printf ("\rPhase 7");
haarkon 1:d95546f84105 654 }
haarkon 0:f00e68bef80c 655 break;
haarkon 0:f00e68bef80c 656 case 7 :
haarkon 0:f00e68bef80c 657 PIXY_red -= 5;
haarkon 0:f00e68bef80c 658 PIXY_green -= 5;
haarkon 1:d95546f84105 659 PIXY_blue -=5;
haarkon 1:d95546f84105 660 if (PIXY_red == 0) {
haarkon 1:d95546f84105 661 phase = 0;
haarkon 1:d95546f84105 662 //Pc.printf ("\rPhase 0");
haarkon 1:d95546f84105 663 }
haarkon 0:f00e68bef80c 664 break;
haarkon 0:f00e68bef80c 665 }
haarkon 0:f00e68bef80c 666 Pixy.putc(0);
haarkon 1:d95546f84105 667 Pixy.putc(0xFD);
haarkon 0:f00e68bef80c 668 Pixy.putc(PIXY_red);
haarkon 0:f00e68bef80c 669 Pixy.putc(PIXY_green);
haarkon 0:f00e68bef80c 670 Pixy.putc(PIXY_blue);
haarkon 0:f00e68bef80c 671
haarkon 1:d95546f84105 672 while (Pixy_nbObjet) {
haarkon 1:d95546f84105 673 Pc.printf ("\r%5hd = %5hd,%5hd : %5hdx%5hd", Pixy_FIFO[Pixy_rObjet].CCbloc.signature, Pixy_FIFO[Pixy_rObjet].CCbloc.x, Pixy_FIFO[Pixy_rObjet].CCbloc.y, Pixy_FIFO[Pixy_rObjet].CCbloc.width, Pixy_FIFO[Pixy_rObjet].CCbloc.height);
haarkon 1:d95546f84105 674
haarkon 1:d95546f84105 675 if (Pixy_nbObjet>1) Pc.printf("\n");
haarkon 1:d95546f84105 676
haarkon 1:d95546f84105 677 if (Pixy_rObjet<19) Pixy_rObjet++;
haarkon 1:d95546f84105 678 else Pixy_rObjet = 0;
haarkon 1:d95546f84105 679
haarkon 1:d95546f84105 680 Pixy_nbObjet--;
haarkon 1:d95546f84105 681 }
haarkon 1:d95546f84105 682 FlagPixy = 0;
haarkon 0:f00e68bef80c 683 }
haarkon 1:d95546f84105 684
haarkon 1:d95546f84105 685 if (FlagTickLed) {
haarkon 1:d95546f84105 686 Led1 = !Led1;
haarkon 1:d95546f84105 687 FlagTickLed = 0;
haarkon 1:d95546f84105 688 }
haarkon 0:f00e68bef80c 689 } while(!Pc.readable());
haarkon 0:f00e68bef80c 690 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 691 break;
haarkon 0:f00e68bef80c 692
haarkon 0:f00e68bef80c 693 }
haarkon 0:f00e68bef80c 694 }
haarkon 0:f00e68bef80c 695 }