#include <WiFi.h>
#include <Wire.h>
#include <WebServer.h>
#include <uri/UriBraces.h>
#include "Config.hpp"
#include "MahonyQuaternionUpdate.hpp"
////////////////////////////////////////////////
// UTILISATION DES PIN
// LED pour verification allumee eteint
const int LED_ON = 26;
const int LED_OFF = 27;
#define SDA1 21
#define SCL1 22
// Replace with your network credentials
const char* ssid_AP = "JIFFY_WIFI";
const char* password_AP = "12345678";
//const char* password_AP = NULL;
// Set web server port number to 80
//WiFiServer server(80);
WebServer server_AP(80); // Object of WebServer(HTTP port, 80 is defult)
IPAddress local_ip(192, 168, 1, 1);
IPAddress gateway(192, 168, 1, 1);
IPAddress subnet(255, 255, 255, 0);
// definition du Timer
hw_timer_t *timer = NULL;
//definition d'un mutex de protection donnee partagee IT et loop
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
/***********************************************************************/
/***********************************************************************/
/***********************************************************************/
//#define TIME_TIMER 1000000 // 1s valeur en us
#define TIME_TIMER 100000 // 1s valeur en us
// Initial time
long int tinitial;
long int tcourant;
long int temps_mesures;
volatile bool intFlag = false;
// Fonction de l'IT
void IRAM_ATTR onTimer() {
portENTER_CRITICAL_ISR(&timerMux);
intFlag = true;
portEXIT_CRITICAL_ISR(&timerMux);
}
void InitTimer()
{
Serial.print("\nInit Timer de ");
Serial.print(TIME_TIMER);
Serial.println(" us ...");
timer = timerBegin(0, 80, true); // Frequence du proc divisee par valeur si 80Mhz => 1MHz d'ou avec inverse prog a 1us
timerAttachInterrupt(timer, &onTimer, true);
timerAlarmWrite(timer, TIME_TIMER, true); // Programmation a la seconde
timerAlarmEnable(timer);
Serial.println("Init Timer done");
}
/***********************************************************************/
/***********************************************************************/
/***********************************************************************/
// Init de l'ecran LCD utilise : AZDelivrery I2C Display 128x64 Pixel 0.96 Pouce
void InitLCD()
{
Serial.println("\nInit Ecran LCD");
Serial.println("Ecran LCD not used");
Serial.println("Init Ecran LCD => done");
}
/***********************************************************************/
/***********************************************************************/
/***********************************************************************/
bool PA_on_off = false;
bool PA_CAL_on_off = false;
bool PA_LED_on = false;
bool PA_LED_off = true;
bool PA_MPU9250_connect = false;
volatile int cap_cmde_int = 0;
volatile int cap_reel_int = 0;
volatile int delta_cap_int = 0;
// a suprimer ensuite
int magXOffset_int = 11;
int magYOffset_int = 22;
int magZOffset_int = 33;
void sendHtml() {
String response = R"(
<!DOCTYPE html><html>
<head>
<title>ESP32 Web Server Demo</title>
<meta name="viewport" content="width=device-width, initial-scale=1">
<style>
html { font-family: sans-serif; text-align: center; height: 100%;}
body { display: inline-flex; flex-direction: column; min-height: 100%;}
h1 { margin-bottom: 0.5em; }
h2 { margin: 0; }
div { display: grid; grid-template-columns: 1fr 1fr; grid-template-rows: auto auto; grid-auto-flow: column; grid-gap: 1em; }
.btn { background-color: #5B5; border: 8px solid black; color: #fff; padding: 0.5em 1em;
font-size: 2em; text-decoration: none }
.btn.OFF { background-color: rgb(85, 84, 84); }
.cap_cmde { background-color: rgb(228, 236, 228); border: 8px solid black; color: rgb(10, 10, 10); padding: 0.5em 1em;
font-size: 2em; text-decoration: none }
.btn_cmde { background-color: rgb(63, 112, 204); border: 8px solid black; color: #fff; padding: 0.5em 1em;
font-size: 2em; text-decoration: none }
</style>
</head>
<body>
<h1>PA JIFFY Web Server</h1>
<div>
<h2>PA</h2>
<a href="/toggle/1" class="btn PA_ON_OFF_TXT">PA_ON_OFF_TXT</a>
<h2>CAP CMDE</h2>
<a class="cap_cmde CAP_CMDE_TXT">CAP_CMDE_TXT</a>
</div>
</body>
<h1>==========================================</h1>
<body>
<div>
<h2>CAP REEL</h2>
<a class="cap_cmde CAP_REEL_TXT">CAP_REEL_TXT</a>
<h2>DELTA</h2>
<a class="cap_cmde DELTA_CAP_TXT">DELTA_CAP_TXT</a>
</div>
</body>
<h1>==========================================</h1>
<h1>COMMANDE</h1>
<body>
<div>
<a href="/toggle/2" class="btn_cmde -1">-1°</a>
<a href="/toggle/3" class="btn_cmde -10">-10°</a>
<a href="/toggle/4" class="btn_cmde +1">+-1°</a>
<a href="/toggle/5" class="btn_cmde +10">+10°</a>
</div>
</body>
<h1>==========================================</h1>
<body>
<div>
<h2>CAL</h2>
<a href="/toggle/6" class="btn CALIB">CALIB</a>
<h2>CAL X</h2>
<a class="cap_cmde CAL_X_TXT">CAL_X_TXT</a>
<h2>CAL Y</h2>
<a class="cap_cmde CAL_Y_TXT">CAL_Y_TXT</a>
<h2>CAL Z</h2>
<a class="cap_cmde CAL_Z_TXT">CAL_Z_TXT</a>
</div>
</body>
</html>)";
response.replace("PA_ON_OFF_TXT", PA_on_off ? "ON" : "OFF");
response.replace("CAP_CMDE_TXT", PA_on_off ? String(cap_cmde_int) : "---");
response.replace("CAP_REEL_TXT", String(cap_reel_int));
response.replace("DELTA_CAP_TXT", PA_on_off ? String(delta_cap_int) : "---");
response.replace("CALIB", PA_CAL_on_off ? "ON" : "OFF");
response.replace("CAL_X_TXT", String(magXOffset_int));
response.replace("CAL_Y_TXT", String(magYOffset_int));
response.replace("CAL_Z_TXT", String(magZOffset_int));
server_AP.send(200, "text/html", response);
}
/***********************************************************************/
/***********************************************************************/
/***********************************************************************/
// Init de l'ESP32 en WIFI Point d'Acces
void InitServer_AP_WIFI()
{
Serial.println("\n");
Serial.print("Creation du Serveur AP (Acces Point Wifi) ssid : ");
Serial.println(ssid_AP);
WiFi.mode(WIFI_AP);
WiFi.softAP(ssid_AP, password_AP);// Initialisation avec WiFi.softAP / ssid et password
//WiFi.softAPConfig(local_ip, gateway, subnet); // option, peut etre suprime
Serial.print("AP Created with Adress IP: ");
Serial.println(WiFi.softAPIP()); // Affiche l'adresse IP de l'ESP32 avec WiFi.SoftIP
Serial.println("Creation du Serveur AP => done");
Serial.print("Connect to My access point: ");
Serial.println(ssid_AP);
server_AP.on("/", sendHtml);
// DEB Traitement des possibilites de la page de commande
server_AP.on(UriBraces("/toggle/{}"), []() {
String cmde_PA = server_AP.pathArg(0);
switch (cmde_PA.toInt()) {
case 1:
PA_on_off = !PA_on_off;
PA_LED_on = !PA_LED_on;
PA_LED_off = !PA_LED_off;
digitalWrite(LED_ON, PA_LED_on);
digitalWrite(LED_OFF, PA_LED_off);
if (PA_on_off == true)
{
// passage a on, prise en compte le CAP courant pour l'init du PA
cap_cmde_int = cap_reel_int;
delta_cap_int = cap_reel_int - cap_cmde_int;
Serial.print("cap_cmde_int = "); Serial.println(cap_cmde_int);
}
break;
case 2:
cap_cmde_int -= 1;
delta_cap_int = cap_reel_int - cap_cmde_int;
Serial.print("cap_cmde_int = "); Serial.println(cap_cmde_int);
break;
case 3:
cap_cmde_int -= 10;
delta_cap_int = cap_reel_int - cap_cmde_int;
Serial.print("cap_cmde_int = "); Serial.println(cap_cmde_int);
break;
case 4:
cap_cmde_int += 1;
delta_cap_int = cap_reel_int - cap_cmde_int;
Serial.print("cap_cmde_int = "); Serial.println(cap_cmde_int);
break;
case 5:
cap_cmde_int += 10;
delta_cap_int = cap_reel_int - cap_cmde_int;
Serial.print("cap_cmde_int = "); Serial.println(cap_cmde_int);
break;
case 6:
PA_CAL_on_off = !PA_CAL_on_off;
if (PA_CAL_on_off == true)
{
}
PA_CAL_on_off = !PA_CAL_on_off;
break;
}
sendHtml();
});
// FIN Traitement des possibilites de la page de commande
server_AP.begin();
Serial.println("HTTP server started");
delay(100);
}
/***********************************************************************/
/***********************************************************************/
/***********************************************************************/
void Scan_I2C()
{
byte error, address;
int nDevices;
Serial.println("\nScanning I2C ...");
nDevices = 0;
for (address = 8; address < 120; address++ )
{
Wire.beginTransmission(address);
error = Wire.endTransmission();
if (error == 0)
{
Serial.print("I2C device found at address 0x");
if (address < 16)
{
Serial.print("0");
}
Serial.println(address, HEX);
nDevices++;
}
else if (error == 4)
{
Serial.print("Unknow error at address 0x");
if (address < 16)
{
Serial.print("0");
}
Serial.println(address, HEX);
}
}
if (nDevices == 0)
{
Serial.println("No I2C devices found");
}
else
{
}
Serial.println("Scanning I2C done");
}
/***********************************************************************/
/***********************************************************************/
/***********************************************************************/
float MagHorizDirection(float mxf, float myf)
{
return atan2(mxf, myf) * 180. / PI;
}
/***********************************************************************/
/***********************************************************************/
/***********************************************************************/
void Display_values(float x, float y, float z)
{
#ifdef DISPLAY_VALUE_PRINT
Serial.print (x);
Serial.print (" ");
Serial.print (y);
Serial.print (" ");
Serial.print (z);
Serial.print (" | ");
#endif
}
/***********************************************************************/
/***********************************************************************/
/***********************************************************************/
// Code lance au demarrage de l'ESP32
void setup() {
// put your setup code here, to run once:
// Wire.begin(I2C_SDA, I2C_SCL);
Wire.begin();
//SDA d'un OLED et un capteur à G21 et les broches SCL respectives à G22.
//Pour le deuxième bus I²C, connectez les broches SDA des deux modules à G17 et les broches SCL / SCK à G16.
//resume OLED : G21 SDA, G22 SCL
// capteur : G17 SDA, G16 SCL
// Initialisation de la Ligne serie
Serial.begin(115200);
Serial.println("Pilote Automatique : JIFFY V1.1");
Serial.println("Begin all setup...");
delay(1000);
Serial.println("Initialisation Serial Ligne => done");
// Init des LED pour le on off du PA
pinMode(LED_ON, OUTPUT);
pinMode(LED_OFF, OUTPUT);
digitalWrite(LED_ON, PA_LED_on);
digitalWrite(LED_OFF, PA_LED_off);
// Initialisation du Serveur AP WIFI
InitServer_AP_WIFI();
// Recherche des ports I2C presents
Scan_I2C();
//Init de l'ecran LCD utilise
InitLCD();
// Init du timer pour la lecture MPU9250
InitTimer();
Init_MPU();
// Pour le QMC5883
Init_5883();
calibration_mag(TIME_CALIBRATION);
Serial.println("\nEnd all setup => done => Start in 5s ...");
delay(5000);
}
/***********************************************************************/
/***********************************************************************/
/***********************************************************************/
void loop() {
// put your main code here, to run repeatedly:
server_AP.handleClient();
if (intFlag == true )
{
float deltatf;
float ax, ay, az, gx, gy, gz, mx, my, mz;
float yaw, pitch, roll; //Euler angle output
int valid_mag;
tcourant = millis();
temps_mesures = (tcourant - tinitial);
tinitial = tcourant;
deltatf = temps_mesures * 0.001;
portENTER_CRITICAL_ISR(&timerMux);
intFlag = false;
portEXIT_CRITICAL_ISR(&timerMux);
/* recuperation des donnees avec 9250 uniquement du capteur avec 6500 prise en compte aussi 5883*/
Get_MPU(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz, &valid_mag);
Display_values(ax, ay, az); // Accelerometer
Display_values(gx, gy, gz); // Gyroscope
if ( valid_mag == 1)
{
Display_values(mx, my, mz); // Magnetometer
}
else
{
//Serial.println(" Utilisation du 5883");
// dans le cas invalide, on retente avec le 5883 mais normalement effectue dans Get_MPU
Get_5883(&mx, &my, &mz, &valid_mag, 1);
if ( valid_mag == 1)
{
Display_values(mx, my, mz); // Magnetometer
}
else
{
#ifdef DISPLAY_VALUE_PRINT
Serial.print(" xxx.xx xxx.xx xxx.xx "); Serial.print(" | ");
#endif
}
}
Serial.println("");
// Fusion des donnees
//MahonyQuaternionUpdate(-ax, ay, az, (gx * PI / 180.), -(gy * PI / 180.), -(gz * PI / 180.), my, -mx, mz, deltatf);
// Les 2 capteurs sont references dans le systeme ENU d'ou passage en NED
MahonyQuaternionUpdate(ay, ax, -az, (gy * PI / 180.), (gx * PI / 180.), -(gz * PI / 180.), my, mx, -mz, deltatf);
//NWU
//MahonyQuaternionUpdate(ay, -ax, az, (gy * PI / 180.), -(gx * PI / 180.), (gz * PI / 180.), my, -mx, mz, deltatf);
MahonyQuaternionUpdatecomputeAngles(&roll, &pitch, &yaw);
// to degrees
yaw *= 180.0 / PI;
pitch *= 180.0 / PI;
roll *= 180.0 / PI;
//Display_values(roll, pitch, yaw);
#ifdef PRINT_ORIENTATION_1
Serial.print("Orientation1: "); Serial.print(yaw); Serial.print(", "); Serial.print(pitch); Serial.print(", "); Serial.println(roll);
#endif
#ifdef PRINT_ORIENTATION_VISU_MAHONY
Serial.print("Orientation: "); Serial.print(yaw); Serial.print(", "); Serial.print(pitch); Serial.print(", "); Serial.println(roll);
#endif
Get_MPU_Pose(&roll, &pitch, &yaw);
//Display_values(roll, pitch, yaw);
#ifdef PRINT_ORIENTATION_2
Serial.print("Orientation2: "); Serial.print(yaw); Serial.print(", "); Serial.print(pitch); Serial.print(", "); Serial.println(roll);
#endif
#ifdef PRINT_ORIENTATION_VISU_MPU
Serial.print("Orientation: "); Serial.print(yaw); Serial.print(", "); Serial.print(pitch); Serial.print(", "); Serial.println(roll);
#endif
{
float mon_pitch, mon_roll, mon_yaw;
float accelX, accelY, accelZ;
float magReadX, magReadY, magReadZ;
float mag_x, mag_y;
accelX = ay;
accelY = ax;
accelZ = -az;
// inversion !!! entre orientation1 et MONmon_pitch = 180 * atan2(-accelX, accelZ)/PI;
//mon_roll = 180 * atan2(accelY, sqrt(accelX*accelX + accelZ*accelZ))/PI;
mon_roll = 180 * atan2(accelY, accelZ)/PI;
mon_pitch = 180 * atan2(-accelX, sqrt(accelY*accelY + accelZ*accelZ))/PI;
magReadX = my;
magReadY = mx;
magReadZ = -mz;
mag_x = magReadX*cos(pitch) + magReadY*sin(roll)*sin(pitch) + magReadZ*cos(roll)*sin(pitch);
mag_y = magReadY * cos(roll) - magReadZ * sin(roll);
mon_yaw = 180 * atan2(-mag_y,mag_x)/PI;
Serial.print("MON Orientat: "); Serial.print(mon_yaw); Serial.print(", "); Serial.print(mon_pitch); Serial.print(", "); Serial.println(mon_roll);
}
// End of line
#ifdef DISPLAY_VALUE_PRINT
Serial.println("");
#endif
}/* intflag = true */
delay(2); // this speeds up the simulation
}
// RAF
// Moteur
// LCD
// calibration => OK
// algo suivi magis... => OK
// repere angles = OK
// yaap gyro -Y -Z accel -X mag X=Y Y=-X
// Attention trouve pour le mag lecture de X puis Z puis Y
// Correction suivant les axes dans :
// voir
// https://fr.mathworks.com/help/supportpkg/arduinoio/ug/estimating-orientation-using-inertial-sensor-fusion-and-mpu9250.html?s_tid=answers_rc2-2_p5_MLT
// https://invensense.tdk.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf
// version quantum de https://github.com/jremington/MPU-9250-AHRS/blob/master/MPU9250_Mahony/MPU9250_Mahony.ino
//
// 2ieme fonction de
// https://bitbucket.org/cinqlair/mpu9250/src/master/mpu9250_OpenGL/src/MadgwickAHRS.cpp
// Faire la calib dans la loop i demande de calib calcul des offset suivant le lien suivant:
// https://github.com/asukiaaa/MPU9250_asukiaaa/blob/master/examples/GetMagOffset/GetMagOffset.ino
// SI pb pour la lecture et programmation voir le site suivant : https://github.com/bolderflight/mpu9250
// faire un ajout avec cette version
// https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU/MPU6050_HMC5883L/MPU6050_HMC5883L.ino
// motor
// https://github.com/pypilot/pypilot/blob/master/arduino/motor/motor.ino
// implement le moteur l298N https://github.com/AndreaLombardo/L298N/tree/master/src
// pour le moteur aussi voir pour algo sur les temps https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino
// voir pour le stockage des clibrations dans ESP32
// https://learn.adafruit.com/how-to-fuse-motion-sensor-data-into-ahrs-orientation-euler-quaternions/storing-calibrations
// voir https://adafruit.github.io/Adafruit_WebSerial_3DModelViewer/