![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TRR2018 omar
Fork of biniou by
stateMachines.h@13:af9a59ccf60b, 2018-09-10 (annotated)
- Committer:
- GaspardD
- Date:
- Mon Sep 10 21:00:47 2018 +0000
- Revision:
- 13:af9a59ccf60b
- Parent:
- 12:51b1b40cc017
- Child:
- 14:d471faa7d1a2
debbuging states
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
GaspardD | 12:51b1b40cc017 | 1 | #include "mbed.h" |
GaspardD | 8:1d8c3ca5e508 | 2 | #include <stdint.h> |
GaspardD | 8:1d8c3ca5e508 | 3 | |
GaspardD | 13:af9a59ccf60b | 4 | #define DEBUG |
GaspardD | 13:af9a59ccf60b | 5 | |
GaspardD | 11:bc24b3ba51a9 | 6 | #define NB_ECHANTILLONS_IR 4 |
GaspardD | 11:bc24b3ba51a9 | 7 | #define NB_ECHANTILLONS_LIDAR 4 |
GaspardD | 11:bc24b3ba51a9 | 8 | |
GaspardD | 11:bc24b3ba51a9 | 9 | #define LIDAR_STRENGTH_THRESOLD 200 //pour la gestion de la vitesse MAX (MAXSpeed_ST) |
GaspardD | 11:bc24b3ba51a9 | 10 | #define LIDAR_STRENGH_DELTA 50 |
GaspardD | 11:bc24b3ba51a9 | 11 | |
GaspardD | 11:bc24b3ba51a9 | 12 | #define IR_DEADZONE_U16_22cm 49677 |
GaspardD | 11:bc24b3ba51a9 | 13 | #define IR_FAR_U16_105cm 11915 |
GaspardD | 11:bc24b3ba51a9 | 14 | |
GaspardD | 11:bc24b3ba51a9 | 15 | #define SPEED_DELTA_CMPS 2 |
GaspardD | 11:bc24b3ba51a9 | 16 | |
GaspardD | 11:bc24b3ba51a9 | 17 | //#define DLVV |
GaspardD | 11:bc24b3ba51a9 | 18 | |
GaspardD | 11:bc24b3ba51a9 | 19 | |
GaspardD | 8:1d8c3ca5e508 | 20 | typedef enum{ |
GaspardD | 8:1d8c3ca5e508 | 21 | ARRET, |
GaspardD | 8:1d8c3ca5e508 | 22 | LOADING_SECTION, |
GaspardD | 8:1d8c3ca5e508 | 23 | RUNNING_SECTION |
GaspardD | 8:1d8c3ca5e508 | 24 | }SECTION_ST; |
GaspardD | 8:1d8c3ca5e508 | 25 | |
GaspardD | 8:1d8c3ca5e508 | 26 | typedef enum{ |
GaspardD | 8:1d8c3ca5e508 | 27 | UNDER_SPEED, |
GaspardD | 8:1d8c3ca5e508 | 28 | OVER_SPEED, |
GaspardD | 8:1d8c3ca5e508 | 29 | AT_SPEED |
GaspardD | 8:1d8c3ca5e508 | 30 | }THROTTLE_ST; |
GaspardD | 8:1d8c3ca5e508 | 31 | |
GaspardD | 8:1d8c3ca5e508 | 32 | typedef enum{ |
GaspardD | 11:bc24b3ba51a9 | 33 | ATTRACTIF_G, |
GaspardD | 11:bc24b3ba51a9 | 34 | ATTRACTIF_D, |
GaspardD | 11:bc24b3ba51a9 | 35 | EQUILIBRAGE_REPULSIF |
GaspardD | 8:1d8c3ca5e508 | 36 | }MUR_ST; |
GaspardD | 8:1d8c3ca5e508 | 37 | |
GaspardD | 8:1d8c3ca5e508 | 38 | typedef enum{ |
GaspardD | 11:bc24b3ba51a9 | 39 | SPEED_MAX,//pas de mur, on va a fond |
GaspardD | 11:bc24b3ba51a9 | 40 | SPEED_VARIABLE,//signal suffisement fort pour gérer la vitesse |
GaspardD | 11:bc24b3ba51a9 | 41 | SPEED_LIMITED//nous somme dans la plage de distance ou la vitesse doit être limitée |
GaspardD | 11:bc24b3ba51a9 | 42 | }MAX_SPEED_ST; |
GaspardD | 11:bc24b3ba51a9 | 43 | |
GaspardD | 11:bc24b3ba51a9 | 44 | #ifdef DLVV |
GaspardD | 11:bc24b3ba51a9 | 45 | typedef enum{//uniquement activé pour dlvv, se rajoute au PID de murOutput |
GaspardD | 11:bc24b3ba51a9 | 46 | FRONT_CLEAR,//pas d'obstacle droit devant |
GaspardD | 11:bc24b3ba51a9 | 47 | FRONT_OBSTRUCTED//un obstacle à été detecté à moins de 20/30 cm tbd |
GaspardD | 8:1d8c3ca5e508 | 48 | }OBSTACLE_ST; |
GaspardD | 11:bc24b3ba51a9 | 49 | #endif |
GaspardD | 8:1d8c3ca5e508 | 50 | |
GaspardD | 8:1d8c3ca5e508 | 51 | typedef struct s_section { |
GaspardD | 11:bc24b3ba51a9 | 52 | int targetSpeed_cmps; |
GaspardD | 11:bc24b3ba51a9 | 53 | 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 | 54 | int brakingCoefficient; |
GaspardD | 11:bc24b3ba51a9 | 55 | float ms_accel; |
GaspardD | 11:bc24b3ba51a9 | 56 | float ms_decel; |
GaspardD | 11:bc24b3ba51a9 | 57 | int lidarWarningDist_cm; |
GaspardD | 8:1d8c3ca5e508 | 58 | int lng_section_cm; |
GaspardD | 12:51b1b40cc017 | 59 | double coef_p; |
GaspardD | 12:51b1b40cc017 | 60 | double coef_i; |
GaspardD | 12:51b1b40cc017 | 61 | double coef_d; |
GaspardD | 8:1d8c3ca5e508 | 62 | s_section* nextSection; |
GaspardD | 8:1d8c3ca5e508 | 63 | }s_Section; |
GaspardD | 8:1d8c3ca5e508 | 64 | |
GaspardD | 11:bc24b3ba51a9 | 65 | //*************** fonctions utilitaires *************** |
GaspardD | 8:1d8c3ca5e508 | 66 | |
GaspardD | 11:bc24b3ba51a9 | 67 | void getTachySpeed(); |
GaspardD | 12:51b1b40cc017 | 68 | uint16_t getDistMoy(uint16_t* tab, int size); |
GaspardD | 12:51b1b40cc017 | 69 | void it4cm(); |
GaspardD | 12:51b1b40cc017 | 70 | void it_serial(); |
GaspardD | 11:bc24b3ba51a9 | 71 | |
GaspardD | 11:bc24b3ba51a9 | 72 | //*************** fonctions states machines *************** |
GaspardD | 8:1d8c3ca5e508 | 73 | //gestion de la direction quand la vue est dégagée |
GaspardD | 11:bc24b3ba51a9 | 74 | void mursInit(void); |
GaspardD | 11:bc24b3ba51a9 | 75 | void mursUpdate(void); |
GaspardD | 11:bc24b3ba51a9 | 76 | void mursOutput(void); |
GaspardD | 8:1d8c3ca5e508 | 77 | |
GaspardD | 11:bc24b3ba51a9 | 78 | #ifdef DLVV |
GaspardD | 8:1d8c3ca5e508 | 79 | //gestion de la direction en mode course et de l'évitement en mode DLVV |
GaspardD | 11:bc24b3ba51a9 | 80 | void obstacleInit(void); |
GaspardD | 11:bc24b3ba51a9 | 81 | void obstacleUpdate(void); |
GaspardD | 11:bc24b3ba51a9 | 82 | void obstacleOutput(void); |
GaspardD | 11:bc24b3ba51a9 | 83 | #endif |
GaspardD | 8:1d8c3ca5e508 | 84 | |
GaspardD | 11:bc24b3ba51a9 | 85 | //gestion du comportement en fn de l'avancement sur le parcours |
GaspardD | 11:bc24b3ba51a9 | 86 | void sectionInit(void); |
GaspardD | 11:bc24b3ba51a9 | 87 | void sectionUpdate(void); |
GaspardD | 11:bc24b3ba51a9 | 88 | void sectionOutput(void); |
GaspardD | 11:bc24b3ba51a9 | 89 | |
GaspardD | 11:bc24b3ba51a9 | 90 | //gestion de la gesion de la vitesse max/rotation max des roues la rotation est plus importante que la vitesse :) |
GaspardD | 11:bc24b3ba51a9 | 91 | void maxSpeedInit(void); |
GaspardD | 11:bc24b3ba51a9 | 92 | void maxSpeedUpdate(void); |
GaspardD | 11:bc24b3ba51a9 | 93 | void maxSpeedOutput(void); |
GaspardD | 8:1d8c3ca5e508 | 94 | |
GaspardD | 8:1d8c3ca5e508 | 95 | //gestion de l'allure en fonction des paramètres de la section |
GaspardD | 11:bc24b3ba51a9 | 96 | void throttleInit(void); |
GaspardD | 11:bc24b3ba51a9 | 97 | void throttleUpdate(void); |
GaspardD | 11:bc24b3ba51a9 | 98 | void throttleOutput(void); |