![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TRR2018 omar
Fork of biniou by
stateMachines.h@50:44c0a075e78d, 2018-09-21 (annotated)
- Committer:
- ohlimi2
- Date:
- Fri Sep 21 12:53:52 2018 +0000
- Revision:
- 50:44c0a075e78d
- Parent:
- 41:4dd36f607279
probleme de demarage
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
GaspardD | 39:de3638276b7e | 1 | #ifndef STATE_MACHINES_H |
GaspardD | 29:fc984fe08ca7 | 2 | #define STATE_MACHINES_H |
GaspardD | 29:fc984fe08ca7 | 3 | |
GaspardD | 12:51b1b40cc017 | 4 | #include "mbed.h" |
GaspardD | 8:1d8c3ca5e508 | 5 | #include <stdint.h> |
GaspardD | 8:1d8c3ca5e508 | 6 | |
GaspardD | 19:771bf61be276 | 7 | //#define DLVV |
GaspardD | 19:771bf61be276 | 8 | #define SAMPLING//commenter pour désactiver |
ohlimi2 | 50:44c0a075e78d | 9 | #define DUMP_SAMPLIG_PERIOD 20000 |
ohlimi2 | 50:44c0a075e78d | 10 | #define DEBUG 2//-1 pour avoir uniquement la gestion de l'interruption bouton |
GaspardD | 17:8c465656eea4 | 11 | |
GaspardD | 41:4dd36f607279 | 12 | #define NB_INTEGRAL_SAMPLES 20 |
GaspardD | 21:de7a0a47f8a3 | 13 | |
ohlimi2 | 36:bccddd02966a | 14 | #define TACHY_CM 8.0 // distance arbre |
GaspardD | 29:fc984fe08ca7 | 15 | #define COEF_RAYON_BR_S_DIST 1 |
GaspardD | 27:f8c3f1524a64 | 16 | //#define FREINAGE_ADAPTATIF |
GaspardD | 24:698fefbbee00 | 17 | #define PUISSANCE_FREINAGE_W 1000 //en W |
GaspardD | 28:af53caf04446 | 18 | #define MASSE_BINIOU_KG 1.8 |
GaspardD | 29:fc984fe08ca7 | 19 | #define DEMI_LARGEUR_BINIOU_CM 10.0 |
GaspardD | 22:26fc6e6f7a55 | 20 | |
ohlimi2 | 36:bccddd02966a | 21 | #define DIRECTION_PERIOD_MS 10000 |
GaspardD | 17:8c465656eea4 | 22 | #define DIRECTION_PULSE_MAX 2100 |
GaspardD | 17:8c465656eea4 | 23 | #define DIRECTION_PULSE_MIN 800 |
GaspardD | 23:04d393220daa | 24 | #define DIRECTION_PULSE_MIDDLE 1530 |
GaspardD | 17:8c465656eea4 | 25 | |
GaspardD | 41:4dd36f607279 | 26 | #define DIST_MIN_LONG_CM 40 |
GaspardD | 29:fc984fe08ca7 | 27 | |
GaspardD | 19:771bf61be276 | 28 | #define SPEED_PERIOD_US 20000 |
GaspardD | 19:771bf61be276 | 29 | #define BRAKING_PULSE_US 1350 |
GaspardD | 19:771bf61be276 | 30 | #define MAX_PULSE_WIDTH_FOR_TACHY_US 1850 |
GaspardD | 19:771bf61be276 | 31 | #define INITAL_PULSE_SPEED_US 1590 |
GaspardD | 19:771bf61be276 | 32 | #define ZERO_PULSE_SPEED_US 1500 |
GaspardD | 13:af9a59ccf60b | 33 | |
GaspardD | 23:04d393220daa | 34 | #define NB_ECHANTILLONS_IR 8 |
GaspardD | 11:bc24b3ba51a9 | 35 | |
GaspardD | 21:de7a0a47f8a3 | 36 | #define LIDAR_STRENGTH_THRESOLD 25 //pour la gestion de la vitesse MAX (MAXSpeed_ST) |
GaspardD | 21:de7a0a47f8a3 | 37 | #define LIDAR_ERR 65535 |
GaspardD | 11:bc24b3ba51a9 | 38 | |
GaspardD | 18:38504337d2fc | 39 | #define SPEED_DELTA_CMPS 30 |
GaspardD | 11:bc24b3ba51a9 | 40 | |
GaspardD | 22:26fc6e6f7a55 | 41 | #define CAPT_45_DROITE PC_2 |
ohlimi2 | 34:c98fd59c690a | 42 | #define CAPT_45_GAUCHE PA_4 |
GaspardD | 22:26fc6e6f7a55 | 43 | #define CAPT_90_DROITE PA_1 |
GaspardD | 22:26fc6e6f7a55 | 44 | #define CAPT_90_GAUCHE PC_1 |
ohlimi2 | 34:c98fd59c690a | 45 | #define CAPT_90_GAUCHE_SHORT PC_0 |
ohlimi2 | 34:c98fd59c690a | 46 | #define CAPT_90_DROITE_SHORT PA_0 |
GaspardD | 22:26fc6e6f7a55 | 47 | #define CAPT_10_GAUCHE PB_0 |
GaspardD | 22:26fc6e6f7a55 | 48 | #define CAPT_10_DROITE PC_3 |
ohlimi2 | 34:c98fd59c690a | 49 | #define CAPT_DEVANT PC_3 |
GaspardD | 22:26fc6e6f7a55 | 50 | #define ODOMETRE PA_11 |
GaspardD | 22:26fc6e6f7a55 | 51 | |
GaspardD | 19:771bf61be276 | 52 | #ifdef SAMPLING |
ohlimi2 | 50:44c0a075e78d | 53 | #define TAILLE_SAMPLES 1500 |
GaspardD | 19:771bf61be276 | 54 | typedef struct states |
GaspardD | 19:771bf61be276 | 55 | { |
GaspardD | 19:771bf61be276 | 56 | char murs_dlvv; |
GaspardD | 19:771bf61be276 | 57 | char section; |
GaspardD | 19:771bf61be276 | 58 | char maxSpeed; |
GaspardD | 19:771bf61be276 | 59 | char throttle; |
GaspardD | 19:771bf61be276 | 60 | }s_States; |
GaspardD | 11:bc24b3ba51a9 | 61 | |
GaspardD | 29:fc984fe08ca7 | 62 | typedef struct sample |
GaspardD | 19:771bf61be276 | 63 | { |
GaspardD | 19:771bf61be276 | 64 | s_States states; |
GaspardD | 22:26fc6e6f7a55 | 65 | int time; |
GaspardD | 22:26fc6e6f7a55 | 66 | int16_t pwm_thro_us; |
GaspardD | 22:26fc6e6f7a55 | 67 | int16_t pwm_dir_us; |
ohlimi2 | 50:44c0a075e78d | 68 | double largeurPiste45; |
ohlimi2 | 50:44c0a075e78d | 69 | double largeurPiste90; |
GaspardD | 29:fc984fe08ca7 | 70 | double position45; |
GaspardD | 29:fc984fe08ca7 | 71 | double position90; |
GaspardD | 29:fc984fe08ca7 | 72 | double dist; |
GaspardD | 23:04d393220daa | 73 | int16_t distLidar; |
GaspardD | 23:04d393220daa | 74 | int16_t strLidar; |
GaspardD | 19:771bf61be276 | 75 | } s_Sample; |
GaspardD | 19:771bf61be276 | 76 | |
GaspardD | 19:771bf61be276 | 77 | #endif |
GaspardD | 11:bc24b3ba51a9 | 78 | |
GaspardD | 8:1d8c3ca5e508 | 79 | typedef enum{ |
GaspardD | 29:fc984fe08ca7 | 80 | REF_BIDIR, |
GaspardD | 29:fc984fe08ca7 | 81 | REF_A_DROITE, |
GaspardD | 29:fc984fe08ca7 | 82 | REF_A_GAUCHE |
GaspardD | 17:8c465656eea4 | 83 | }MUR_ST; |
GaspardD | 17:8c465656eea4 | 84 | |
GaspardD | 17:8c465656eea4 | 85 | #ifdef DLVV |
GaspardD | 17:8c465656eea4 | 86 | typedef enum{//uniquement activé pour dlvv, se rajoute au PID de murOutput |
GaspardD | 29:fc984fe08ca7 | 87 | ALL_CLEAR, |
GaspardD | 29:fc984fe08ca7 | 88 | FRONT_OBSTRUCTED,//pas d'obstacle droit devant |
GaspardD | 29:fc984fe08ca7 | 89 | RIGHT_OBSTRUCTED,//un obstacle à été detecté à gauche |
GaspardD | 29:fc984fe08ca7 | 90 | LEFT_OBSTRUCTED//un obstacle à été detecté à droite |
GaspardD | 17:8c465656eea4 | 91 | }OBSTACLE_ST; |
GaspardD | 17:8c465656eea4 | 92 | #endif |
GaspardD | 17:8c465656eea4 | 93 | |
GaspardD | 17:8c465656eea4 | 94 | typedef enum{ |
GaspardD | 8:1d8c3ca5e508 | 95 | ARRET, |
GaspardD | 8:1d8c3ca5e508 | 96 | LOADING_SECTION, |
GaspardD | 8:1d8c3ca5e508 | 97 | RUNNING_SECTION |
GaspardD | 8:1d8c3ca5e508 | 98 | }SECTION_ST; |
GaspardD | 8:1d8c3ca5e508 | 99 | |
GaspardD | 8:1d8c3ca5e508 | 100 | typedef enum{ |
GaspardD | 17:8c465656eea4 | 101 | SPEED_MAX,//pas de mur, on va a fond |
GaspardD | 21:de7a0a47f8a3 | 102 | SPEED_WARNING,//signal suffisement fort pour gérer la vitesse |
GaspardD | 21:de7a0a47f8a3 | 103 | BLOCKED//nous somme dans la plage de distance ou la vitesse doit être limitée |
GaspardD | 17:8c465656eea4 | 104 | }MAX_SPEED_ST; |
GaspardD | 17:8c465656eea4 | 105 | |
GaspardD | 17:8c465656eea4 | 106 | typedef enum{ |
GaspardD | 21:de7a0a47f8a3 | 107 | REGULATION_SPEED, |
GaspardD | 16:63690703b5b6 | 108 | AT_SPEED, |
GaspardD | 19:771bf61be276 | 109 | BRAKING, |
GaspardD | 19:771bf61be276 | 110 | STOPPED |
GaspardD | 8:1d8c3ca5e508 | 111 | }THROTTLE_ST; |
GaspardD | 8:1d8c3ca5e508 | 112 | |
GaspardD | 8:1d8c3ca5e508 | 113 | |
GaspardD | 8:1d8c3ca5e508 | 114 | typedef struct s_section { |
GaspardD | 29:fc984fe08ca7 | 115 | double targetSpeed_cmps;//supérieure a 328 |
GaspardD | 29:fc984fe08ca7 | 116 | double slowSpeed_cmps;//supérieure a 328; pour effectuer les virages: pour ne pas avoir d'a coups entre la vitesse variable et la vitesse limitée |
GaspardD | 29:fc984fe08ca7 | 117 | double coef_p_speed; |
GaspardD | 29:fc984fe08ca7 | 118 | double consigne_position; |
GaspardD | 29:fc984fe08ca7 | 119 | double lidarWarningDist_cm; |
GaspardD | 29:fc984fe08ca7 | 120 | double lng_section_cm; |
ohlimi2 | 50:44c0a075e78d | 121 | double coef_p90; |
ohlimi2 | 50:44c0a075e78d | 122 | double coef_p45; |
ohlimi2 | 50:44c0a075e78d | 123 | double coef_i90; |
ohlimi2 | 50:44c0a075e78d | 124 | double coef_i45; |
ohlimi2 | 50:44c0a075e78d | 125 | double coef_d90; |
ohlimi2 | 50:44c0a075e78d | 126 | double coef_d45; |
GaspardD | 8:1d8c3ca5e508 | 127 | s_section* nextSection; |
GaspardD | 8:1d8c3ca5e508 | 128 | }s_Section; |
GaspardD | 8:1d8c3ca5e508 | 129 | |
GaspardD | 11:bc24b3ba51a9 | 130 | //*************** fonctions utilitaires *************** |
GaspardD | 8:1d8c3ca5e508 | 131 | |
ohlimi2 | 36:bccddd02966a | 132 | //void getTachySpeed(); |
GaspardD | 29:fc984fe08ca7 | 133 | double getDistMoy(AnalogIn* p, double *tab, int size); |
GaspardD | 12:51b1b40cc017 | 134 | void it4cm(); |
GaspardD | 12:51b1b40cc017 | 135 | void it_serial(); |
GaspardD | 23:04d393220daa | 136 | void sampleLog(); |
GaspardD | 29:fc984fe08ca7 | 137 | void initSamples(); |
GaspardD | 29:fc984fe08ca7 | 138 | void initIntegrationTable(); |
GaspardD | 29:fc984fe08ca7 | 139 | void pressed(); |
GaspardD | 11:bc24b3ba51a9 | 140 | |
GaspardD | 33:88d8f254c0b7 | 141 | |
GaspardD | 11:bc24b3ba51a9 | 142 | //*************** fonctions states machines *************** |
GaspardD | 8:1d8c3ca5e508 | 143 | //gestion de la direction quand la vue est dégagée |
GaspardD | 11:bc24b3ba51a9 | 144 | void mursInit(void); |
GaspardD | 11:bc24b3ba51a9 | 145 | void mursUpdate(void); |
GaspardD | 11:bc24b3ba51a9 | 146 | void mursOutput(void); |
GaspardD | 8:1d8c3ca5e508 | 147 | |
GaspardD | 11:bc24b3ba51a9 | 148 | #ifdef DLVV |
GaspardD | 8:1d8c3ca5e508 | 149 | //gestion de la direction en mode course et de l'évitement en mode DLVV |
GaspardD | 11:bc24b3ba51a9 | 150 | void obstacleInit(void); |
GaspardD | 11:bc24b3ba51a9 | 151 | void obstacleUpdate(void); |
GaspardD | 11:bc24b3ba51a9 | 152 | void obstacleOutput(void); |
GaspardD | 11:bc24b3ba51a9 | 153 | #endif |
GaspardD | 8:1d8c3ca5e508 | 154 | |
GaspardD | 11:bc24b3ba51a9 | 155 | //gestion du comportement en fn de l'avancement sur le parcours |
GaspardD | 11:bc24b3ba51a9 | 156 | void sectionInit(void); |
GaspardD | 11:bc24b3ba51a9 | 157 | void sectionUpdate(void); |
GaspardD | 11:bc24b3ba51a9 | 158 | void sectionOutput(void); |
GaspardD | 11:bc24b3ba51a9 | 159 | |
GaspardD | 11:bc24b3ba51a9 | 160 | //gestion de la gesion de la vitesse max/rotation max des roues la rotation est plus importante que la vitesse :) |
GaspardD | 11:bc24b3ba51a9 | 161 | void maxSpeedInit(void); |
GaspardD | 11:bc24b3ba51a9 | 162 | void maxSpeedUpdate(void); |
GaspardD | 11:bc24b3ba51a9 | 163 | void maxSpeedOutput(void); |
GaspardD | 8:1d8c3ca5e508 | 164 | |
GaspardD | 8:1d8c3ca5e508 | 165 | //gestion de l'allure en fonction des paramètres de la section |
GaspardD | 11:bc24b3ba51a9 | 166 | void throttleInit(void); |
GaspardD | 11:bc24b3ba51a9 | 167 | void throttleUpdate(void); |
GaspardD | 11:bc24b3ba51a9 | 168 | void throttleOutput(void); |
GaspardD | 29:fc984fe08ca7 | 169 | |
GaspardD | 29:fc984fe08ca7 | 170 | #endif |