![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Projet d'interfaçage - LSM6DS3 : Test de Stabilité Quentin NOIRET LP MECSE 2020 -------------------------------------------------- L’application réalisée permet de tester la stabilité d’un système (ex : drone) en affichant différents seuils d’inclinaison en fonction des valeurs de l’accéléromètre.
Dependencies: Interfacage_MECSE_LSM6DS3 mbed LSM6DS3
main.cpp@7:e5f8936d11af, 2020-06-24 (annotated)
- Committer:
- raketitch
- Date:
- Wed Jun 24 21:34:53 2020 +0000
- Revision:
- 7:e5f8936d11af
- Parent:
- 6:16ba206a4253
version finale
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
raketitch | 7:e5f8936d11af | 1 | // Projet interfaçage : LSM6DS3 Demo |
raketitch | 7:e5f8936d11af | 2 | //Quentin NOIRET |
raketitch | 7:e5f8936d11af | 3 | //LP MECSE - IUT de CACHAN |
raketitch | 7:e5f8936d11af | 4 | //2020 |
beanmachine44 | 0:9632b831b6c1 | 5 | |
raketitch | 7:e5f8936d11af | 6 | //Bibliothèques |
beanmachine44 | 0:9632b831b6c1 | 7 | #include "mbed.h" |
raketitch | 6:16ba206a4253 | 8 | #include "LSM6DS3.h" //sensor |
raketitch | 6:16ba206a4253 | 9 | #include "LCD_DISCO_F746NG.h" //LCD Screen |
raketitch | 6:16ba206a4253 | 10 | #include "TS_DISCO_F746NG.h" //Touch Screen |
raketitch | 7:e5f8936d11af | 11 | #include "logo.h" //logo IUT |
raketitch | 7:e5f8936d11af | 12 | #include "math.h" //nécessaire aux calculs d'angle |
raketitch | 7:e5f8936d11af | 13 | #include "F746_GUI.hpp" //bibliothèques d'interface |
raketitch | 6:16ba206a4253 | 14 | |
raketitch | 6:16ba206a4253 | 15 | |
raketitch | 6:16ba206a4253 | 16 | |
raketitch | 7:e5f8936d11af | 17 | //Constantes |
raketitch | 7:e5f8936d11af | 18 | #define REFRESH_TIME_MS 50 |
raketitch | 7:e5f8936d11af | 19 | #define M_PI 3.141593 |
raketitch | 7:e5f8936d11af | 20 | #define SEUIL_1 5 |
raketitch | 7:e5f8936d11af | 21 | #define SEUIL_2 20 |
raketitch | 7:e5f8936d11af | 22 | #define SEUIL_3 45 |
raketitch | 7:e5f8936d11af | 23 | |
raketitch | 7:e5f8936d11af | 24 | #define UP_PADDING 15 |
raketitch | 7:e5f8936d11af | 25 | #define LEFT_PADDING 15 |
raketitch | 7:e5f8936d11af | 26 | |
raketitch | 7:e5f8936d11af | 27 | |
raketitch | 7:e5f8936d11af | 28 | LSM6DS3 sensor(PB_9, PB_8); //instance du capteur : (PIN_SCL, PIN_SDA) |
raketitch | 7:e5f8936d11af | 29 | LCD_DISCO_F746NG lcd; //écran LCD |
raketitch | 7:e5f8936d11af | 30 | TS_DISCO_F746NG ts; //écran tactile |
raketitch | 7:e5f8936d11af | 31 | Serial pc(USBTX, USBRX); //liaison série |
raketitch | 7:e5f8936d11af | 32 | |
raketitch | 7:e5f8936d11af | 33 | //Prototypes des fonctions |
raketitch | 7:e5f8936d11af | 34 | |
raketitch | 7:e5f8936d11af | 35 | void setup(void); //initialisation du capteur |
raketitch | 7:e5f8936d11af | 36 | void readSensor(void); //lecture du capteur |
raketitch | 7:e5f8936d11af | 37 | void printSensor(void); //affichage des valeurs du capteur |
raketitch | 7:e5f8936d11af | 38 | void drawImageLogo(int offsetX, int offsetY); //affichage du logo de l'IUT |
raketitch | 7:e5f8936d11af | 39 | |
raketitch | 7:e5f8936d11af | 40 | //Variables globales |
raketitch | 7:e5f8936d11af | 41 | char text[30]; |
raketitch | 7:e5f8936d11af | 42 | |
raketitch | 7:e5f8936d11af | 43 | float accelX, accelY, accelZ, // units m/s/s i.e. accelZ if often 9.8 (gravity) |
raketitch | 7:e5f8936d11af | 44 | gyroX, gyroY, gyroZ, // units dps (degrees per second) |
raketitch | 7:e5f8936d11af | 45 | accRoll=-1, accPitch=-1; // units degrees (roll and pitch noisy, yaw not possible) |
raketitch | 7:e5f8936d11af | 46 | |
raketitch | 7:e5f8936d11af | 47 | |
raketitch | 7:e5f8936d11af | 48 | int etat1=0, etat2=0; //variable d'etat |
raketitch | 7:e5f8936d11af | 49 | |
raketitch | 6:16ba206a4253 | 50 | using namespace std; |
raketitch | 6:16ba206a4253 | 51 | |
raketitch | 7:e5f8936d11af | 52 | //--------------------------------------------------------------------// |
raketitch | 7:e5f8936d11af | 53 | //--------------------------- MAIN -----------------------------------// |
raketitch | 7:e5f8936d11af | 54 | //--------------------------------------------------------------------// |
raketitch | 7:e5f8936d11af | 55 | |
raketitch | 6:16ba206a4253 | 56 | int main() |
raketitch | 6:16ba206a4253 | 57 | { |
raketitch | 7:e5f8936d11af | 58 | //Variables locales |
raketitch | 7:e5f8936d11af | 59 | TS_StateTypeDef TS_State; //Etat de l'écran tactile |
raketitch | 7:e5f8936d11af | 60 | uint16_t touch, oldTouch; //pour détecter les front montant d'un double appui |
raketitch | 7:e5f8936d11af | 61 | bool touchFlag = false; //flag qui s'active quand on réalise un double appui |
raketitch | 7:e5f8936d11af | 62 | |
raketitch | 7:e5f8936d11af | 63 | //------------------------------------------------------// |
raketitch | 7:e5f8936d11af | 64 | //------------------ INITIALISATION --------------------// |
raketitch | 7:e5f8936d11af | 65 | //------------------------------------------------------// |
raketitch | 7:e5f8936d11af | 66 | setup(); |
raketitch | 7:e5f8936d11af | 67 | |
raketitch | 7:e5f8936d11af | 68 | //------------------------------------------------------// |
raketitch | 7:e5f8936d11af | 69 | //--------------- DEMARRAGE INTERFACE ------------------// |
raketitch | 7:e5f8936d11af | 70 | //------------------------------------------------------// |
raketitch | 7:e5f8936d11af | 71 | |
raketitch | 7:e5f8936d11af | 72 | //------------------------------------------------------// |
raketitch | 7:e5f8936d11af | 73 | //---------------------- ECRAN 1 -----------------------// |
raketitch | 7:e5f8936d11af | 74 | //------------------------------------------------------// |
raketitch | 7:e5f8936d11af | 75 | BSP_LCD_Clear(LCD_COLOR_WHITE); |
raketitch | 7:e5f8936d11af | 76 | BSP_LCD_SetFont(&LCD_DEFAULT_FONT); |
raketitch | 7:e5f8936d11af | 77 | BSP_LCD_SetBackColor(LCD_COLOR_WHITE); |
raketitch | 7:e5f8936d11af | 78 | BSP_LCD_SetTextColor(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 79 | |
raketitch | 7:e5f8936d11af | 80 | BSP_LCD_DisplayStringAt(0, 1, (uint8_t *)"PROJET INTERFACAGE", CENTER_MODE); |
raketitch | 7:e5f8936d11af | 81 | drawImageLogo(150,150); |
raketitch | 7:e5f8936d11af | 82 | HAL_Delay(2000); |
raketitch | 7:e5f8936d11af | 83 | |
raketitch | 7:e5f8936d11af | 84 | //------------------------------------------------------// |
raketitch | 7:e5f8936d11af | 85 | //---------------------- ECRAN 2 -----------------------// |
raketitch | 7:e5f8936d11af | 86 | //------------------------------------------------------// |
raketitch | 7:e5f8936d11af | 87 | |
raketitch | 7:e5f8936d11af | 88 | BSP_LCD_Clear(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 89 | BSP_LCD_DisplayStringAt(0, 1, (uint8_t *)"Toucher pour commencer", CENTER_MODE); |
raketitch | 7:e5f8936d11af | 90 | |
raketitch | 7:e5f8936d11af | 91 | //Attend que l'utilisateur touche l'écra, |
raketitch | 7:e5f8936d11af | 92 | do |
raketitch | 7:e5f8936d11af | 93 | { |
raketitch | 7:e5f8936d11af | 94 | ts.GetState(&TS_State); |
raketitch | 7:e5f8936d11af | 95 | |
raketitch | 7:e5f8936d11af | 96 | }while(!TS_State.touchDetected); |
raketitch | 7:e5f8936d11af | 97 | HAL_Delay(50); |
raketitch | 7:e5f8936d11af | 98 | |
raketitch | 7:e5f8936d11af | 99 | //------------------------------------------------------// |
raketitch | 7:e5f8936d11af | 100 | //----------- DECLARATIONS OBJETS INTERFACE -----------// |
raketitch | 7:e5f8936d11af | 101 | //------------------------------------------------------// |
raketitch | 6:16ba206a4253 | 102 | |
raketitch | 7:e5f8936d11af | 103 | Button btnStart(LEFT_PADDING, UP_PADDING+35, 100, 75, "Start"); |
raketitch | 7:e5f8936d11af | 104 | Button btnStop(LEFT_PADDING+100+5, UP_PADDING+35, 100, 75, "Stop"); |
raketitch | 7:e5f8936d11af | 105 | Button btnDebug_On(LEFT_PADDING+200+10, UP_PADDING+35, 100, 75, "Afficher Val"); |
raketitch | 7:e5f8936d11af | 106 | Button btnDebug_Off(LEFT_PADDING+300+15, UP_PADDING+35, 100, 75, "Masquer Val"); |
raketitch | 7:e5f8936d11af | 107 | Button btnReset(480-LEFT_PADDING-100, 272-UP_PADDING-50, 100, 50, "Reset"); |
raketitch | 7:e5f8936d11af | 108 | |
raketitch | 7:e5f8936d11af | 109 | Label lbTitre(240, 2, "Test de Stabilite", Label::CENTER, Font16); |
raketitch | 7:e5f8936d11af | 110 | Label lbInfo(240, 20, "Poser 2 doigts sur l'ecran pour afficher/masquer le menu", Label::CENTER); |
raketitch | 7:e5f8936d11af | 111 | Label lbSystem(260, 180, "", Label::CENTER, Font24); |
raketitch | 7:e5f8936d11af | 112 | |
raketitch | 7:e5f8936d11af | 113 | NumericLabel<float> lbl_ax(LEFT_PADDING, 150, "ax : %4.2f", accelX, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK);//, Font12, Label::LCD_COLOR_BLACK, Label::LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 114 | NumericLabel<float> lbl_ay(LEFT_PADDING, 160, "ay : %4.2f", accelY, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 115 | NumericLabel<float> lbl_az(LEFT_PADDING, 170, "az : %4.2f", accelZ, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 116 | |
raketitch | 7:e5f8936d11af | 117 | NumericLabel<float> lbl_roll(LEFT_PADDING, 180, "Roll : %4.2f", accRoll, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 118 | NumericLabel<float> lbl_pitch(LEFT_PADDING, 190, "Pitch: %4.2f", accPitch, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 119 | |
raketitch | 7:e5f8936d11af | 120 | //NumericLabel<int> lbl_etat1(LEFT_PADDING, 200, "etat1: %d", etat1, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 121 | //NumericLabel<int> lbl_etat2(LEFT_PADDING, 210, "etat2: %d", etat2, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 122 | |
raketitch | 7:e5f8936d11af | 123 | //------------------------------------------------------// |
raketitch | 7:e5f8936d11af | 124 | //---------- INITIALISATION OBJETS INTERFACE ----------// |
raketitch | 7:e5f8936d11af | 125 | //------------------------------------------------------// |
raketitch | 6:16ba206a4253 | 126 | |
raketitch | 7:e5f8936d11af | 127 | btnStart.Erase(); |
raketitch | 7:e5f8936d11af | 128 | btnStop.Erase(); |
raketitch | 7:e5f8936d11af | 129 | btnDebug_On.Erase(); |
raketitch | 7:e5f8936d11af | 130 | btnDebug_Off.Erase(); |
raketitch | 7:e5f8936d11af | 131 | btnReset.Erase(); |
raketitch | 7:e5f8936d11af | 132 | |
raketitch | 7:e5f8936d11af | 133 | btnStart.Activate(); |
raketitch | 7:e5f8936d11af | 134 | btnStop.Inactivate(); |
raketitch | 7:e5f8936d11af | 135 | btnDebug_On.Inactivate(); |
raketitch | 7:e5f8936d11af | 136 | btnDebug_Off.Inactivate(); |
raketitch | 7:e5f8936d11af | 137 | btnReset.Inactivate(); |
raketitch | 7:e5f8936d11af | 138 | |
raketitch | 7:e5f8936d11af | 139 | lbl_ax.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 140 | lbl_ay.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 141 | lbl_az.Redraw(LCD_COLOR_BLACK); |
beanmachine44 | 0:9632b831b6c1 | 142 | |
raketitch | 7:e5f8936d11af | 143 | lbl_roll.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 144 | lbl_pitch.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 145 | |
raketitch | 7:e5f8936d11af | 146 | //lbl_etat1.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 147 | //lbl_etat2.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 148 | |
raketitch | 7:e5f8936d11af | 149 | HAL_Delay(500); |
raketitch | 7:e5f8936d11af | 150 | |
raketitch | 7:e5f8936d11af | 151 | btnReset.Activate(); |
raketitch | 7:e5f8936d11af | 152 | |
raketitch | 7:e5f8936d11af | 153 | //BOUCLE INFINI |
raketitch | 6:16ba206a4253 | 154 | while (true) |
raketitch | 6:16ba206a4253 | 155 | { |
raketitch | 7:e5f8936d11af | 156 | ts.GetState(&TS_State);//lecture ecran |
raketitch | 7:e5f8936d11af | 157 | touch = TS_State.touchDetected; |
raketitch | 6:16ba206a4253 | 158 | |
raketitch | 7:e5f8936d11af | 159 | //gestion interface |
raketitch | 7:e5f8936d11af | 160 | switch (etat1) |
raketitch | 7:e5f8936d11af | 161 | { |
raketitch | 7:e5f8936d11af | 162 | case 0: //idle |
raketitch | 7:e5f8936d11af | 163 | if(btnStart.Touched()) |
raketitch | 7:e5f8936d11af | 164 | { |
raketitch | 7:e5f8936d11af | 165 | etat1 = 1; |
raketitch | 7:e5f8936d11af | 166 | btnStart.Inactivate(); |
raketitch | 7:e5f8936d11af | 167 | btnStop.Activate(); |
raketitch | 7:e5f8936d11af | 168 | btnDebug_On.Activate(); |
raketitch | 7:e5f8936d11af | 169 | } |
raketitch | 7:e5f8936d11af | 170 | break; |
raketitch | 7:e5f8936d11af | 171 | |
raketitch | 7:e5f8936d11af | 172 | case 1://started masquer valeurs |
raketitch | 7:e5f8936d11af | 173 | readSensor(); |
raketitch | 7:e5f8936d11af | 174 | accRoll = atan2(accelY, accelZ) * 180/M_PI; |
raketitch | 7:e5f8936d11af | 175 | accPitch = atan2(-accelX, sqrt(accelY*accelY + accelZ*accelZ)) * 180/M_PI; |
raketitch | 7:e5f8936d11af | 176 | if (btnDebug_On.Touched()) |
raketitch | 7:e5f8936d11af | 177 | { |
raketitch | 7:e5f8936d11af | 178 | btnDebug_On.Inactivate(); |
raketitch | 7:e5f8936d11af | 179 | btnDebug_Off.Activate(); |
raketitch | 7:e5f8936d11af | 180 | |
raketitch | 7:e5f8936d11af | 181 | etat1 = 2; |
raketitch | 7:e5f8936d11af | 182 | } |
raketitch | 7:e5f8936d11af | 183 | if(btnStop.Touched()) |
raketitch | 7:e5f8936d11af | 184 | { |
raketitch | 7:e5f8936d11af | 185 | etat1 = 0; |
raketitch | 7:e5f8936d11af | 186 | btnStart.Activate(); |
raketitch | 7:e5f8936d11af | 187 | btnStop.Inactivate(); |
raketitch | 7:e5f8936d11af | 188 | btnDebug_On.Inactivate(); |
raketitch | 7:e5f8936d11af | 189 | btnDebug_Off.Inactivate(); |
raketitch | 7:e5f8936d11af | 190 | } |
raketitch | 7:e5f8936d11af | 191 | |
raketitch | 7:e5f8936d11af | 192 | break; |
raketitch | 7:e5f8936d11af | 193 | |
raketitch | 7:e5f8936d11af | 194 | case 2 ://Afficher valeurs |
raketitch | 7:e5f8936d11af | 195 | readSensor(); |
raketitch | 7:e5f8936d11af | 196 | accRoll = atan2(accelY, accelZ) * 180/M_PI; |
raketitch | 7:e5f8936d11af | 197 | accPitch = atan2(-accelX, sqrt(accelY*accelY + accelZ*accelZ)) * 180/M_PI; |
raketitch | 7:e5f8936d11af | 198 | lbl_ax.Draw(accelX); |
raketitch | 7:e5f8936d11af | 199 | lbl_ay.Draw(accelY); |
raketitch | 7:e5f8936d11af | 200 | lbl_az.Draw(accelZ); |
raketitch | 6:16ba206a4253 | 201 | |
raketitch | 7:e5f8936d11af | 202 | lbl_roll.Draw(accRoll); |
raketitch | 7:e5f8936d11af | 203 | lbl_pitch.Draw(accPitch); |
raketitch | 7:e5f8936d11af | 204 | |
raketitch | 7:e5f8936d11af | 205 | //lbl_etat1.Draw(etat1); |
raketitch | 7:e5f8936d11af | 206 | //lbl_etat2.Draw(etat2); |
raketitch | 7:e5f8936d11af | 207 | |
raketitch | 7:e5f8936d11af | 208 | if(btnDebug_Off.Touched()) |
raketitch | 7:e5f8936d11af | 209 | { |
raketitch | 7:e5f8936d11af | 210 | etat1 = 1; |
raketitch | 7:e5f8936d11af | 211 | btnDebug_Off.Inactivate(); |
raketitch | 7:e5f8936d11af | 212 | btnDebug_On.Activate(); |
raketitch | 7:e5f8936d11af | 213 | |
raketitch | 7:e5f8936d11af | 214 | lbl_ax.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 215 | lbl_ay.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 216 | lbl_az.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 217 | |
raketitch | 7:e5f8936d11af | 218 | lbl_roll.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 219 | lbl_pitch.Redraw(LCD_COLOR_BLACK); |
raketitch | 6:16ba206a4253 | 220 | |
raketitch | 7:e5f8936d11af | 221 | //lbl_etat1.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 222 | //lbl_etat2.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 223 | } |
raketitch | 7:e5f8936d11af | 224 | if(btnStop.Touched()) |
raketitch | 7:e5f8936d11af | 225 | { |
raketitch | 7:e5f8936d11af | 226 | etat1 = 0; |
raketitch | 7:e5f8936d11af | 227 | btnStart.Activate(); |
raketitch | 7:e5f8936d11af | 228 | btnStop.Inactivate(); |
raketitch | 7:e5f8936d11af | 229 | btnDebug_On.Inactivate(); |
raketitch | 7:e5f8936d11af | 230 | btnDebug_Off.Inactivate(); |
raketitch | 7:e5f8936d11af | 231 | |
raketitch | 7:e5f8936d11af | 232 | lbl_ax.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 233 | lbl_ay.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 234 | lbl_az.Redraw(LCD_COLOR_BLACK); |
raketitch | 6:16ba206a4253 | 235 | |
raketitch | 7:e5f8936d11af | 236 | lbl_roll.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 237 | lbl_pitch.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 238 | |
raketitch | 7:e5f8936d11af | 239 | //lbl_etat1.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 240 | //lbl_etat2.Redraw(LCD_COLOR_BLACK); |
raketitch | 7:e5f8936d11af | 241 | } |
raketitch | 7:e5f8936d11af | 242 | break; |
raketitch | 7:e5f8936d11af | 243 | |
raketitch | 6:16ba206a4253 | 244 | } |
raketitch | 6:16ba206a4253 | 245 | |
raketitch | 7:e5f8936d11af | 246 | switch(etat2) |
raketitch | 7:e5f8936d11af | 247 | { |
raketitch | 7:e5f8936d11af | 248 | case 0: |
raketitch | 7:e5f8936d11af | 249 | //ecran bleu |
raketitch | 7:e5f8936d11af | 250 | if((abs(accRoll)>SEUIL_1) || (abs(accPitch)>SEUIL_1)) |
raketitch | 7:e5f8936d11af | 251 | { |
raketitch | 7:e5f8936d11af | 252 | etat2 = 1; |
raketitch | 7:e5f8936d11af | 253 | BSP_LCD_Clear(LCD_COLOR_YELLOW); |
raketitch | 7:e5f8936d11af | 254 | touchFlag = true; |
raketitch | 7:e5f8936d11af | 255 | lbSystem.Draw("legerement incline"); |
raketitch | 7:e5f8936d11af | 256 | |
raketitch | 7:e5f8936d11af | 257 | } |
raketitch | 7:e5f8936d11af | 258 | break; |
raketitch | 7:e5f8936d11af | 259 | |
raketitch | 7:e5f8936d11af | 260 | case 1: //attention ecran jaune |
raketitch | 7:e5f8936d11af | 261 | if((abs(accRoll)<SEUIL_1) && (abs(accPitch)<SEUIL_1)) |
raketitch | 7:e5f8936d11af | 262 | { |
raketitch | 7:e5f8936d11af | 263 | etat2 = 0; |
raketitch | 7:e5f8936d11af | 264 | BSP_LCD_Clear(LCD_COLOR_GREEN); |
raketitch | 7:e5f8936d11af | 265 | touchFlag = true; |
raketitch | 7:e5f8936d11af | 266 | lbSystem.Draw("plat"); |
raketitch | 7:e5f8936d11af | 267 | } |
raketitch | 7:e5f8936d11af | 268 | if((abs(accRoll)>SEUIL_2) || (abs(accPitch)>SEUIL_2)) |
raketitch | 7:e5f8936d11af | 269 | { |
raketitch | 7:e5f8936d11af | 270 | etat2 = 2; |
raketitch | 7:e5f8936d11af | 271 | BSP_LCD_Clear(LCD_COLOR_ORANGE); |
raketitch | 7:e5f8936d11af | 272 | touchFlag = true; |
raketitch | 7:e5f8936d11af | 273 | lbSystem.Draw("incline"); |
raketitch | 7:e5f8936d11af | 274 | } |
raketitch | 7:e5f8936d11af | 275 | break; |
raketitch | 7:e5f8936d11af | 276 | |
raketitch | 7:e5f8936d11af | 277 | case 2: //orange |
raketitch | 7:e5f8936d11af | 278 | if((abs(accRoll)<SEUIL_2) && (abs(accPitch)<SEUIL_2)) |
raketitch | 7:e5f8936d11af | 279 | { |
raketitch | 7:e5f8936d11af | 280 | etat2 = 1; |
raketitch | 7:e5f8936d11af | 281 | BSP_LCD_Clear(LCD_COLOR_YELLOW); |
raketitch | 7:e5f8936d11af | 282 | touchFlag = true; |
raketitch | 7:e5f8936d11af | 283 | lbSystem.Draw("legerement incline"); |
raketitch | 7:e5f8936d11af | 284 | } |
raketitch | 7:e5f8936d11af | 285 | if((abs(accRoll)>SEUIL_3) || (abs(accPitch)>SEUIL_3)) |
raketitch | 7:e5f8936d11af | 286 | { |
raketitch | 7:e5f8936d11af | 287 | etat2 = 3; |
raketitch | 7:e5f8936d11af | 288 | BSP_LCD_Clear(LCD_COLOR_RED); |
raketitch | 7:e5f8936d11af | 289 | touchFlag = true; |
raketitch | 7:e5f8936d11af | 290 | lbSystem.Draw("tres incline"); |
raketitch | 7:e5f8936d11af | 291 | } |
raketitch | 7:e5f8936d11af | 292 | //danger ecran rouge + clignotement |
raketitch | 7:e5f8936d11af | 293 | break; |
raketitch | 7:e5f8936d11af | 294 | |
raketitch | 7:e5f8936d11af | 295 | case 3: //rouge |
raketitch | 7:e5f8936d11af | 296 | if((abs(accRoll)<SEUIL_3) && (abs(accPitch)<SEUIL_3)) |
raketitch | 7:e5f8936d11af | 297 | { |
raketitch | 7:e5f8936d11af | 298 | etat2 = 2; |
raketitch | 7:e5f8936d11af | 299 | BSP_LCD_Clear(LCD_COLOR_ORANGE); |
raketitch | 7:e5f8936d11af | 300 | touchFlag = true; |
raketitch | 7:e5f8936d11af | 301 | lbSystem.Draw("incline"); |
raketitch | 7:e5f8936d11af | 302 | } |
raketitch | 7:e5f8936d11af | 303 | //danger ecran rouge + clignotement |
raketitch | 7:e5f8936d11af | 304 | break; |
raketitch | 7:e5f8936d11af | 305 | |
raketitch | 7:e5f8936d11af | 306 | } |
raketitch | 7:e5f8936d11af | 307 | |
raketitch | 7:e5f8936d11af | 308 | |
raketitch | 7:e5f8936d11af | 309 | //lcd_print(250, 10, "etat1 : %0.0f", (float) etat1); |
raketitch | 7:e5f8936d11af | 310 | //lcd_print(250, 11, "etat2 : %0.0f", (float) etat2); |
raketitch | 7:e5f8936d11af | 311 | //lcd_print(250, 12, "touchstate : %0.0f", (float) touch); |
raketitch | 7:e5f8936d11af | 312 | //---------------------------- DEBUG ------------------------------// |
raketitch | 7:e5f8936d11af | 313 | if(touch == 2 && oldTouch != touch) |
raketitch | 7:e5f8936d11af | 314 | { |
raketitch | 7:e5f8936d11af | 315 | if(touchFlag) |
raketitch | 7:e5f8936d11af | 316 | { |
raketitch | 7:e5f8936d11af | 317 | pc.printf("touchflag true / etat1 : %d\r\n", etat1); |
raketitch | 7:e5f8936d11af | 318 | btnStart.Draw(); |
raketitch | 7:e5f8936d11af | 319 | btnStop.Draw(); |
raketitch | 7:e5f8936d11af | 320 | btnDebug_On.Draw(); |
raketitch | 7:e5f8936d11af | 321 | btnDebug_Off.Draw(); |
raketitch | 7:e5f8936d11af | 322 | btnReset.Draw(); |
raketitch | 7:e5f8936d11af | 323 | if(etat1 == 0) |
raketitch | 7:e5f8936d11af | 324 | { |
raketitch | 7:e5f8936d11af | 325 | btnStart.Activate(); |
raketitch | 7:e5f8936d11af | 326 | btnStop.Inactivate(); |
raketitch | 7:e5f8936d11af | 327 | btnDebug_On.Inactivate(); |
raketitch | 7:e5f8936d11af | 328 | btnDebug_Off.Inactivate(); |
raketitch | 7:e5f8936d11af | 329 | } |
raketitch | 7:e5f8936d11af | 330 | else if(etat1 == 1) |
raketitch | 7:e5f8936d11af | 331 | { |
raketitch | 7:e5f8936d11af | 332 | btnStart.Inactivate(); |
raketitch | 7:e5f8936d11af | 333 | btnDebug_Off.Inactivate(); |
raketitch | 7:e5f8936d11af | 334 | } |
raketitch | 7:e5f8936d11af | 335 | else |
raketitch | 7:e5f8936d11af | 336 | { |
raketitch | 7:e5f8936d11af | 337 | btnStart.Inactivate(); |
raketitch | 7:e5f8936d11af | 338 | btnDebug_On.Inactivate(); |
raketitch | 7:e5f8936d11af | 339 | } |
raketitch | 7:e5f8936d11af | 340 | } |
raketitch | 7:e5f8936d11af | 341 | else |
raketitch | 7:e5f8936d11af | 342 | { |
raketitch | 7:e5f8936d11af | 343 | pc.printf("touchflag false / etat1 : %d\r\n", etat1); |
raketitch | 7:e5f8936d11af | 344 | btnStart.Erase(); |
raketitch | 7:e5f8936d11af | 345 | btnStop.Erase(); |
raketitch | 7:e5f8936d11af | 346 | btnDebug_On.Erase(); |
raketitch | 7:e5f8936d11af | 347 | btnDebug_Off.Erase(); |
raketitch | 7:e5f8936d11af | 348 | } |
raketitch | 7:e5f8936d11af | 349 | touchFlag ^= 1; |
raketitch | 7:e5f8936d11af | 350 | } |
raketitch | 7:e5f8936d11af | 351 | |
raketitch | 7:e5f8936d11af | 352 | if(btnReset.Touched()) |
raketitch | 7:e5f8936d11af | 353 | { |
raketitch | 7:e5f8936d11af | 354 | NVIC_SystemReset(); |
raketitch | 7:e5f8936d11af | 355 | } |
raketitch | 7:e5f8936d11af | 356 | |
raketitch | 7:e5f8936d11af | 357 | |
raketitch | 7:e5f8936d11af | 358 | oldTouch = TS_State.touchDetected; |
raketitch | 7:e5f8936d11af | 359 | lbTitre.Draw(); |
raketitch | 7:e5f8936d11af | 360 | lbInfo.Draw(); |
raketitch | 6:16ba206a4253 | 361 | wait_ms(REFRESH_TIME_MS); |
raketitch | 6:16ba206a4253 | 362 | } |
raketitch | 6:16ba206a4253 | 363 | } |
raketitch | 6:16ba206a4253 | 364 | |
raketitch | 7:e5f8936d11af | 365 | //------------------- FONCTIONS ------------------------------------// |
raketitch | 7:e5f8936d11af | 366 | |
beanmachine44 | 0:9632b831b6c1 | 367 | void setup() |
beanmachine44 | 0:9632b831b6c1 | 368 | { |
raketitch | 7:e5f8936d11af | 369 | pc.printf("\r\n------ LSM6DS3 Demo -----------\r\n"); |
raketitch | 7:e5f8936d11af | 370 | pc.printf("LDSM6DS3 Init\r\n"); |
raketitch | 7:e5f8936d11af | 371 | uint16_t status = sensor.begin(sensor.G_SCALE_245DPS, //plage de mesure minimale du gyroscope : jusqu'à +/- 245 deg/s (déjà assez rapide) |
raketitch | 7:e5f8936d11af | 372 | sensor.A_SCALE_2G, //plage de mesure minimale de l'accéléromètre : +/- 2G (accélération relativement faible) |
raketitch | 7:e5f8936d11af | 373 | sensor.G_ODR_1660, //fréquence d'échantillonage des valeurs du gyroscope max à 1,66 kEch/s (précision max) |
raketitch | 7:e5f8936d11af | 374 | sensor.A_ODR_6660); //fréquence d'échantillonage des valeurs de l'accéléromètre à 6.66 kEch/s (précision max) |
raketitch | 7:e5f8936d11af | 375 | |
raketitch | 7:e5f8936d11af | 376 | pc.printf("LSM6DS3 WHO_AM_I's returned: 0x%X\r\n", status); //renvoie l'adresse i2c du capteur |
5hel2l2y | 1:acf696b18c52 | 377 | pc.printf("Should be 0x69\r\n"); |
raketitch | 7:e5f8936d11af | 378 | |
raketitch | 7:e5f8936d11af | 379 | pc.printf("LCD Init size(%d, %d)\r\n", BSP_LCD_GetXSize(), BSP_LCD_GetYSize()); |
raketitch | 7:e5f8936d11af | 380 | |
beanmachine44 | 0:9632b831b6c1 | 381 | } |
beanmachine44 | 0:9632b831b6c1 | 382 | |
raketitch | 7:e5f8936d11af | 383 | void readSensor() |
beanmachine44 | 0:9632b831b6c1 | 384 | { |
raketitch | 7:e5f8936d11af | 385 | sensor.readTemp(); //température en °C |
raketitch | 7:e5f8936d11af | 386 | |
raketitch | 7:e5f8936d11af | 387 | sensor.readAccel();//accélération en x, y, z en G(environ 9 m/s²) |
raketitch | 7:e5f8936d11af | 388 | accelX = sensor.ax; |
raketitch | 7:e5f8936d11af | 389 | accelY = sensor.ay; |
raketitch | 7:e5f8936d11af | 390 | accelZ = sensor.az; |
raketitch | 7:e5f8936d11af | 391 | |
raketitch | 7:e5f8936d11af | 392 | sensor.readGyro();//vitesse de rotation en x, y, z en deg/s |
raketitch | 7:e5f8936d11af | 393 | gyroX = sensor.gx; |
raketitch | 7:e5f8936d11af | 394 | gyroY = sensor.gy; |
raketitch | 7:e5f8936d11af | 395 | gyroZ = sensor.gz; |
raketitch | 7:e5f8936d11af | 396 | |
raketitch | 7:e5f8936d11af | 397 | } |
raketitch | 7:e5f8936d11af | 398 | void printSensor() |
raketitch | 7:e5f8936d11af | 399 | { |
raketitch | 7:e5f8936d11af | 400 | pc.printf("accelX : %3.2f\n\r", accelX); |
raketitch | 7:e5f8936d11af | 401 | pc.printf("accelY : %3.2f\n\r", accelY); |
raketitch | 7:e5f8936d11af | 402 | pc.printf("accelZ : %3.2f\n\r", accelZ); |
raketitch | 7:e5f8936d11af | 403 | |
raketitch | 7:e5f8936d11af | 404 | pc.printf("accRoll : %3.2f\n\r", accRoll); |
raketitch | 7:e5f8936d11af | 405 | pc.printf("accPitch : %3.2f\n\r", accPitch); |
raketitch | 6:16ba206a4253 | 406 | } |
beanmachine44 | 0:9632b831b6c1 | 407 | |
raketitch | 7:e5f8936d11af | 408 | |
raketitch | 7:e5f8936d11af | 409 | void drawImageLogo(int offsetX, int offsetY) |
raketitch | 6:16ba206a4253 | 410 | { |
raketitch | 7:e5f8936d11af | 411 | int x = 0; |
raketitch | 7:e5f8936d11af | 412 | int y = 0; |
raketitch | 7:e5f8936d11af | 413 | uint32_t* dataPtr = (uint32_t*)logo.data; |
raketitch | 7:e5f8936d11af | 414 | while(y < logo.height) { |
raketitch | 7:e5f8936d11af | 415 | while(x < logo.width) { |
raketitch | 7:e5f8936d11af | 416 | BSP_LCD_DrawPixel(x + offsetX, y + offsetY, *dataPtr); |
raketitch | 7:e5f8936d11af | 417 | dataPtr++; |
raketitch | 7:e5f8936d11af | 418 | x++; |
raketitch | 7:e5f8936d11af | 419 | } |
raketitch | 7:e5f8936d11af | 420 | x = 0; |
raketitch | 7:e5f8936d11af | 421 | y++; |
raketitch | 7:e5f8936d11af | 422 | } |
raketitch | 7:e5f8936d11af | 423 | } |