wip: add reference to motionDetection to motion

src/Dezibot.cpp

@@ -12,15 +12,14 @@
 
 #include "Dezibot.h"
 
-Dezibot::Dezibot():multiColorLight(),motionDetection(),display(128, 64, &Wire, -1){
+MotionDetection motionDetection;
+static MotionDetection* motionDetectionPTR = &motionDetection;
+Dezibot::Dezibot():multiColorLight(){
 
 };
 void Dezibot::begin(void) {
-    motion.begin();
+    //motion.begin();
     multiColorLight.begin();
-    motionDetection.begin();
+    //motionDetection.begin();
     Wire.begin(1,2);
-    display.begin(SSD1306_SWITCHCAPVCC,0x3C);
-    display.clearDisplay();
-    display.display();
 };

src/Dezibot.h

@@ -18,21 +18,21 @@
 #include "motionDetection/MotionDetection.h"
 #include "Arduino.h"
 #include "Wire.h"
-#include "Adafruit_SSD1306.h"
+
+extern static MotionDetection* motionDetectionPTR;
 
 class Dezibot {
 protected:
-
+    void setIMU(Motion& m){
+        m.imuInst = motionDetection;
+    };
 public:
     Dezibot();
-    Motion motion;
     LightDetection lightDetection;
     ColorDetection colorDetection;
     MultiColorLight multiColorLight;
-    MotionDetection motionDetection;
-    //temporary, display component is not implemented yet
-    Adafruit_SSD1306 display;
-    
+    Motion motion = Motion(motionDetectionPTR);
+   static MotionDetection motionDetection = *motionDetectionPTR;
     void begin(void);
 /*
 Display display

src/motion/Motion.cpp

@@ -16,6 +16,8 @@ TaskHandle_t xClockwiseTaskHandle = NULL;
 TaskHandle_t xAntiClockwiseTaskHandle = NULL;
 
 // Initialize the movement component.
+
+
 void Motion::begin(void) {
     ledc_timer_config_t motor_timer = {
         .speed_mode       = LEDC_MODE,
@@ -31,7 +33,8 @@ void Motion::begin(void) {
 void Motion::moveTask(void * args) {
     Motion::left.setSpeed(LEFT_MOTOR_DUTY);
     Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
-    vTaskDelay((uint32_t) args / portTICK_PERIOD_MS);
+    //moveCMD cmd = (moveCMD)args;
+    //vTaskDelay( cmd.duration/ portTICK_PERIOD_MS);
     Motion::left.setSpeed(0);
     Motion::right.setSpeed(0);
     vTaskDelete(xMoveTaskHandle);
@@ -39,12 +42,10 @@ void Motion::moveTask(void * args) {
 
 // Move forward for a certain amount of time.
 void Motion::move(uint32_t moveForMs) {
-    if (moveForMs > 0){
-    xTaskCreate(moveTask, "Move", 4096, (void*)moveForMs, 10, &xMoveTaskHandle);
-    } else{
+       // moveCMD cmd = {moveForMs,imuInst};
+      //  xTaskCreate(moveTask, "Move", 4096, (void*)cmd, 10, &xMoveTaskHandle);
         Motion::left.setSpeed(LEFT_MOTOR_DUTY);
         Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
-    }
 };
 
 void Motion::leftMotorTask(void * args) {
@@ -86,5 +87,7 @@ void Motion::rotateAntiClockwise(uint32_t rotateForMs) {
 void Motion::stop(void){
     Motion::left.setSpeed(0);
     Motion::right.setSpeed(0);
+    //just for testing
+ //   Serial.println(imu->getAcceleration().z);
 }
  

src/motion/Motion.h

@@ -16,12 +16,18 @@
 #include <freertos/FreeRTOS.h>
 #include <freertos/task.h>
 #include "driver/ledc.h"
+#include "motionDetection/MotionDetection.h"
 #define LEDC_MODE          LEDC_LOW_SPEED_MODE
 #define TIMER              LEDC_TIMER_2
 #define CHANNEL_LEFT       LEDC_CHANNEL_3 
 #define CHANNEL_RIGHT      LEDC_CHANNEL_4  
 #define DUTY_RES           LEDC_TIMER_13_BIT // Set duty resolution to 13 bits
 #define FREQUENCY          (5000) // Frequency in Hertz. Set frequency at 5 kHz
+
+struct moveCMD{
+    uint32_t duration;
+    MotionDetection* imu;
+};
 class Motor{
     public:
         Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel);
@@ -38,6 +44,7 @@ class Motor{
 
 class Motion{
 protected:
+    static MotionDetection* imuInst;
     static const uint16_t RIGHT_MOTOR_DUTY = 4096;
     static const uint16_t LEFT_MOTOR_DUTY = 4096;
     static const int MOTOR_RIGHT_PIN = 11; 
@@ -50,6 +57,11 @@ public:
     //Shared Timer to sync movement
     static inline Motor left = Motor(MOTOR_LEFT_PIN,TIMER,CHANNEL_LEFT);
     static inline Motor right = Motor(MOTOR_RIGHT_PIN,TIMER,CHANNEL_RIGHT);
+    friend class Dezibot;
+
+    /**
+     * constructor gets a pointer to the motiondetection class, which enables correction of the motion
+    */
 
     /**
      * @brief Initialize the movement component.

src/motionDetection/MotionDetection.cpp

@@ -1,4 +1,5 @@
 #include "MotionDetection.h"
+#include <math.h>
 
 MotionDetection::MotionDetection(){//:handler(FSPI){
     handler = new SPIClass(FSPI);
@@ -132,13 +133,13 @@ void MotionDetection::resetRegisterBankAccess(){
 
 void MotionDetection::testFIFO(){
     uint16_t fifocount;
-    while(fifocount < 20){
+    
         fifocount = 0;
         fifocount = (this->readRegister(FIFO_COUNTH)<<8);
         fifocount |= this->readRegister(FIFO_COUNTL);
         Serial.print("FiFo Count: ");
         Serial.println(fifocount);
-    }
+    
     Serial.print("FiFoData: ");
      handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
     digitalWrite(34,LOW);
@@ -147,11 +148,19 @@ void MotionDetection::testFIFO(){
     //delayMicroseconds(10);
     digitalWrite(34,HIGH);
     handler->endTransaction();
-    Serial.println("=============");
+    for(int i=0;i<fifocount;i++){
+
+       /* Serial.println(buf[8*i]);
+        Serial.println((buf[0x02+8*i]<<8)|buf[0x01+8*i]);
+        Serial.println((buf[0x04+8*i]<<8)|buf[0x03+8*i]);
+        Serial.println((buf[0x06+8*i]<<8)|buf[0x05+8*i]);
+        Serial.println("=============");*/
+    }
     
     writeRegister(0x02,0x04);
     delayMicroseconds(10);
     fifocount = 0;
+    
         fifocount = (this->readRegister(FIFO_COUNTH)<<8);
         fifocount |= this->readRegister(FIFO_COUNTL);
         Serial.print("FiFo Count: ");
@@ -167,11 +176,45 @@ void MotionDetection::initFIFO(){
     //set TMST_CONFIG1_MREG1 TMST_CONFIIG1_TMST_EN
     this->writeToRegisterBank(MREG1,TMST_CONFIG1,0x00);
     //set FiFO config 5 GYRO_EN,TMST_FSYNC, ACCEL_EN, WM_GT_TH_EN
-    this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x21);
+    this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23);
     //set FOF_CONFIG2 0x1 (INT triggerd each packaged)
     this->writeRegister(FIFO_CONFIG2,0x0A);
 };
 
+uint MotionDetection::getDataFromFIFO(FIFO_Package* buffer){
+    int16_t fifocount = 0;
+    int8_t fifohigh = this->readRegister(FIFO_COUNTH);
+    int8_t fifolow = this->readRegister(FIFO_COUNTL);  
+    fifocount = (fifohigh<<8)|fifolow;
+    //fifocount |= this->readRegister(FIFO_COUNTL);
+    //fifocount = (this->readRegister(FIFO_COUNTH)<<8);
+    Serial.println(fifolow);
+    Serial.println(fifohigh);
+    Serial.println(fifocount);
+    handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
+    digitalWrite(34,LOW);
+    handler->transfer(cmdRead(FIFO_DATA));
+    handler->transfer(buf,16*fifocount);
+    digitalWrite(34,HIGH);
+    handler->endTransaction();
+    
+    writeRegister(0x02,0x04);
+    delayMicroseconds(10);
+
+    for(int i = 0; i<fifocount;i++){
+        buffer[i].header = buf[0x00+16*i];
+        buffer[i].accel.x = (buf[0x02+16*i]<<8)|buf[0x01+16*i]; 
+        buffer[i].accel.y = (buf[0x04+16*i]<<8)|buf[0x03+16*i]; 
+        buffer[i].accel.z = (buf[0x06+16*i]<<8)|buf[0x05+16*i]; 
+        buffer[i].gyro.x = (buf[0x08+16*i]<<8)|buf[0x07+16*i]; 
+        buffer[i].gyro.y = (buf[0x0A+16*i]<<8)|buf[0x09+16*i]; 
+        buffer[i].gyro.z = (buf[0x0C+16*i]<<8)|buf[0x0B+16*i]; 
+        buffer[i].temperature = buf[0x0D+16*i]; 
+        buffer[i].timestamp = (buf[0x0F+16*i]<<8)|buf[0x0E +16*i]; 
+    }
+    return fifocount;
+};
+
 void MotionDetection::writeRegister(uint8_t reg, uint8_t value){
     handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
     digitalWrite(34,LOW);

src/motionDetection/MotionDetection.h

@@ -19,12 +19,21 @@ struct IMUResult{
     int16_t z;
 };
 
+struct FIFO_Package{
+    int8_t header;
+    IMUResult gyro;
+    IMUResult accel;
+    int16_t temperature;
+    int16_t timestamp;
+};
+
 
 class MotionDetection{
 protected:
     enum registerBank{MREG1,MREG2,MREG3};
-    static const uint frequency = 10000000;
-    int8_t* buf = new int8_t[16*200];
+    static const uint frequency = 24000000;
+    const uint bufferLength = 500*16;
+    int8_t* buf = new int8_t[bufferLength];
     uint8_t readFromRegisterBank(registerBank bank,uint8_t reg);
     void writeToRegisterBank(registerBank bank, uint8_t reg, uint8_t value);
     void resetRegisterBankAccess();
@@ -88,6 +97,14 @@ public:
      * @return the value of the whoami register of the ICM-42670 
      */
     int8_t getWhoAmI();
+    /**
+     * @brief will read all availible packages from fifo, after 40ms Fifo is full
+     * 
+     * @param buffer pointer to FIFO_Package Struct that at least must have size 64 (this is the max package count with APEX Enabled)
+     * 
+     * @return the amount of acutally fetched packages 
+    */
+    uint getDataFromFIFO(FIFO_Package* buffer);
 
     void testFIFO();
 };