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@11:df0957bf8f16, 2020-03-09 (annotated)

Committer:
gvaquette
Date:
Mon Mar 09 05:54:45 2020 +0000
Revision:
11:df0957bf8f16
Parent:
10:c6db803927b6
Child:
12:ee9e5009f7ce
Some english translate;

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