added tiltdetection

src/motionDetection/MotionDetection.cpp

@@ -1,5 +1,5 @@
 #include "MotionDetection.h"
-#include <cmath.h>
+
 #include <limits.h>
 MotionDetection::MotionDetection(){//:handler(FSPI){
     handler = new SPIClass(FSPI);
@@ -10,13 +10,20 @@ void MotionDetection::begin(void){
     digitalWrite(34,HIGH);
     handler->begin(36,37,35,34);
      
+     
     handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); 
     digitalWrite(34,LOW);
     // set Accel and Gyroscop to Low Noise
+    handler->transfer(PWR_MGMT0);
     handler->transfer(0x1F);
-    handler->transfer(0x0F);
     //busy Wait for startup 
-    delayMicroseconds(200);
+    delayMicroseconds(250);
+    //set Gyroconfig
+    handler->transfer(0x20);
+    handler->transfer(0x25);
+    //set Gyro Filter
+    handler->transfer(0x23);
+    handler->transfer(0x37);
     digitalWrite(34,HIGH);
     handler->endTransaction();
 };
@@ -31,12 +38,9 @@ void MotionDetection::end(void){
 };
 IMUResult MotionDetection::getAcceleration(){
     IMUResult result;
-    result.x = readRegister(ACCEL_DATA_X_HIGH)<<8;
+    result.x = readRegister(ACCEL_DATA_X_HIGH)<<8 | readRegister(ACCEL_DATA_X_LOW);
-    result.x |= readRegister(ACCEL_DATA_X_LOW);
+    result.y = readRegister(ACCEL_DATA_Y_HIGH)<<8 | readRegister(ACCEL_DATA_Y_LOW);
-    result.y = readRegister(ACCEL_DATA_Y_HIGH)<<8;
+    result.z = readRegister(ACCEL_DATA_Z_HIGH)<<8 | readRegister(ACCEL_DATA_Z_LOW);
-    result.y |= readRegister(ACCEL_DATA_Y_LOW);
-    result.z = readRegister(ACCEL_DATA_Z_HIGH)<<8;
-    result.z |= readRegister(ACCEL_DATA_Z_LOW);
     return result;
 };
 IMUResult MotionDetection::getRotation(){
@@ -59,7 +63,7 @@ int8_t MotionDetection::getWhoAmI(){
     return readRegister(WHO_AM_I);    
 };
 
-bool MotionDetection::isShaken(uint32_t threshold = defaultShakeThreshold,uint8_t axis = xAxis|yAxis|zAxis){
+bool MotionDetection::isShaken(uint32_t threshold ,uint8_t axis){
 
 };
 
@@ -75,23 +79,73 @@ Orientation MotionDetection::getTilt(){
     //check if reading is valid
     if (abs(accelrationStrenght-this->gForceCalib) > tolerance ){
         //total accelration is not gravitational force, error
-        return Orientation{xRotation = UINT_MAX,yRotation = UINT_MAX};
+        return Orientation{INT_MAX,INT_MAX};
     }
 
     //calculates the angle between the two axis, therefore value between 0-90
-    uint yAngle;
+    int yAngle;
-    uint xAngle;
+    int xAngle;
-    if(result.z != 0){
+    if(reading.z != 0){
-        yAngle = atan((result.x)/result.z)+0.5;
+        yAngle = atan(float(reading.x)/reading.z)*180/3.1415+0.5;
-        xAngle = atan(float(result.y)/result.z)+0.5;
+        xAngle = atan(float(reading.y)/reading.z)*180/3.1415+0.5;
     } else {
         yAngle = 90;
         xAngle = 90;
     }
 
-
     //shift quadrants depending on signum
+        //esp is facing down (normal position)
+    if (reading.z > 0){
+        if(reading.y < 0){
+            xAngle = xAngle+180;
+        } else{
+            xAngle = -1*(180-xAngle);
+        }
 
+        if(reading.x < 0){
+            yAngle = yAngle+180;
+        } else{
+            yAngle = -1*(180-yAngle);
+        }
+        //yAngle = -1*yAngle-90;
+    }
+          
+
+    return Orientation{xAngle,yAngle};
+
+};
+
+Direction MotionDetection::getTiltDirection(uint tolerance){
+    if (this->getAcceleration().z > 0){
+        return Flipped;
+    }
+    Orientation Rot = this->getTilt();
+    Serial.println(Rot.xRotation);
+    Serial.println(Rot.xRotation == INT_MAX);
+    if ((Rot.xRotation == INT_MAX)){
+        return Error;
+    }
+    if(abs(abs(Rot.xRotation)-abs(Rot.yRotation))>tolerance){
+        //determine which axis is more tiltet
+        if (abs(Rot.xRotation)>abs(Rot.yRotation)){
+            //test if dezibot is tilted left or right
+            if (Rot.xRotation > 0){
+                return Right;
+            } else {
+                return Left;
+            }
+        } else {
+            //test if robot is tilted front or back
+            if (Rot.yRotation > 0){
+                return Front;
+            } else {
+                return Back;
+            }
+        }
+    } else {
+        //dezibot is (with tolerance) leveled
+        return Neutral;
+    }
 };
 
 uint8_t MotionDetection::cmdRead(uint8_t reg){
@@ -101,7 +155,7 @@ uint8_t MotionDetection::cmdWrite(uint8_t reg){
     return (CMD_WRITE | (reg & ADDR_MASK));
 };
 
-int8_t MotionDetection::readRegister(uint8_t reg){
+uint8_t MotionDetection::readRegister(uint8_t reg){
     handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
     digitalWrite(34,LOW);
     uint8_t result;

src/motionDetection/MotionDetection.h

@@ -24,10 +24,19 @@ enum Axis{
 };
 
 struct Orientation{
-    uint xRotation;
+    int xRotation;
-    uint yRotation;
+    int yRotation;
 };
 
+enum Direction{
+    Front,
+    Left,
+    Right,
+    Back,
+    Neutral,
+    Flipped,
+    Error
+};
 
 class MotionDetection{
 protected:
@@ -57,7 +66,7 @@ protected:
     static const uint8_t PWR_MGMT0 =         0x1F;
     static const uint8_t WHO_AM_I =          0x75;
 
-    static const uint frequency = 24000000;
+    static const uint frequency = 10000000;
     static const uint16_t defaultShakeThreshold = 500;
 
     uint16_t cmdRead(uint8_t regHigh,uint8_t regLow);
@@ -65,12 +74,12 @@ protected:
     uint8_t cmdRead(uint8_t reg);
     uint8_t cmdWrite(uint8_t reg);
 
-    int8_t readRegister(uint8_t reg);
+    uint8_t readRegister(uint8_t reg);
     int16_t readDoubleRegister(uint8_t lowerReg);
 
     SPIClass * handler = NULL;
 
-    gForceCalib = 2000;
+    uint gForceCalib = 4050;
      
     
 public:
@@ -148,6 +157,21 @@ public:
     Orientation getTilt();
 
 
+    /**
+     * @brief Checks in which direction (Front, Left, Right, Back) the robot is tilted. 
+     * 
+     * @attention Does only work if the robot is not moving
+     *  
+     * @param tolerance (optional, default = 10) how many degrees can the robot be tilted, and still will be considerd as neutral. 
+     * 
+     * 
+     * @return Direction the direction in that the robot is tilted most. Front is onsiderd as the direction of driving.
+     * If robot is not tilted more than the tolerance in any direction, return is Neutral.
+     * If Robot is upside down, return is Flipped.
+     * If Robot is moved, return is Error
+     */
+    Direction getTiltDirection(uint tolerance = 10);
+
     /**
      * can be used to set a custom value for the gforceReading of the zaxis, which will improve the getTiltFunction.
      * 

src/multiColorLight/MultiColorLight.cpp

@@ -60,7 +60,6 @@ void MultiColorLight::blink(uint16_t amount,uint32_t color, leds leds, uint32_t
         MultiColorLight::turnOffLed(leds);
         vTaskDelay(interval);
     }
-
 };
 
 void MultiColorLight::turnOffLed(leds leds){