Tmx unification for ros2

.github/workflows/build.yaml

@@ -3,8 +3,6 @@ name: Arduino Compile
 on:
     push:
     pull_request:
-        branches:
-            - main
 
 jobs:
     build:
@@ -16,7 +14,11 @@ jobs:
                     "robotdyn_blackpill_f303cc", 
                     "nanoatmega328",
                     "adafruit_itsybitsy_m4",
-                    "program_via_AVRISP_mkII"
+                    "program_via_AVRISP_mkII",
+                    "robotdyn_blackpill_f103",
+                    "pico",
+                    "esp32doit-devkit-v1",
+                    "pico2w"
                   ]
             fail-fast: false
 

README.md

@@ -156,7 +156,7 @@ Receive:
 To read a digital/analog pin, you need to send a command to set the pin mode and then, on-change, the data will be sent. It's possible to switch modes during operation, even input <-> output. Just send a new ```SET_PIN_MODE``` message.
 
 #### Analog
-The pin must be in range of [0..5] (pico: 26+adc_pin == gpio_pin, 4==temp_sensor). differential is the minimal change for the value or it is not reported.
+The pin must be in range of [0..MAX_PINS], check before that it's an analog pin. differential is the minimal change for the value or it is not reported.
 
 Differential is ```uint16_t```, with ```diff_high = diff>>8``` and ```diff_low = diff&0xFF```. 
 
@@ -205,7 +205,7 @@ If you only want to en/disable a single pin or type, send this message.
 modify_type can be any of
 - REPORTING_DISABLE_ALL = 0. Disable analog and digital inputs reporting. Set pin to 0.
 - REPORTING_ANALOG_ENABLE = 1. Enable a single analog pin. Needs to have been set as analog pin otherwise UB.
-- REPORTING_ANALOG_DISABLE = 3. Disable a single analog pin. Pin in range ```[0..5]```
+- REPORTING_ANALOG_DISABLE = 3. Disable a single analog pin. Pin in range ```[0..MAX_PINS]```
 - REPORTING_DIGITAL_ENABLE = 2. Enable a single digital pin. Needs to have been set as digital input, otherwise UB.
 - REPORTING_DIGITAL_DISABLE = 4. Disable a single digital pin.
 

include/boards/esp32devkit.hpp

@@ -1,13 +1,17 @@
 #pragma once
 
 #if defined(ARDUINO_ARCH_ESP32)
-
 #include <Arduino.h>
+#include <Wire.h>
 // This board does not have a normal list of analog pins
-// this shifts the analog pins
 #define A1 2047
 #define A2 2047
 #define A8 2047
 #define A9 2047
+void hw_init();
 
+#define SECOND_I2C_PORT 1
+#define SECOND_I2C_PORT_SDA 16
+#define SECOND_I2C_PORT_SCL 17
+extern TwoWire Wire2; // Use GPIO 16 and 17 for I2C on ESP32
 #endif

include/boards/itsybitsy_m4.hpp

@@ -18,4 +18,5 @@ const auto A18 = 2047;
 const auto A19 = 2047;
 
 const auto MAX_SERVOS = 0;
+void hw_init();
 #endif

include/boards/nanoatmega.hpp

@@ -15,5 +15,6 @@ const auto A16 = 2047;
 const auto A17 = 2047;
 const auto A18 = 2047;
 const auto A19 = 2047;
-const uint8_t MAX_SERVOS = 4; // PWM pins on Nano
+#define MAX_SERVOS 4 // PWM pins on Nano
+void hw_init();
 #endif

include/boards/pico.hpp

@@ -0,0 +1,42 @@
+#pragma once
+#include <Arduino.h>
+
+// Maybe some more are required, for pico2 and pico1w
+#if defined(RASPBERRY_PI_PICO) || defined(ARDUINO_RASPBERRY_PI_PICO_W) ||      \
+    defined(ARDUINO_ARCH_RP2040)
+
+#include <Wire.h>
+#include <hardware/i2c.h>
+
+const auto A4 = 2047;
+const auto A5 = 2047;
+const auto A6 = 2047;
+const auto A7 = 2047;
+const auto A8 = 2047;
+const auto A9 = 2047;
+const auto A10 = 2047;
+const auto A11 = 2047;
+const auto A12 = 2047;
+const auto A13 = 2047;
+const auto A14 = 2047;
+const auto A15 = 2047;
+const auto A16 = 2047;
+const auto A17 = 2047;
+const auto A18 = 2047;
+const auto A19 = 2047;
+#define SECOND_I2C_PORT 1
+// Change the pins to match SDA and SCL for your board
+#define SECOND_I2C_PORT_SDA 10
+#define SECOND_I2C_PORT_SCL 11
+#if !defined(ARDUINO_RASPBERRY_PI_PICO_2W)
+extern TwoWire Wire2;
+
+#define MAX_SERVOS 12 // according to the servo lib
+#else
+#define MAX_SERVOS 0
+extern TwoWire Wire2; // Use GPIO 10 and 11 for I2C on Pico 2W
+#endif
+#define I2C_COUNT 2
+#define MAX_SONARS 6 // the current library does not work somehow on pico
+void hw_init();
+#endif

include/boards/stm32blackpill.hpp

@@ -12,7 +12,8 @@ const auto A16 = 2047;
 const auto A17 = 2047;
 const auto A18 = 2047;
 const auto A19 = 2047;
-const auto MAX_SERVOS = 8; // PWM pins on BlackPill
+const auto MAX_SERVOS = 8;
+void hw_init();
 #endif
 
 #if defined(ARDUINO_ARCH_STM32) && defined(ARDUINO_BLACKPILL_F303CC)
@@ -21,5 +22,6 @@ const auto A16 = 2047;
 const auto A17 = 2047;
 const auto A18 = 2047;
 const auto A19 = 2047;
-const auto MAX_SERVOS = 8; // PWM pins on BlackPill
+const auto MAX_SERVOS = 8;
+void hw_init();
 #endif

include/commands.hpp

@@ -55,7 +55,9 @@ enum MESSAGE_OUT_TYPE : uint8_t {
   ENCODER_REPORT = 14,
   DEBUG_PRINT = 99,
   SENSOR_REPORT = 20,
+  SENSOR_MAIN_REPORT = 21,
   PONG_REPORT = 32,
+  MODULE_MAIN_REPORT = 33,
   MODULE_REPORT = 34,
   MAX_OUT_MESSAGE_TYPE
 };

include/config.hpp

@@ -1,9 +1,17 @@
 #pragma once
-
+#include "boards/esp32devkit.hpp"
+#include "boards/itsybitsy_m4.hpp"
+#include "boards/nanoatmega.hpp"
+#include "boards/pico.hpp"
+#include "boards/stm32blackpill.hpp"
 #ifndef MAX_SONARS
 #define MAX_SONARS 6
 #endif
 
 #ifndef ENABLE_ADAFRUIT_WATCHDOG
 #define ENABLE_ADAFRUIT_WATCHDOG 1
+#endif
+
+#ifndef I2C_COUNT
+#define I2C_COUNT 1
 #endif

include/i2c.hpp

@@ -1,5 +1,6 @@
 #pragma once
-
+#include <Wire.h>
+#include <config.hpp>
 void i2c_begin();
 void i2c_read();
 void i2c_write();
@@ -44,3 +45,10 @@ void i2c_write();
 
 // Indicator that no i2c register is being specified in the command
 #define I2C_NO_REGISTER 254
+
+extern TwoWire *i2c_buses[I2C_COUNT];
+
+bool write_i2c(int i2c_port, int device_address, const uint8_t *data,
+               size_t length);
+bool read_i2c(int i2c_port, int device_address, const uint8_t *registers,
+              size_t register_length, uint8_t *data, size_t data_length);

include/main.hpp

@@ -1,9 +1,5 @@
 #pragma once
 
-#include "boards/esp32devkit.hpp"
-#include "boards/itsybitsy_m4.hpp"
-#include "boards/nanoatmega.hpp"
-#include "boards/stm32blackpill.hpp"
 #include <Arduino.h>
 void get_unique_id();
 
@@ -48,6 +44,11 @@ void ping();
 
 void feature_detection();
 
+void send_debug_info(byte id, int value);
+
+void module_new();
+void module_data();
+void sensor_new();
 enum PIN_MODES : uint8_t {
   NOT_SET = 255,
   INPUT_MODE = 0,
@@ -59,3 +60,5 @@ enum PIN_MODES : uint8_t {
   SONAR_MODE = 7,
   DHT_MODE = 8
 };
+
+void send_message(const uint8_t *message, size_t length);

include/modules.hpp

@@ -0,0 +1,51 @@
+#pragma once
+#include <Arduino.h>
+#ifndef MAX_MODULES_COUNT
+#define MAX_MODULES_COUNT 4 // Max number of modules that can be added
+#endif                      // MAX_MODULES_COUNT
+
+#if MAX_MODULES_COUNT > 0
+/***********************************************/
+/***************MODULES*************************/
+void module_new_i(uint8_t command_buffer[], size_t packet_size);
+
+void module_data_i(uint8_t command_buffer[], size_t packet_size);
+
+enum MODULE_TYPES : uint8_t { // Max 255 modules, but will always fit in a
+                              // single byte!
+  PCA9685 = 0,                // 16x 12bit PWM
+  HIWONDER_SERVO = 1,
+  SHUTDOWN_RELAY = 2,
+  TMX_SSD1306 = 3,
+  MAX_MODULES
+};
+
+class Module {
+public:
+  virtual void readModule() = 0;
+  virtual void writeModule(uint8_t data[], size_t size) = 0;
+  virtual void resetModule() = 0;
+  bool stop = false;
+  void publishData(const uint8_t data[], size_t size);
+
+  int num = 0;
+  MODULE_TYPES type = MODULE_TYPES::MAX_MODULES;
+  // called at every loop, only used when needed (Oled update)
+  virtual void updModule(){};
+};
+void scan_modules();
+void upd_modules();
+extern Module *modules[MAX_MODULES_COUNT]; // Array of pointers to modules
+extern size_t module_count;                // Number of modules in the array
+
+#include "modules/ssd1306.hpp" // Include the SSD1306 module header
+
+#else
+
+// No modules supported, so we define empty functions
+inline void module_new_i(uint8_t command_buffer[], size_t packet_size) {}
+inline void module_data_i(uint8_t command_buffer[], size_t packet_size) {}
+inline void scan_modules() {}
+inline void upd_modules() {}
+
+#endif

include/modules/ssd1306.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <main.hpp>
+#include <modules.hpp>
+class TmxSSD1306 : public Module {
+public:
+  TmxSSD1306(uint8_t data[], size_t packet_size) {
+    num = data[0];
+    type = MODULE_TYPES::TMX_SSD1306;
+    // TODO: this is just for testing the module system.
+  }
+  void readModule() override {
+    // probably nothing to read
+  }
+  void writeModule(uint8_t data[], size_t size) override {
+    // // TODO: implement writing to the SSD1306 display
+    // send_debug_info(30, size);
+    // send_debug_info(31, data[0]);
+    if (data[0] == 1) { // test reply on the "booting..." text commands.
+      uint8_t display_data[32] = {1, 10, 10, 1};
+
+      this->publishData(display_data, 4);
+    }
+  }
+  void resetModule() override {}
+  void updModule() override {
+    // MAYBE: write a few bytes to the display, or not, depends on lib.
+  }
+};

include/sensors.hpp

@@ -0,0 +1,50 @@
+#pragma once
+#include <Arduino.h>
+#ifndef MAX_SENSORS_COUNT
+#define MAX_SENSORS_COUNT 4 // Max number of modules that can be added
+#endif                      // MAX_MODULES_COUNT
+
+#if MAX_SENSORS_COUNT > 0
+/*****************************************************************************/
+/****SENSORS*/
+/*****************************************************************************/
+
+enum SENSOR_TYPES : uint8_t { // Max 255 sensors, but will always fit in a
+                              // single byte!
+  GPS = 0,
+  LOAD_CELL = 1,
+  MPU_9250 = 2,
+  TOF_VL53 = 3,
+  VEML6040 = 4, // Color sensor
+  ADXL345 = 5,  // 3 axis accel
+  INA226a = 6,
+  HMC5883l = 7,
+  AS5600_t = 8, // Magnetic angle sensor
+  MAX_SENSORS
+};
+
+class Sensor {
+public:
+  virtual void readSensor() = 0;
+  virtual void resetSensor() = 0;
+  bool stop = false;
+  void writeSensorData(const uint8_t data[], size_t size);
+  static Sensor *create(uint8_t *data, size_t size) {
+    // This function is used to create a sensor object based on the type
+    // and data provided. It should be overridden in derived classes.
+    return nullptr;
+  }
+  int num;
+  SENSOR_TYPES type = SENSOR_TYPES::MAX_SENSORS;
+};
+
+const int SENSORS_MAX_SETTINGS_A = 6;
+void sensor_new_i(uint8_t command_buffer[], size_t packet_size);
+void readSensors();
+
+#else
+
+// No sensors supported, so we define empty functions
+void sensor_new_i(uint8_t command_buffer[], size_t packet_size);
+void readSensors();
+#endif