Merge pull request #3 from RoboticsBrno/mpu

Mpu + OLED

Author: Tasssadar

.gitignore

@@ -39,5 +39,6 @@
 
 # Doxygen export
 doc
+
 .pio
 src/main.cpp

Makefile

@@ -1,3 +1,6 @@
 all:
 	pio ci examples/logger/main.cpp  --lib src --project-conf platformio.ini
-	pio ci examples/motors/main.cpp  --lib src --project-conf platformio.ini
+	pio ci examples/motors/main.cpp  --lib src --project-conf platformio.ini
+
+format:
+	clang-format -i src/*.h src/*.cpp

examples/mpu/main.cpp

@@ -0,0 +1,38 @@
+#include "RBCX.h"
+#include <Arduino.h>
+#include <string>
+
+void clear() {
+    rb::Manager::get().oled().fill(rb::Oled::Black);
+    rb::Manager::get().oled().updateScreen();
+}
+
+void waitToNextTest() {
+    delay(3000);
+    clear();
+}
+
+void setup() {
+    printf("RB3204-RBCX\n");
+
+    delay(500);
+
+    printf("Init manager\n");
+    auto& man = rb::Manager::get();
+    man.install();
+
+    man.leds().red(true);
+
+    auto& mpu = rb::Manager::get().mpu();
+
+    mpu.init();
+    mpu.sendStart();
+
+    while (true) {
+        printf("MPU - angle: X: %2.2f Y: %2.2f Z: %2.2f\n", mpu.getAngleX(), mpu.getAngleY(), mpu.getAngleZ());
+        delay(100);
+    }
+
+}
+
+void loop() {}

examples/oled/main.cpp

@@ -0,0 +1,98 @@
+#include <Arduino.h>
+#include "RBCX.h"
+
+void clear(){
+    rb::Manager::get().oled().fill(rb::Oled::Black);
+    rb::Manager::get().oled().updateScreen();
+}
+
+void waitToNextTest() {
+    delay(3000);
+    clear();
+}
+
+void setup() {
+    printf("RB3204-RBCX\n");
+
+    delay(500);
+
+    printf("Init manager\n");
+    auto& man = rb::Manager::get();
+    man.install();
+
+    man.leds().red(true);
+    
+    auto& oled = rb::Manager::get().oled();
+
+    oled.init(rb::Oled::Oled_128x64, true, false);
+    // oled.init(rb::Oled::Oled_128x32, true, false);
+
+    while (true) {
+        ///////// fill ////////////////////////
+        printf("fill\n");
+        oled.fill(rb::Oled::White);
+        oled.updateScreen();
+        delay(1000);
+
+        oled.fill(rb::Oled::Black);
+        oled.updateScreen();
+        delay(1000);
+
+
+        ///////// drawPixel ////////////////////////
+        printf("drawPixel\n");
+        for(int i = 0; i<500; i++) {
+            oled.drawPixel(random(0,oled.getWidth()), random(0, oled.getHeight()), rb::Oled::White);
+        }
+        oled.updateScreen();
+        waitToNextTest();
+
+
+        ///////// setCursor ////////////////////////
+        ///////// writeString ////////////////////////
+        printf("setCursor + writeString\n");
+        oled.setCursor(45, 0);
+        oled.writeString("OLED", rb::Oled::Font_11x18, rb::Oled::White);
+        
+        oled.setCursor(5, oled.getHeight()/2);
+        String text = "OLED w:" + String(oled.getWidth()) + " | h:" + String(oled.getHeight());
+        oled.writeString(text, rb::Oled::Font_7x10, rb::Oled::White);
+        oled.updateScreen();        
+        waitToNextTest();
+
+
+        ///////// drawLine ////////////////////////
+        printf("drawLine\n");
+        oled.drawLine(0, 0, oled.getWidth(), oled.getHeight(), rb::Oled::White);
+        oled.drawLine(0, oled.getHeight(), oled.getWidth(), 0, rb::Oled::White);
+        oled.updateScreen();
+        waitToNextTest();
+
+
+        ///////// drawArc ////////////////////////
+        printf("drawArc\n");
+        oled.drawArc(oled.getWidth()/2, oled.getHeight()/2, 15, 20, 270, rb::Oled::White);
+        oled.updateScreen();
+        waitToNextTest();
+
+
+        ///////// drawCircle ////////////////////////
+        printf("drawCircle\n");
+        for(uint32_t delta = 0; delta < 5; delta ++) {
+            oled.drawCircle(20* delta+30, 15, 10, rb::Oled::White);
+        }
+        oled.updateScreen();
+        waitToNextTest();
+
+        ///////// drawRectangle ////////////////////////
+        printf("drawRectangle\n");
+        for(int i = 5; i< oled.getWidth(); i+=5) {
+            oled.drawRectangle(i, 0, i+i, oled.getHeight()-1, rb::Oled::White);
+        }
+        oled.updateScreen();      
+        waitToNextTest();
+
+    }
+}
+
+void loop() {}

library.json

@@ -35,7 +35,7 @@
       "version": "https://github.com/RoboticsBrno/Esp32-lx16a/archive/refs/tags/v1.2.1.zip"
     }
   ],
-  "version": "1.4.1",
+  "version": "1.5.0",
   "frameworks": ["espidf", "arduino"],
   "platforms": "espressif32",
   "build": {

src/RBCXManager.cpp

@@ -155,6 +155,12 @@ void Manager::consumerRoutine() {
         case CoprocStat_stupidServoStat_tag:
             // Ignore
             break;
+        case CoprocStat_mpuStat_tag:
+            m_mpu.setState(msg.payload.mpuStat);
+            break;
+        case CoprocStat_faultStat_tag:
+            fault(msg.payload.faultStat);
+            break;
         default:
             printf("Received message of unknown type from stm32: %d\n",
                 msg.which_payload);
@@ -261,6 +267,21 @@ bool Manager::printTasksDebugInfo() {
 }
 #endif
 
+void Manager::fault(CoprocStat_FaultStat faultStat) {
+    switch (faultStat.which_fault) {
+    case CoprocStat_FaultStat_oledFault_tag:
+        ESP_LOGE(TAG, "Oled not connected");
+        break;
+
+    case CoprocStat_FaultStat_mpuFault_tag:
+        ESP_LOGE(TAG, "MPU6050 not connected");
+        break;
+
+    default:
+        ESP_LOGE(TAG, "CoprocStat - non specific error");
+    }
+}
+
 MotorChangeBuilder::MotorChangeBuilder() {}
 
 MotorChangeBuilder::MotorChangeBuilder(MotorChangeBuilder&& o)

src/RBCXManager.h

@@ -1,8 +1,10 @@
 #pragma once
 
+// clang-format off
 #include <freertos/FreeRTOS.h>
 #include <freertos/queue.h>
 #include "freertos/semphr.h"
+// clang-format on
 #include <functional>
 #include <list>
 #include <memory>
@@ -13,6 +15,8 @@
 #include "RBCXButtons.h"
 #include "RBCXLeds.h"
 #include "RBCXMotor.h"
+#include "RBCXMpu.h"
+#include "RBCXOled.h"
 #include "RBCXPiezo.h"
 #include "RBCXStupidServo.h"
 #include "RBCXTimers.h"
@@ -48,7 +52,7 @@ inline ManagerInstallFlags operator|(
 }
 
 // Periodically print info about all RBCX tasks to the console
-//#define RB_DEBUG_MONITOR_TASKS 1
+// #define RB_DEBUG_MONITOR_TASKS 1
 
 /**
  * \brief The main library class for working with the RBCX board.
@@ -99,6 +103,9 @@ class Manager {
 
     StupidServo& stupidServo(uint8_t index);
 
+    Oled& oled() { return m_oled; } //!< Get the {@link Piezo} controller
+    Mpu& mpu() { return m_mpu; } //!< Get the {@link Piezo} controller
+
     // Pass to Esp32-lx16a library's begin()
     SmartServoBusBackend &smartServoBusBackend();
 
@@ -159,6 +166,7 @@ class Manager {
     std::vector<TaskHandle_t> m_tasks;
     std::mutex m_tasks_mutex;
 #endif
+    void fault(CoprocStat_FaultStat faultStat);
 
     TaskHandle_t m_keepaliveTask;
 
@@ -174,6 +182,8 @@ class Manager {
     TickType_t m_motors_last_set;
     Motor m_motors[size_t(MotorId::MAX)];
 
+    rb::Oled m_oled;
+    rb::Mpu m_mpu;
     rb::Piezo m_piezo;
     rb::Leds m_leds;
     rb::Buttons m_buttons;