From d4cb8af3b3ccbf4eda9ee24a2d5a98b9c89a7cdf Mon Sep 17 00:00:00 2001 From: Hans Haupt Date: Thu, 13 Jun 2024 17:20:22 +0200 Subject: [PATCH] wip: add reference to motionDetection to motion --- src/Dezibot.cpp | 11 +++--- src/Dezibot.h | 14 +++---- src/motion/Motion.cpp | 13 ++++--- src/motion/Motion.h | 12 ++++++ src/motionDetection/MotionDetection.cpp | 51 +++++++++++++++++++++++-- src/motionDetection/MotionDetection.h | 21 +++++++++- 6 files changed, 98 insertions(+), 24 deletions(-) diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp index f708ccd..45af406 100644 --- a/src/Dezibot.cpp +++ b/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(); }; diff --git a/src/Dezibot.h b/src/Dezibot.h index 001744d..a78a76c 100644 --- a/src/Dezibot.h +++ b/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 diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp index 812ca92..b411a35 100644 --- a/src/motion/Motion.cpp +++ b/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); } diff --git a/src/motion/Motion.h b/src/motion/Motion.h index 13ae523..f252042 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -16,12 +16,18 @@ #include #include #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. diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp index 03c650c..b907b73 100644 --- a/src/motionDetection/MotionDetection.cpp +++ b/src/motionDetection/MotionDetection.cpp @@ -1,4 +1,5 @@ #include "MotionDetection.h" +#include 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;ireadRegister(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; ibeginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); digitalWrite(34,LOW); diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h index 3f5664f..b145495 100644 --- a/src/motionDetection/MotionDetection.h +++ b/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(); };