M5 test

gistfile1.ino

#include "M5GFX.h"
#include "M5Unified.h"
#include <WiFi.h>
#include <FastLED.h>
#include "M5AtomicMotion.h"
#include <BasicTag.h>
#include <SparkplugNode.h>
#include <NTPClient.h>
#include <MQTT.h>
#include "WiFiUdp.h"

//Wifi client
WiFiClient net;

// MQTT variables
MQTTClient mqttClient;

// Sparkplug variables
const char* group_id = "ryans-group";  // Sparkplug group id
const char* node_id = "sparkbeest-node";  // Sparkplug node id
size_t payload_buffer_size = 1024;  // How many bytes to allocate for storing the payloads
SparkplugNodeConfig* nodeData = NULL; 
int scanCount = 0;
SparkplugNodeState nodeState;
uint64_t loop_count_var = 0;

// NTP details
WiFiUDP ntpUDP;
NTPClient timeClient(ntpUDP);

// Specify the number of RGB LEDs (25 for M5Atom Matrix).
#define NUM_LEDS 25
// Specify DATA PIN of RGB LEDs.
#define LED_DATA_PIN 27

const char *ssid = "wifinetwork";
const char *password = "wifipassword";

uint64_t pwm = 500;
FunctionalBasicTag* pwm_tag;
CRGB leds[NUM_LEDS];

M5AtomicMotion AtomicMotion;

void getServoStatus() {
    for (int ch = 0; ch < 4; ch++) {
        Serial.printf("Servo Channel %d: %d \n", ch, AtomicMotion.getServoPulse(ch));
    }
}

void startYourEngines() {
  Serial.println("Atomic Motion beginning...");
  while (!AtomicMotion.begin(&Wire, M5_ATOMIC_MOTION_I2C_ADDR, 25, 21, 100000)) {
    Serial.println("Atomic Motion begin failed");
    delay(1000);
  }
  Serial.println("Atomic Motion initiated");
}

void connectToWifi() {
  int tryDelay = 500;
  int numberOfTries = 20;
  leds[12] = CRGB::Yellow;
  // Wait for the WiFi event
  while (true) {
    switch (WiFi.status()) {
      case WL_NO_SSID_AVAIL: Serial.println("[WiFi] SSID not found"); break;
      case WL_CONNECT_FAILED:
        Serial.print("[WiFi] Failed - WiFi not connected! Reason: ");
        leds[12] = CRGB::Red;
        return;
        break;
      case WL_CONNECTION_LOST: Serial.println("[WiFi] Connection was lost"); break;
      case WL_SCAN_COMPLETED: Serial.println("[WiFi] Scan is completed"); break;
      case WL_DISCONNECTED:
        Serial.println("[WiFi] WiFi is disconnected");
        leds[12] = CRGB::Red;
        break;
      case WL_CONNECTED:
        Serial.println("[WiFi] WiFi is connected!");
        leds[12] = CRGB::Green;
        Serial.print("[WiFi] IP address: ");
        Serial.println(WiFi.localIP());
        timeClient.begin();
        timeClient.update();
        Serial.print("[NTP] Time retrieved: ");
        Serial.println(timeClient.getFormattedTime());
        Serial.println("[MQTT] Connecting to MQTT Broker...");
        connectToMQTTBroker();
        return;
        break;
      default:
        Serial.print("[WiFi] WiFi Status: ");
        Serial.println(WiFi.status());
        break;
    }
    delay(tryDelay);

    if (numberOfTries <= 0) {
      Serial.print("[WiFi] Failed to connect to WiFi!");
      leds[12] = CRGB::Red;
      // Use disconnect function to force stop trying to connect
      WiFi.disconnect();
      return;
    } else {
      numberOfTries--;
    }
  }
}

void connectToMQTTBroker() {
  while (!mqttClient.connect("sparkbeest-node")) {
    Serial.print(".");
    delay(1000);
  }
  Serial.println("[MQTT] Successfully connected!");
}

uint64_t getTimestamp() {
  return timeClient.getEpochTime() * 1000;
}

void printSparkplugNodeConfig(SparkplugNodeConfig* node) {
  // Prints relevant data from a sparkplug node
  if (node == NULL) {
    Serial.println("Provided SparkplugNodeConfig* is NULL!");
    return;
  }
  Serial.println("SparkplugNodeConfig:");

  // Print the topics
  Serial.println("Topics:");
  Serial.println(node->topics.NBIRTH);
  Serial.println(node->topics.NDEATH);
  Serial.println(node->topics.NCMD);
  Serial.println(node->topics.NDATA);
  Serial.println();

  // Print the node's tags
  Serial.print("bdSeq value: ");
  Serial.println(*(node->vars.bd_seq_tag_value));
  Serial.print("rebirth value: ");
  Serial.println(*(node->vars.rebirth_tag_value));
  Serial.print("Scan Rate value: ");
  Serial.println(*(node->vars.scan_rate_tag_value));
  Serial.println();
}

void speedChangedHandler(FunctionalBasicTag* tag) {
  Serial.println("[Sparkplug] Speed tag changed!");
}

void setupSparkplug() {
  // Initialize the sparkplug node
  nodeData = createSparkplugNode(group_id, node_id, payload_buffer_size, getTimestamp);
  if (nodeData == NULL) {
    Serial.println("FAILED TO CREATE SPARKPLUG NODE");
    return;
  }

  // Change scan rate to 5 seconds
  *(nodeData->vars.scan_rate_tag_value) = 5000;

  // Print the newly instantiated node
  printSparkplugNodeConfig(nodeData);

  // create a loop count tag
  FunctionalBasicTag* loop_count_tag = createUInt64Tag("testing/loop count", &loop_count_var, getNextAlias(), true, true);
  pwm_tag = createUInt64Tag("pwm", &pwm, getNextAlias(), true, true);
  FunctionalBasicTag* test_tag = createUInt64Tag("test", &pwm, getNextAlias(), true, true);
  
  addOnChangeCallback(pwm_tag, speedChangedHandler);
  
  // Show tags count
  Serial.print("Tag count: ");
  Serial.println(getTagsCount());

  if (loop_count_tag == NULL || pwm_tag == NULL) {
    Serial.println("[Sparkplug] FAILED TO CREATE TAGS!");
    return;
  }

  // Initiate mqtt broker connection
  spnOnMQTTConnected(nodeData);
}

bool publishMQTT(const char* topic, uint8_t* buffer, size_t buffer_length) {
  Serial.println();
  Serial.println("PUBLISH MQTT MESSAGE:");
  Serial.print("Topic: ");
  Serial.println(topic);
  mqttClient.publish(topic, (char*) buffer, buffer_length);
  return true;
}

void updateSparkplug() {
  if (nodeData == NULL) {
    Serial.println("FAILED TO START, IGNORING LOOP!");
    delay(60000);
    return;
  }

  // increment loop count
  loop_count_var++;
  scanCount++;
  
  // Call the tick node function
  nodeState = tickSparkplugNode(nodeData);

  // Check the resulting SparkplugNodeState and take appropriate action
  switch(nodeState) {
    case spn_ERROR_NODE_NULL:
      scanCount -= 1;
      Serial.println("[Sparkplug] FAILED TO START, IGNORING LOOP!");
      delay(60000);
      return;
    case spn_SCAN_NOT_DUE:
      scanCount -= 1;
      return;
    case spn_SCAN_FAILED:
      scanCount -= 1;
      Serial.println("[Sparkplug] ERROR: SCAN TAGS FAILED");
      return;
    case spn_MAKE_NBIRTH_FAILED:
      Serial.println("[Sparkplug] ERROR: FAILED TO MAKE NBIRTH PAYLOAD");
      break;
    case spn_NBIRTH_PL_READY:
      Serial.println("[Sparkplug] PUBLISHING NBIRTH");
      if (publishMQTT(nodeData->mqtt_message.topic, nodeData->mqtt_message.payload->buffer, nodeData->mqtt_message.payload->written_length)) {
        spnOnPublishNBIRTH(nodeData);
      }
      break;
    case spn_PROCESS_NCMD_FAILED:
      Serial.println("[Sparkplug] NCMD PROCESS FAILED");
      break;
    case spn_PROCESS_NCMD_SUCCESS:
      Serial.println("[Sparkplug] NCMD PROCESS SUCCEEDED");
      break;
    case spn_VALUES_UNCHANGED:
      Serial.println("[Sparkplug] Skipping, values unchanged.");
      break;
    case spn_MAKE_NDATA_FAILED:
      Serial.println("[Sparkplug] ERROR: FAILED TO MAKE NDATA PAYLOAD");
      break;
    case spn_NDATA_PL_READY:
      Serial.println("[Sparkplug] PUBLISHING NDATA");
      if (publishMQTT(nodeData->mqtt_message.topic, nodeData->mqtt_message.payload->buffer, nodeData->mqtt_message.payload->written_length)) {
        spnOnPublishNDATA(nodeData);
      }
      break;
    default:
      scanCount -= 1;
      Serial.print("[Sparkplug] ERROR: Unknown State returned: ");
      int nodeStateInt = (int)nodeState;
      Serial.print("[Sparkplug]");
      Serial.println(nodeStateInt);
      return;
  }
  
  // Periodically simulate a rebirth command
  if (scanCount == 5) {
    Serial.println("[Sparkplug] Rebirting...");
    // reset scanCount
    scanCount = 0;
    *(nodeData->vars.rebirth_tag_value) = true;
  }
}

void setup() {
  Serial.println("[Board] Starting up...");
  auto cfg = M5.config();
  M5.begin(cfg);

  FastLED.addLeds<WS2812, LED_DATA_PIN, GRB>(leds, NUM_LEDS);
  FastLED.setBrightness(20);
  delay(50);
  
  Serial.begin(115200);
  delay(100);

  Serial.println();
  WiFi.begin(ssid, password);
  mqttClient.begin("192.168.178.55", net);

  Serial.println();
  Serial.print("[WiFi] Connecting to ");
  Serial.print(ssid);
  Serial.println("...");
  connectToWifi();

  Serial.println();
  Serial.print("[Motion] Starting motion...");
  startYourEngines();

  Serial.println();
  Serial.println("[Sparkplug] Initiating Sparkplug...");
  setupSparkplug();
}


void loop() {
  // Generic components updates
  // Update the board for button presses and such
  M5.update();
  // Update the known time to ensure Sparkplug stuff is synced
  timeClient.update();
  // Loop the MQTT Client to check subs/pubs
  mqttClient.loop();
  // Update Sparkplug to ensure sparkpluggy stuff is working
  updateSparkplug();

  // Update the currently displayed status LED
  leds[12] = CRGB::Black; // set LED to black
  FastLED.show(); // show LEDs (lights off because they are black)
  
  // @TODO: Movement! 
  // AtomicMotion.setServoPulse(1, pwm);
  // AtomicMotion.setServoPulse(3, pwm);

  if (M5.BtnA.wasPressed()) {
    pwm += 100;
    // Disconnect from WiFi
    leds[12] = CRGB::Red;
    Serial.println("[WiFi] Disconnecting from WiFi!");
    // This function will disconnect and turn off the WiFi (NVS WiFi data is kept)
    if (WiFi.disconnect(false, false)) {
      Serial.println("[WiFi] Disconnected from WiFi!");
      connectToWifi();
    }
    getServoStatus();
    for (int ch = 0; ch < 4; ch++) {
    // AtomicMotion.setServoPulse(ch, pwm);
      Serial.printf("Servo Channel %d: %d \n", ch, AtomicMotion.getServoPulse(ch));
    }
  }

}