anthony peltriaux / Mbed 2 deprecated DISCO-F746NG_blink_led

Projet interfaçage

Dependencies:   mbed BSP_DISCO_F746NG

main.cpp@0:1c6757f4b61c, 2021-06-21 (annotated)

Committer:
anthonyp08
Date:
Mon Jun 21 08:21:33 2021 +0000
Revision:
0:1c6757f4b61c
Child:
1:eb044e6a9033
Projet interface

Who changed what in which revision?

UserRevisionLine numberNew contents of line
anthonyp08 0:1c6757f4b61c 1 #include "mbed.h"
anthonyp08 0:1c6757f4b61c 2 #include "MPU6050.h"
anthonyp08 0:1c6757f4b61c 3 #include "stm32746g_discovery_lcd.h" // include les bibliothèques
anthonyp08 0:1c6757f4b61c 4 #include <math.h>
anthonyp08 0:1c6757f4b61c 5
anthonyp08 0:1c6757f4b61c 6
anthonyp08 0:1c6757f4b61c 7
anthonyp08 0:1c6757f4b61c 8 Serial pc(USBTX, USBRX);
anthonyp08 0:1c6757f4b61c 9 I2C i2c(I2C_SDA, I2C_SCL); //Déclaration de SDA et SCL ainsi que I2C
anthonyp08 0:1c6757f4b61c 10 MPU6050 mpu(i2c);
anthonyp08 0:1c6757f4b61c 11
anthonyp08 0:1c6757f4b61c 12
anthonyp08 0:1c6757f4b61c 13 PwmOut servo1 (D9); //Déclaration des pins des servo-moteurs
anthonyp08 0:1c6757f4b61c 14 PwmOut servo2 (D10);
anthonyp08 0:1c6757f4b61c 15
anthonyp08 0:1c6757f4b61c 16 int16_t ax, ay, az;
anthonyp08 0:1c6757f4b61c 17 int16_t gx, gy, gz;
anthonyp08 0:1c6757f4b61c 18 int moyx, moyy, moyz ; //Déclaration des variables
anthonyp08 0:1c6757f4b61c 19
anthonyp08 0:1c6757f4b61c 20 Timer timer;
anthonyp08 0:1c6757f4b61c 21
anthonyp08 0:1c6757f4b61c 22
anthonyp08 0:1c6757f4b61c 23 Ticker flipper;
anthonyp08 0:1c6757f4b61c 24
anthonyp08 0:1c6757f4b61c 25 double t1, t2;
anthonyp08 0:1c6757f4b61c 26
anthonyp08 0:1c6757f4b61c 27
anthonyp08 0:1c6757f4b61c 28 void servo_ctrl()
anthonyp08 0:1c6757f4b61c 29 {
anthonyp08 0:1c6757f4b61c 30 if (t2 <0) {
anthonyp08 0:1c6757f4b61c 31 servo1.pulsewidth_us(2000);
anthonyp08 0:1c6757f4b61c 32
anthonyp08 0:1c6757f4b61c 33 }
anthonyp08 0:1c6757f4b61c 34 else if (t2 > 0) {
anthonyp08 0:1c6757f4b61c 35 servo1.pulsewidth_us(1200);
anthonyp08 0:1c6757f4b61c 36
anthonyp08 0:1c6757f4b61c 37 } //fonctionnalité des servo-moteurs
anthonyp08 0:1c6757f4b61c 38 if (t1 <0) {
anthonyp08 0:1c6757f4b61c 39 servo2.pulsewidth_us(2000);
anthonyp08 0:1c6757f4b61c 40
anthonyp08 0:1c6757f4b61c 41 }
anthonyp08 0:1c6757f4b61c 42 else if (t1 > 0) {
anthonyp08 0:1c6757f4b61c 43 servo2.pulsewidth_us(1200);
anthonyp08 0:1c6757f4b61c 44
anthonyp08 0:1c6757f4b61c 45 }
anthonyp08 0:1c6757f4b61c 46 }
anthonyp08 0:1c6757f4b61c 47
anthonyp08 0:1c6757f4b61c 48 int main()
anthonyp08 0:1c6757f4b61c 49 {
anthonyp08 0:1c6757f4b61c 50
anthonyp08 0:1c6757f4b61c 51 BSP_LCD_Init();
anthonyp08 0:1c6757f4b61c 52 BSP_LCD_LayerDefaultInit(LTDC_ACTIVE_LAYER, LCD_FB_START_ADDRESS);
anthonyp08 0:1c6757f4b61c 53 BSP_LCD_SelectLayer(LTDC_ACTIVE_LAYER);
anthonyp08 0:1c6757f4b61c 54
anthonyp08 0:1c6757f4b61c 55 BSP_LCD_Clear(LCD_COLOR_BLACK);
anthonyp08 0:1c6757f4b61c 56 BSP_LCD_SetFont(&LCD_DEFAULT_FONT);
anthonyp08 0:1c6757f4b61c 57 BSP_LCD_SetBackColor(LCD_COLOR_WHITE);
anthonyp08 0:1c6757f4b61c 58 BSP_LCD_SetTextColor(LCD_COLOR_RED);
anthonyp08 0:1c6757f4b61c 59
anthonyp08 0:1c6757f4b61c 60 int32_t xPos=240,yPos=150; // déclaration position initiale de la bille
anthonyp08 0:1c6757f4b61c 61
anthonyp08 0:1c6757f4b61c 62
anthonyp08 0:1c6757f4b61c 63 char buffer[10] = {0};
anthonyp08 0:1c6757f4b61c 64 char tab [10] = {0};
anthonyp08 0:1c6757f4b61c 65
anthonyp08 0:1c6757f4b61c 66 pc.printf("MPU6050 test\r\n");
anthonyp08 0:1c6757f4b61c 67 pc.printf("MPU6050 initialize \r\n");
anthonyp08 0:1c6757f4b61c 68
anthonyp08 0:1c6757f4b61c 69 mpu.initialize(); //Initialisation du MPU-6050
anthonyp08 0:1c6757f4b61c 70
anthonyp08 0:1c6757f4b61c 71
anthonyp08 0:1c6757f4b61c 72
anthonyp08 0:1c6757f4b61c 73 pc.printf("MPU6050 testConnection\r\n");
anthonyp08 0:1c6757f4b61c 74
anthonyp08 0:1c6757f4b61c 75 bool mpu6050TestResult = mpu.testConnection();
anthonyp08 0:1c6757f4b61c 76 if(mpu6050TestResult) {
anthonyp08 0:1c6757f4b61c 77 pc.printf("MPU6050 test passed \r\n");
anthonyp08 0:1c6757f4b61c 78 } else {
anthonyp08 0:1c6757f4b61c 79 pc.printf("MPU6050 test failed \r\n");
anthonyp08 0:1c6757f4b61c 80 }
anthonyp08 0:1c6757f4b61c 81
anthonyp08 0:1c6757f4b61c 82
anthonyp08 0:1c6757f4b61c 83 servo1.period_ms(20); // déclaration de la période des servo-moteurs
anthonyp08 0:1c6757f4b61c 84 servo2.period_ms(20);
anthonyp08 0:1c6757f4b61c 85 flipper.attach(&servo_ctrl, 1.0); // nous demandons toute les secondes si il y a une action
anthonyp08 0:1c6757f4b61c 86 while(1) {
anthonyp08 0:1c6757f4b61c 87
anthonyp08 0:1c6757f4b61c 88
anthonyp08 0:1c6757f4b61c 89
anthonyp08 0:1c6757f4b61c 90 BSP_LCD_Clear(LCD_COLOR_BLACK);
anthonyp08 0:1c6757f4b61c 91 BSP_LCD_FillCircle(xPos,yPos,4);
anthonyp08 0:1c6757f4b61c 92
anthonyp08 0:1c6757f4b61c 93 sprintf(buffer, "%d", xPos);
anthonyp08 0:1c6757f4b61c 94 sprintf(tab, "%d", yPos);
anthonyp08 0:1c6757f4b61c 95 BSP_LCD_DisplayStringAt(0,0,(uint8_t*)buffer, LEFT_MODE);
anthonyp08 0:1c6757f4b61c 96 BSP_LCD_DisplayStringAt(60,0,(uint8_t*)tab, LEFT_MODE);
anthonyp08 0:1c6757f4b61c 97
anthonyp08 0:1c6757f4b61c 98
anthonyp08 0:1c6757f4b61c 99 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // récupération des valeurs bruts du MPU-6050
anthonyp08 0:1c6757f4b61c 100 t1=atan2(-(double)ay,-(double)ax)*(180/3.1415); // calcul en degrès des valeurs bruts
anthonyp08 0:1c6757f4b61c 101 t2=atan2(-(double)az,-(double)ax)*(180/3.1415);
anthonyp08 0:1c6757f4b61c 102 pc.printf("%6d %6d %6d %6.2f %6.2f\r\n",ax,ay,az,t1,t2); //affichage des valeurs bruts et degrès (voir sur putty)
anthonyp08 0:1c6757f4b61c 103
anthonyp08 0:1c6757f4b61c 104 if (t2 < 0) {
anthonyp08 0:1c6757f4b61c 105 xPos = xPos - 3;
anthonyp08 0:1c6757f4b61c 106
anthonyp08 0:1c6757f4b61c 107 }
anthonyp08 0:1c6757f4b61c 108 else if (t2 > 0) {
anthonyp08 0:1c6757f4b61c 109 xPos = xPos + 3;
anthonyp08 0:1c6757f4b61c 110
anthonyp08 0:1c6757f4b61c 111 // Selon l'angle des axes du MPU-6050 la position de la bille change
anthonyp08 0:1c6757f4b61c 112 }
anthonyp08 0:1c6757f4b61c 113
anthonyp08 0:1c6757f4b61c 114 if (t1 <0) {
anthonyp08 0:1c6757f4b61c 115 yPos = yPos + 3;
anthonyp08 0:1c6757f4b61c 116
anthonyp08 0:1c6757f4b61c 117
anthonyp08 0:1c6757f4b61c 118 }
anthonyp08 0:1c6757f4b61c 119 else if (t1 > 0) {
anthonyp08 0:1c6757f4b61c 120 yPos = yPos - 3;
anthonyp08 0:1c6757f4b61c 121
anthonyp08 0:1c6757f4b61c 122
anthonyp08 0:1c6757f4b61c 123 }
anthonyp08 0:1c6757f4b61c 124
anthonyp08 0:1c6757f4b61c 125
anthonyp08 0:1c6757f4b61c 126 if (xPos<=4) {
anthonyp08 0:1c6757f4b61c 127 xPos = 4;
anthonyp08 0:1c6757f4b61c 128 } else if (xPos>=475) {
anthonyp08 0:1c6757f4b61c 129 xPos = 475;
anthonyp08 0:1c6757f4b61c 130 }
anthonyp08 0:1c6757f4b61c 131
anthonyp08 0:1c6757f4b61c 132 if (yPos<=4) { // Avec cela la bille ne peut pas sortir de l'écran
anthonyp08 0:1c6757f4b61c 133 yPos = 4;
anthonyp08 0:1c6757f4b61c 134 } else if (yPos>=267) {
anthonyp08 0:1c6757f4b61c 135 yPos = 267;
anthonyp08 0:1c6757f4b61c 136 }
anthonyp08 0:1c6757f4b61c 137
anthonyp08 0:1c6757f4b61c 138 }
anthonyp08 0:1c6757f4b61c 139 }