From e03bde591da62e832830d0d067725b5095a61612 Mon Sep 17 00:00:00 2001 From: hhaupt Date: Sun, 12 May 2024 18:55:54 +0200 Subject: [PATCH 01/12] first working example of IMU Based Motioncorrection --- .../motion_correction/.theia/launch.json | 8 ++ .../motion_correction/motion_correction.ino | 112 ++++++++++++++++++ src/Dezibot.cpp | 6 +- src/Dezibot.h | 7 +- 4 files changed, 131 insertions(+), 2 deletions(-) create mode 100644 example/IMU/Motion_Correction/motion_correction/.theia/launch.json create mode 100644 example/IMU/Motion_Correction/motion_correction/motion_correction.ino diff --git a/example/IMU/Motion_Correction/motion_correction/.theia/launch.json b/example/IMU/Motion_Correction/motion_correction/.theia/launch.json new file mode 100644 index 0000000..9a49ac9 --- /dev/null +++ b/example/IMU/Motion_Correction/motion_correction/.theia/launch.json @@ -0,0 +1,8 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + "version": "0.2.0", + "configurations": [ + + ] +} diff --git a/example/IMU/Motion_Correction/motion_correction/motion_correction.ino b/example/IMU/Motion_Correction/motion_correction/motion_correction.ino new file mode 100644 index 0000000..58416df --- /dev/null +++ b/example/IMU/Motion_Correction/motion_correction/motion_correction.ino @@ -0,0 +1,112 @@ +#include "Dezibot.h" +#define interval 30 +Dezibot dezibot = Dezibot(); +#define maxDuty 160 +#define minDuty 90 +#define baseDuty 110 +void setup() { + // put your setup code here, to run once: + dezibot.begin(); + Serial.begin(115200); + //dezibot.motion.move(); + //analogWrite(11, 2000); + //analogWrite(12, 2000); +} +uint8_t leftDuty= baseDuty; +uint8_t rightDuty = baseDuty; +unsigned long previous = millis(); +unsigned long current = millis(); +long averageZRotation = 0; +long averageXAccel = 0; +int left=0; +int right=0; +int resultcounter = 0; +void loop() { + // put your main code here, to run repeatedly: + current = millis(); + IMUResult result = dezibot.motionDetection.getRotation(); + averageXAccel += dezibot.motionDetection.getAcceleration().x; + averageZRotation += result.z; + //Serial.println(result.z); + if (result.z > 100){ + right++; + } else if(result.z < -100) { + left++; + } + resultcounter++; + if ((current - previous) > interval){ + Serial.println("============================================="); + previous = current; + averageZRotation = averageZRotation / resultcounter; + averageXAccel = averageXAccel / resultcounter; + Serial.print("Z:"); + Serial.println(averageZRotation); + dezibot.display.clearDisplay(); + dezibot.display.setCursor(0, 0); + dezibot.display.setTextColor(WHITE); + dezibot.display.setTextSize(2); // Draw 2X-scale text + dezibot.display.println(averageZRotation); + dezibot.display.print(left); + dezibot.display.print(" "); + dezibot.display.println(right); + Serial.println("============================================="); + /*if(averageZRotation < -20){ + //turns right + if((rightDuty < maxDuty || leftDuty <= minDuty) && rightDuty<255){ + rightDuty++; + } else{ + leftDuty--; + } + } else if(averageZRotation > 20){ + //turns left + if(leftDuty < maxDuty || rightDuty <= minDuty && leftDuty<255){ + leftDuty++; + } else { + rightDuty--; + } + }*/ + if(left>right){ //rotates anticlock + leftDuty++; + rightDuty--; + /*if(abs(leftDuty-baseDuty)abs(rightDuty-baseDuty)){ + rightDuty++; + } else{ + leftDuty--; + } + if( leftDuty == minDuty){ + rightDuty += 5; + leftDuty += 1; + } else{ + leftDuty--; + }*/ + } + analogWrite(12,leftDuty); + analogWrite(11, rightDuty); + + dezibot.display.println(leftDuty); + dezibot.display.println(rightDuty); + dezibot.display.display(); + + + averageZRotation = 0; + averageXAccel = 0; + resultcounter = 0; + left = 0; + right = 0; + } +} diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp index c412e89..f708ccd 100644 --- a/src/Dezibot.cpp +++ b/src/Dezibot.cpp @@ -12,11 +12,15 @@ #include "Dezibot.h" -Dezibot::Dezibot():multiColorLight(),motionDetection(){ +Dezibot::Dezibot():multiColorLight(),motionDetection(),display(128, 64, &Wire, -1){ }; void Dezibot::begin(void) { motion.begin(); multiColorLight.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 50fcca6..001744d 100644 --- a/src/Dezibot.h +++ b/src/Dezibot.h @@ -16,6 +16,9 @@ #include "colorDetection/ColorDetection.h" #include "multiColorLight/MultiColorLight.h" #include "motionDetection/MotionDetection.h" +#include "Arduino.h" +#include "Wire.h" +#include "Adafruit_SSD1306.h" class Dezibot { protected: @@ -27,7 +30,9 @@ public: ColorDetection colorDetection; MultiColorLight multiColorLight; MotionDetection motionDetection; - + //temporary, display component is not implemented yet + Adafruit_SSD1306 display; + void begin(void); /* Display display From 8fb1ab7651f42fa2aed423e530b8c2b21850eef7 Mon Sep 17 00:00:00 2001 From: hhaupt Date: Sun, 12 May 2024 23:25:08 +0200 Subject: [PATCH 02/12] improved the calibrated movement example --- .../motion_correction/motion_correction.ino | 98 +++++++------------ src/motionDetection/MotionDetection.cpp | 8 +- 2 files changed, 43 insertions(+), 63 deletions(-) diff --git a/example/IMU/Motion_Correction/motion_correction/motion_correction.ino b/example/IMU/Motion_Correction/motion_correction/motion_correction.ino index 58416df..95cc821 100644 --- a/example/IMU/Motion_Correction/motion_correction/motion_correction.ino +++ b/example/IMU/Motion_Correction/motion_correction/motion_correction.ino @@ -1,9 +1,8 @@ #include "Dezibot.h" -#define interval 30 Dezibot dezibot = Dezibot(); -#define maxDuty 160 -#define minDuty 90 -#define baseDuty 110 +#define maxDuty 6000 +#define minDuty 3500 +#define baseDuty 3900 void setup() { // put your setup code here, to run once: dezibot.begin(); @@ -12,14 +11,18 @@ void setup() { //analogWrite(11, 2000); //analogWrite(12, 2000); } -uint8_t leftDuty= baseDuty; -uint8_t rightDuty = baseDuty; +uint16_t leftDuty= baseDuty; +uint16_t rightDuty = baseDuty; unsigned long previous = millis(); unsigned long current = millis(); long averageZRotation = 0; long averageXAccel = 0; int left=0; int right=0; +int difference = 0; +int changerate = 0; +int interval = 40; +int threshold = 150; int resultcounter = 0; void loop() { // put your main code here, to run repeatedly: @@ -28,76 +31,46 @@ void loop() { averageXAccel += dezibot.motionDetection.getAcceleration().x; averageZRotation += result.z; //Serial.println(result.z); - if (result.z > 100){ + if (result.z > threshold){ right++; - } else if(result.z < -100) { + } else if(result.z < -threshold) { left++; } resultcounter++; if ((current - previous) > interval){ - Serial.println("============================================="); - previous = current; - averageZRotation = averageZRotation / resultcounter; - averageXAccel = averageXAccel / resultcounter; - Serial.print("Z:"); - Serial.println(averageZRotation); + //averageZRotation = averageZRotation / resultcounter; + difference = abs(left-right); + if (difference>25){ + changerate = 100; + } else if(difference>20){ + changerate = 50; + } else if(difference >15){ + changerate = 30; + } else if(difference > 10){ + changerate = 10; + } else{ + changerate = 5; + } + //Serial.print("Z:"); + //Serial.println(averageZRotation); dezibot.display.clearDisplay(); dezibot.display.setCursor(0, 0); dezibot.display.setTextColor(WHITE); dezibot.display.setTextSize(2); // Draw 2X-scale text - dezibot.display.println(averageZRotation); + dezibot.display.println(averageZRotation); + //dezibot.display.println(resultcounter); dezibot.display.print(left); dezibot.display.print(" "); dezibot.display.println(right); - Serial.println("============================================="); - /*if(averageZRotation < -20){ - //turns right - if((rightDuty < maxDuty || leftDuty <= minDuty) && rightDuty<255){ - rightDuty++; - } else{ - leftDuty--; - } - } else if(averageZRotation > 20){ - //turns left - if(leftDuty < maxDuty || rightDuty <= minDuty && leftDuty<255){ - leftDuty++; - } else { - rightDuty--; - } - }*/ if(left>right){ //rotates anticlock - leftDuty++; - rightDuty--; - /*if(abs(leftDuty-baseDuty)abs(rightDuty-baseDuty)){ - rightDuty++; - } else{ - leftDuty--; - } - if( leftDuty == minDuty){ - rightDuty += 5; - leftDuty += 1; - } else{ - leftDuty--; - }*/ + leftDuty+=changerate; + rightDuty-=changerate; + } else if(lefttransfer(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(); }; From 09cbbd7a5ba23b5b306666ab81ced821bf4d4528 Mon Sep 17 00:00:00 2001 From: amorgner Date: Sat, 25 May 2024 11:22:20 +0000 Subject: [PATCH 03/12] Update README.md with information to including library --- README.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/README.md b/README.md index c0d6b18..dd50512 100644 --- a/README.md +++ b/README.md @@ -140,6 +140,14 @@ Using `arduino-cli` to compile and upload: `arduino-cli upload /Users/jo/Documents/Arduino/theSketch -p /dev/cu.usbmodem101 -b esp32:esp32:nora_w10` `arduino-cli compile /Users/jo/Documents/Arduino/theSketch -p /dev/cu.usbmodem101 -b esp32:esp32:nora_w10` +##### Including Library + +Arduino IDE -> Sketch -> Include Library -> add .ZIP Library -> this library + +If there is any other error like 'Adafruit_SSD1306' not found, you have to include this library also. + +Arduino IDE -> Sketch -> Manage Library -> Search for missing Library + #### Display It is important to specify the SDA and SCL ports by using `Wire.begin(SDA, SCL)`. From 5e2b8698d9246b9edbe78a82885b65d7ba8a5086 Mon Sep 17 00:00:00 2001 From: hhaupt Date: Mon, 10 Jun 2024 00:38:26 +0200 Subject: [PATCH 04/12] WIP: Read/Write from Registerbanks --- src/motionDetection/MotionDetection.cpp | 105 +++++++++++++++++++++++- src/motionDetection/MotionDetection.h | 35 ++------ 2 files changed, 111 insertions(+), 29 deletions(-) diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp index e22fe65..d0e99c1 100644 --- a/src/motionDetection/MotionDetection.cpp +++ b/src/motionDetection/MotionDetection.cpp @@ -12,8 +12,8 @@ void MotionDetection::begin(void){ 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(250); //set Gyroconfig @@ -22,8 +22,14 @@ void MotionDetection::begin(void){ //set Gyro Filter handler->transfer(0x23); handler->transfer(0x37); + //Disable FIFO Bypass + handler->transfer(FIFO_CONFIG1); + handler->transfer(0x00); digitalWrite(34,HIGH); handler->endTransaction(); + this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23); + //Enable Gyro and Acceldata in FIFO + }; void MotionDetection::end(void){ handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); @@ -71,7 +77,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; @@ -80,4 +86,99 @@ int8_t MotionDetection::readRegister(uint8_t reg){ digitalWrite(34,HIGH); handler->endTransaction(); return result; +}; + +uint8_t MotionDetection::readFromRegisterBank(registerBank bank,uint8_t reg){ + uint8_t result = 0; + handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + digitalWrite(34,LOW); + switch(bank){ + case(MREG1): + result = handler->transfer(BLK_SEL_R); + result = handler->transfer(0x00); + break; + case(MREG2): + result = handler->transfer(BLK_SEL_R); + result = handler->transfer(0x28); + break; + case(MREG3): + result = handler->transfer(BLK_SEL_R); + result = handler->transfer(0x50); + break; + } + result = handler->transfer(MADDR_R); + result = handler->transfer(reg); + digitalWrite(34,HIGH); + handler->endTransaction(); + delayMicroseconds(10); + result=this->readRegister(M_R); + delayMicroseconds(10); + this->resetRegisterBankAccess(); + return result; +}; + +void MotionDetection::writeToRegisterBank(registerBank bank, uint8_t reg, uint8_t value){ + while((this->readRegister(MCLK_RDY))&0x08!=0x08){ + Serial.println("CLK not rdy"); + delay(100); + } + Serial.print("MADDR_W: "); + Serial.println(readRegister(MADDR_W)); + handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + digitalWrite(34,LOW); + switch(bank){ + case(MREG1): + handler->transfer(BLK_SEL_W); + handler->transfer(0x00); + break; + case(MREG2): + handler->transfer(BLK_SEL_W); + handler->transfer(0x28); + break; + case(MREG3): + handler->transfer(BLK_SEL_W); + handler->transfer(0x50); + break; + } + handler->transfer(MADDR_W); + handler->transfer(reg); + //handler->transfer(reg); + handler->transfer(M_W); + handler->transfer(value); + digitalWrite(34,HIGH); + handler->endTransaction(); + Serial.print("MADDR_W: "); + Serial.println(readRegister(MADDR_W)); + delayMicroseconds(10); + this->resetRegisterBankAccess(); + +}; + +void MotionDetection::resetRegisterBankAccess(){ + handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + digitalWrite(34,LOW); + handler->transfer(BLK_SEL_R); + handler->transfer(0x00); + handler->transfer(BLK_SEL_W); + handler->transfer(0x00); + handler->transfer(MADDR_R); + handler->transfer(0x00); + handler->transfer(MADDR_W); + handler->transfer(0x00); + digitalWrite(34,HIGH); + handler->endTransaction(); +}; + +void MotionDetection::testFIFO(){ + this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23); + //Serial.println(this->readFromRegisterBank(MREG1,0x47)); + //uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; + //fifocount |= this->readRegister(FIFO_COUNTL); + //Serial.print("FiFo Count: "); + //Serial.println(fifocount); + + Serial.print("INT_CONFIG1: "); + Serial.println(readFromRegisterBank(MREG1,0x05)); + Serial.print("FiFoData: "); + Serial.println(readRegister(0x3F)); }; \ No newline at end of file diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h index 6d9bd5a..7f76b5d 100644 --- a/src/motionDetection/MotionDetection.h +++ b/src/motionDetection/MotionDetection.h @@ -12,6 +12,7 @@ #define MotionDetection_h #include #include +#include "IMU_CMDs.h" struct IMUResult{ int16_t x; int16_t y; @@ -21,40 +22,18 @@ struct IMUResult{ class MotionDetection{ protected: - - static const uint8_t CMD_READ = 0x80; - static const uint8_t CMD_WRITE = 0x00; - static const uint8_t ADDR_MASK = 0x7F; - - //Registers - static const uint8_t REG_TEMP_LOW = 0x0A; - static const uint8_t REG_TEMP_HIGH = 0X09; - - static const uint8_t ACCEL_DATA_X_HIGH = 0x0B; - static const uint8_t ACCEL_DATA_X_LOW = 0x0C; - static const uint8_t ACCEL_DATA_Y_HIGH = 0x0D; - static const uint8_t ACCEL_DATA_Y_LOW = 0x0E; - static const uint8_t ACCEL_DATA_Z_HIGH = 0x0F; - static const uint8_t ACCEL_DATA_Z_LOW = 0x10; - - static const uint8_t GYRO_DATA_X_HIGH = 0x11; - static const uint8_t GYRO_DATA_X_LOW = 0x12; - static const uint8_t GYRO_DATA_Y_HIGH = 0x13; - static const uint8_t GYRO_DATA_Y_LOW = 0x14; - static const uint8_t GYRO_DATA_Z_HIGH = 0x15; - static const uint8_t GYRO_DATA_Z_LOW = 0x16; - - static const uint8_t PWR_MGMT0 = 0x1F; - static const uint8_t WHO_AM_I = 0x75; - + enum registerBank{MREG1,MREG2,MREG3}; static const uint frequency = 10000000; + uint8_t readFromRegisterBank(registerBank bank,uint8_t reg); + void writeToRegisterBank(registerBank bank, uint8_t reg, uint8_t value); + void resetRegisterBankAccess(); 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; @@ -106,5 +85,7 @@ public: * @return the value of the whoami register of the ICM-42670 */ int8_t getWhoAmI(); + + void testFIFO(); }; #endif //MotionDetection \ No newline at end of file From 00c76166f81ad1eb2c2f95378be771ade47c03f9 Mon Sep 17 00:00:00 2001 From: hhaupt Date: Mon, 10 Jun 2024 23:15:24 +0200 Subject: [PATCH 05/12] added CMD declarations --- src/motionDetection/IMU_CMDs.h | 46 ++++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 src/motionDetection/IMU_CMDs.h diff --git a/src/motionDetection/IMU_CMDs.h b/src/motionDetection/IMU_CMDs.h new file mode 100644 index 0000000..cdbe607 --- /dev/null +++ b/src/motionDetection/IMU_CMDs.h @@ -0,0 +1,46 @@ +#ifndef IMU_CMDs +#define IMU_CMDs + +#define CMD_READ 0x80 +#define CMD_WRITE 0x00 +#define ADDR_MASK 0x7F + +//Registers +#define MCLK_RDY 0x00 + +#define REG_TEMP_LOW 0x0A +#define REG_TEMP_HIGH 0X09 + +#define ACCEL_DATA_X_HIGH 0x0B +#define ACCEL_DATA_X_LOW 0x0C +#define ACCEL_DATA_Y_HIGH 0x0D +#define ACCEL_DATA_Y_LOW 0x0E +#define ACCEL_DATA_Z_HIGH 0x0F +#define ACCEL_DATA_Z_LOW 0x10 + +#define GYRO_DATA_X_HIGH 0x11 +#define GYRO_DATA_X_LOW 0x12 +#define GYRO_DATA_Y_HIGH 0x13 +#define GYRO_DATA_Y_LOW 0x14 +#define GYRO_DATA_Z_HIGH 0x15 +#define GYRO_DATA_Z_LOW 0x16 + +#define PWR_MGMT0 0x1F +#define WHO_AM_I 0x75 + +#define BLK_SEL_W 0x79 +#define BLK_SEL_R 0x7C +#define MADDR_W 0x7A +#define MADDR_R 0x7D +#define M_W 0x7B +#define M_R 0x7E + +#define FIFO_COUNTH 0x3D +#define FIFO_COUNTL 0x3E +#define FIFO_DATA 0x3F +#define FIFO_CONFIG1 0x28 + + +//MREG1 +#define FIFO_CONFIG5 0x01 +#endif \ No newline at end of file From a058b3f2057cdc2a28eedb7ade24d2c9e1f16a2e Mon Sep 17 00:00:00 2001 From: hhaupt Date: Wed, 12 Jun 2024 20:26:38 +0200 Subject: [PATCH 06/12] implemented access to registerbanks, fixed wrong write access --- src/motionDetection/IMU_CMDs.h | 5 +- src/motionDetection/MotionDetection.cpp | 208 ++++++++++++++++-------- src/motionDetection/MotionDetection.h | 2 + 3 files changed, 146 insertions(+), 69 deletions(-) diff --git a/src/motionDetection/IMU_CMDs.h b/src/motionDetection/IMU_CMDs.h index cdbe607..ab420c7 100644 --- a/src/motionDetection/IMU_CMDs.h +++ b/src/motionDetection/IMU_CMDs.h @@ -28,6 +28,8 @@ #define PWR_MGMT0 0x1F #define WHO_AM_I 0x75 +#define INTF_CONFIG0 0x35 + #define BLK_SEL_W 0x79 #define BLK_SEL_R 0x7C #define MADDR_W 0x7A @@ -39,8 +41,9 @@ #define FIFO_COUNTL 0x3E #define FIFO_DATA 0x3F #define FIFO_CONFIG1 0x28 - +#define FIFO_CONFIG2 0x29 //MREG1 #define FIFO_CONFIG5 0x01 +#define TMST_CONFIG1 0x00 #endif \ No newline at end of file diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp index d0e99c1..cdc4ab7 100644 --- a/src/motionDetection/MotionDetection.cpp +++ b/src/motionDetection/MotionDetection.cpp @@ -9,36 +9,39 @@ void MotionDetection::begin(void){ digitalWrite(34,HIGH); handler->begin(36,37,35,34); - handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - digitalWrite(34,LOW); + //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); + this->writeRegister(PWR_MGMT0,0x1F); + //handler->transfer(PWR_MGMT0); + //handler->transfer(0x1F); //busy Wait for startup delayMicroseconds(250); //set Gyroconfig - handler->transfer(0x20); - handler->transfer(0x25); + this->writeRegister(0x20,0x25); + //handler->transfer(0x20); + //handler->transfer(0x25); //set Gyro Filter - handler->transfer(0x23); - handler->transfer(0x37); + this->writeRegister(0x23,0x37); + //handler->transfer(0x23); + //handler->transfer(0x37); //Disable FIFO Bypass - handler->transfer(FIFO_CONFIG1); - handler->transfer(0x00); - digitalWrite(34,HIGH); - handler->endTransaction(); - this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23); + //handler->transfer(FIFO_CONFIG1); + //handler->transfer(0x00); + //digitalWrite(34,HIGH); + //handler->endTransaction(); //Enable Gyro and Acceldata in FIFO - + this->initFIFO(); }; void MotionDetection::end(void){ - handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - digitalWrite(34,LOW); - handler->transfer(cmdWrite(PWR_MGMT0)); + //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + //digitalWrite(34,LOW); + this->writeRegister(PWR_MGMT0,0x00); + //handler->transfer(cmdWrite(PWR_MGMT0)); //turn Accel and Gyroscope off - handler->transfer(0x00); - digitalWrite(34,HIGH); - handler->end(); + //handler->transfer(0x00); + //digitalWrite(34,HIGH); + //handler->end(); }; IMUResult MotionDetection::getAcceleration(){ IMUResult result; @@ -90,28 +93,32 @@ uint8_t MotionDetection::readRegister(uint8_t reg){ uint8_t MotionDetection::readFromRegisterBank(registerBank bank,uint8_t reg){ uint8_t result = 0; - handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - digitalWrite(34,LOW); + //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + //digitalWrite(34,LOW); switch(bank){ case(MREG1): - result = handler->transfer(BLK_SEL_R); - result = handler->transfer(0x00); + this->writeRegister(BLK_SEL_R,0x00); + //result = handler->transfer(BLK_SEL_R); + //result = handler->transfer(0x00); break; case(MREG2): - result = handler->transfer(BLK_SEL_R); - result = handler->transfer(0x28); + this->writeRegister(BLK_SEL_R,0x28); + //result = handler->transfer(BLK_SEL_R); + //result = handler->transfer(0x28); break; case(MREG3): - result = handler->transfer(BLK_SEL_R); - result = handler->transfer(0x50); + this->writeRegister(BLK_SEL_R,0x50); + //result = handler->transfer(BLK_SEL_R); + //result = handler->transfer(0x50); break; } - result = handler->transfer(MADDR_R); - result = handler->transfer(reg); - digitalWrite(34,HIGH); - handler->endTransaction(); + this->writeRegister(MADDR_R,reg); + //result = handler->transfer(MADDR_R); + //result = handler->transfer(reg); + //digitalWrite(34,HIGH); + //handler->endTransaction(); delayMicroseconds(10); - result=this->readRegister(M_R); + result=this->readRegister(M_R); delayMicroseconds(10); this->resetRegisterBankAccess(); return result; @@ -122,63 +129,128 @@ void MotionDetection::writeToRegisterBank(registerBank bank, uint8_t reg, uint8_ Serial.println("CLK not rdy"); delay(100); } + uint8_t result = this->readRegister(PWR_MGMT0); Serial.print("MADDR_W: "); Serial.println(readRegister(MADDR_W)); - handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - digitalWrite(34,LOW); + //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + //digitalWrite(34,LOW); + //set Idle Bit + this->writeRegister(PWR_MGMT0,result|0x10); + //handler->transfer(PWR_MGMT0); + //handler->transfer(result|0x10); switch(bank){ case(MREG1): - handler->transfer(BLK_SEL_W); - handler->transfer(0x00); + this->writeRegister(BLK_SEL_W,0x00); + // handler->transfer(BLK_SEL_W); + // handler->transfer(0x00); break; case(MREG2): - handler->transfer(BLK_SEL_W); - handler->transfer(0x28); + this->writeRegister(BLK_SEL_W,0x28); + //handler->transfer(BLK_SEL_W); + //handler->transfer(0x28); break; case(MREG3): - handler->transfer(BLK_SEL_W); - handler->transfer(0x50); + this->writeRegister(BLK_SEL_W,0x50); + //handler->transfer(BLK_SEL_W); + //handler->transfer(0x50); break; } - handler->transfer(MADDR_W); - handler->transfer(reg); + this->writeRegister(MADDR_W,reg); + //handler->transfer(MADDR_W); //handler->transfer(reg); - handler->transfer(M_W); - handler->transfer(value); - digitalWrite(34,HIGH); - handler->endTransaction(); + this->writeRegister(M_W,value); + //handler->transfer(M_W); + //handler->transfer(value); + delayMicroseconds(10); + this->writeRegister(PWR_MGMT0,result&0xEF); + //handler->transfer(PWR_MGMT0); + //handler->transfer(result&0xEF); + //digitalWrite(34,HIGH); + //handler->endTransaction(); Serial.print("MADDR_W: "); Serial.println(readRegister(MADDR_W)); - delayMicroseconds(10); this->resetRegisterBankAccess(); }; void MotionDetection::resetRegisterBankAccess(){ - handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - digitalWrite(34,LOW); - handler->transfer(BLK_SEL_R); - handler->transfer(0x00); - handler->transfer(BLK_SEL_W); - handler->transfer(0x00); - handler->transfer(MADDR_R); - handler->transfer(0x00); - handler->transfer(MADDR_W); - handler->transfer(0x00); - digitalWrite(34,HIGH); - handler->endTransaction(); + //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + //digitalWrite(34,LOW); + this->writeRegister(BLK_SEL_R,0x00); + //handler->transfer(BLK_SEL_R); + //handler->transfer(0x00); + this->writeRegister(BLK_SEL_W,0x00); + //handler->transfer(BLK_SEL_W); + //handler->transfer(0x00); + this->writeRegister(MADDR_R,0x00); + //handler->transfer(MADDR_R); + //handler->transfer(0x00); + this->writeRegister(MADDR_W,0x00); + //handler->transfer(MADDR_W); + //handler->transfer(0x00); + //digitalWrite(34,HIGH); + //handler->endTransaction(); }; void MotionDetection::testFIFO(){ - this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23); - //Serial.println(this->readFromRegisterBank(MREG1,0x47)); - //uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; - //fifocount |= this->readRegister(FIFO_COUNTL); - //Serial.print("FiFo Count: "); - //Serial.println(fifocount); + //this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23); + Serial.println(this->readFromRegisterBank(MREG1,FIFO_CONFIG5)); + uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; + fifocount |= this->readRegister(FIFO_COUNTL); + Serial.print("FiFo Count: "); + Serial.println(fifocount); + uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; + fifocount |= this->readRegister(FIFO_COUNTL); + Serial.print("FiFo Count: "); + Serial.println(fifocount); + uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; + fifocount |= this->readRegister(FIFO_COUNTL); + Serial.print("FiFo Count: "); + Serial.println(fifocount); - Serial.print("INT_CONFIG1: "); - Serial.println(readFromRegisterBank(MREG1,0x05)); + //Serial.print("INT_CONFIG1: "); + //Serial.println(readFromRegisterBank(MREG1,0x05)); Serial.print("FiFoData: "); Serial.println(readRegister(0x3F)); +}; + +void MotionDetection::initFIFO(){ + + //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + //digitalWrite(34,LOW); + + //set INTF_CONFIG0 FIFO_COUNT_REC_RECORD und Little Endian + this->writeRegister(INTF_CONFIG0,0x70); + //handler->transfer(INTF_CONFIG0); + //handler->transfer(0x70); + //set FIFO_CONFIG1 to Mode Snapshot and BYPASS Off + this->writeRegister(FIFO_CONFIG1,0x02); + //handler->transfer(FIFO_CONFIG1); + //handler->transfer(0x02); + //set TMST_CONFIG1_MREG1 TMST_CONFIIG1_TMST_EN + //digitalWrite(34,HIGH); + //handler->endTransaction(); + this->writeToRegisterBank(MREG1,TMST_CONFIG1,0x03); + //set FiFO config 5 GYRO_EN,TMST_FSYNC, ACCEL_EN, WM_GT_TH_EN + this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x27); + //set FOF_CONFIG2 0x1 (INT triggerd each packaged) + //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + //digitalWrite(34,LOW); + this->writeRegister(FIFO_CONFIG2,0x0A); + //handler->transfer(FIFO_CONFIG2); + //handler->transfer(0x0A); + //disable Data Read Interrupt + //digitalWrite(34,HIGH); + //handler->endTransaction(); + +}; + +void MotionDetection::writeRegister(uint8_t reg, uint8_t value){ + handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + digitalWrite(34,LOW); + handler->transfer(reg); + handler->transfer(value); + delayMicroseconds(10); + digitalWrite(34,HIGH); + handler->endTransaction(); }; \ No newline at end of file diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h index 7f76b5d..f668d79 100644 --- a/src/motionDetection/MotionDetection.h +++ b/src/motionDetection/MotionDetection.h @@ -35,6 +35,8 @@ protected: uint8_t readRegister(uint8_t reg); int16_t readDoubleRegister(uint8_t lowerReg); + void writeRegister(uint8_t reg, uint8_t value); + void initFIFO(); SPIClass * handler = NULL; From 841202897b48ded0d7640643fa9461c09ff30a10 Mon Sep 17 00:00:00 2001 From: hhaupt Date: Thu, 13 Jun 2024 04:17:12 +0200 Subject: [PATCH 07/12] Fixed FiFo access --- src/motionDetection/MotionDetection.cpp | 141 ++++++------------------ src/motionDetection/MotionDetection.h | 1 + 2 files changed, 35 insertions(+), 107 deletions(-) diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp index cdc4ab7..03c650c 100644 --- a/src/motionDetection/MotionDetection.cpp +++ b/src/motionDetection/MotionDetection.cpp @@ -8,40 +8,21 @@ void MotionDetection::begin(void){ pinMode(34,OUTPUT); 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 this->writeRegister(PWR_MGMT0,0x1F); - //handler->transfer(PWR_MGMT0); - //handler->transfer(0x1F); //busy Wait for startup delayMicroseconds(250); + //set accelconfig + this->writeRegister(0x21,0x05); //set Gyroconfig this->writeRegister(0x20,0x25); - //handler->transfer(0x20); - //handler->transfer(0x25); //set Gyro Filter this->writeRegister(0x23,0x37); - //handler->transfer(0x23); - //handler->transfer(0x37); - //Disable FIFO Bypass - //handler->transfer(FIFO_CONFIG1); - //handler->transfer(0x00); - //digitalWrite(34,HIGH); - //handler->endTransaction(); //Enable Gyro and Acceldata in FIFO this->initFIFO(); }; void MotionDetection::end(void){ - //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - //digitalWrite(34,LOW); this->writeRegister(PWR_MGMT0,0x00); - //handler->transfer(cmdWrite(PWR_MGMT0)); - //turn Accel and Gyroscope off - //handler->transfer(0x00); - //digitalWrite(34,HIGH); - //handler->end(); }; IMUResult MotionDetection::getAcceleration(){ IMUResult result; @@ -86,37 +67,25 @@ uint8_t MotionDetection::readRegister(uint8_t reg){ uint8_t result; result = handler->transfer(cmdRead(reg)); result = handler->transfer(0x00); - digitalWrite(34,HIGH); + digitalWrite(34,HIGH); handler->endTransaction(); return result; }; uint8_t MotionDetection::readFromRegisterBank(registerBank bank,uint8_t reg){ uint8_t result = 0; - //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - //digitalWrite(34,LOW); switch(bank){ case(MREG1): this->writeRegister(BLK_SEL_R,0x00); - //result = handler->transfer(BLK_SEL_R); - //result = handler->transfer(0x00); break; case(MREG2): this->writeRegister(BLK_SEL_R,0x28); - //result = handler->transfer(BLK_SEL_R); - //result = handler->transfer(0x28); break; case(MREG3): this->writeRegister(BLK_SEL_R,0x50); - //result = handler->transfer(BLK_SEL_R); - //result = handler->transfer(0x50); break; } this->writeRegister(MADDR_R,reg); - //result = handler->transfer(MADDR_R); - //result = handler->transfer(reg); - //digitalWrite(34,HIGH); - //handler->endTransaction(); delayMicroseconds(10); result=this->readRegister(M_R); delayMicroseconds(10); @@ -132,117 +101,75 @@ void MotionDetection::writeToRegisterBank(registerBank bank, uint8_t reg, uint8_ uint8_t result = this->readRegister(PWR_MGMT0); Serial.print("MADDR_W: "); Serial.println(readRegister(MADDR_W)); - //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - //digitalWrite(34,LOW); //set Idle Bit this->writeRegister(PWR_MGMT0,result|0x10); - //handler->transfer(PWR_MGMT0); - //handler->transfer(result|0x10); switch(bank){ case(MREG1): this->writeRegister(BLK_SEL_W,0x00); - // handler->transfer(BLK_SEL_W); - // handler->transfer(0x00); break; case(MREG2): this->writeRegister(BLK_SEL_W,0x28); - //handler->transfer(BLK_SEL_W); - //handler->transfer(0x28); break; case(MREG3): this->writeRegister(BLK_SEL_W,0x50); - //handler->transfer(BLK_SEL_W); - //handler->transfer(0x50); break; } this->writeRegister(MADDR_W,reg); - //handler->transfer(MADDR_W); - //handler->transfer(reg); this->writeRegister(M_W,value); - //handler->transfer(M_W); - //handler->transfer(value); delayMicroseconds(10); this->writeRegister(PWR_MGMT0,result&0xEF); - //handler->transfer(PWR_MGMT0); - //handler->transfer(result&0xEF); - //digitalWrite(34,HIGH); - //handler->endTransaction(); Serial.print("MADDR_W: "); Serial.println(readRegister(MADDR_W)); this->resetRegisterBankAccess(); - }; void MotionDetection::resetRegisterBankAccess(){ - //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - //digitalWrite(34,LOW); this->writeRegister(BLK_SEL_R,0x00); - //handler->transfer(BLK_SEL_R); - //handler->transfer(0x00); this->writeRegister(BLK_SEL_W,0x00); - //handler->transfer(BLK_SEL_W); - //handler->transfer(0x00); this->writeRegister(MADDR_R,0x00); - //handler->transfer(MADDR_R); - //handler->transfer(0x00); this->writeRegister(MADDR_W,0x00); - //handler->transfer(MADDR_W); - //handler->transfer(0x00); - //digitalWrite(34,HIGH); - //handler->endTransaction(); }; void MotionDetection::testFIFO(){ - //this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23); - Serial.println(this->readFromRegisterBank(MREG1,FIFO_CONFIG5)); - uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; - fifocount |= this->readRegister(FIFO_COUNTL); - Serial.print("FiFo Count: "); - Serial.println(fifocount); - uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; - fifocount |= this->readRegister(FIFO_COUNTL); - Serial.print("FiFo Count: "); - Serial.println(fifocount); - uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; - fifocount |= this->readRegister(FIFO_COUNTL); - Serial.print("FiFo Count: "); - Serial.println(fifocount); - - //Serial.print("INT_CONFIG1: "); - //Serial.println(readFromRegisterBank(MREG1,0x05)); + 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: "); - Serial.println(readRegister(0x3F)); -}; + handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + digitalWrite(34,LOW); + handler->transfer(cmdRead(0x3f)); + handler->transfer(buf,8*fifocount); + //delayMicroseconds(10); + digitalWrite(34,HIGH); + handler->endTransaction(); + Serial.println("============="); + + writeRegister(0x02,0x04); + delayMicroseconds(10); + fifocount = 0; + fifocount = (this->readRegister(FIFO_COUNTH)<<8); + fifocount |= this->readRegister(FIFO_COUNTL); + Serial.print("FiFo Count: "); + Serial.println(fifocount); +} void MotionDetection::initFIFO(){ - - //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - //digitalWrite(34,LOW); - + delay(60); //set INTF_CONFIG0 FIFO_COUNT_REC_RECORD und Little Endian - this->writeRegister(INTF_CONFIG0,0x70); - //handler->transfer(INTF_CONFIG0); - //handler->transfer(0x70); + this->writeRegister(INTF_CONFIG0,0x60); //set FIFO_CONFIG1 to Mode Snapshot and BYPASS Off - this->writeRegister(FIFO_CONFIG1,0x02); - //handler->transfer(FIFO_CONFIG1); - //handler->transfer(0x02); + this->writeRegister(FIFO_CONFIG1,0x00); //set TMST_CONFIG1_MREG1 TMST_CONFIIG1_TMST_EN - //digitalWrite(34,HIGH); - //handler->endTransaction(); - this->writeToRegisterBank(MREG1,TMST_CONFIG1,0x03); + 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,0x27); + this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x21); //set FOF_CONFIG2 0x1 (INT triggerd each packaged) - //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - //digitalWrite(34,LOW); this->writeRegister(FIFO_CONFIG2,0x0A); - //handler->transfer(FIFO_CONFIG2); - //handler->transfer(0x0A); - //disable Data Read Interrupt - //digitalWrite(34,HIGH); - //handler->endTransaction(); - }; void MotionDetection::writeRegister(uint8_t reg, uint8_t value){ @@ -250,7 +177,7 @@ void MotionDetection::writeRegister(uint8_t reg, uint8_t value){ digitalWrite(34,LOW); handler->transfer(reg); handler->transfer(value); - delayMicroseconds(10); digitalWrite(34,HIGH); + delayMicroseconds(10); handler->endTransaction(); }; \ No newline at end of file diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h index f668d79..3f5664f 100644 --- a/src/motionDetection/MotionDetection.h +++ b/src/motionDetection/MotionDetection.h @@ -24,6 +24,7 @@ class MotionDetection{ protected: enum registerBank{MREG1,MREG2,MREG3}; static const uint frequency = 10000000; + int8_t* buf = new int8_t[16*200]; uint8_t readFromRegisterBank(registerBank bank,uint8_t reg); void writeToRegisterBank(registerBank bank, uint8_t reg, uint8_t value); void resetRegisterBankAccess(); From d4cb8af3b3ccbf4eda9ee24a2d5a98b9c89a7cdf Mon Sep 17 00:00:00 2001 From: Hans Haupt Date: Thu, 13 Jun 2024 17:20:22 +0200 Subject: [PATCH 08/12] 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(); }; From 4f308412e4a94d0a724bfb59b58fed44feff593f Mon Sep 17 00:00:00 2001 From: hhaupt Date: Thu, 13 Jun 2024 23:35:17 +0200 Subject: [PATCH 09/12] added FIFOdata fetch method --- src/Dezibot.cpp | 7 +++---- src/Dezibot.h | 25 +++---------------------- src/motionDetection/MotionDetection.cpp | 2 +- 3 files changed, 7 insertions(+), 27 deletions(-) diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp index 45af406..1c1b19d 100644 --- a/src/Dezibot.cpp +++ b/src/Dezibot.cpp @@ -12,14 +12,13 @@ #include "Dezibot.h" -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); }; diff --git a/src/Dezibot.h b/src/Dezibot.h index a78a76c..d9d9d80 100644 --- a/src/Dezibot.h +++ b/src/Dezibot.h @@ -19,37 +19,18 @@ #include "Arduino.h" #include "Wire.h" -extern static MotionDetection* motionDetectionPTR; - class Dezibot { protected: - void setIMU(Motion& m){ - m.imuInst = motionDetection; - }; + public: Dezibot(); LightDetection lightDetection; ColorDetection colorDetection; MultiColorLight multiColorLight; - Motion motion = Motion(motionDetectionPTR); - static MotionDetection motionDetection = *motionDetectionPTR; + Motion motion ; + MotionDetection motionDetection; void begin(void); -/* -Display display -IRCommuncation irCommuncation (beinhaltet Kommuniaktion / Annhärung...) -Battery battery -Extension extension -WiFi wifi //wie wird WiFi geschrieben? -//nur lesender Zugriff, in dieser Klasse sind andere Instanzen mit dem Dezibotinterface gekapselt -Friends friends -OperatingSystem operatingSystem -USBCommunication usbCommunication -Button button -//nicht unique, initzial Dezibot -String robotName - -*/ }; #endif //Dezibot_h \ No newline at end of file diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp index b907b73..14da520 100644 --- a/src/motionDetection/MotionDetection.cpp +++ b/src/motionDetection/MotionDetection.cpp @@ -1,7 +1,7 @@ #include "MotionDetection.h" #include -MotionDetection::MotionDetection(){//:handler(FSPI){ +MotionDetection::MotionDetection(){ handler = new SPIClass(FSPI); }; From 2701446915a5e7d2517b717e85cf264b2a1f5640 Mon Sep 17 00:00:00 2001 From: hhaupt Date: Fri, 14 Jun 2024 01:22:46 +0200 Subject: [PATCH 10/12] updated movement Tasks to handle multiple calling properbly and prepare calibration feature --- src/Dezibot.cpp | 1 - src/Dezibot.h | 1 - src/motion/Motion.cpp | 113 ++++++++++++++++++++++++++++++++++-------- src/motion/Motion.h | 8 ++- 4 files changed, 100 insertions(+), 23 deletions(-) diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp index 1c1b19d..8a90184 100644 --- a/src/Dezibot.cpp +++ b/src/Dezibot.cpp @@ -19,6 +19,5 @@ Dezibot::Dezibot():multiColorLight(){ void Dezibot::begin(void) { motion.begin(); multiColorLight.begin(); - motionDetection.begin(); Wire.begin(1,2); }; diff --git a/src/Dezibot.h b/src/Dezibot.h index d9d9d80..619315b 100644 --- a/src/Dezibot.h +++ b/src/Dezibot.h @@ -28,7 +28,6 @@ public: ColorDetection colorDetection; MultiColorLight multiColorLight; Motion motion ; - MotionDetection motionDetection; void begin(void); }; diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp index b411a35..4b4f513 100644 --- a/src/motion/Motion.cpp +++ b/src/motion/Motion.cpp @@ -11,9 +11,6 @@ #include "Motion.h" -TaskHandle_t xMoveTaskHandle = NULL; -TaskHandle_t xClockwiseTaskHandle = NULL; -TaskHandle_t xAntiClockwiseTaskHandle = NULL; // Initialize the movement component. @@ -29,36 +26,83 @@ void Motion::begin(void) { ledc_timer_config(&motor_timer); Motion::left.begin(); Motion::right.begin(); + detection.begin(); }; void Motion::moveTask(void * args) { + uint32_t runtime = (uint32_t)args; + Motion::left.setSpeed(LEFT_MOTOR_DUTY); Motion::right.setSpeed(RIGHT_MOTOR_DUTY); - //moveCMD cmd = (moveCMD)args; - //vTaskDelay( cmd.duration/ portTICK_PERIOD_MS); - Motion::left.setSpeed(0); - Motion::right.setSpeed(0); - vTaskDelete(xMoveTaskHandle); + Motion::xLastWakeTime = xTaskGetTickCount(); + while(1){ + if(runtime>40||runtime==0){ + vTaskDelayUntil(&xLastWakeTime,40); + runtime -= 40; + //calc new parameters + //set new parameters + } else { + vTaskDelayUntil(&xLastWakeTime,runtime); + Motion::left.setSpeed(0); + Motion::right.setSpeed(0); + vTaskDelete(xMoveTaskHandle); + } + } }; // Move forward for a certain amount of time. void Motion::move(uint32_t moveForMs) { // moveCMD cmd = {moveForMs,imuInst}; - // xTaskCreate(moveTask, "Move", 4096, (void*)cmd, 10, &xMoveTaskHandle); - Motion::left.setSpeed(LEFT_MOTOR_DUTY); - Motion::right.setSpeed(RIGHT_MOTOR_DUTY); + + if(xMoveTaskHandle){ + vTaskDelete(xMoveTaskHandle); + xMoveTaskHandle = NULL; + } + if(xClockwiseTaskHandle){ + + vTaskDelete(xClockwiseTaskHandle); + xClockwiseTaskHandle = NULL; + } + if(xAntiClockwiseTaskHandle){ + vTaskDelete(xAntiClockwiseTaskHandle); + xAntiClockwiseTaskHandle = NULL; + + } + xTaskCreate(moveTask, "Move", 4096, (void*)moveForMs, 10, &xMoveTaskHandle); + }; void Motion::leftMotorTask(void * args) { - Motion::left.setSpeed(LEFT_MOTOR_DUTY); + uint32_t runtime = (uint32_t)args; + if(xMoveTaskHandle){ + vTaskDelete(xMoveTaskHandle); + xMoveTaskHandle = NULL; + } + if(xAntiClockwiseTaskHandle){ + vTaskDelete(xAntiClockwiseTaskHandle); + xAntiClockwiseTaskHandle = NULL; + } Motion::right.setSpeed(0); - vTaskDelay((uint32_t) args / portTICK_PERIOD_MS); - Motion::left.setSpeed(0); - vTaskDelete(xClockwiseTaskHandle); + Motion::left.setSpeed(LEFT_MOTOR_DUTY); + while(1){ + if((runtime>40)||(runtime==0)){ + vTaskDelayUntil(&xLastWakeTime,40); + runtime -=40; + } else { + vTaskDelayUntil(&xLastWakeTime,runtime); + Motion::left.setSpeed(0); + vTaskDelete(xClockwiseTaskHandle); + } + vTaskDelayUntil(&xLastWakeTime,40); + } }; // Rotate clockwise for a certain amount of time. void Motion::rotateClockwise(uint32_t rotateForMs) { + if (rotateForMs > 0){ + if(xClockwiseTaskHandle){ + vTaskDelete(xClockwiseTaskHandle); + } xTaskCreate(leftMotorTask, "LeftMotor", 4096, (void*)rotateForMs, 10, &xClockwiseTaskHandle); } else { Motion::left.setSpeed(LEFT_MOTOR_DUTY); @@ -67,16 +111,35 @@ void Motion::rotateClockwise(uint32_t rotateForMs) { }; void Motion::rightMotorTask(void * args) { + uint32_t runtime = (uint32_t)args; + if(xMoveTaskHandle){ + vTaskDelete(xMoveTaskHandle); + xMoveTaskHandle = NULL; + } + if(xClockwiseTaskHandle){ + vTaskDelete(xClockwiseTaskHandle); + xClockwiseTaskHandle = NULL; + } Motion::right.setSpeed(RIGHT_MOTOR_DUTY); Motion::left.setSpeed(0); - vTaskDelay((uint32_t) args / portTICK_PERIOD_MS); - Motion::right.setSpeed(0); - vTaskDelete(xAntiClockwiseTaskHandle); + while(1){ + if(runtime>40||runtime==0){ + vTaskDelayUntil(&xLastWakeTime,40); + runtime -= 40; + } else { + vTaskDelayUntil(&xLastWakeTime,runtime); + Motion::right.setSpeed(0); + vTaskDelete(xAntiClockwiseTaskHandle); + } + } }; // Rotate anticlockwise for a certain amount of time. void Motion::rotateAntiClockwise(uint32_t rotateForMs) { if(rotateForMs > 0){ + if(xAntiClockwiseTaskHandle){ + vTaskDelete(xAntiClockwiseTaskHandle); + } xTaskCreate(rightMotorTask, "RightMotor", 4096, (void*)rotateForMs, 10, &xAntiClockwiseTaskHandle); } else { Motion::right.setSpeed(RIGHT_MOTOR_DUTY); @@ -85,9 +148,19 @@ void Motion::rotateAntiClockwise(uint32_t rotateForMs) { }; void Motion::stop(void){ + if(xMoveTaskHandle){ + vTaskDelete(xMoveTaskHandle); + xMoveTaskHandle = NULL; + } + if(xAntiClockwiseTaskHandle){ + vTaskDelete(xAntiClockwiseTaskHandle); + xAntiClockwiseTaskHandle = NULL; + } + if(xClockwiseTaskHandle){ + vTaskDelete(xClockwiseTaskHandle); + xClockwiseTaskHandle = NULL; + } 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 f252042..30dffff 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -52,12 +52,18 @@ protected: static void moveTask(void * args); static void leftMotorTask(void * args); static void rightMotorTask(void * args); + static inline TaskHandle_t xMoveTaskHandle = NULL; + static inline TaskHandle_t xClockwiseTaskHandle = NULL; + static inline TaskHandle_t xAntiClockwiseTaskHandle = NULL; + static inline TickType_t xLastWakeTime; 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; + + //MotionDetection instance, for motion Correction and user (access with dezibot.motion.detection) + static inline MotionDetection detection; /** * constructor gets a pointer to the motiondetection class, which enables correction of the motion From b1bb29aa1c18f2adddef83c1e63a81265d1d6147 Mon Sep 17 00:00:00 2001 From: hhaupt Date: Fri, 14 Jun 2024 02:06:37 +0200 Subject: [PATCH 11/12] added basic motioncorrection for the move function --- src/motion/Motion.cpp | 49 +++++++++++++++++++++++++++++++++++++------ src/motion/Motion.h | 36 +++++++++++++++++++++---------- 2 files changed, 68 insertions(+), 17 deletions(-) diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp index 4b4f513..2448f04 100644 --- a/src/motion/Motion.cpp +++ b/src/motion/Motion.cpp @@ -40,6 +40,40 @@ void Motion::moveTask(void * args) { runtime -= 40; //calc new parameters //set new parameters + int fifocount = detection.getDataFromFIFO(buffer); + int rightCounter = 0; + int leftCounter = 0; + int changerate = 0; + for(int i = 0;icorrectionThreshold){ + rightCounter++; + } else if(buffer[i].gyro.z<-correctionThreshold){ + leftCounter++; + } + } + int difference = abs(leftCounter-rightCounter); + if (difference>25){ + changerate = 200; + } else if(difference>20){ + changerate = 100; + } else if(difference >15){ + changerate = 50; + } else if(difference > 10){ + changerate = 20; + } else{ + changerate = 5; + } + + if(leftCounter>rightCounter){ //rotates anticlock + LEFT_MOTOR_DUTY+=changerate; + RIGHT_MOTOR_DUTY-=changerate; + } else if(leftCounter 0){ if(xClockwiseTaskHandle){ vTaskDelete(xClockwiseTaskHandle); @@ -135,7 +170,9 @@ void Motion::rightMotorTask(void * args) { }; // Rotate anticlockwise for a certain amount of time. -void Motion::rotateAntiClockwise(uint32_t rotateForMs) { +void Motion::rotateAntiClockwise(uint32_t rotateForMs,uint baseValue) { + LEFT_MOTOR_DUTY = baseValue; + RIGHT_MOTOR_DUTY = baseValue; if(rotateForMs > 0){ if(xAntiClockwiseTaskHandle){ vTaskDelete(xAntiClockwiseTaskHandle); diff --git a/src/motion/Motion.h b/src/motion/Motion.h index 30dffff..d0a6eb2 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -45,8 +45,8 @@ 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 inline uint16_t RIGHT_MOTOR_DUTY = 3900; + static inline uint16_t LEFT_MOTOR_DUTY = 3900; static const int MOTOR_RIGHT_PIN = 11; static const int MOTOR_LEFT_PIN = 12; static void moveTask(void * args); @@ -57,6 +57,9 @@ protected: static inline TaskHandle_t xAntiClockwiseTaskHandle = NULL; static inline TickType_t xLastWakeTime; + static inline FIFO_Package* buffer = new FIFO_Package[64]; + static inline int correctionThreshold = 150; + public: //Shared Timer to sync movement static inline Motor left = Motor(MOTOR_LEFT_PIN,TIMER,CHANNEL_LEFT); @@ -65,10 +68,6 @@ public: //MotionDetection instance, for motion Correction and user (access with dezibot.motion.detection) static inline MotionDetection detection; - /** - * constructor gets a pointer to the motiondetection class, which enables correction of the motion - */ - /** * @brief Initialize the movement component. * @@ -78,23 +77,30 @@ public: /** * @brief Move forward for a certain amount of time. * Call with moveForMs 0 will start movement, that must be stopped explicit by call to stop(). + * The function applys a basic algorithm to improve the straigthness of the movement. + * Lifting the robot from the desk may corrupt the results and is not recommended. + * * @param moveForMs Representing the duration of forward moving in milliseconds. + * @param baseValue The value that is used to start with the calibrated movement. Defaults to 3900. + * If the Dezibot is not moving forward at all increasing the value may help. If the robot is just jumping up and down but not forward, try a lower value. */ - static void move(uint32_t moveForMs=0); + static void move(uint32_t moveForMs=0,uint baseValue=3900); /** * @brief Rotate clockwise for a certain amount of time. * Call with moveForMs 0 will start movement, that must be stopped explicit by call to stop(). - * @param rotateForMs Representing the duration of rotating clockwise in milliseconds. + * @param rotateForMs Representing the duration of rotating clockwise in milliseconds, or 0 to rotate until another movecmd is issued. Default is 0 + * @param baseValue The value that is used to start with the calibrated movement (not released yet, currently just the used value) */ - static void rotateClockwise(uint32_t rotateForMs=0); + static void rotateClockwise(uint32_t rotateForMs=0,uint baseValue=3900); /** * @brief Rotate anticlockwise for a certain amount of time. * Call with moveForMs 0 will start movement, that must be stopped explicit by call to stop(). - * @param rotateForMs Representing the duration of rotating anticlockwise in milliseconds. + * @param rotateForMs Representing the duration of rotating anticlockwise in milliseconds or 0 to let the robot turn until another movecommand is issued. Default is 0. + * @param baseValue The value that is used to start with the calibrated movement (not released yet, currently just the used value). */ - static void rotateAntiClockwise(uint32_t rotateForMs=0); + static void rotateAntiClockwise(uint32_t rotateForMs=0,uint baseValue=3900); /** * @brief stops any current movement, no matter if timebased or endless @@ -102,6 +108,14 @@ public: */ static void stop(void); + /** + * @brief Does the same as the move function, but this function does not apply any kind of algorithm to improve the result. + * + * @param moveForMs how many ms should the robot move, or 0 to let the robot move until another move command is mentioned, default is 0 + * @param baseValue the duty value that is used for the movement, default is 0 + */ + static void moveWithoutCorrection(uint32_t moveForMs=0, uint baseValue = 3900); + }; From 2b0cd00ddd72bbb96eb1eb72eeca63f6d386f3f5 Mon Sep 17 00:00:00 2001 From: hhaupt Date: Fri, 14 Jun 2024 02:23:12 +0200 Subject: [PATCH 12/12] cleaned Motion class --- src/motion/Motion.h | 38 ++++++++++++++++++++++++++------------ src/motion/Motor.cpp | 21 ++++++++++++++++++--- 2 files changed, 44 insertions(+), 15 deletions(-) diff --git a/src/motion/Motion.h b/src/motion/Motion.h index d0a6eb2..8bb7cdf 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -23,16 +23,30 @@ #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; -}; +#define DEFAULT_BASE_VALUE 3900 class Motor{ public: Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel); + + /** + * @brief Initializes the motor + */ void begin(void); + + /** + * @brief Set the Speed by changing the pwm. To avoid current peaks, a linear ramp-up is used. + * + * @attention it is requried at any time to use that method to access the motors or methods of the motionclass to avoid such peaks. + * + * @param duty the duty cyle that should be set, can be between 0-8192 + */ void setSpeed(uint16_t duty); + + /** + * @brief returns the currently activ speed + * + * @return current speedvalue of the motor + */ uint16_t getSpeed(void); protected: uint8_t pin; @@ -45,8 +59,8 @@ class Motor{ class Motion{ protected: static MotionDetection* imuInst; - static inline uint16_t RIGHT_MOTOR_DUTY = 3900; - static inline uint16_t LEFT_MOTOR_DUTY = 3900; + static inline uint16_t RIGHT_MOTOR_DUTY = DEFAULT_BASE_VALUE; + static inline uint16_t LEFT_MOTOR_DUTY = DEFAULT_BASE_VALUE; static const int MOTOR_RIGHT_PIN = 11; static const int MOTOR_LEFT_PIN = 12; static void moveTask(void * args); @@ -61,7 +75,7 @@ protected: static inline int correctionThreshold = 150; public: - //Shared Timer to sync movement + //Instances of the motors, so they can also be used from outside to set values for the motors directly. static inline Motor left = Motor(MOTOR_LEFT_PIN,TIMER,CHANNEL_LEFT); static inline Motor right = Motor(MOTOR_RIGHT_PIN,TIMER,CHANNEL_RIGHT); @@ -84,7 +98,7 @@ public: * @param baseValue The value that is used to start with the calibrated movement. Defaults to 3900. * If the Dezibot is not moving forward at all increasing the value may help. If the robot is just jumping up and down but not forward, try a lower value. */ - static void move(uint32_t moveForMs=0,uint baseValue=3900); + static void move(uint32_t moveForMs=0,uint baseValue=DEFAULT_BASE_VALUE); /** * @brief Rotate clockwise for a certain amount of time. @@ -92,7 +106,7 @@ public: * @param rotateForMs Representing the duration of rotating clockwise in milliseconds, or 0 to rotate until another movecmd is issued. Default is 0 * @param baseValue The value that is used to start with the calibrated movement (not released yet, currently just the used value) */ - static void rotateClockwise(uint32_t rotateForMs=0,uint baseValue=3900); + static void rotateClockwise(uint32_t rotateForMs=0,uint baseValue=DEFAULT_BASE_VALUE); /** * @brief Rotate anticlockwise for a certain amount of time. @@ -100,7 +114,7 @@ public: * @param rotateForMs Representing the duration of rotating anticlockwise in milliseconds or 0 to let the robot turn until another movecommand is issued. Default is 0. * @param baseValue The value that is used to start with the calibrated movement (not released yet, currently just the used value). */ - static void rotateAntiClockwise(uint32_t rotateForMs=0,uint baseValue=3900); + static void rotateAntiClockwise(uint32_t rotateForMs=0,uint baseValue=DEFAULT_BASE_VALUE); /** * @brief stops any current movement, no matter if timebased or endless @@ -114,7 +128,7 @@ public: * @param moveForMs how many ms should the robot move, or 0 to let the robot move until another move command is mentioned, default is 0 * @param baseValue the duty value that is used for the movement, default is 0 */ - static void moveWithoutCorrection(uint32_t moveForMs=0, uint baseValue = 3900); + static void moveWithoutCorrection(uint32_t moveForMs=0, uint baseValue = DEFAULT_BASE_VALUE); }; diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp index 4dfc3a4..0c26a58 100644 --- a/src/motion/Motor.cpp +++ b/src/motion/Motor.cpp @@ -23,9 +23,24 @@ void Motor::begin(void){ }; void Motor::setSpeed(uint16_t duty){ - this->duty = duty; - ledc_set_duty(LEDC_MODE,this->channel,duty); - ledc_update_duty(LEDC_MODE,this->channel); + + int difference = duty-this->getSpeed(); + if (difference > 0){ + for(int i = 0;iduty += difference/20; + ledc_set_duty(LEDC_MODE,this->channel,duty); + ledc_update_duty(LEDC_MODE,this->channel); + delayMicroseconds(5); + } + } else { + for(int i = 0;i>difference;i-=abs(difference/20)){ + this->duty -= abs(difference/20); + ledc_set_duty(LEDC_MODE,this->channel,duty); + ledc_update_duty(LEDC_MODE,this->channel); + delayMicroseconds(5); + } + } + }; uint16_t Motor::getSpeed(void){