Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.h@18:38504337d2fc, 2018-09-12 (annotated)

Committer:
GaspardD
Date:
Wed Sep 12 22:37:07 2018 +0000
Revision:
18:38504337d2fc
Parent:
17:8c465656eea4
Child:
19:771bf61be276
fonctionne de mani?re tr?s propre; d?couverte: m?me ? en lui disant d'aller ? la vitesse minimale en proportionnel, la vitesse du robot d?passe toujours la consigne

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GaspardD 12:51b1b40cc017 1 #include "mbed.h"
GaspardD 8:1d8c3ca5e508 2 #include <stdint.h>
GaspardD 8:1d8c3ca5e508 3
GaspardD 18:38504337d2fc 4 //#define DEBUG 3
GaspardD 17:8c465656eea4 5
GaspardD 17:8c465656eea4 6 #define DIERCTION_PERIOD 10000
GaspardD 17:8c465656eea4 7 #define DIRECTION_PULSE_MAX 2100
GaspardD 17:8c465656eea4 8 #define DIRECTION_PULSE_MIN 800
GaspardD 17:8c465656eea4 9 #define DIRECTION_PULSE_MIDDLE 1500
GaspardD 17:8c465656eea4 10
GaspardD 17:8c465656eea4 11 #define SPEED_PERIOD 20000
GaspardD 17:8c465656eea4 12 #define BRAKING_PWM 1350
GaspardD 17:8c465656eea4 13 #define MAX_PULSE_WIDTH_FOR_TACHY 1700
GaspardD 17:8c465656eea4 14 #define INITAL_PULSE_SPEED 1590
GaspardD 13:af9a59ccf60b 15
GaspardD 11:bc24b3ba51a9 16 #define NB_ECHANTILLONS_IR 4
GaspardD 11:bc24b3ba51a9 17 #define NB_ECHANTILLONS_LIDAR 4
GaspardD 11:bc24b3ba51a9 18
GaspardD 11:bc24b3ba51a9 19 #define LIDAR_STRENGTH_THRESOLD 200 //pour la gestion de la vitesse MAX (MAXSpeed_ST)
GaspardD 11:bc24b3ba51a9 20 #define LIDAR_STRENGH_DELTA 50
GaspardD 11:bc24b3ba51a9 21
GaspardD 11:bc24b3ba51a9 22 #define IR_DEADZONE_U16_22cm 49677
GaspardD 11:bc24b3ba51a9 23 #define IR_FAR_U16_105cm 11915
GaspardD 11:bc24b3ba51a9 24
GaspardD 18:38504337d2fc 25 #define SPEED_DELTA_CMPS 30
GaspardD 11:bc24b3ba51a9 26
GaspardD 11:bc24b3ba51a9 27 //#define DLVV
GaspardD 11:bc24b3ba51a9 28
GaspardD 11:bc24b3ba51a9 29
GaspardD 8:1d8c3ca5e508 30 typedef enum{
GaspardD 17:8c465656eea4 31 ATTRACTIF_G,
GaspardD 17:8c465656eea4 32 ATTRACTIF_D,
GaspardD 17:8c465656eea4 33 EQUILIBRAGE_REPULSIF
GaspardD 17:8c465656eea4 34 }MUR_ST;
GaspardD 17:8c465656eea4 35
GaspardD 17:8c465656eea4 36 #ifdef DLVV
GaspardD 17:8c465656eea4 37 typedef enum{//uniquement activé pour dlvv, se rajoute au PID de murOutput
GaspardD 17:8c465656eea4 38 FRONT_CLEAR,//pas d'obstacle droit devant
GaspardD 17:8c465656eea4 39 FRONT_OBSTRUCTED//un obstacle à été detecté à moins de 20/30 cm tbd
GaspardD 17:8c465656eea4 40 }OBSTACLE_ST;
GaspardD 17:8c465656eea4 41 #endif
GaspardD 17:8c465656eea4 42
GaspardD 17:8c465656eea4 43 typedef enum{
GaspardD 8:1d8c3ca5e508 44 ARRET,
GaspardD 8:1d8c3ca5e508 45 LOADING_SECTION,
GaspardD 8:1d8c3ca5e508 46 RUNNING_SECTION
GaspardD 8:1d8c3ca5e508 47 }SECTION_ST;
GaspardD 8:1d8c3ca5e508 48
GaspardD 8:1d8c3ca5e508 49 typedef enum{
GaspardD 17:8c465656eea4 50 SPEED_MAX,//pas de mur, on va a fond
GaspardD 17:8c465656eea4 51 SPEED_VARIABLE,//signal suffisement fort pour gérer la vitesse
GaspardD 17:8c465656eea4 52 SPEED_LIMITED//nous somme dans la plage de distance ou la vitesse doit être limitée
GaspardD 17:8c465656eea4 53 }MAX_SPEED_ST;
GaspardD 17:8c465656eea4 54
GaspardD 17:8c465656eea4 55 typedef enum{
GaspardD 8:1d8c3ca5e508 56 UNDER_SPEED,
GaspardD 8:1d8c3ca5e508 57 OVER_SPEED,
GaspardD 16:63690703b5b6 58 AT_SPEED,
GaspardD 16:63690703b5b6 59 BRAKING
GaspardD 8:1d8c3ca5e508 60 }THROTTLE_ST;
GaspardD 8:1d8c3ca5e508 61
GaspardD 8:1d8c3ca5e508 62
GaspardD 8:1d8c3ca5e508 63 typedef struct s_section {
GaspardD 11:bc24b3ba51a9 64 int targetSpeed_cmps;
GaspardD 11:bc24b3ba51a9 65 int slowSpeed_cmps;//pour effectuer les virages... pour ne pas avoir d'a coups entre la vitesse variable et la vitesse limitée: brakingCoefficient = 30*((targetSpeed_cmps/slowSpeed_cmps)-1)
GaspardD 11:bc24b3ba51a9 66 int brakingCoefficient;
GaspardD 17:8c465656eea4 67 int us_accel;
GaspardD 17:8c465656eea4 68 int us_decel;
GaspardD 11:bc24b3ba51a9 69 int lidarWarningDist_cm;
GaspardD 8:1d8c3ca5e508 70 int lng_section_cm;
GaspardD 17:8c465656eea4 71 int coef_p;
GaspardD 17:8c465656eea4 72 int coef_i;
GaspardD 17:8c465656eea4 73 int coef_d;
GaspardD 8:1d8c3ca5e508 74 s_section* nextSection;
GaspardD 8:1d8c3ca5e508 75 }s_Section;
GaspardD 8:1d8c3ca5e508 76
GaspardD 11:bc24b3ba51a9 77 //*************** fonctions utilitaires ***************
GaspardD 8:1d8c3ca5e508 78
GaspardD 11:bc24b3ba51a9 79 void getTachySpeed();
GaspardD 12:51b1b40cc017 80 uint16_t getDistMoy(uint16_t* tab, int size);
GaspardD 12:51b1b40cc017 81 void it4cm();
GaspardD 12:51b1b40cc017 82 void it_serial();
GaspardD 11:bc24b3ba51a9 83
GaspardD 11:bc24b3ba51a9 84 //*************** fonctions states machines ***************
GaspardD 8:1d8c3ca5e508 85 //gestion de la direction quand la vue est dégagée
GaspardD 11:bc24b3ba51a9 86 void mursInit(void);
GaspardD 11:bc24b3ba51a9 87 void mursUpdate(void);
GaspardD 11:bc24b3ba51a9 88 void mursOutput(void);
GaspardD 8:1d8c3ca5e508 89
GaspardD 11:bc24b3ba51a9 90 #ifdef DLVV
GaspardD 8:1d8c3ca5e508 91 //gestion de la direction en mode course et de l'évitement en mode DLVV
GaspardD 11:bc24b3ba51a9 92 void obstacleInit(void);
GaspardD 11:bc24b3ba51a9 93 void obstacleUpdate(void);
GaspardD 11:bc24b3ba51a9 94 void obstacleOutput(void);
GaspardD 11:bc24b3ba51a9 95 #endif
GaspardD 8:1d8c3ca5e508 96
GaspardD 11:bc24b3ba51a9 97 //gestion du comportement en fn de l'avancement sur le parcours
GaspardD 11:bc24b3ba51a9 98 void sectionInit(void);
GaspardD 11:bc24b3ba51a9 99 void sectionUpdate(void);
GaspardD 11:bc24b3ba51a9 100 void sectionOutput(void);
GaspardD 11:bc24b3ba51a9 101
GaspardD 11:bc24b3ba51a9 102 //gestion de la gesion de la vitesse max/rotation max des roues la rotation est plus importante que la vitesse :)
GaspardD 11:bc24b3ba51a9 103 void maxSpeedInit(void);
GaspardD 11:bc24b3ba51a9 104 void maxSpeedUpdate(void);
GaspardD 11:bc24b3ba51a9 105 void maxSpeedOutput(void);
GaspardD 8:1d8c3ca5e508 106
GaspardD 8:1d8c3ca5e508 107 //gestion de l'allure en fonction des paramètres de la section
GaspardD 11:bc24b3ba51a9 108 void throttleInit(void);
GaspardD 11:bc24b3ba51a9 109 void throttleUpdate(void);
GaspardD 11:bc24b3ba51a9 110 void throttleOutput(void);