Chafik ZERROUKI / Mbed 2 deprecated TestBoardv2_boussole_pixi

hkb

Dependencies:   Encoder_Nucleo_16_bits PwmIn mbed

Fork of TestBoardv2_boussole_pixi by ilan Sandoz

main.cpp@9:7584e3904be6, 2017-06-09 (annotated)

Committer:
Cachan1
Date:
Fri Jun 09 11:14:04 2017 +0000
Revision:
9:7584e3904be6
Parent:
8:527f5db9b3dc 
jnpj;

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;
illan 8:527f5db9b3dc 236 char BOUSSOLE_status[1] = {0};
illan 8:527f5db9b3dc 237 char I2C_registerValue[4];
illan 8:527f5db9b3dc 238
haarkon 0:f00e68bef80c 239 // Dialogue avec la Pixy
haarkon 3:3dfe26a10796 240 T_pixyCCData Pixy_CCFIFO[20];
haarkon 3:3dfe26a10796 241 T_pixyNMData Pixy_NMFIFO[20];
haarkon 3:3dfe26a10796 242 Byte Pixy_CCObjet, Pixy_NMObjet;
haarkon 0:f00e68bef80c 243
haarkon 0:f00e68bef80c 244 // Gestion des capteurs Ultrason
haarkon 3:3dfe26a10796 245 long Echo1Start, Echo2Start, Echo3Start;
haarkon 3:3dfe26a10796 246 double DistUS1, DistUS2, DistUS3;
illan 8:527f5db9b3dc 247 int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/;
illan 8:527f5db9b3dc 248 int SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400;
illan 8:527f5db9b3dc 249 T_SERVODIR SERVO_dir = S_monte;
illan 8:527f5db9b3dc 250 double MOTG_duty = 0.5, MOTD_duty = 0.5;
illan 8:527f5db9b3dc 251 double CAP_I2C, CAP_PWM;
haarkon 0:f00e68bef80c 252
haarkon 0:f00e68bef80c 253 /** Liste des interruptions
haarkon 0:f00e68bef80c 254 *
haarkon 0:f00e68bef80c 255 */
illan 8:527f5db9b3dc 256
illan 8:527f5db9b3dc 257 void capteur_ultrason(void);
illan 8:527f5db9b3dc 258 void boussole(void);
illan 8:527f5db9b3dc 259 void capteur_infrarouge(void);
illan 8:527f5db9b3dc 260 void CNY(void);
illan 8:527f5db9b3dc 261 void servomoteur(void);
illan 8:527f5db9b3dc 262 void avancer(void);
illan 8:527f5db9b3dc 263 void tour_droit(void);
illan 8:527f5db9b3dc 264 void tour_gauche(void);
haarkon 0:f00e68bef80c 265
haarkon 0:f00e68bef80c 266 void tickTime()
haarkon 0:f00e68bef80c 267 {
haarkon 0:f00e68bef80c 268 Tick++;
haarkon 0:f00e68bef80c 269 FlagTick = 1;
haarkon 0:f00e68bef80c 270 if ((Tick%100)==0) FlagTickLed = 1;
haarkon 0:f00e68bef80c 271 }
haarkon 0:f00e68bef80c 272
haarkon 0:f00e68bef80c 273 void BPevent ()
haarkon 0:f00e68bef80c 274 {
haarkon 0:f00e68bef80c 275 BPPressed = 1;
haarkon 0:f00e68bef80c 276 }
haarkon 0:f00e68bef80c 277
haarkon 6:88b4805d33e1 278 void PWM_motGRise (void)
haarkon 6:88b4805d33e1 279 {
haarkon 6:88b4805d33e1 280 static lWord oldTime;
haarkon 6:88b4805d33e1 281 lWord now;
haarkon 6:88b4805d33e1 282 now = times.read_us();
haarkon 6:88b4805d33e1 283 Gperiod = now-oldTime;
haarkon 6:88b4805d33e1 284 oldTime = now;
haarkon 6:88b4805d33e1 285 nbTurnG++;
haarkon 6:88b4805d33e1 286 }
haarkon 6:88b4805d33e1 287
haarkon 6:88b4805d33e1 288 void PWM_motDRise (void)
haarkon 6:88b4805d33e1 289 {
haarkon 6:88b4805d33e1 290 static lWord oldTime;
haarkon 6:88b4805d33e1 291 lWord now;
haarkon 6:88b4805d33e1 292 now = times.read_us();
haarkon 6:88b4805d33e1 293 Dperiod = now-oldTime;
haarkon 6:88b4805d33e1 294 oldTime = now;
haarkon 6:88b4805d33e1 295 nbTurnD++;
haarkon 6:88b4805d33e1 296 }
haarkon 6:88b4805d33e1 297
haarkon 0:f00e68bef80c 298 void echo1Rise ()
haarkon 0:f00e68bef80c 299 {
haarkon 2:1d440e938c44 300 Echo1Start = times.read_us();
haarkon 0:f00e68bef80c 301 }
haarkon 0:f00e68bef80c 302
haarkon 0:f00e68bef80c 303 void echo2Rise ()
haarkon 0:f00e68bef80c 304 {
haarkon 2:1d440e938c44 305 Echo2Start = times.read_us();
haarkon 0:f00e68bef80c 306 }
haarkon 0:f00e68bef80c 307
haarkon 0:f00e68bef80c 308 void echo3Rise ()
haarkon 0:f00e68bef80c 309 {
haarkon 2:1d440e938c44 310 Echo3Start = times.read_us();
haarkon 0:f00e68bef80c 311 }
haarkon 0:f00e68bef80c 312
haarkon 0:f00e68bef80c 313 void echo1Fall ()
haarkon 0:f00e68bef80c 314 {
haarkon 2:1d440e938c44 315 DistUS1 = (double)(times.read_us() - Echo1Start)/5.8;
haarkon 0:f00e68bef80c 316 FlagUS1 = 1;
haarkon 0:f00e68bef80c 317 }
haarkon 0:f00e68bef80c 318
haarkon 0:f00e68bef80c 319 void echo2Fall ()
haarkon 0:f00e68bef80c 320 {
haarkon 2:1d440e938c44 321 DistUS2 = (double)(times.read_us() - Echo2Start)/5.8;
haarkon 0:f00e68bef80c 322 FlagUS2 = 1;
haarkon 0:f00e68bef80c 323 }
haarkon 0:f00e68bef80c 324
haarkon 0:f00e68bef80c 325 void echo3Fall ()
haarkon 0:f00e68bef80c 326 {
haarkon 2:1d440e938c44 327 DistUS3 = (double)(times.read_us() - Echo3Start)/5.8;
haarkon 0:f00e68bef80c 328 FlagUS3 = 1;
haarkon 0:f00e68bef80c 329 }
haarkon 0:f00e68bef80c 330
haarkon 0:f00e68bef80c 331 void getPixyByte ()
haarkon 0:f00e68bef80c 332 {
haarkon 2:1d440e938c44 333 static T_tmpBuffer tmpBuffer;
haarkon 2:1d440e938c44 334 static T_structBuffer msgBuffer;
haarkon 3:3dfe26a10796 335 static T_pixyState PIXY_state = none;
haarkon 2:1d440e938c44 336 static Byte byteCount = 0;
haarkon 3:3dfe26a10796 337 static int PIXY_synced = 0, dummy;
haarkon 2:1d440e938c44 338 int i, somme;
haarkon 3:3dfe26a10796 339 static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0;
haarkon 3:3dfe26a10796 340 static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0;
haarkon 0:f00e68bef80c 341
haarkon 2:1d440e938c44 342 Pixy_check = 0;
haarkon 0:f00e68bef80c 343
haarkon 3:3dfe26a10796 344 if (!PIXY_synced) { // On n'a pas trouvé le START (0x55aa0000)
haarkon 2:1d440e938c44 345 tmpBuffer.tab[byteCount] = Pixy.getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire
haarkon 2:1d440e938c44 346
haarkon 2:1d440e938c44 347 if (byteCount < 3) { // Si on n'a pas encore reçu les 4 premier octets
haarkon 2:1d440e938c44 348 byteCount++; // On passe à la case suivante du tableau temporaire
haarkon 2:1d440e938c44 349 } else { // Lorsqu'on a 4 octets
haarkon 2:1d440e938c44 350 if (tmpBuffer.mot != 0xaa550000) { // Si le code n'est pas le code de START
haarkon 2:1d440e938c44 351 for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau
haarkon 2:1d440e938c44 352 byteCount = 3; // Et on attend le caractère suivant
haarkon 2:1d440e938c44 353 } else { // Si on a trouvé le code de START
haarkon 3:3dfe26a10796 354 PIXY_synced = 1; // On passe en mode synchronisé
haarkon 3:3dfe26a10796 355 PIXY_state = begin;
haarkon 2:1d440e938c44 356 byteCount = 0;
haarkon 0:f00e68bef80c 357 }
haarkon 0:f00e68bef80c 358 }
haarkon 2:1d440e938c44 359 }
haarkon 2:1d440e938c44 360
haarkon 3:3dfe26a10796 361 if (PIXY_synced) {
haarkon 2:1d440e938c44 362
haarkon 3:3dfe26a10796 363 switch (PIXY_state) {
haarkon 2:1d440e938c44 364 case begin : // l'aiguillage est là !
haarkon 2:1d440e938c44 365
haarkon 2:1d440e938c44 366 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus
haarkon 2:1d440e938c44 367 byteCount++;
haarkon 2:1d440e938c44 368 if (byteCount == 2) { // Quand on a 2 octets
haarkon 2:1d440e938c44 369
haarkon 2:1d440e938c44 370 if (msgBuffer.mot == 0xaa55) { // Si c'est un bloc normal (code 0xAA55)
haarkon 3:3dfe26a10796 371 PIXY_state = normal; // On part vers le traitement spécifique
haarkon 2:1d440e938c44 372 }
haarkon 2:1d440e938c44 373
haarkon 2:1d440e938c44 374 if (msgBuffer.mot == 0xaa56) { // Si c'est un bloc Color Code (code 0xAA56)
haarkon 3:3dfe26a10796 375 PIXY_state = colorCode; // On part vers le traitement spécifique
haarkon 2:1d440e938c44 376 }
haarkon 2:1d440e938c44 377
haarkon 2:1d440e938c44 378 if (msgBuffer.mot == 0) { // Si on a un debut de trame (code 0000)
haarkon 3:3dfe26a10796 379 PIXY_state = doubleZero; // On part vers le traitement spécifique
haarkon 2:1d440e938c44 380 }
haarkon 3:3dfe26a10796 381 if ((PIXY_state == begin) || (PIXY_state == none)) { // Si c'est autre chose
haarkon 3:3dfe26a10796 382 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
haarkon 3:3dfe26a10796 383 PIXY_state = none; // Ceinture et bretelle
haarkon 2:1d440e938c44 384 }
haarkon 2:1d440e938c44 385 byteCount = 0;
haarkon 2:1d440e938c44 386 }
haarkon 2:1d440e938c44 387 break;
haarkon 2:1d440e938c44 388
haarkon 2:1d440e938c44 389 case normal : // Si on a un bloc normal
haarkon 2:1d440e938c44 390
haarkon 3:3dfe26a10796 391 Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = Pixy.getc(); // On stocke les octets un à un dans la structure Bloc
haarkon 2:1d440e938c44 392 if (byteCount < 11) { // Tant que la structure n'est pas pleine
haarkon 2:1d440e938c44 393 byteCount++; // On passe à l'octet suivant
haarkon 2:1d440e938c44 394 } else { // Quand elle est pleine
haarkon 2:1d440e938c44 395 byteCount = 0; // On réinitialise
haarkon 3:3dfe26a10796 396 PIXY_state = begin; // On retourne à l'aiguillage
haarkon 2:1d440e938c44 397 // On calcule la somme de contrôle
haarkon 3:3dfe26a10796 398 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 399
haarkon 3:3dfe26a10796 400 if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) { // Si le checksum est bon, on valide la réception
haarkon 3:3dfe26a10796 401 if (PIXY_wNMObjet < 19) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet
haarkon 3:3dfe26a10796 402 else PIXY_wNMObjet = 0;
haarkon 3:3dfe26a10796 403 if (PIXY_nbNMObjet < 19) PIXY_nbNMObjet++; // On dit que l'on a un objet CC de plus
haarkon 2:1d440e938c44 404 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
haarkon 2:1d440e938c44 405 }
haarkon 2:1d440e938c44 406 }
haarkon 2:1d440e938c44 407 break;
haarkon 2:1d440e938c44 408
haarkon 2:1d440e938c44 409 case colorCode : // Si on a un bloc colorCode
haarkon 2:1d440e938c44 410
haarkon 3:3dfe26a10796 411 Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc
haarkon 2:1d440e938c44 412 if (byteCount < 13) byteCount++; // tant que la structure n'est pas pleine on passe à l'octet suivant
haarkon 2:1d440e938c44 413 else { // Quand elle est pleine
haarkon 2:1d440e938c44 414 byteCount = 0; // On réinitialise
haarkon 3:3dfe26a10796 415 PIXY_state = begin; // On retourne à l'aiguillage
haarkon 2:1d440e938c44 416 // On calcule la somme de contrôle
haarkon 3:3dfe26a10796 417 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 418
haarkon 3:3dfe26a10796 419 if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) { // Si le checksum est bon
haarkon 3:3dfe26a10796 420 if (PIXY_wCCObjet < 19) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet
haarkon 3:3dfe26a10796 421 else PIXY_wCCObjet = 0;
haarkon 3:3dfe26a10796 422 if (PIXY_nbCCObjet < 19) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter
haarkon 2:1d440e938c44 423 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
haarkon 2:1d440e938c44 424 }
haarkon 2:1d440e938c44 425 }
haarkon 2:1d440e938c44 426 break;
haarkon 2:1d440e938c44 427
haarkon 2:1d440e938c44 428 case doubleZero : // Si on a reçu le code de début d'une nouvelle trame.
haarkon 2:1d440e938c44 429
haarkon 2:1d440e938c44 430 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus
haarkon 2:1d440e938c44 431 byteCount++;
haarkon 2:1d440e938c44 432 if (byteCount == 2) { // Quand on a 2 octets
haarkon 2:1d440e938c44 433 if (msgBuffer.mot == 0xaa55) { // On doit impérativement trouver le code 0xAA55
haarkon 3:3dfe26a10796 434 PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage
haarkon 3:3dfe26a10796 435 Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement
haarkon 3:3dfe26a10796 436 Pixy_CCObjet = PIXY_nbCCObjet;
haarkon 3:3dfe26a10796 437 PIXY_nbCCObjet = 0;
haarkon 3:3dfe26a10796 438 PIXY_nbNMObjet = 0;
haarkon 3:3dfe26a10796 439 FlagPixy = 1; // On valide le traitement de la trame précédente.
haarkon 2:1d440e938c44 440 } else { // Si on trouve autre chose
haarkon 3:3dfe26a10796 441 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
haarkon 3:3dfe26a10796 442 PIXY_state = none; // Ceinture et bretelle
haarkon 2:1d440e938c44 443 }
haarkon 2:1d440e938c44 444 byteCount = 0;
haarkon 2:1d440e938c44 445 }
haarkon 2:1d440e938c44 446 break;
haarkon 0:f00e68bef80c 447 }
haarkon 0:f00e68bef80c 448 }
haarkon 0:f00e68bef80c 449 }
haarkon 0:f00e68bef80c 450
haarkon 2:1d440e938c44 451
haarkon 0:f00e68bef80c 452 int main()
haarkon 0:f00e68bef80c 453 {
haarkon 0:f00e68bef80c 454
Cachan1 9:7584e3904be6 455 int i = 0, val_H_balle = 0, val_W_balle=0, max_val_W_balle = 0, indice_balle = 0, val_carre_balle=0, val_x_balle=0, A=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
illan 8:527f5db9b3dc 460
haarkon 0:f00e68bef80c 461
haarkon 0:f00e68bef80c 462 char MENU_choix = 0;
haarkon 0:f00e68bef80c 463
haarkon 0:f00e68bef80c 464
illan 8:527f5db9b3dc 465 double BOUSSOLE_periode;
illan 8:527f5db9b3dc 466
illan 8:527f5db9b3dc 467 double Vbat_val;
illan 8:527f5db9b3dc 468
illan 8:527f5db9b3dc 469
illan 8:527f5db9b3dc 470
haarkon 0:f00e68bef80c 471
haarkon 6:88b4805d33e1 472 int MOTG_evol = 1, MOTD_evol = 1;
illan 8:527f5db9b3dc 473
haarkon 5:4955cb4b3646 474
haarkon 2:1d440e938c44 475 times.reset();
haarkon 2:1d440e938c44 476 times.start();
haarkon 0:f00e68bef80c 477
haarkon 6:88b4805d33e1 478 // Initialisation des interruptions
haarkon 2:1d440e938c44 479 tick.attach(&tickTime, 0.001);
haarkon 0:f00e68bef80c 480
haarkon 6:88b4805d33e1 481 BP.rise (&BPevent);
haarkon 0:f00e68bef80c 482
haarkon 6:88b4805d33e1 483 Echo1.rise (&echo1Rise);
haarkon 6:88b4805d33e1 484 Echo2.rise (&echo2Rise);
haarkon 6:88b4805d33e1 485 Echo3.rise (&echo3Rise);
haarkon 6:88b4805d33e1 486 Echo1.fall (&echo1Fall);
haarkon 6:88b4805d33e1 487 Echo2.fall (&echo2Fall);
haarkon 6:88b4805d33e1 488 Echo3.fall (&echo3Fall);
haarkon 6:88b4805d33e1 489
haarkon 6:88b4805d33e1 490 PWMG.rise (&PWM_motGRise);
haarkon 6:88b4805d33e1 491 PWMD.rise (&PWM_motDRise);
haarkon 6:88b4805d33e1 492
haarkon 6:88b4805d33e1 493 Pixy.attach (&getPixyByte);
haarkon 0:f00e68bef80c 494
haarkon 1:d95546f84105 495 BP.enable_irq();
haarkon 0:f00e68bef80c 496 IG.enable_irq();
haarkon 0:f00e68bef80c 497 Echo1.enable_irq();
haarkon 0:f00e68bef80c 498 Echo2.enable_irq();
haarkon 0:f00e68bef80c 499 Echo3.enable_irq();
haarkon 6:88b4805d33e1 500 PWMG.enable_irq();
haarkon 6:88b4805d33e1 501 PWMD.enable_irq();
haarkon 0:f00e68bef80c 502
haarkon 6:88b4805d33e1 503 // Initialisation des périphériques
haarkon 6:88b4805d33e1 504 // Bus I2C
haarkon 6:88b4805d33e1 505 Bus_I2C.frequency (100000);
haarkon 0:f00e68bef80c 506
haarkon 6:88b4805d33e1 507 // PWM des moteurs
haarkon 0:f00e68bef80c 508 Pwm_MG.period_us(50);
haarkon 0:f00e68bef80c 509 Pwm_MD.period_us(50);
haarkon 0:f00e68bef80c 510 En = 0;
haarkon 5:4955cb4b3646 511
haarkon 6:88b4805d33e1 512 // Bus SPI
haarkon 6:88b4805d33e1 513 SPIG.format (16,1);
haarkon 6:88b4805d33e1 514 SPIG.frequency (1000000);
haarkon 6:88b4805d33e1 515
haarkon 6:88b4805d33e1 516 SPID.format (16,1);
haarkon 6:88b4805d33e1 517 SPID.frequency (1000000);
haarkon 6:88b4805d33e1 518
haarkon 6:88b4805d33e1 519 SS = 1;
haarkon 6:88b4805d33e1 520
haarkon 6:88b4805d33e1 521 // Led
haarkon 0:f00e68bef80c 522 Led2 = 0;
haarkon 0:f00e68bef80c 523
haarkon 6:88b4805d33e1 524 Servo.period_ms (20);
haarkon 6:88b4805d33e1 525 Servo.pulsewidth_us(200);
haarkon 0:f00e68bef80c 526
haarkon 0:f00e68bef80c 527 while(1) {
haarkon 0:f00e68bef80c 528
haarkon 0:f00e68bef80c 529 do {
haarkon 0:f00e68bef80c 530 Led1 = 0;
haarkon 0:f00e68bef80c 531 Pc.printf ("\n\n\n\n\rProgramme de test\n\n\rEntrez le code du test a effectuer :\n\n");
haarkon 0:f00e68bef80c 532 Pc.printf ("\r1- Capteurs Ultra Son (les 3)\n");
haarkon 0:f00e68bef80c 533 Pc.printf ("\r2- Boussole et I2C\n");
haarkon 0:f00e68bef80c 534 Pc.printf ("\r3- Capteurs GP2 (les 4)\n");
haarkon 0:f00e68bef80c 535 Pc.printf ("\r4- Capteurs CNY70 (les 3)\n");
haarkon 6:88b4805d33e1 536 Pc.printf ("\r5- VBAT \t! erreur de composant la mesure est fausse !\n");
haarkon 6:88b4805d33e1 537 Pc.printf ("\r6- Moteur Gauche\n");
haarkon 6:88b4805d33e1 538 Pc.printf ("\r7- Moteur Droit\n");
haarkon 0:f00e68bef80c 539 Pc.printf ("\r8- Servomoteur\n");
haarkon 6:88b4805d33e1 540 Pc.printf ("\r9- PIXY (CMUCAM5)\n");
haarkon 0:f00e68bef80c 541 MENU_choix = Pc.getc ();
haarkon 0:f00e68bef80c 542 } while (((MENU_choix-'0')<1) || ((MENU_choix-'0')>9));
haarkon 0:f00e68bef80c 543
haarkon 0:f00e68bef80c 544 switch (MENU_choix-'0') {
haarkon 0:f00e68bef80c 545
haarkon 0:f00e68bef80c 546 case 1 :
haarkon 0:f00e68bef80c 547 Pc.printf ("\n\n\rTest des captreurs Ultrason\n");
haarkon 0:f00e68bef80c 548 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
illan 8:527f5db9b3dc 549 do
illan 8:527f5db9b3dc 550 {
Cachan1 9:7584e3904be6 551 if(CNY1>0.5 && A==1) {
Cachan1 9:7584e3904be6 552 avancer();
Cachan1 9:7584e3904be6 553 }
Cachan1 9:7584e3904be6 554 else
Cachan1 9:7584e3904be6 555 {
Cachan1 9:7584e3904be6 556 En = 0;
Cachan1 9:7584e3904be6 557 A=0;
Cachan1 9:7584e3904be6 558 }
illan 8:527f5db9b3dc 559 }
illan 8:527f5db9b3dc 560 while(!Pc.readable());
haarkon 0:f00e68bef80c 561 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 562 break;
haarkon 0:f00e68bef80c 563
haarkon 0:f00e68bef80c 564 case 2 :
haarkon 0:f00e68bef80c 565 Pc.printf ("\n\n\rBoussole\n");
haarkon 0:f00e68bef80c 566 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 567
haarkon 0:f00e68bef80c 568 Pc.printf ("\n\rVerif du bus I2C :");
haarkon 0:f00e68bef80c 569 I2C_check = Bus_I2C.write (BOUSSOLE_adress,BOUSSOLE_status,1,false);
haarkon 0:f00e68bef80c 570 if (I2C_check==0) {
haarkon 0:f00e68bef80c 571 Pc.printf (" OK\n");
haarkon 0:f00e68bef80c 572 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true);
haarkon 0:f00e68bef80c 573 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4);
haarkon 0:f00e68bef80c 574 Pc.printf ("\rVersion Firmware boussole : %03d\n", I2C_registerValue[0]);
haarkon 0:f00e68bef80c 575 } else {
haarkon 0:f00e68bef80c 576 Pc.printf (" FAIL\n");
haarkon 0:f00e68bef80c 577 }
haarkon 0:f00e68bef80c 578
haarkon 3:3dfe26a10796 579 BOUSSOLE_periode = PWMB.period();
haarkon 0:f00e68bef80c 580 Pc.printf ("\rVerif de la PWM :");
haarkon 3:3dfe26a10796 581 if ((BOUSSOLE_periode > 0.11) || (BOUSSOLE_periode < 0.06)) {
haarkon 0:f00e68bef80c 582 Pc.printf (" FAIL\n\n");
haarkon 0:f00e68bef80c 583 } else {
haarkon 0:f00e68bef80c 584 Pc.printf (" OK\n\n");
haarkon 0:f00e68bef80c 585 BOUSSOLE_check = 0;
haarkon 0:f00e68bef80c 586 }
haarkon 0:f00e68bef80c 587
haarkon 0:f00e68bef80c 588 do {
illan 8:527f5db9b3dc 589 boussole();
illan 8:527f5db9b3dc 590 if(CAP_PWM<160)
illan 8:527f5db9b3dc 591 {
illan 8:527f5db9b3dc 592 tour_droit();
illan 8:527f5db9b3dc 593 }
Cachan1 9:7584e3904be6 594 else if(CAP_PWM>230)
illan 8:527f5db9b3dc 595 {
illan 8:527f5db9b3dc 596 tour_gauche();
illan 8:527f5db9b3dc 597 }
illan 8:527f5db9b3dc 598 else
illan 8:527f5db9b3dc 599 {
illan 8:527f5db9b3dc 600 avancer();
illan 8:527f5db9b3dc 601 }
haarkon 0:f00e68bef80c 602 } while(!Pc.readable());
haarkon 0:f00e68bef80c 603 MENU_choix = Pc.getc();
illan 8:527f5db9b3dc 604 En=0;
haarkon 0:f00e68bef80c 605 break;
haarkon 0:f00e68bef80c 606
haarkon 0:f00e68bef80c 607 case 3 :
haarkon 0:f00e68bef80c 608 Pc.printf ("\n\n\rGP2xx\n");
haarkon 0:f00e68bef80c 609 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 610 do {
illan 8:527f5db9b3dc 611 capteur_infrarouge();
illan 8:527f5db9b3dc 612
haarkon 0:f00e68bef80c 613 } while(!Pc.readable());
haarkon 0:f00e68bef80c 614 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 615 break;
haarkon 0:f00e68bef80c 616
haarkon 0:f00e68bef80c 617 case 4 :
haarkon 0:f00e68bef80c 618 Pc.printf ("\n\n\rCNY70\n");
haarkon 0:f00e68bef80c 619 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 620 do {
illan 8:527f5db9b3dc 621 CNY();
haarkon 0:f00e68bef80c 622 } while(!Pc.readable());
haarkon 0:f00e68bef80c 623 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 624 break;
haarkon 0:f00e68bef80c 625
haarkon 0:f00e68bef80c 626 case 5 :
haarkon 0:f00e68bef80c 627 Pc.printf ("\n\n\rVbat\n");
haarkon 0:f00e68bef80c 628 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 0:f00e68bef80c 629 do {
haarkon 0:f00e68bef80c 630 if (FlagTickLed) {
haarkon 0:f00e68bef80c 631 Led1 = !Led1;
haarkon 0:f00e68bef80c 632 FlagTickLed = 0;
haarkon 0:f00e68bef80c 633
haarkon 0:f00e68bef80c 634 Vbat_val = Vbat;
haarkon 0:f00e68bef80c 635 Pc.printf ("\rVbat = %5.3lf V", Vbat_val*3.3*4.3);
haarkon 0:f00e68bef80c 636 }
haarkon 0:f00e68bef80c 637 } while(!Pc.readable());
haarkon 0:f00e68bef80c 638 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 639 break;
haarkon 0:f00e68bef80c 640
haarkon 0:f00e68bef80c 641 case 6 :
haarkon 0:f00e68bef80c 642 Pc.printf ("\n\n\rMoteur Gauche\n");
haarkon 0:f00e68bef80c 643 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 5:4955cb4b3646 644
haarkon 5:4955cb4b3646 645 En = 1;
haarkon 5:4955cb4b3646 646 SensG = 1;
haarkon 6:88b4805d33e1 647 Led2 = 1;
haarkon 5:4955cb4b3646 648 MOTG_duty = 0.5;
haarkon 6:88b4805d33e1 649 MOTG_evol = 1;
haarkon 5:4955cb4b3646 650 Pwm_MG = 0.5;
haarkon 6:88b4805d33e1 651
haarkon 0:f00e68bef80c 652 do {
haarkon 0:f00e68bef80c 653 if (FlagTickLed) {
haarkon 0:f00e68bef80c 654 Led1 = !Led1;
haarkon 0:f00e68bef80c 655 FlagTickLed = 0;
haarkon 0:f00e68bef80c 656 }
haarkon 5:4955cb4b3646 657 if (BPPressed) {
haarkon 5:4955cb4b3646 658 BPPressed = 0;
haarkon 6:88b4805d33e1 659 SensG = !SensG;
haarkon 6:88b4805d33e1 660 Led2 = !Led2;
haarkon 6:88b4805d33e1 661 }
haarkon 5:4955cb4b3646 662
haarkon 6:88b4805d33e1 663 if (((Tick%1000)==0) && FlagTick) {
haarkon 6:88b4805d33e1 664 FlagTick = 0;
haarkon 6:88b4805d33e1 665 if (MOTG_evol) {
haarkon 6:88b4805d33e1 666 if (MOTG_duty < 0.9) MOTG_duty += 0.1;
haarkon 6:88b4805d33e1 667 else {
haarkon 6:88b4805d33e1 668 MOTG_evol = 0;
haarkon 6:88b4805d33e1 669 MOTG_duty = 0.9;
haarkon 6:88b4805d33e1 670 }
haarkon 5:4955cb4b3646 671 } else {
haarkon 6:88b4805d33e1 672 if (MOTG_duty > 0.1) MOTG_duty -= 0.1;
haarkon 6:88b4805d33e1 673 else {
haarkon 6:88b4805d33e1 674 MOTG_evol = 1;
haarkon 6:88b4805d33e1 675 MOTG_duty = 0.1;
haarkon 6:88b4805d33e1 676 }
haarkon 5:4955cb4b3646 677 }
haarkon 5:4955cb4b3646 678 }
haarkon 6:88b4805d33e1 679
haarkon 6:88b4805d33e1 680 Pwm_MG = MOTG_duty;
haarkon 6:88b4805d33e1 681 Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTG_duty, Gauche.GetCounter(), Gperiod);
haarkon 6:88b4805d33e1 682
haarkon 6:88b4805d33e1 683
haarkon 0:f00e68bef80c 684 } while(!Pc.readable());
haarkon 0:f00e68bef80c 685 MENU_choix = Pc.getc();
haarkon 6:88b4805d33e1 686
haarkon 5:4955cb4b3646 687 En = 0;
haarkon 5:4955cb4b3646 688 if (SensG == 0) Pwm_MG = 0;
haarkon 5:4955cb4b3646 689 else Pwm_MG = 1;
haarkon 6:88b4805d33e1 690
haarkon 0:f00e68bef80c 691 break;
haarkon 0:f00e68bef80c 692
haarkon 0:f00e68bef80c 693 case 7 :
haarkon 0:f00e68bef80c 694 Pc.printf ("\n\n\rMoteur Droit\n");
haarkon 0:f00e68bef80c 695 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 5:4955cb4b3646 696
haarkon 5:4955cb4b3646 697 En = 1;
haarkon 5:4955cb4b3646 698 SensD = 1;
haarkon 6:88b4805d33e1 699 Led2 = 1;
haarkon 5:4955cb4b3646 700 MOTD_duty = 0.5;
haarkon 6:88b4805d33e1 701 MOTD_evol = 1;
haarkon 5:4955cb4b3646 702 Pwm_MD = 0.5;
haarkon 5:4955cb4b3646 703
haarkon 0:f00e68bef80c 704 do {
haarkon 0:f00e68bef80c 705 if (FlagTickLed) {
haarkon 0:f00e68bef80c 706 Led1 = !Led1;
haarkon 0:f00e68bef80c 707 FlagTickLed = 0;
haarkon 0:f00e68bef80c 708 }
haarkon 5:4955cb4b3646 709 if (BPPressed) {
haarkon 5:4955cb4b3646 710 BPPressed = 0;
haarkon 6:88b4805d33e1 711 SensD = !SensD;
haarkon 6:88b4805d33e1 712 Led2 = !Led2;
haarkon 6:88b4805d33e1 713 }
haarkon 6:88b4805d33e1 714
haarkon 6:88b4805d33e1 715 if (((Tick%1000)==0) && FlagTick) {
haarkon 6:88b4805d33e1 716 FlagTick = 0;
haarkon 6:88b4805d33e1 717 if (MOTD_evol) {
haarkon 6:88b4805d33e1 718 if (MOTD_duty < 0.9) MOTD_duty += 0.1;
haarkon 6:88b4805d33e1 719 else {
haarkon 6:88b4805d33e1 720 MOTD_evol = 0;
haarkon 6:88b4805d33e1 721 MOTD_duty = 0.9;
haarkon 6:88b4805d33e1 722 }
haarkon 5:4955cb4b3646 723 } else {
haarkon 6:88b4805d33e1 724 if (MOTD_duty > 0.1) MOTD_duty -= 0.1;
haarkon 6:88b4805d33e1 725 else {
haarkon 6:88b4805d33e1 726 MOTD_evol = 1;
haarkon 6:88b4805d33e1 727 MOTD_duty = 0.1;
haarkon 6:88b4805d33e1 728 }
haarkon 5:4955cb4b3646 729 }
haarkon 5:4955cb4b3646 730 }
haarkon 6:88b4805d33e1 731 Pwm_MD = MOTD_duty;
haarkon 6:88b4805d33e1 732 Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTD_duty, Droite.GetCounter(), Dperiod);
haarkon 6:88b4805d33e1 733
haarkon 0:f00e68bef80c 734 } while(!Pc.readable());
haarkon 0:f00e68bef80c 735 MENU_choix = Pc.getc();
haarkon 6:88b4805d33e1 736
haarkon 5:4955cb4b3646 737 En = 0;
haarkon 5:4955cb4b3646 738 if (SensD == 0) Pwm_MD = 0;
haarkon 5:4955cb4b3646 739 else Pwm_MD = 1;
haarkon 6:88b4805d33e1 740
haarkon 0:f00e68bef80c 741 break;
haarkon 0:f00e68bef80c 742
haarkon 0:f00e68bef80c 743 case 8 :
haarkon 0:f00e68bef80c 744 Pc.printf ("\n\n\rServo Moteur\n");
haarkon 0:f00e68bef80c 745 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 6:88b4805d33e1 746
haarkon 0:f00e68bef80c 747 do {
illan 8:527f5db9b3dc 748 servomoteur();
haarkon 0:f00e68bef80c 749 } while(!Pc.readable());
haarkon 0:f00e68bef80c 750 MENU_choix = Pc.getc();
illan 8:527f5db9b3dc 751
illan 8:527f5db9b3dc 752 En = 0;
illan 8:527f5db9b3dc 753
haarkon 0:f00e68bef80c 754 break;
haarkon 0:f00e68bef80c 755
haarkon 0:f00e68bef80c 756 case 9 :
haarkon 0:f00e68bef80c 757 Pc.printf ("\n\n\rPixy\n");
haarkon 0:f00e68bef80c 758 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
haarkon 2:1d440e938c44 759
haarkon 2:1d440e938c44 760 if (Pixy_check == 0) Pc.printf ("\n\rPIXY is ALIVE\n");
haarkon 2:1d440e938c44 761 else Pc.printf ("\n\rPIXY don't talk\n");
haarkon 2:1d440e938c44 762
haarkon 0:f00e68bef80c 763 do {
haarkon 2:1d440e938c44 764 if (FlagPixy) {
haarkon 2:1d440e938c44 765
haarkon 2:1d440e938c44 766 if (FlagPixyOverflow) {
haarkon 2:1d440e938c44 767 Pc.printf ("\rSome Data were lost\n");
haarkon 2:1d440e938c44 768 FlagPixyOverflow = 0;
haarkon 0:f00e68bef80c 769 }
haarkon 3:3dfe26a10796 770 for (PIXY_objet = 0; PIXY_objet < Pixy_NMObjet; PIXY_objet++) {
haarkon 3:3dfe26a10796 771 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 772
haarkon 3:3dfe26a10796 773 if (PIXY_rNMObjet<19) PIXY_rNMObjet++;
haarkon 3:3dfe26a10796 774 else PIXY_rNMObjet = 0;
haarkon 2:1d440e938c44 775 }
haarkon 3:3dfe26a10796 776 Pixy_NMObjet = 0;
haarkon 0:f00e68bef80c 777
haarkon 3:3dfe26a10796 778 for (PIXY_objet = 0; PIXY_objet < Pixy_CCObjet; PIXY_objet++) {
haarkon 3:3dfe26a10796 779 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 780
haarkon 2:1d440e938c44 781 if (PIXY_rCCObjet<19) PIXY_rCCObjet++;
haarkon 2:1d440e938c44 782 else PIXY_rCCObjet = 0;
haarkon 2:1d440e938c44 783 }
illan 8:527f5db9b3dc 784 val_W_balle = 0;
illan 8:527f5db9b3dc 785 val_H_balle = 0;
illan 8:527f5db9b3dc 786 val_carre_balle = 0;
illan 8:527f5db9b3dc 787 max_val_W_balle = 0;
illan 8:527f5db9b3dc 788 indice_balle = 0;
illan 8:527f5db9b3dc 789 for(i=0;i<14;i++){
illan 8:527f5db9b3dc 790 val_W_balle=Pixy_NMFIFO[i].NMbloc.width;
illan 8:527f5db9b3dc 791 val_H_balle=Pixy_NMFIFO[i].NMbloc.height;
illan 8:527f5db9b3dc 792
illan 8:527f5db9b3dc 793 val_carre_balle = val_H_balle/val_W_balle;
illan 8:527f5db9b3dc 794
illan 8:527f5db9b3dc 795 val_x_balle =Pixy_NMFIFO[i].NMbloc.x;
illan 8:527f5db9b3dc 796
illan 8:527f5db9b3dc 797
illan 8:527f5db9b3dc 798 if(val_W_balle>max_val_W_balle){
illan 8:527f5db9b3dc 799 max_val_W_balle=val_W_balle;
illan 8:527f5db9b3dc 800 indice_balle=i;
Cachan1 9:7584e3904be6 801 }
illan 8:527f5db9b3dc 802 if(val_x_balle>130 && val_x_balle<170){
illan 8:527f5db9b3dc 803 avancer();
illan 8:527f5db9b3dc 804 }
illan 8:527f5db9b3dc 805 else{
illan 8:527f5db9b3dc 806 En = 0;
illan 8:527f5db9b3dc 807 }
illan 8:527f5db9b3dc 808
Cachan1 9:7584e3904be6 809
illan 8:527f5db9b3dc 810 }
illan 8:527f5db9b3dc 811 Pc.printf("\rNMobj sig = %hd : X=%5hd,Y=%5hd (W=%5hd, H=%5hd)\n",Pixy_NMFIFO[indice_balle].NMbloc.signature,Pixy_NMFIFO[indice_balle].NMbloc.x,Pixy_NMFIFO[indice_balle].NMbloc.y,Pixy_NMFIFO[indice_balle].NMbloc.width,Pixy_NMFIFO[indice_balle].NMbloc.height);
illan 8:527f5db9b3dc 812
haarkon 3:3dfe26a10796 813 Pixy_CCObjet = 0;
haarkon 3:3dfe26a10796 814 Pc.printf("\n\r");
haarkon 1:d95546f84105 815 FlagPixy = 0;
illan 8:527f5db9b3dc 816 wait_ms(500);
haarkon 0:f00e68bef80c 817 }
haarkon 1:d95546f84105 818
haarkon 1:d95546f84105 819 if (FlagTickLed) {
haarkon 1:d95546f84105 820 Led1 = !Led1;
haarkon 1:d95546f84105 821 FlagTickLed = 0;
haarkon 1:d95546f84105 822 }
haarkon 0:f00e68bef80c 823 } while(!Pc.readable());
haarkon 0:f00e68bef80c 824 MENU_choix = Pc.getc();
haarkon 0:f00e68bef80c 825 break;
haarkon 0:f00e68bef80c 826
haarkon 0:f00e68bef80c 827 }
haarkon 0:f00e68bef80c 828 }
illan 8:527f5db9b3dc 829 }
illan 8:527f5db9b3dc 830
illan 8:527f5db9b3dc 831 void capteur_ultrason(void)
illan 8:527f5db9b3dc 832 {
illan 8:527f5db9b3dc 833 if (FlagTickLed) {
illan 8:527f5db9b3dc 834 Led1 = !Led1;
illan 8:527f5db9b3dc 835 FlagTickLed = 0;
illan 8:527f5db9b3dc 836 }
illan 8:527f5db9b3dc 837
illan 8:527f5db9b3dc 838 // Gestion des US
illan 8:527f5db9b3dc 839 if (((Tick%150)==0) && FlagTick) {
illan 8:527f5db9b3dc 840 Trig1 = 1;
illan 8:527f5db9b3dc 841 wait_us(20);
illan 8:527f5db9b3dc 842 Trig1 = 0;
illan 8:527f5db9b3dc 843 FlagTick = 0;
illan 8:527f5db9b3dc 844 }
illan 8:527f5db9b3dc 845
illan 8:527f5db9b3dc 846 if (((Tick%150)==50) && FlagTick) {
illan 8:527f5db9b3dc 847 Trig2 = 1;
illan 8:527f5db9b3dc 848 wait_us(20);
illan 8:527f5db9b3dc 849 Trig2 = 0;
illan 8:527f5db9b3dc 850 FlagTick = 0;
illan 8:527f5db9b3dc 851 }
illan 8:527f5db9b3dc 852
illan 8:527f5db9b3dc 853 if (((Tick%150)==100) && FlagTick) {
illan 8:527f5db9b3dc 854 Trig3 = 1;
illan 8:527f5db9b3dc 855 wait_us(20);
illan 8:527f5db9b3dc 856 Trig3 = 0;
illan 8:527f5db9b3dc 857 FlagTick = 0;
illan 8:527f5db9b3dc 858 }
illan 8:527f5db9b3dc 859
illan 8:527f5db9b3dc 860 if (FlagUS1==1) {
illan 8:527f5db9b3dc 861 Pc.printf ("\rUS 1 = %04d mm", (int)DistUS1);
illan 8:527f5db9b3dc 862 FlagUS1 = 0;
illan 8:527f5db9b3dc 863 }
illan 8:527f5db9b3dc 864
illan 8:527f5db9b3dc 865 if (FlagUS2==1) {
illan 8:527f5db9b3dc 866 Pc.printf ("\r\t\t\tUS 2 = %04d mm", (int)DistUS2);
illan 8:527f5db9b3dc 867 FlagUS2 = 0;
illan 8:527f5db9b3dc 868 }
illan 8:527f5db9b3dc 869
illan 8:527f5db9b3dc 870 if (FlagUS3==1) {
illan 8:527f5db9b3dc 871 Pc.printf ("\r\t\t\t\t\t\tUS 3 = %04d mm", (int)DistUS3);
illan 8:527f5db9b3dc 872 FlagUS3 = 0;
illan 8:527f5db9b3dc 873 }
illan 8:527f5db9b3dc 874 }
illan 8:527f5db9b3dc 875
illan 8:527f5db9b3dc 876 void boussole(void)
illan 8:527f5db9b3dc 877 {
illan 8:527f5db9b3dc 878
illan 8:527f5db9b3dc 879
illan 8:527f5db9b3dc 880
illan 8:527f5db9b3dc 881 if (FlagTickLed) {
illan 8:527f5db9b3dc 882 Led1 = !Led1;
illan 8:527f5db9b3dc 883 FlagTickLed = 0;
illan 8:527f5db9b3dc 884 }
illan 8:527f5db9b3dc 885
illan 8:527f5db9b3dc 886 if (((Tick%150)==0) && FlagTick) {
illan 8:527f5db9b3dc 887 FlagTick = 0;
illan 8:527f5db9b3dc 888 if (BOUSSOLE_check==0) {
illan 8:527f5db9b3dc 889 CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10;
illan 8:527f5db9b3dc 890 Pc.printf ("\r PWM = %4.1lf", CAP_PWM);
illan 8:527f5db9b3dc 891 }
illan 8:527f5db9b3dc 892 if (I2C_check==0) {
illan 8:527f5db9b3dc 893 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true);
illan 8:527f5db9b3dc 894 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4);
illan 8:527f5db9b3dc 895 CAP_I2C = (double)(((unsigned short)I2C_registerValue[2]<<8)+(unsigned short)I2C_registerValue[3])/10.0;
illan 8:527f5db9b3dc 896 Pc.printf ("\r\t\t I2C = %4.1lf", CAP_I2C);
illan 8:527f5db9b3dc 897 }
illan 8:527f5db9b3dc 898 }
illan 8:527f5db9b3dc 899 }
illan 8:527f5db9b3dc 900
illan 8:527f5db9b3dc 901 void capteur_infrarouge(void)
illan 8:527f5db9b3dc 902 {
illan 8:527f5db9b3dc 903 double SD1_val, SD2_val, LD1_val, LD2_val;
illan 8:527f5db9b3dc 904 double SD1_dist, SD2_dist, LD1_dist, LD2_dist;
illan 8:527f5db9b3dc 905
illan 8:527f5db9b3dc 906 if (FlagTickLed) {
illan 8:527f5db9b3dc 907 Led1 = !Led1;
illan 8:527f5db9b3dc 908 FlagTickLed = 0;
illan 8:527f5db9b3dc 909
illan 8:527f5db9b3dc 910 SD1_val = SD1;
illan 8:527f5db9b3dc 911 SD2_val = SD2;
illan 8:527f5db9b3dc 912 LD1_val = LD1;
illan 8:527f5db9b3dc 913 LD2_val = LD2;
illan 8:527f5db9b3dc 914
illan 8:527f5db9b3dc 915 if (SD1_val < 0.06) {
illan 8:527f5db9b3dc 916 SD1_val = 0;
illan 8:527f5db9b3dc 917 SD1_dist = 40;
illan 8:527f5db9b3dc 918 } else {
illan 8:527f5db9b3dc 919 SD1_dist = 11.611/(SD1_val*3.3-0.0237);
illan 8:527f5db9b3dc 920 if (SD1_dist > 40) SD1_dist = 40;
illan 8:527f5db9b3dc 921 }
illan 8:527f5db9b3dc 922
illan 8:527f5db9b3dc 923 if (SD2_val < 0.06) {
illan 8:527f5db9b3dc 924 SD2_val = 0;
illan 8:527f5db9b3dc 925 SD2_dist = 40;
illan 8:527f5db9b3dc 926 } else {
illan 8:527f5db9b3dc 927 SD2_dist = 11.611/(SD2_val*3.3-0.0237);
illan 8:527f5db9b3dc 928 if (SD2_dist > 40) SD2_dist = 40;
illan 8:527f5db9b3dc 929 }
illan 8:527f5db9b3dc 930
illan 8:527f5db9b3dc 931 if (LD1_val < 0.1) {
illan 8:527f5db9b3dc 932 LD1_val = 0;
illan 8:527f5db9b3dc 933 LD1_dist = 150;
illan 8:527f5db9b3dc 934 } else {
illan 8:527f5db9b3dc 935 LD1_dist = 59.175/(LD1_val*3.3-0.0275);
illan 8:527f5db9b3dc 936 if (LD1_dist > 150) LD1_dist = 150;
illan 8:527f5db9b3dc 937 }
illan 8:527f5db9b3dc 938
illan 8:527f5db9b3dc 939 if (LD2_val < 0.1) {
illan 8:527f5db9b3dc 940 LD2_val = 0;
illan 8:527f5db9b3dc 941 LD2_dist = 150;
illan 8:527f5db9b3dc 942 } else {
illan 8:527f5db9b3dc 943 LD2_dist = 59.175/(LD2_val*3.3-0.0275);
illan 8:527f5db9b3dc 944 if (LD2_dist > 150) LD2_dist = 150;
illan 8:527f5db9b3dc 945 }
illan 8:527f5db9b3dc 946
illan 8:527f5db9b3dc 947 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);
illan 8:527f5db9b3dc 948 }
illan 8:527f5db9b3dc 949 }
illan 8:527f5db9b3dc 950
illan 8:527f5db9b3dc 951 void CNY(void)
illan 8:527f5db9b3dc 952 {
illan 8:527f5db9b3dc 953 double CNY1_val, CNY2_val, CNY3_val;
illan 8:527f5db9b3dc 954
illan 8:527f5db9b3dc 955 if (FlagTickLed) {
illan 8:527f5db9b3dc 956 Led1 = !Led1;
illan 8:527f5db9b3dc 957 FlagTickLed = 0;
illan 8:527f5db9b3dc 958
illan 8:527f5db9b3dc 959 CNY1_val = CNY1;
illan 8:527f5db9b3dc 960 CNY2_val = CNY2;
illan 8:527f5db9b3dc 961 CNY3_val = CNY3;
illan 8:527f5db9b3dc 962
illan 8:527f5db9b3dc 963 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);
illan 8:527f5db9b3dc 964 }
illan 8:527f5db9b3dc 965 }
illan 8:527f5db9b3dc 966
illan 8:527f5db9b3dc 967
illan 8:527f5db9b3dc 968 void servomoteur(void)
illan 8:527f5db9b3dc 969 {
illan 8:527f5db9b3dc 970
illan 8:527f5db9b3dc 971
illan 8:527f5db9b3dc 972 if (FlagTickLed) {
illan 8:527f5db9b3dc 973 Led1 = !Led1;
illan 8:527f5db9b3dc 974 FlagTickLed = 0;
illan 8:527f5db9b3dc 975 }
illan 8:527f5db9b3dc 976
illan 8:527f5db9b3dc 977 if (((Tick%250)==0) && FlagTick) {
illan 8:527f5db9b3dc 978 FlagTick = 0;
illan 8:527f5db9b3dc 979 if (SERVO_dir == S_monte) {
illan 8:527f5db9b3dc 980 if (SERVO_pulseWidth < (SERVO_max - 100)) SERVO_pulseWidth +=100;
illan 8:527f5db9b3dc 981 else {
illan 8:527f5db9b3dc 982 SERVO_dir = S_descente;
illan 8:527f5db9b3dc 983 SERVO_pulseWidth = SERVO_max;
illan 8:527f5db9b3dc 984 }
illan 8:527f5db9b3dc 985 } else {
illan 8:527f5db9b3dc 986 if (SERVO_pulseWidth > (SERVO_min + 100)) SERVO_pulseWidth -=100;
illan 8:527f5db9b3dc 987 else {
illan 8:527f5db9b3dc 988 SERVO_dir = S_monte;
illan 8:527f5db9b3dc 989 SERVO_pulseWidth = SERVO_min;
illan 8:527f5db9b3dc 990 }
illan 8:527f5db9b3dc 991 }
illan 8:527f5db9b3dc 992
illan 8:527f5db9b3dc 993 Servo.pulsewidth_us (SERVO_pulseWidth);
illan 8:527f5db9b3dc 994 Pc.printf ("\rPulse = %d",SERVO_pulseWidth);
illan 8:527f5db9b3dc 995 }
illan 8:527f5db9b3dc 996 }
illan 8:527f5db9b3dc 997
illan 8:527f5db9b3dc 998 void avancer(void)
illan 8:527f5db9b3dc 999 {
illan 8:527f5db9b3dc 1000 En = 1;
illan 8:527f5db9b3dc 1001 SensD = 1;
illan 8:527f5db9b3dc 1002 Led2 = 1;
Cachan1 9:7584e3904be6 1003 MOTD_duty = 0.2;
illan 8:527f5db9b3dc 1004 SensG = 0;
Cachan1 9:7584e3904be6 1005 MOTG_duty = 0.87;
illan 8:527f5db9b3dc 1006
illan 8:527f5db9b3dc 1007 Pwm_MG = MOTG_duty;
illan 8:527f5db9b3dc 1008 Pwm_MD = MOTD_duty;
illan 8:527f5db9b3dc 1009 }
illan 8:527f5db9b3dc 1010 void tour_droit(void)
illan 8:527f5db9b3dc 1011 {
illan 8:527f5db9b3dc 1012 En = 1;
illan 8:527f5db9b3dc 1013 SensD = 0;
illan 8:527f5db9b3dc 1014 Led2 = 1;
illan 8:527f5db9b3dc 1015 MOTD_duty = 0.5;
Cachan1 9:7584e3904be6 1016 Pwm_MD = 0.3;
illan 8:527f5db9b3dc 1017 SensG = 0;
illan 8:527f5db9b3dc 1018 MOTG_duty = 0.5;
Cachan1 9:7584e3904be6 1019 Pwm_MG = 0.3;
illan 8:527f5db9b3dc 1020
illan 8:527f5db9b3dc 1021 Pwm_MG = MOTG_duty;
illan 8:527f5db9b3dc 1022 Pwm_MD = MOTD_duty;
illan 8:527f5db9b3dc 1023 }
illan 8:527f5db9b3dc 1024 void tour_gauche(void)
illan 8:527f5db9b3dc 1025 {
illan 8:527f5db9b3dc 1026 En = 1;
illan 8:527f5db9b3dc 1027 SensD = 1;
illan 8:527f5db9b3dc 1028 Led2 = 1;
illan 8:527f5db9b3dc 1029 MOTD_duty = 0.5;
illan 8:527f5db9b3dc 1030 Pwm_MD = 0.5;
illan 8:527f5db9b3dc 1031 SensG = 1;
illan 8:527f5db9b3dc 1032 MOTG_duty = 0.5;
illan 8:527f5db9b3dc 1033 Pwm_MG = 0.5;
illan 8:527f5db9b3dc 1034
illan 8:527f5db9b3dc 1035 Pwm_MG = MOTG_duty;
illan 8:527f5db9b3dc 1036 Pwm_MD = MOTD_duty;
illan 8:527f5db9b3dc 1037 }