Merged Tiltdetection

example/IMU/Shake_Detection/shake_detection/shake_detection.ino

@@ -0,0 +1,19 @@
+#include "Dezibot.h"
+
+Dezibot dezibot = Dezibot();
+void setup() {
+  // put your setup code here, to run once:
+
+dezibot.begin();
+dezibot.multiColorLight.turnOffLed();
+ 
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+if(dezibot.motionDetection.isShaken(1000,zAxis)){
+  dezibot.multiColorLight.setTopLeds(0xFF0000);
+} else if(dezibot.motionDetection.isShaken(1000,xAxis|yAxis)) {
+  dezibot.multiColorLight.setTopLeds(0x00FF00);
+}
+}

example/IMU/Tilt_Detection/tilt_detection/tilt_detection.ino

@@ -0,0 +1,37 @@
+#include "Dezibot.h"
+
+Dezibot dezibot = Dezibot();
+
+void setup() {
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  dezibot.begin();
+}
+
+void loop() {
+  switch (dezibot.motionDetection.getTiltDirection()) {
+    case Front:
+      dezibot.multiColorLight.setTopLeds(GREEN);
+      break;
+    case Left:
+      dezibot.multiColorLight.setTopLeds(YELLOW);
+      break;
+    case Right:
+      dezibot.multiColorLight.setTopLeds(TURQUOISE);
+      break;
+    case Back:
+      dezibot.multiColorLight.setTopLeds(BLUE);
+      break;
+    case Flipped:
+      dezibot.multiColorLight.setTopLeds(PINK);
+      break;
+    case Neutral:
+      dezibot.multiColorLight.turnOffLed();
+      break;
+
+    case Error:
+      dezibot.multiColorLight.setTopLeds(RED);
+      break;
+  }
+  delay(100);
+}

src/motionDetection/MotionDetection.cpp

@@ -9,13 +9,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();
 };
@@ -30,12 +37,9 @@ void MotionDetection::end(void){
 };
 IMUResult MotionDetection::getAcceleration(){
     IMUResult result;
-    result.x = readRegister(ACCEL_DATA_X_HIGH)<<8;
-    result.x |= readRegister(ACCEL_DATA_X_LOW);
-    result.y = readRegister(ACCEL_DATA_Y_HIGH)<<8;
-    result.y |= readRegister(ACCEL_DATA_Y_LOW);
-    result.z = readRegister(ACCEL_DATA_Z_HIGH)<<8;
-    result.z |= readRegister(ACCEL_DATA_Z_LOW);
+    result.x = readRegister(ACCEL_DATA_X_HIGH)<<8 | readRegister(ACCEL_DATA_X_LOW);
+    result.y = readRegister(ACCEL_DATA_Y_HIGH)<<8 | readRegister(ACCEL_DATA_Y_LOW);
+    result.z = readRegister(ACCEL_DATA_Z_HIGH)<<8 | readRegister(ACCEL_DATA_Z_LOW);
     return result;
 };
 IMUResult MotionDetection::getRotation(){
@@ -58,6 +62,106 @@ int8_t MotionDetection::getWhoAmI(){
     return readRegister(WHO_AM_I);    
 };
 
+bool MotionDetection::isShaken(uint32_t threshold ,uint8_t axis){
+    IMUResult measurment1;
+    IMUResult measurment2;
+    uint count = 0;
+    for(uint i = 0;i<20;i++){
+        measurment1 = this->getAcceleration();
+        delayMicroseconds(10);
+        measurment2 = this->getAcceleration();
+        if(
+            (((axis && xAxis) > 0) && (abs(abs(measurment1.x)-abs(measurment2.x))>threshold)) ||
+            (((axis && yAxis) > 0) && (abs(abs(measurment1.y)-abs(measurment2.y))>threshold)) ||
+            (((axis && zAxis) > 0) && (abs(abs(measurment1.z)-abs(measurment2.z))>threshold))){
+                count++;
+        }
+        delayMicroseconds(15);
+    }
+    return (count > 6);
+};
+
+void MotionDetection::calibrateZAxis(uint gforceValue){
+    this->gForceCalib = gforceValue;
+};
+
+Orientation MotionDetection::getTilt(){
+    uint tolerance = 200;
+    IMUResult reading = this->getAcceleration();
+    bool flipped = reading.z < 0;
+    float accelrationStrenght = sqrt(reading.x*reading.x+reading.y*reading.y+reading.z*reading.z);
+    //check if reading is valid
+    if (abs(accelrationStrenght-this->gForceCalib) > tolerance ){
+        //total accelration is not gravitational force, error
+        return Orientation{INT_MAX,INT_MAX};
+    }
+
+    //calculates the angle between the two axis, therefore value between 0-90
+    int yAngle;
+    int xAngle;
+    if(reading.z != 0){
+        yAngle = atan(float(reading.x)/reading.z)*180/3.1415+0.5;
+        xAngle = atan(float(reading.y)/reading.z)*180/3.1415+0.5;
+    } else {
+        yAngle = 90*(reading.x > 0) - (reading.x < 0);
+        xAngle = 90*(reading.y > 0) - (reading.y < 0);
+    }
+
+    //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){
     return (CMD_READ | (reg & ADDR_MASK));
 };
@@ -65,7 +169,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

@@ -17,7 +17,26 @@ struct IMUResult{
     int16_t y;
     int16_t z;
 };
+enum Axis{
+    xAxis = 0x01,
+    yAxis = 0x02,
+    zAxis = 0x04
+};
 
+struct Orientation{
+    int xRotation;
+    int yRotation;
+};
+
+enum Direction{
+    Front,
+    Left,
+    Right,
+    Back,
+    Neutral,
+    Flipped,
+    Error
+};
 
 class MotionDetection{
 protected:
@@ -48,17 +67,20 @@ protected:
     static const uint8_t WHO_AM_I =          0x75;
 
     static const uint frequency = 10000000;
+    static const uint16_t defaultShakeThreshold = 500;
 
     uint16_t cmdRead(uint8_t regHigh,uint8_t regLow);
     uint16_t cmdWrite(uint8_t regHigh,uint8_t regLow);
     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;
 
+    uint gForceCalib = 4050;
+     
     
 public:
     MotionDetection();
@@ -83,21 +105,21 @@ public:
      * 
      * @return IMUResult that contains the new read values
      */
-    IMUResult getAcceleration();
+    IMUResult getAcceleration(void);
 
     /**
      * @brief Triggers a new reading of the gyroscope and reads the values from the imu
      * 
      * @return IMUResult 
      */
-    IMUResult getRotation();
+    IMUResult getRotation(void);
 
     /**
      * @brief Reads the current On Chip temperature of the IMU 
      * 
      * @return normalized temperature in degree Centigrade
      */
-    float getTemperature();
+    float getTemperature(void);
 
     /**
      * @brief Returns the value of reading the whoAmI register
@@ -105,6 +127,62 @@ public:
      * 
      * @return the value of the whoami register of the ICM-42670 
      */
-    int8_t getWhoAmI();
+    int8_t getWhoAmI(void);
+
+    /**
+     * @brief Detects if at the time of calling is shaken. Therefore the sum over all accelerationvalues is calculated 
+     * and checked against threshold. If sum > threshold a shake is detected, else not
+     * 
+     * @param threshold (optional) the level of acceleration that must be reached to detect a shake
+     * @param axis (optional) select which axis should be used for detection. Possible values ar xAxis,yAxis,zAxis
+     * It's possible to combine multiple axis with the bitwise or Operator | 
+     * For Example: to detect x and y axis: axis = xAxis|yAxis
+     * 
+     * @return true if a shake is detected, false else
+    */
+    bool isShaken(uint32_t threshold = defaultShakeThreshold,uint8_t axis = xAxis|yAxis|zAxis);
+
+    /**
+     * @brief calculates how the robot is tilted. It is set, that when the robot is placed normally on a flat table, the result will be (0,0)
+     * Tilting the robot, so that the front leg is deeper than the other to results in an increasing degrees, tilting the front leg up will increase negativ degrees
+     * Tilting the robot to the right will increase the degrees until 180° (upside down), tilting it left will result in increasing negativ degrees (-1,-2,...,-180).
+     * On the top there is a jump of the values from 180->-180 and vice versa.
+     * 
+     * Precision is rounded to 1 deg steps
+     * 
+     * @attention The results are only valid, if the robot is not moved in any way during the measurment, as the calculation is made by using the accelration values.
+     * If it's detected, that the robot is accelerated while measuring, the method will return max(int).
+     * Please note that the imu is pretty sensitiv, even walking next to the table may influcene the result. 
+     * 
+    */
+    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.
+     * 
+     * @attention this method is not persisten, so the value is not stored when the programm is restarted / the robot is powerd off
+     * 
+     * @param gforceValue the value the IMU returns for the gravitationforce -> to get this value, place the robot on a leveled surface
+     * and read the value getAcceleration().z
+    */
+    void calibrateZAxis(uint gforceValue);
+
+
 };
 #endif //MotionDetection

src/multiColorLight/MultiColorLight.cpp

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