Diff: main.cpp

Revision:

0:1cfd66c3a181

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,295 @@
+#include "mbed.h"
+
+// Eléments:
+#include "Base.h"
+#include "Moteur.h"
+#include "Encodeur.h"
+#include "Odometrie.h"
+#include "Servo.h"
+#include "Infra.h"
+#include <math.h>
+
+/*** Paramètres ***/
+
+// Pin tirette:
+#define pin_tirette  A4
+
+// Pin interrupteur:
+#define pin_switch_color  A5
+
+// Pin Servo:
+#define pin_servo  A0
+
+// Pins des moteurs:
+#define pins_moteurG_enA_in1_in2  D8, D11, D12
+#define pins_moteurD_enB_in1_in2  D7, D14, D15
+
+// Pin pompe à air:
+#define pin_pompe  D6
+
+// Pins Infrarouge:
+#define pin_infra1  A1
+#define pin_infra2  A2
+#define pin_infra3  A3
+
+// Paramètres des encodeurs:
+#define encodeur_res       1024
+#define encodeur_diametre  40        // en mm
+#define encodeurs_entraxe  97/2.0f  // en mm
+
+// Paramètre de l'odometrie et asservissement:
+#define update_delay_us    500       // en µs
+
+/*** Création des objets ***/
+
+// Moteurs:
+Moteur moteurG(pins_moteurG_enA_in1_in2);
+Moteur moteurD(pins_moteurD_enB_in1_in2);
+
+// Encodeurs:
+Encodeur encodeurG('G', encodeur_res, encodeur_diametre);
+Encodeur encodeurD('D', encodeur_res, encodeur_diametre);
+
+// Odometrie:
+Odometrie odometrie(&encodeurG, &encodeurD, encodeurs_entraxe);
+
+// Base:
+Base base(&moteurG, &moteurD, &odometrie, update_delay_us);
+#define base_max_erreur_distance  10
+#define base_max_erreur_orientation  0.1f
+
+// Tirette:
+DigitalIn tirette(pin_tirette);
+
+// Interrupteur:
+DigitalIn switch_color(pin_switch_color);
+
+// Pompe:
+DigitalOut pompe(pin_pompe);
+
+// Servo:
+Servo servo(pin_servo);
+
+// Infrarouge:
+Infra infraFrontD(pin_infra1);
+Infra infraFrontG(pin_infra2);
+Infra infraBack(pin_infra3);
+int detection();
+bool isStopped = false;
+
+// Buzzer:
+void buzzer_on();
+void buzzer_off();
+void buzzer_note();
+
+// Fin de l'épreuve:
+#define epreuve_duration  100 // s
+Timeout epreuve_timeout;
+void finish() { base.stop(); }
+
+// Prototypes - Machines d'états:
+extern int PURPLE_steps(int state);
+extern int YELLOW_steps(int state);
+
+int main() {
+    
+  printf("STM32 Base\r\n");
+  printf("Adresses de variables: (pour STM-Studio)\r\n");
+  printf(" - encodeurG.tours   = %p\r\n", encodeurG.getTours_ptr());
+  printf(" - encodeurD.tours   = %p\r\n", encodeurD.getTours_ptr());
+  printf(" - odometrie.X       = %p\r\n", odometrie.getX_ptr());
+  printf(" - odometrie.Y       = %p\r\n", odometrie.getY_ptr());
+  printf(" - odometrie.Theta   = %p\r\n", odometrie.getTheta_ptr());
+  printf(" - base.erreur_distance    = %p\r\n", base.getErreur_distance_ptr());
+  printf(" - base.erreur_orientation = %p\r\n", base.getErreur_orientation_ptr());
+  printf(" - base.pwm_distance       = %p\r\n", base.getPID_PWM_distance_ptr());
+  printf(" - base.pwm_orientation    = %p\r\n", base.getPID_PWM_orientation_ptr());
+  printf(" - base.pwm_gauche   = %p\r\n", base.getPID_PWM_gauche_ptr());
+  printf(" - base.pwm_droite   = %p\r\n", base.getPID_PWM_droite_ptr());
+  
+  // PID:
+  base.setPID_distance(1, 0, 1);
+  base.setPID_orientation(10, 0, 1);
+  
+  base.start();
+  
+  // Devant:
+  //base.forward(-1400);
+  base.forward(1000);
+  while(1) wait(1);
+  //base.turn(360.0f*10);
+  wait(6);
+  base.turn(90.0f);
+  wait(3);
+  base.forward(-600);
+  wait(3);
+  
+  while(1) wait(1);
+  
+  /*while(1) {
+    buzzer_on();
+    wait(4);
+    buzzer_off();
+    wait(2);    
+  }*/
+  
+  /*while(1) {
+    infraFrontG.mesure();
+    infraFrontD.mesure();
+    infraBack.mesure();
+    float distanceG = infraFrontG.getDistance();
+    float distanceD = infraFrontD.getDistance();
+    float distanceB = infraBack.getDistance();
+    
+    printf("f_G: %f \t f_D: %f \t f_B: %f\r\n", distanceG, distanceD, distanceB);
+    wait(0.1);
+  }*/
+  
+  // Démarre le servo:
+  servo.enable(1500, 20000);
+  servo.pos(90);
+  
+  // Tirette:
+  char count = 0;
+  /*while (count < 5) {
+    if(!tirette) count++;
+    else count = 0;
+    wait(0.1);
+  }*/
+  
+  //wait(1);
+  
+  // Fin de l'épreuve:
+  epreuve_timeout.attach(&finish, epreuve_duration);
+  
+  // Couleur du robot:
+  bool color_side = 1;//switch_color;
+  
+  // Démarre les moteurs:
+  base.start();
+  
+  // Eteint la pompe:
+  pompe = 0;
+  
+  // Désactive capteur arrière:
+  infraBack.off();
+  
+  // Déplacement:
+  int time = 0, timeOut = 500, state = 0;
+  while (1) {
+    // Gestion du temps:
+    wait_ms(1);
+    
+    // Détection:
+    //if (time % 50 == 0 || isStopped)
+    //    time += detection();
+    //else
+        time++;
+    
+    // Etape suivante:
+    
+    // Récupère les erreurs:
+    //float erreur_distance = base.getErreurDistance();
+    //float erreur_orientation = base.getErreurOrientation();
+    //if (time % 100 == 0) printf("d: %f, o: %f\r\n", erreur_distance, erreur_orientation);
+    
+    // Si la position est respectée:
+    //if (state <= 11 && (time > timeOut || abs(erreur_distance) < base_max_erreur_distance && abs(erreur_orientation) < base_max_erreur_orientation)){
+    if (state <= 14 && time > timeOut){
+      if (color_side == 0) timeOut = PURPLE_steps(state);
+      else timeOut = YELLOW_steps(state);
+      
+      state++;
+      time = 0; 
+    }
+    
+  }
+
+}
+
+// Détection de l'environnement;
+int detection() {
+    bool detectG = infraFrontG.mesure();
+    bool detectD = infraFrontD.mesure();
+    bool detectB = infraBack.mesure();
+    
+    //if (detectG || detectD || detectB) {
+    if (detectG || detectD) {
+        base.stop();
+        buzzer_on();
+        
+        // Si première détection:
+        if (isStopped == false) {
+            isStopped = true;
+            moteurG.setPWM(0.1);
+            moteurD.setPWM(0.1);
+            moteurG.forward();
+            moteurD.forward();
+            wait(0.5);
+            moteurD.stop();
+            moteurG.stop();
+        } else {
+            wait(1);
+        }
+        
+    } else if(detectB) {
+        base.stop();
+        buzzer_on();
+        
+        // Si première détection:
+        if (isStopped == false) {
+            isStopped = true;
+            moteurG.setPWM(0.1);
+            moteurD.setPWM(0.1);
+            moteurG.backward();
+            moteurD.backward();
+            wait(0.5);
+            moteurD.stop();
+            moteurG.stop();
+        } else {
+            wait(1);
+        }
+    }
+    else { 
+        if (isStopped == true) base.start();
+        buzzer_off();
+        isStopped = false;
+        return 1;
+    }
+    
+    return 0;
+}
+
+
+/*PwmOut buzzer(PB_1);
+Ticker buzzer_ticker;
+Timeout buzzer_timeout;
+DigitalOut buzzer_led(D2, 0);
+bool buzzer_state = false;
+*/
+void buzzer_note() {
+    /*float period;
+    if (buzzer_state) period =  1000000.0f / 440.0f;
+    else period = 1000000.0f / 349.0f;
+    buzzer_state = !buzzer_state;
+    
+    buzzer_led  = !buzzer_led;
+    
+    buzzer.period_us(period);
+    buzzer.write(0.50f); // 50% duty cycle
+    */
+}
+
+void buzzer_turn_off() {
+    /*buzzer_ticker.detach();
+    buzzer.write(0);
+    buzzer_led = 0;*/
+}
+
+void buzzer_on() {
+    //buzzer_ticker.attach(&buzzer_note, 0.4);
+}
+
+void buzzer_off() {
+   //buzzer_timeout.attach(&buzzer_turn_off, 1.0f);
+}