// setting libraries
#include <Wire.h>
#include <MPU6050.h>
#include <KalmanFilter.h>
// Webserial setup:
#include <Arduino.h>
#include <WiFi.h>
#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <WebSerial.h>
/*-----------------------------------------------------------------------------------------------*/
// Installing IMU as MPU
MPU6050 mpu;
// Installing WebSerial
AsyncWebServer server(80);
// Adding taskhandler
TaskHandle_t TaskSerial;
// setting variables for swingphase detection
KalmanFilter kalmanX(0.001, 0.003, 0.03);
KalmanFilter kalmanY(0.001, 0.003, 0.03);
float accPitch = 0;
float accRoll = 0;
float kalPitch = 0;
float kalRoll = 0;
float lowerTreshHold = -5; // degrees
float upperTreshHold = 20; // degrees
float lastMeasurements[5] = {0,0,0,0,0};
bool swingPhase = false;
// Webserial variables
const char* ssid = "WSLDemo"; // WiFi AP SSID
const char* password = ""; // WiFi AP Password
/*-----------------------------------------------------------------------------------------------*/
void setup()
{
Serial.begin(115200);
// Initialize MPU6050
while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
{
Serial.print("Cannot find IMU");
delay(500);
}
// Calibrate gyroscope. The calibration must be at rest.
mpu.calibrateGyro();
// Webserial Setup
webSerialSetup();
//Task setup to use a second core
xTaskCreatePinnedToCore(
taskUpdateSerial, /* Function to implement the task */
"TaskSerial", /* Name of the task */
10000, /* Stack size in words */
NULL, /* Task input parameter */
0, /* Priority of the task */
&TaskSerial, /* Task handle. */
0); /* Core where the task should run */
}
/*-----------------------------------------------------------------------------------------------*/
void taskUpdateSerial(void * pvParameters){
for(;;){
WebSerial.loop(); // WebSerial
delay(5000);
}
}
/*-----------------------------------------------------------------------------------------------*/
void loop(){
updateIMUData(); // Updates the IMU data
updateSwingPhase(); // Updates the SwingPhase state
updateArray(lastMeasurements, kalPitch); // Updates the array
}
/*-----------------------------------------------------------------------------------------------*/
void updateIMUData(){
/* This function updates all the values of the IMU */
// Reading IMU
Vector acc = mpu.readNormalizeAccel();
Vector gyr = mpu.readNormalizeGyro();
// Calculate Pitch & Roll from accelerometer (deg)
accPitch = -(atan2(acc.XAxis, sqrt(acc.YAxis*acc.YAxis + acc.ZAxis*acc.ZAxis))*180.0)/M_PI;
accRoll = (atan2(acc.YAxis, acc.ZAxis)*180.0)/M_PI;
// Kalman filter
kalPitch = kalmanY.update(accPitch, gyr.YAxis);
kalRoll = kalmanX.update(accRoll, gyr.XAxis);
Serial.print("accPitch:");
Serial.print(accPitch);
Serial.print(",");
Serial.print("kalPitch:");
Serial.print(kalPitch);
Serial.print(",");
Serial.print("accRoll:");
Serial.print(accRoll);
Serial.print(",");
Serial.print("kalRoll:");
Serial.print(kalRoll);
Serial.print(",");
Serial.print("swingphase:");
Serial.println(swingPhase);
}
/*-----------------------------------------------------------------------------------------------*/
void updateSwingPhase(){
/* This function checks if the foot is in swing phase and updates if it is/isn't */
if(swingPhase){
// Checks if the foot is returning from swingphase
if(checkSmaller(lastMeasurements, kalPitch)){
swingPhase = false;
}
}else{
// Checks if the foot is returning to swingphase
if(checkBigger(lastMeasurements, kalPitch)){
swingPhase = true;
}
}
}
/*-----------------------------------------------------------------------------------------------*/
void updateArray(float Array[], float newElement){
/* This function updates the array, by removing the first entry, moving them all one position forward and adding the new element. */
for(int i=0; i < sizeof(Array)/sizeof(Array[0]) - 1; i++){
Array[i] = Array[i+1];
}
Array[-1] = newElement;
}
/*-----------------------------------------------------------------------------------------------*/
bool checkSmaller(float Array[], float Element){
/* This function checks if an element is smaller than all elements of an array. */
for(int i=0; i < sizeof(Array)/sizeof(Array[0]); i++){
if (Array[i] > Element){
return false;
}
}
return true;
}
/*-----------------------------------------------------------------------------------------------*/
bool checkBigger(float Array[], float Element){
/* This function checks if an element is bigger than all elements of an array. */
for(int i=0; i < sizeof(Array)/sizeof(Array[0]); i++){
if (Array[i] < Element){
return false;
}
}
return true;
}
In my project I was working on swingphase detection, using a MPU6050. I ran into troubles trying to use my bool.
I tried to print the bool and it results into values ranging from 0-255. And I also tried using an interger instead of an bool, but it also results into some kind of random values. The values seem to be influeced by the IMU (as soon as the IMU returns a negative number, this random number also becomes negative) and I don't understand why this is the case and how to solve this.
Anyone that can help me? Any other ideas to improve the code are also welcome. 