dfgqf
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of TestBoardv2_boussole_pixi by
Diff: main.cpp
- Revision:
- 6:88b4805d33e1
- Parent:
- 5:4955cb4b3646
- Child:
- 8:527f5db9b3dc
diff -r 4955cb4b3646 -r 88b4805d33e1 main.cpp --- a/main.cpp Sun Jun 04 15:15:19 2017 +0000 +++ b/main.cpp Wed Jun 07 20:18:43 2017 +0000 @@ -11,14 +11,33 @@ #include "PwmIn.h" #include "Nucleo_Encoder_16_bits.h" -#define BOUSSOLE_adress 0xC0 -#define PIXY_adress 0x54 +#define BOUSSOLE_adress 0xC0 +#define PIXY_adress 0x54 + +#define CC_BLOCSIZE 14 +#define N_BLOCSIZE 12 + +#define N_BLOCCODE 0xAA55 +#define CC_BLOCCODE 0xAA56 + -#define CC_BLOCSIZE 14 -#define N_BLOCSIZE 12 +// Comme la nucleo est Little Endian et que l'AS5047D est Big Endian, les codes sont inversés +// Registre Valeur (BE) Valeur (LE) +// NOP 0xC000 0x0003 +// ANGLE 0xFFFF 0xFFFF +// ERROR 0x4001 0x8002 +// SETTING1 (R) 0xC018 0x1803 +// SETTING2 (R) 0x4019 0x8902 +// SETTING1 (W) 0x0018 0x1800 +// SETTING2 (W) 0x8019 0x9801 -#define N_BLOCCODE 0xAA55 -#define CC_BLOCCODE 0xAA56 +#define SPI_READ_NOP 0x0003 +#define SPI_READ_ANGLE 0xFFFF +#define SPI_READ_ERROR 0x8002 +#define SPI_READ_SETTINGS1 0x1803 +#define SPI_READ_SETTINGS2 0x8902 +#define SPI_WRTIE_SETTINGS1 0x1800 +#define SPI_WRITE_SETTINGS2 0x9801 typedef unsigned char Byte; typedef unsigned short Word; @@ -178,9 +197,9 @@ InterruptIn Echo3 (PB_12); InterruptIn BP (PC_13); InterruptIn IG (PC_7); +InterruptIn PWMG (PB_2); +InterruptIn PWMD (PB_1); -PwmIn PWMG (PB_2); -PwmIn PWMD (PB_1); PwmIn PWMB (PA_15); PwmOut Pwm_MG (PB_10); @@ -189,8 +208,8 @@ I2C Bus_I2C (PB_9, PB_8); -SPI MotG (PC_12, PC_11, PC_10); -SPI MotD (PB_15, PB_14, PB_13); +SPI SPIG (PC_12, PC_11, PC_10); +SPI SPID (PB_15, PB_14, PB_13); Nucleo_Encoder_16_bits Gauche (TIM4); // A = PB_6, B = PB_7 Nucleo_Encoder_16_bits Droite (TIM3); // A = PB_4, B = PB_5 @@ -208,11 +227,11 @@ */ // Structure de temps -lWord Tick = 0; +lWord Tick = 0, Gperiod, Dperiod; // Sémaphore d'interruption int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0; -int FlagTick = 0, FlagTickLed = 0, BPPressed = 0; +int FlagTick = 0, FlagTickLed = 0, BPPressed = 0, nbTurnG = 0, nbTurnD = 0; int Pixy_check = -1; // Dialogue avec la Pixy @@ -240,6 +259,26 @@ BPPressed = 1; } +void PWM_motGRise (void) +{ + static lWord oldTime; + lWord now; + now = times.read_us(); + Gperiod = now-oldTime; + oldTime = now; + nbTurnG++; +} + +void PWM_motDRise (void) +{ + static lWord oldTime; + lWord now; + now = times.read_us(); + Dperiod = now-oldTime; + oldTime = now; + nbTurnD++; +} + void echo1Rise () { Echo1Start = times.read_us(); @@ -402,8 +441,7 @@ Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0; int PIXY_objet; - double SERVO_angle = 0, SERVO_angleMAX = 180, SERVO_angleMIN = 0; - Word SERVO_pos; + int SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400; T_SERVODIR SERVO_dir = S_monte; char MENU_choix = 0; @@ -416,40 +454,60 @@ double SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val; double SD1_dist, SD2_dist, LD1_dist, LD2_dist; - double MOTG_duty = 0, MOTD_duty = 0; + int MOTG_evol = 1, MOTD_evol = 1; + double MOTG_duty = 0.5, MOTD_duty = 0.5; times.reset(); times.start(); + // Initialisation des interruptions tick.attach(&tickTime, 0.001); - Bus_I2C.frequency (100000); - - BP.rise (&BPevent); + BP.rise (&BPevent); - Echo1.rise (&echo1Rise); - Echo2.rise (&echo2Rise); - Echo3.rise (&echo3Rise); - Echo1.fall (&echo1Fall); - Echo2.fall (&echo2Fall); - Echo3.fall (&echo3Fall); + Echo1.rise (&echo1Rise); + Echo2.rise (&echo2Rise); + Echo3.rise (&echo3Rise); + Echo1.fall (&echo1Fall); + Echo2.fall (&echo2Fall); + Echo3.fall (&echo3Fall); + + PWMG.rise (&PWM_motGRise); + PWMD.rise (&PWM_motDRise); + + Pixy.attach (&getPixyByte); BP.enable_irq(); IG.enable_irq(); Echo1.enable_irq(); Echo2.enable_irq(); Echo3.enable_irq(); + PWMG.enable_irq(); + PWMD.enable_irq(); - Pixy.attach (&getPixyByte); + // Initialisation des périphériques + // Bus I2C + Bus_I2C.frequency (100000); + // PWM des moteurs Pwm_MG.period_us(50); Pwm_MD.period_us(50); - En = 0; + // Bus SPI + SPIG.format (16,1); + SPIG.frequency (1000000); + + SPID.format (16,1); + SPID.frequency (1000000); + + SS = 1; + + // Led Led2 = 0; - Servo.period_ms(20); + Servo.period_ms (20); + Servo.pulsewidth_us(200); while(1) { @@ -460,11 +518,11 @@ Pc.printf ("\r2- Boussole et I2C\n"); Pc.printf ("\r3- Capteurs GP2 (les 4)\n"); Pc.printf ("\r4- Capteurs CNY70 (les 3)\n"); - Pc.printf ("\r5- VBAT\n"); - Pc.printf ("\r6- Moteur Gauche -- NON CODE --\n"); - Pc.printf ("\r7- Moteur Droit -- NON CODE --\n"); + Pc.printf ("\r5- VBAT \t! erreur de composant la mesure est fausse !\n"); + Pc.printf ("\r6- Moteur Gauche\n"); + Pc.printf ("\r7- Moteur Droit\n"); Pc.printf ("\r8- Servomoteur\n"); - Pc.printf ("\r9- PIXY (CMUCAM5)\n\r"); + Pc.printf ("\r9- PIXY (CMUCAM5)\n"); MENU_choix = Pc.getc (); } while (((MENU_choix-'0')<1) || ((MENU_choix-'0')>9)); @@ -657,33 +715,50 @@ En = 1; SensG = 1; + Led2 = 1; MOTG_duty = 0.5; + MOTG_evol = 1; Pwm_MG = 0.5; - + do { if (FlagTickLed) { Led1 = !Led1; FlagTickLed = 0; } - if (BPPressed) { BPPressed = 0; + SensG = !SensG; + Led2 = !Led2; + } - if (!SensG) { - SensG = 1; + if (((Tick%1000)==0) && FlagTick) { + FlagTick = 0; + if (MOTG_evol) { + if (MOTG_duty < 0.9) MOTG_duty += 0.1; + else { + MOTG_evol = 0; + MOTG_duty = 0.9; + } } else { - SensG = 0; + if (MOTG_duty > 0.1) MOTG_duty -= 0.1; + else { + MOTG_evol = 1; + MOTG_duty = 0.1; + } } - Pwm_MG = MOTG_duty; } - - Pc.printf ("\rPos = %ld Tick", Gauche.GetCounter()); - + + Pwm_MG = MOTG_duty; + Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTG_duty, Gauche.GetCounter(), Gperiod); + + } while(!Pc.readable()); MENU_choix = Pc.getc(); + En = 0; if (SensG == 0) Pwm_MG = 0; else Pwm_MG = 1; + break; case 7 : @@ -692,7 +767,9 @@ En = 1; SensD = 1; + Led2 = 1; MOTD_duty = 0.5; + MOTD_evol = 1; Pwm_MD = 0.5; do { @@ -702,26 +779,42 @@ } if (BPPressed) { BPPressed = 0; - if (SensD) { - SensD = 0; + SensD = !SensD; + Led2 = !Led2; + } + + if (((Tick%1000)==0) && FlagTick) { + FlagTick = 0; + if (MOTD_evol) { + if (MOTD_duty < 0.9) MOTD_duty += 0.1; + else { + MOTD_evol = 0; + MOTD_duty = 0.9; + } } else { - SensD = 1; + if (MOTD_duty > 0.1) MOTD_duty -= 0.1; + else { + MOTD_evol = 1; + MOTD_duty = 0.1; + } } - Pwm_MD = MOTD_duty; } - - Pc.printf ("\rPos = %ld Tick", Droite.GetCounter()); - + Pwm_MD = MOTD_duty; + Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTD_duty, Droite.GetCounter(), Dperiod); + } while(!Pc.readable()); MENU_choix = Pc.getc(); + En = 0; if (SensD == 0) Pwm_MD = 0; else Pwm_MD = 1; + break; case 8 : Pc.printf ("\n\n\rServo Moteur\n"); Pc.printf ("\rAppuyez sur Entree pour quitter\n"); + do { if (FlagTickLed) { Led1 = !Led1; @@ -729,25 +822,25 @@ } if (((Tick%250)==0) && FlagTick) { + FlagTick = 0; if (SERVO_dir == S_monte) { - if (SERVO_angle >= SERVO_angleMAX) { + if (SERVO_pulseWidth < (SERVO_max - 100)) SERVO_pulseWidth +=100; + else { SERVO_dir = S_descente; - SERVO_angle = SERVO_angleMAX; - } else { - SERVO_angle += 5; + SERVO_pulseWidth = SERVO_max; } } else { - if (SERVO_angle <= SERVO_angleMIN) { + if (SERVO_pulseWidth > (SERVO_min + 100)) SERVO_pulseWidth -=100; + else { SERVO_dir = S_monte; - SERVO_angle = SERVO_angleMIN; - } else { - SERVO_angle -= 5; + SERVO_pulseWidth = SERVO_min; } } - SERVO_pos = (lWord)(SERVO_angle*50/9) + 1000; - Servo.pulsewidth_us(SERVO_pos); - Pc.printf ("\rAngle = %4.1lf", SERVO_angle); + + Servo.pulsewidth_us (SERVO_pulseWidth); + Pc.printf ("\rPulse = %d",SERVO_pulseWidth); } + } while(!Pc.readable()); MENU_choix = Pc.getc(); break;