Jonas DOREL / Mbed 2 deprecated Asservissement

Asservissement

Dependencies:   Encoder_Nucleo_16_bits PwmIn mbed

Fork of TestBoard by IUT DE CACHAN GE1

main.cpp@10:4e1c8b3edee7, 2017-06-10 (annotated)

Committer:
DOREL
Date:
Sat Jun 10 12:23:42 2017 +0000
Revision:
10:4e1c8b3edee7
Parent:
9:9e8b83cf5083 
Motors work;

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 0:f00e68bef80c 118 * PA_0 -> Pixy RX (Serial)
haarkon 0:f00e68bef80c 119 * PA_1 -> Pixy TX (Serial)
haarkon 0:f00e68bef80c 120 * PA_2 -> PC TX (Serial)
haarkon 0:f00e68bef80c 121 * PA_3 -> PX RX (Serial)
haarkon 0:f00e68bef80c 122 * PA_4 -> GP2 SD #2 (Analog In)
haarkon 0:f00e68bef80c 123 * PA_5 -> LED1 (Digital Out)
haarkon 0:f00e68bef80c 124 * PA_6 -> CNY3 (Analog In)
haarkon 0:f00e68bef80c 125 * PA_7 -> CNY2 (Analog In)
haarkon 0:f00e68bef80c 126 * PA_8 -> Servomoteur (PWM Out)
haarkon 0:f00e68bef80c 127 * PA_9 -> US Trigger #3 (Digital Out)
haarkon 1:d95546f84105 128 * PA_10 -> US Echo #1 (Pwm In)
haarkon 0:f00e68bef80c 129 * PA_11 -> US Echo #2 (Pwm In)
haarkon 1:d95546f84105 130 * PA_12 -> SS (SPI Slave Select) (Digital Out)
haarkon 0:f00e68bef80c 131 * PA_13
haarkon 0:f00e68bef80c 132 * PA_14
haarkon 0:f00e68bef80c 133 * PA_15 -> Boussole (Pwm In)
haarkon 0:f00e68bef80c 134 *
haarkon 0:f00e68bef80c 135 * PB_0 -> GP2 SD #1 (Analog In)
haarkon 0:f00e68bef80c 136 * PB_1 -> Position D (Pwm In)
haarkon 0:f00e68bef80c 137 * PB_2 -> Position G (Pwm In)
haarkon 0:f00e68bef80c 138 * PB_3 -> PWM Mot D (PWM Out)
haarkon 0:f00e68bef80c 139 * PB_4 -> Enocdeur Droit A (QE)
haarkon 0:f00e68bef80c 140 * PB_5 -> Enocdeur Droit B (QE)
haarkon 0:f00e68bef80c 141 * PB_6 -> Enocdeur Gauche A (QE)
haarkon 0:f00e68bef80c 142 * PB_7 -> Enocdeur Gauche B (QE)
haarkon 0:f00e68bef80c 143 * PB_8 -> SCL (I2C)
haarkon 0:f00e68bef80c 144 * PB_9 -> SDA (I2C)
haarkon 0:f00e68bef80c 145 * PB_10 -> PWM Mot G (PWM Out)
haarkon 0:f00e68bef80c 146 * PB_11
haarkon 0:f00e68bef80c 147 * PB_12 -> US Echo #3 (Pwm In)
haarkon 0:f00e68bef80c 148 * PB_13 -> SCK Encodeur D (SPI)
haarkon 0:f00e68bef80c 149 * PB_14 -> MISO Encodeur D (SPI)
haarkon 0:f00e68bef80c 150 * PB_15 -> MOSI Encodeur D (SPI)
haarkon 0:f00e68bef80c 151 *
haarkon 0:f00e68bef80c 152 * PC_0 -> GP2 LD #1 (Analog In)
haarkon 0:f00e68bef80c 153 * PC_1 -> GP2 LD #2 (Analog In)
haarkon 0:f00e68bef80c 154 * PC_2 -> US Trigger #2 (Digital Out)
haarkon 0:f00e68bef80c 155 * PC_3 -> US Trigger #1 (Digital Out)
haarkon 0:f00e68bef80c 156 * PC_4 -> CNY1 (Analog In)
haarkon 0:f00e68bef80c 157 * PC_5 -> Vbat (Analog In)
haarkon 0:f00e68bef80c 158 * PC_6 -> Dir Mot Droit (Digital Out)
haarkon 0:f00e68bef80c 159 * PC_7 -> I (Encodeur Gauche) (IRQ In)
haarkon 0:f00e68bef80c 160 * PC_8 -> Dir Mot Gauche (Digital Out)
haarkon 0:f00e68bef80c 161 * PC_9 -> Enable Moteurs (Digital Out)
haarkon 0:f00e68bef80c 162 * PC_10 -> SCK Encodeur G (SPI)
haarkon 0:f00e68bef80c 163 * PC_11 -> MISO Encodeur G (SPI)
haarkon 0:f00e68bef80c 164 * PC_12 -> MOSI Encodeur G (SPI)
haarkon 0:f00e68bef80c 165 * PC_13 -> User BP (IRQ In)
haarkon 0:f00e68bef80c 166 * PC_14
haarkon 0:f00e68bef80c 167 * PC_15
haarkon 0:f00e68bef80c 168 *
haarkon 0:f00e68bef80c 169 * PD_1
haarkon 0:f00e68bef80c 170 * PD_2 -> Led2 (Digital Out)
haarkon 0:f00e68bef80c 171 */
haarkon 0:f00e68bef80c 172
haarkon 1:d95546f84105 173 Serial Pixy (PA_0, PA_1, 230400);
haarkon 5:4955cb4b3646 174 Serial Pc (PA_2, PA_3, 921600);
haarkon 0:f00e68bef80c 175
haarkon 0:f00e68bef80c 176 AnalogIn CNY1 (PC_4);
haarkon 0:f00e68bef80c 177 AnalogIn CNY2 (PA_7);
haarkon 0:f00e68bef80c 178 AnalogIn CNY3 (PA_6);
haarkon 0:f00e68bef80c 179 AnalogIn LD1 (PC_0);
haarkon 0:f00e68bef80c 180 AnalogIn LD2 (PC_1);
haarkon 0:f00e68bef80c 181 AnalogIn SD1 (PB_0);
haarkon 0:f00e68bef80c 182 AnalogIn SD2 (PA_4);
haarkon 0:f00e68bef80c 183 AnalogIn Vbat (PC_5);
haarkon 0:f00e68bef80c 184
haarkon 0:f00e68bef80c 185 DigitalOut Led1 (PA_5);
haarkon 0:f00e68bef80c 186 DigitalOut Led2 (PD_2);
haarkon 0:f00e68bef80c 187 DigitalOut Trig1 (PC_3);
haarkon 0:f00e68bef80c 188 DigitalOut Trig2 (PC_2);
haarkon 0:f00e68bef80c 189 DigitalOut Trig3 (PA_9);
haarkon 0:f00e68bef80c 190 DigitalOut En (PC_9);
haarkon 0:f00e68bef80c 191 DigitalOut SensG (PC_8);
haarkon 0:f00e68bef80c 192 DigitalOut SensD (PC_6);
haarkon 1:d95546f84105 193 DigitalOut SS (PA_12);
haarkon 0:f00e68bef80c 194
haarkon 1:d95546f84105 195 InterruptIn Echo1 (PA_10);
haarkon 0:f00e68bef80c 196 InterruptIn Echo2 (PA_11);
haarkon 0:f00e68bef80c 197 InterruptIn Echo3 (PB_12);
haarkon 0:f00e68bef80c 198 InterruptIn BP (PC_13);
haarkon 0:f00e68bef80c 199 InterruptIn IG (PC_7);
haarkon 6:88b4805d33e1 200 InterruptIn PWMG (PB_2);
haarkon 6:88b4805d33e1 201 InterruptIn PWMD (PB_1);
haarkon 1:d95546f84105 202
haarkon 1:d95546f84105 203 PwmIn PWMB (PA_15);
haarkon 0:f00e68bef80c 204
haarkon 0:f00e68bef80c 205 PwmOut Pwm_MG (PB_10);
haarkon 0:f00e68bef80c 206 PwmOut Pwm_MD (PB_3);
haarkon 0:f00e68bef80c 207 PwmOut Servo (PA_8);
haarkon 0:f00e68bef80c 208
haarkon 0:f00e68bef80c 209 I2C Bus_I2C (PB_9, PB_8);
haarkon 0:f00e68bef80c 210
haarkon 6:88b4805d33e1 211 SPI SPIG (PC_12, PC_11, PC_10);
haarkon 6:88b4805d33e1 212 SPI SPID (PB_15, PB_14, PB_13);
haarkon 0:f00e68bef80c 213
haarkon 0:f00e68bef80c 214 Nucleo_Encoder_16_bits Gauche (TIM4); // A = PB_6, B = PB_7
haarkon 0:f00e68bef80c 215 Nucleo_Encoder_16_bits Droite (TIM3); // A = PB_4, B = PB_5
haarkon 0:f00e68bef80c 216
haarkon 2:1d440e938c44 217 Ticker tick;
haarkon 2:1d440e938c44 218 Timer times;
haarkon 0:f00e68bef80c 219
haarkon 0:f00e68bef80c 220 /** Liste des variables globales
haarkon 0:f00e68bef80c 221 *
haarkon 0:f00e68bef80c 222 * 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 223 * FlagIG -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue gauche
haarkon 0:f00e68bef80c 224 * FlagID -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue droite
haarkon 0:f00e68bef80c 225 * EchoXStart -> (long) Donne le temps en µs de début de l'impulsion d'écho de l'US n°X
haarkon 0:f00e68bef80c 226 * DistUSX -> (float) Donne la distance en mm mesurée par l'US n°X
haarkon 0:f00e68bef80c 227 */
haarkon 0:f00e68bef80c 228
haarkon 0:f00e68bef80c 229 // Structure de temps
haarkon 6:88b4805d33e1 230 lWord Tick = 0, Gperiod, Dperiod;
haarkon 0:f00e68bef80c 231
haarkon 0:f00e68bef80c 232 // Sémaphore d'interruption
haarkon 3:3dfe26a10796 233 int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0;
haarkon 6:88b4805d33e1 234 int FlagTick = 0, FlagTickLed = 0, BPPressed = 0, nbTurnG = 0, nbTurnD = 0;
haarkon 3:3dfe26a10796 235 int Pixy_check = -1;
haarkon 0:f00e68bef80c 236
haarkon 0:f00e68bef80c 237 // Dialogue avec la Pixy
haarkon 3:3dfe26a10796 238 T_pixyCCData Pixy_CCFIFO[20];
haarkon 3:3dfe26a10796 239 T_pixyNMData Pixy_NMFIFO[20];
haarkon 3:3dfe26a10796 240 Byte Pixy_CCObjet, Pixy_NMObjet;
haarkon 0:f00e68bef80c 241
haarkon 0:f00e68bef80c 242 // Gestion des capteurs Ultrason
haarkon 3:3dfe26a10796 243 long Echo1Start, Echo2Start, Echo3Start;
haarkon 3:3dfe26a10796 244 double DistUS1, DistUS2, DistUS3;
haarkon 0:f00e68bef80c 245
haarkon 0:f00e68bef80c 246 /** Liste des interruptions
haarkon 0:f00e68bef80c 247 *
haarkon 0:f00e68bef80c 248 */
haarkon 0:f00e68bef80c 249
haarkon 0:f00e68bef80c 250 void tickTime()
haarkon 0:f00e68bef80c 251 {
haarkon 0:f00e68bef80c 252 Tick++;
haarkon 0:f00e68bef80c 253 FlagTick = 1;
haarkon 0:f00e68bef80c 254 if ((Tick%100)==0) FlagTickLed = 1;
haarkon 0:f00e68bef80c 255 }
haarkon 0:f00e68bef80c 256
haarkon 0:f00e68bef80c 257 void BPevent ()
haarkon 0:f00e68bef80c 258 {
haarkon 0:f00e68bef80c 259 BPPressed = 1;
haarkon 0:f00e68bef80c 260 }
haarkon 0:f00e68bef80c 261
haarkon 6:88b4805d33e1 262 void PWM_motGRise (void)
haarkon 6:88b4805d33e1 263 {
haarkon 6:88b4805d33e1 264 static lWord oldTime;
haarkon 6:88b4805d33e1 265 lWord now;
haarkon 6:88b4805d33e1 266 now = times.read_us();
haarkon 6:88b4805d33e1 267 Gperiod = now-oldTime;
haarkon 6:88b4805d33e1 268 oldTime = now;
haarkon 6:88b4805d33e1 269 nbTurnG++;
haarkon 6:88b4805d33e1 270 }
haarkon 6:88b4805d33e1 271
haarkon 6:88b4805d33e1 272 void PWM_motDRise (void)
haarkon 6:88b4805d33e1 273 {
haarkon 6:88b4805d33e1 274 static lWord oldTime;
haarkon 6:88b4805d33e1 275 lWord now;
haarkon 6:88b4805d33e1 276 now = times.read_us();
haarkon 6:88b4805d33e1 277 Dperiod = now-oldTime;
haarkon 6:88b4805d33e1 278 oldTime = now;
haarkon 6:88b4805d33e1 279 nbTurnD++;
haarkon 6:88b4805d33e1 280 }
haarkon 6:88b4805d33e1 281
haarkon 0:f00e68bef80c 282 void echo1Rise ()
haarkon 0:f00e68bef80c 283 {
haarkon 2:1d440e938c44 284 Echo1Start = times.read_us();
haarkon 0:f00e68bef80c 285 }
haarkon 0:f00e68bef80c 286
haarkon 0:f00e68bef80c 287 void echo2Rise ()
haarkon 0:f00e68bef80c 288 {
haarkon 2:1d440e938c44 289 Echo2Start = times.read_us();
haarkon 0:f00e68bef80c 290 }
haarkon 0:f00e68bef80c 291
haarkon 0:f00e68bef80c 292 void echo3Rise ()
haarkon 0:f00e68bef80c 293 {
haarkon 2:1d440e938c44 294 Echo3Start = times.read_us();
haarkon 0:f00e68bef80c 295 }
haarkon 0:f00e68bef80c 296
haarkon 0:f00e68bef80c 297 void echo1Fall ()
haarkon 0:f00e68bef80c 298 {
haarkon 2:1d440e938c44 299 DistUS1 = (double)(times.read_us() - Echo1Start)/5.8;
haarkon 0:f00e68bef80c 300 FlagUS1 = 1;
haarkon 0:f00e68bef80c 301 }
haarkon 0:f00e68bef80c 302
haarkon 0:f00e68bef80c 303 void echo2Fall ()
haarkon 0:f00e68bef80c 304 {
haarkon 2:1d440e938c44 305 DistUS2 = (double)(times.read_us() - Echo2Start)/5.8;
haarkon 0:f00e68bef80c 306 FlagUS2 = 1;
haarkon 0:f00e68bef80c 307 }
haarkon 0:f00e68bef80c 308
haarkon 0:f00e68bef80c 309 void echo3Fall ()
haarkon 0:f00e68bef80c 310 {
haarkon 2:1d440e938c44 311 DistUS3 = (double)(times.read_us() - Echo3Start)/5.8;
haarkon 0:f00e68bef80c 312 FlagUS3 = 1;
haarkon 0:f00e68bef80c 313 }
haarkon 0:f00e68bef80c 314
haarkon 0:f00e68bef80c 315 void getPixyByte ()
haarkon 0:f00e68bef80c 316 {
haarkon 2:1d440e938c44 317 static T_tmpBuffer tmpBuffer;
haarkon 2:1d440e938c44 318 static T_structBuffer msgBuffer;
haarkon 3:3dfe26a10796 319 static T_pixyState PIXY_state = none;
haarkon 2:1d440e938c44 320 static Byte byteCount = 0;
haarkon 3:3dfe26a10796 321 static int PIXY_synced = 0, dummy;
haarkon 2:1d440e938c44 322 int i, somme;
haarkon 3:3dfe26a10796 323 static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0;
haarkon 3:3dfe26a10796 324 static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0;
haarkon 0:f00e68bef80c 325
haarkon 2:1d440e938c44 326 Pixy_check = 0;
haarkon 0:f00e68bef80c 327
haarkon 3:3dfe26a10796 328 if (!PIXY_synced) { // On n'a pas trouvé le START (0x55aa0000)
haarkon 2:1d440e938c44 329 tmpBuffer.tab[byteCount] = Pixy.getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire
haarkon 2:1d440e938c44 330
haarkon 2:1d440e938c44 331 if (byteCount < 3) { // Si on n'a pas encore reçu les 4 premier octets
haarkon 2:1d440e938c44 332 byteCount++; // On passe à la case suivante du tableau temporaire
haarkon 2:1d440e938c44 333 } else { // Lorsqu'on a 4 octets
haarkon 2:1d440e938c44 334 if (tmpBuffer.mot != 0xaa550000) { // Si le code n'est pas le code de START
haarkon 2:1d440e938c44 335 for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau
haarkon 2:1d440e938c44 336 byteCount = 3; // Et on attend le caractère suivant
haarkon 2:1d440e938c44 337 } else { // Si on a trouvé le code de START
haarkon 3:3dfe26a10796 338 PIXY_synced = 1; // On passe en mode synchronisé
haarkon 3:3dfe26a10796 339 PIXY_state = begin;
haarkon 2:1d440e938c44 340 byteCount = 0;
haarkon 0:f00e68bef80c 341 }
haarkon 0:f00e68bef80c 342 }
haarkon 2:1d440e938c44 343 }
haarkon 2:1d440e938c44 344
haarkon 3:3dfe26a10796 345 if (PIXY_synced) {
haarkon 2:1d440e938c44 346
haarkon 3:3dfe26a10796 347 switch (PIXY_state) {
haarkon 2:1d440e938c44 348 case begin : // l'aiguillage est là !
haarkon 2:1d440e938c44 349
haarkon 2:1d440e938c44 350 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus
haarkon 2:1d440e938c44 351 byteCount++;
haarkon 2:1d440e938c44 352 if (byteCount == 2) { // Quand on a 2 octets
haarkon 2:1d440e938c44 353
haarkon 2:1d440e938c44 354 if (msgBuffer.mot == 0xaa55) { // Si c'est un bloc normal (code 0xAA55)
haarkon 3:3dfe26a10796 355 PIXY_state = normal; // On part vers le traitement spécifique
haarkon 2:1d440e938c44 356 }
haarkon 2:1d440e938c44 357
haarkon 2:1d440e938c44 358 if (msgBuffer.mot == 0xaa56) { // Si c'est un bloc Color Code (code 0xAA56)
haarkon 3:3dfe26a10796 359 PIXY_state = colorCode; // On part vers le traitement spécifique
haarkon 2:1d440e938c44 360 }
haarkon 2:1d440e938c44 361
haarkon 2:1d440e938c44 362 if (msgBuffer.mot == 0) { // Si on a un debut de trame (code 0000)
haarkon 3:3dfe26a10796 363 PIXY_state = doubleZero; // On part vers le traitement spécifique
haarkon 2:1d440e938c44 364 }
haarkon 3:3dfe26a10796 365 if ((PIXY_state == begin) || (PIXY_state == none)) { // Si c'est autre chose
haarkon 3:3dfe26a10796 366 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
haarkon 3:3dfe26a10796 367 PIXY_state = none; // Ceinture et bretelle
haarkon 2:1d440e938c44 368 }
haarkon 2:1d440e938c44 369 byteCount = 0;
haarkon 2:1d440e938c44 370 }
haarkon 2:1d440e938c44 371 break;
haarkon 2:1d440e938c44 372
haarkon 2:1d440e938c44 373 case normal : // Si on a un bloc normal
haarkon 2:1d440e938c44 374
haarkon 3:3dfe26a10796 375 Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = Pixy.getc(); // On stocke les octets un à un dans la structure Bloc
haarkon 2:1d440e938c44 376 if (byteCount < 11) { // Tant que la structure n'est pas pleine
haarkon 2:1d440e938c44 377 byteCount++; // On passe à l'octet suivant
haarkon 2:1d440e938c44 378 } else { // Quand elle est pleine
haarkon 2:1d440e938c44 379 byteCount = 0; // On réinitialise
haarkon 3:3dfe26a10796 380 PIXY_state = begin; // On retourne à l'aiguillage
haarkon 2:1d440e938c44 381 // On calcule la somme de contrôle
haarkon 3:3dfe26a10796 382 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 383
haarkon 3:3dfe26a10796 384 if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) { // Si le checksum est bon, on valide la réception
haarkon 3:3dfe26a10796 385 if (PIXY_wNMObjet < 19) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet
haarkon 3:3dfe26a10796 386 else PIXY_wNMObjet = 0;
haarkon 3:3dfe26a10796 387 if (PIXY_nbNMObjet < 19) PIXY_nbNMObjet++; // On dit que l'on a un objet CC de plus
haarkon 2:1d440e938c44 388 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
haarkon 2:1d440e938c44 389 }
haarkon 2:1d440e938c44 390 }
haarkon 2:1d440e938c44 391 break;
haarkon 2:1d440e938c44 392
haarkon 2:1d440e938c44 393 case colorCode : // Si on a un bloc colorCode
haarkon 2:1d440e938c44 394
haarkon 3:3dfe26a10796 395 Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc
haarkon 2:1d440e938c44 396 if (byteCount < 13) byteCount++; // tant que la structure n'est pas pleine on passe à l'octet suivant
haarkon 2:1d440e938c44 397 else { // Quand elle est pleine
haarkon 2:1d440e938c44 398 byteCount = 0; // On réinitialise
haarkon 3:3dfe26a10796 399 PIXY_state = begin; // On retourne à l'aiguillage
haarkon 2:1d440e938c44 400 // On calcule la somme de contrôle
haarkon 3:3dfe26a10796 401 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 402
haarkon 3:3dfe26a10796 403 if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) { // Si le checksum est bon
haarkon 3:3dfe26a10796 404 if (PIXY_wCCObjet < 19) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet
haarkon 3:3dfe26a10796 405 else PIXY_wCCObjet = 0;
haarkon 3:3dfe26a10796 406 if (PIXY_nbCCObjet < 19) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter
haarkon 2:1d440e938c44 407 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
haarkon 2:1d440e938c44 408 }
haarkon 2:1d440e938c44 409 }
haarkon 2:1d440e938c44 410 break;
haarkon 2:1d440e938c44 411
haarkon 2:1d440e938c44 412 case doubleZero : // Si on a reçu le code de début d'une nouvelle trame.
haarkon 2:1d440e938c44 413
haarkon 2:1d440e938c44 414 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus
haarkon 2:1d440e938c44 415 byteCount++;
haarkon 2:1d440e938c44 416 if (byteCount == 2) { // Quand on a 2 octets
haarkon 2:1d440e938c44 417 if (msgBuffer.mot == 0xaa55) { // On doit impérativement trouver le code 0xAA55
haarkon 3:3dfe26a10796 418 PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage
haarkon 3:3dfe26a10796 419 Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement
haarkon 3:3dfe26a10796 420 Pixy_CCObjet = PIXY_nbCCObjet;
haarkon 3:3dfe26a10796 421 PIXY_nbCCObjet = 0;
haarkon 3:3dfe26a10796 422 PIXY_nbNMObjet = 0;
haarkon 3:3dfe26a10796 423 FlagPixy = 1; // On valide le traitement de la trame précédente.
haarkon 2:1d440e938c44 424 } else { // Si on trouve autre chose
haarkon 3:3dfe26a10796 425 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
haarkon 3:3dfe26a10796 426 PIXY_state = none; // Ceinture et bretelle
haarkon 2:1d440e938c44 427 }
haarkon 2:1d440e938c44 428 byteCount = 0;
haarkon 2:1d440e938c44 429 }
haarkon 2:1d440e938c44 430 break;
haarkon 0:f00e68bef80c 431 }
haarkon 0:f00e68bef80c 432 }
haarkon 0:f00e68bef80c 433 }
haarkon 0:f00e68bef80c 434
DOREL 9:9e8b83cf5083 435 void motor_command(int mg_command, int md_command, float* mg_pwm, float* md_pwm, int* mg_sens, int* md_sens) {
DOREL 9:9e8b83cf5083 436 if(mg_command >= 0) {
DOREL 10:4e1c8b3edee7 437 *mg_sens = 1;
DOREL 10:4e1c8b3edee7 438 *mg_pwm = 1-((float)(mg_command%101)/100);
DOREL 9:9e8b83cf5083 439 } else {
DOREL 10:4e1c8b3edee7 440 *mg_sens = 0;
DOREL 10:4e1c8b3edee7 441 *mg_pwm = ((float)(-mg_command%101)/100);
DOREL 9:9e8b83cf5083 442 }
DOREL 9:9e8b83cf5083 443 if(md_command >= 0) {
DOREL 10:4e1c8b3edee7 444 *md_sens = 0;
DOREL 10:4e1c8b3edee7 445 *md_pwm = ((float)(md_command%101)/100);
DOREL 9:9e8b83cf5083 446 } else {
DOREL 9:9e8b83cf5083 447 *md_sens = 1;
DOREL 10:4e1c8b3edee7 448 *md_pwm = 1-((float)(-md_command%101)/100);
DOREL 9:9e8b83cf5083 449 }
DOREL 9:9e8b83cf5083 450 }
haarkon 2:1d440e938c44 451
haarkon 0:f00e68bef80c 452 int main()
haarkon 0:f00e68bef80c 453 {
haarkon 0:f00e68bef80c 454
haarkon 2:1d440e938c44 455 int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/;
haarkon 5:4955cb4b3646 456
haarkon 3:3dfe26a10796 457 Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0;
haarkon 2:1d440e938c44 458 int PIXY_objet;
haarkon 0:f00e68bef80c 459
haarkon 6:88b4805d33e1 460 int SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400;
haarkon 0:f00e68bef80c 461 T_SERVODIR SERVO_dir = S_monte;
haarkon 0:f00e68bef80c 462
haarkon 0:f00e68bef80c 463 char MENU_choix = 0;
haarkon 0:f00e68bef80c 464
haarkon 2:1d440e938c44 465 char BOUSSOLE_status[1] = {0};
haarkon 0:f00e68bef80c 466 char I2C_registerValue[4];
haarkon 3:3dfe26a10796 467 double BOUSSOLE_periode;
haarkon 0:f00e68bef80c 468
haarkon 0:f00e68bef80c 469 double CAP_I2C, CAP_PWM;
haarkon 0:f00e68bef80c 470 double SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val;
haarkon 0:f00e68bef80c 471 double SD1_dist, SD2_dist, LD1_dist, LD2_dist;
haarkon 0:f00e68bef80c 472
DOREL 9:9e8b83cf5083 473 float md_pwm = 0;
DOREL 9:9e8b83cf5083 474 float mg_pwm = 0;
DOREL 9:9e8b83cf5083 475 int md_sens = 1;
DOREL 9:9e8b83cf5083 476 int mg_sens = 1;
DOREL 8:0977c3794f10 477
DOREL 10:4e1c8b3edee7 478 int i = 0;
DOREL 9:9e8b83cf5083 479 float val_carre_balle=0, val_x_balle=0;
DOREL 9:9e8b83cf5083 480
DOREL 10:4e1c8b3edee7 481 float erreur_asservissement, commande_moteur = 30, mesure_angle_pixy;
haarkon 5:4955cb4b3646 482
haarkon 2:1d440e938c44 483 times.reset();
haarkon 2:1d440e938c44 484 times.start();
haarkon 0:f00e68bef80c 485
haarkon 6:88b4805d33e1 486 // Initialisation des interruptions
haarkon 2:1d440e938c44 487 tick.attach(&tickTime, 0.001);
haarkon 0:f00e68bef80c 488
haarkon 6:88b4805d33e1 489 BP.rise (&BPevent);
haarkon 0:f00e68bef80c 490
haarkon 6:88b4805d33e1 491 Echo1.rise (&echo1Rise);
haarkon 6:88b4805d33e1 492 Echo2.rise (&echo2Rise);
haarkon 6:88b4805d33e1 493 Echo3.rise (&echo3Rise);
haarkon 6:88b4805d33e1 494 Echo1.fall (&echo1Fall);
haarkon 6:88b4805d33e1 495 Echo2.fall (&echo2Fall);
haarkon 6:88b4805d33e1 496 Echo3.fall (&echo3Fall);
haarkon 6:88b4805d33e1 497
haarkon 6:88b4805d33e1 498 PWMG.rise (&PWM_motGRise);
haarkon 6:88b4805d33e1 499 PWMD.rise (&PWM_motDRise);
haarkon 6:88b4805d33e1 500
haarkon 6:88b4805d33e1 501 Pixy.attach (&getPixyByte);
haarkon 0:f00e68bef80c 502
haarkon 1:d95546f84105 503 BP.enable_irq();
haarkon 0:f00e68bef80c 504 IG.enable_irq();
haarkon 0:f00e68bef80c 505 Echo1.enable_irq();
haarkon 0:f00e68bef80c 506 Echo2.enable_irq();
haarkon 0:f00e68bef80c 507 Echo3.enable_irq();
haarkon 6:88b4805d33e1 508 PWMG.enable_irq();
haarkon 6:88b4805d33e1 509 PWMD.enable_irq();
haarkon 0:f00e68bef80c 510
haarkon 6:88b4805d33e1 511 // Initialisation des périphériques
haarkon 6:88b4805d33e1 512 // Bus I2C
haarkon 6:88b4805d33e1 513 Bus_I2C.frequency (100000);
haarkon 0:f00e68bef80c 514
haarkon 6:88b4805d33e1 515 // PWM des moteurs
haarkon 0:f00e68bef80c 516 Pwm_MG.period_us(50);
haarkon 0:f00e68bef80c 517 Pwm_MD.period_us(50);
haarkon 0:f00e68bef80c 518 En = 0;
haarkon 5:4955cb4b3646 519
haarkon 6:88b4805d33e1 520 // Bus SPI
haarkon 6:88b4805d33e1 521 SPIG.format (16,1);
haarkon 6:88b4805d33e1 522 SPIG.frequency (1000000);
haarkon 6:88b4805d33e1 523
haarkon 6:88b4805d33e1 524 SPID.format (16,1);
haarkon 6:88b4805d33e1 525 SPID.frequency (1000000);
haarkon 6:88b4805d33e1 526
haarkon 6:88b4805d33e1 527 SS = 1;
haarkon 6:88b4805d33e1 528
haarkon 6:88b4805d33e1 529 // Led
haarkon 0:f00e68bef80c 530 Led2 = 0;
haarkon 0:f00e68bef80c 531
DOREL 10:4e1c8b3edee7 532 En = 1;
DOREL 10:4e1c8b3edee7 533
haarkon 6:88b4805d33e1 534 Servo.period_ms (20);
DOREL 10:4e1c8b3edee7 535 Servo.pulsewidth_us(200);
DOREL 10:4e1c8b3edee7 536
DOREL 10:4e1c8b3edee7 537 Pc.printf("test1\n\r");
DOREL 10:4e1c8b3edee7 538
DOREL 10:4e1c8b3edee7 539 motor_command(60, -60, &mg_pwm, &md_pwm, &mg_sens, &md_sens);
DOREL 10:4e1c8b3edee7 540 Pwm_MD = md_pwm;
DOREL 10:4e1c8b3edee7 541 Pwm_MG = mg_pwm;
DOREL 10:4e1c8b3edee7 542 SensD = md_sens;
DOREL 10:4e1c8b3edee7 543 SensG = mg_sens;
DOREL 10:4e1c8b3edee7 544 wait_ms(3000);
DOREL 10:4e1c8b3edee7 545
DOREL 10:4e1c8b3edee7 546 Pc.printf("Pwm_MG = %lf, Pwm_MD = %lf, SensG = %d, SensD = %d\r\n", mg_pwm, md_pwm, mg_sens, md_sens);
haarkon 0:f00e68bef80c 547
DOREL 10:4e1c8b3edee7 548 Pwm_MD = 0;
DOREL 10:4e1c8b3edee7 549 Pwm_MG = 0;
DOREL 10:4e1c8b3edee7 550 SensD = 0;
DOREL 10:4e1c8b3edee7 551 SensG = 0;
DOREL 10:4e1c8b3edee7 552
haarkon 0:f00e68bef80c 553 while(1) {
DOREL 10:4e1c8b3edee7 554 /*
DOREL 9:9e8b83cf5083 555 val_x_balle = 0;
DOREL 9:9e8b83cf5083 556 i = 0;
DOREL 10:4e1c8b3edee7 557 while ((val_x_balle==0) && (i<19)) {
DOREL 8:0977c3794f10 558 //test carre +/-5%
DOREL 8:0977c3794f10 559 val_carre_balle = (float)Pixy_NMFIFO[i].NMbloc.height/(float)Pixy_NMFIFO[i].NMbloc.width;
DOREL 8:0977c3794f10 560 if ((0.85<val_carre_balle) && (val_carre_balle<1.15)) val_x_balle =Pixy_NMFIFO[i].NMbloc.x+Pixy_NMFIFO[i].NMbloc.width/2;
DOREL 8:0977c3794f10 561 i++;
DOREL 9:9e8b83cf5083 562 }
DOREL 9:9e8b83cf5083 563 mesure_angle_pixy=(val_x_balle-160)*(37.5/160);
DOREL 9:9e8b83cf5083 564 Pc.printf("%lf %lf\r\n", val_x_balle, mesure_angle_pixy);
DOREL 8:0977c3794f10 565 wait_us(50000);
DOREL 8:0977c3794f10 566 erreur_asservissement = 2 * (-mesure_angle_pixy);
DOREL 8:0977c3794f10 567 motor_command(commande_moteur+erreur_asservissement, commande_moteur-erreur_asservissement, &mg_pwm, &md_pwm, &mg_sens, &md_sens);
DOREL 10:4e1c8b3edee7 568 mesure_angle_pixy;
DOREL 10:4e1c8b3edee7 569 Pwm_MD = md_pwm;
DOREL 10:4e1c8b3edee7 570 Pwm_MG = mg_pwm;
DOREL 10:4e1c8b3edee7 571 SensD = md_sens;
DOREL 10:4e1c8b3edee7 572 SensG = mg_sens;
DOREL 10:4e1c8b3edee7 573 */
haarkon 0:f00e68bef80c 574 }
haarkon 0:f00e68bef80c 575 }