WioLTE_KeiganMotor_MQTT_ctrl.ino

/*
 * Copyright (c) 2021 Kohei "Max" MATSUSHITA (ma2shita+git@ma2shita.jp)
 * Released under the MIT license
 * https://opensource.org/licenses/mit-license.php
*/

#define __VERSION__ "0.999"

#define SerialMon SerialUSB
#define LOOP_INTERVAL_MS (100)
#define PING_INTERVAL_MS (30000)

#include <KM1_I2C.h> /* more than 2.0.2 (2.0.2 is not work) */
KeiganMotor Motor(0x20);

#include <WioLTEforArduino.h>
WioLTE Wio;
#include <WioLTEClient.h>
WioLTEClient WioClient(&Wio);

#include <PubSubClient.h>
PubSubClient MqttClient;
struct MqttParams {
  char id[64];
  char report_topic[128];
  char command_topic[128];
};
struct MqttParams mqtt_params;

#include <ArduinoJson.h>

#include <map>

// Required global instances: WioLTE Wio
void mcu_restart() {
  delay(5000); // waiting for flush of serial buffer
  Wio.SystemReset();
}

// Required global instances: KeiganMotor Motor
void km_readMotorMeasurement(float &degree, int32_t &rpm) {
  Motor.readMotorMeasurement();
  degree = fabs(fmod(Motor.degree, 360));
  rpm = (int32_t) Motor.rpm;
}

// Required global instances: KeiganMotor Motor
void km_moveByDistanceDegree(float degree, float rpm) {
  Motor.speedRpm(rpm);
  Motor.moveByDistanceDegree(degree);
}

// Required global instances: KeiganMotor Motor
void km_reboot() {
  Motor.begin();
  Motor.reboot();
  delay(3000);
  Motor.enable();
}

// Required global instances: PubSubClient MqttClient, MqttParams mqtt_params
void reportMotorMeasurement() {
  float degree;
  int32_t rpm;
  km_readMotorMeasurement(degree, rpm);
  unsigned long uptime_s = millis() / 1000;
  
  StaticJsonDocument<192> doc;
  JsonObject state = doc.createNestedObject("state");
  JsonObject state_reported = state.createNestedObject("reported");
  state_reported["degree"] = degree;
  state_reported["rpm"] = rpm;
  JsonObject state_reported__mcu = state_reported.createNestedObject("_mcu");
  state_reported__mcu["uptime_s"] = uptime_s;
  state_reported__mcu["fimware_version"] = __VERSION__;
  state["desired"] = nullptr;
  
  char payload[256];
  serializeJson(doc, payload);
  SerialMon.println(payload);
  MqttClient.publish(mqtt_params.report_topic, payload);
}

volatile bool interval_trigger_flag;
HardwareTimer Timer2(TIM2);
void timer2_callback() {
  interval_trigger_flag = true;
}

// Required global instances: SerialMon
void mqtt_subscribe_callback(char* topic, byte* payload, unsigned int length) {
  String buf_t = String(topic);
  SerialMon.print("Incoming: "); SerialMon.println(buf_t);
  payload[length] = '\0'; // https://hawksnowlog.blogspot.com/2017/06/convert-byte-array-to-string.html
  String buf_p = String((char*) payload);
  SerialMon.print("Payload: "); SerialMon.println(buf_p);
  
  DynamicJsonDocument doc(128);
  DeserializationError error = deserializeJson(doc, buf_p);
  if (error) {
    SerialMon.print(F("deserializeJson() failed: "));
    SerialMon.println(error.f_str());
    return;
  }
  
  std::map<std::string, int> cmd;
  cmd["readMotorMeasurement"] = 1;
  cmd["moveByDistanceDegree"] = 2;
  cmd["reboot"] = 65535;
  
  switch(cmd[doc["method"]]) {
    case 1:
      reportMotorMeasurement();
      break;
    case 2:
      km_moveByDistanceDegree(doc["degree"], doc["rpm"]);
      break;
    case 65535:
      km_reboot();
      delay(5000);
      mcu_restart();
      break;
    default:
    SerialMon.println(F("Ignored."));
      break;
  }
}

// Required global instances: WioLTE Wio, SerialMon
void connect() {
  SerialMon.println("Wio.PowerSupplyLTE");
  Wio.PowerSupplyLTE(true);
  delay(500);
  SerialMon.print("Wio.TurnOnOrReset: ");
  if (!Wio.TurnOnOrReset()) {
    SerialMon.println("failed.");
    mcu_restart();
  }
  SerialMon.println("done.");
  SerialMon.print("Wio.Activate: ");
  if (!Wio.Activate("soracom.io", "sora", "sora")) {
    SerialMon.println("failed.");
    mcu_restart();
  }
  SerialMon.println("done.");
}

// Required global instances: WioLTE Wio, MqttParams mqtt_params, SerialMon
void set_mqtt_id_and_topics() {
  char imsi[16];
  Wio.GetIMSI(imsi, sizeof(imsi));
  
  char buf[10];
  strncpy(buf, imsi+9, 6); // Substr 6 char from tail
  buf[6] = '\0';
  
  sprintf(mqtt_params.id, "wiolte-%s", buf);
  SerialMon.print("MQTT_ID: "); SerialMon.println(mqtt_params.id);
  sprintf(mqtt_params.report_topic, "%s/report", mqtt_params.id);
  SerialMon.print("Report to: "); SerialMon.println(mqtt_params.report_topic);
  sprintf(mqtt_params.command_topic, "%s/command", mqtt_params.id);
  SerialMon.print("Command from: "); SerialMon.println(mqtt_params.command_topic);
}

// Required global instances: PubSubClient MqttClient, MqttParams mqtt_params, WioLTEClient WioClient, SerialMon
void mqtt_connect_and_subscribe() {
  SerialMon.print("MqttClient.connect: ");
  MqttClient.setServer("beam.soracom.io", 1883);
  MqttClient.setClient(WioClient);
  MqttClient.setBufferSize(512);
  MqttClient.setCallback(mqtt_subscribe_callback);
  if (!MqttClient.connect(mqtt_params.id)) {
    SerialMon.println(MqttClient.state());
    mcu_restart();
  }
  SerialMon.println("done.");
  MqttClient.subscribe(mqtt_params.command_topic);
} 

// Required global instances: WioLTE Wio, HardwareTimer Timer3, SerialMon
void setup() {
  delay(1000);
  
  SerialMon.println("");
  SerialMon.println("--- START ---------------------------------------------------");
  SerialMon.println(__VERSION__);
  
  SerialMon.println("### I/O Initialize.");
  Wio.Init();
  SerialMon.println("### Power supply ON.");
  Wio.PowerSupplyGrove(true);
  delay(500);
  SerialMon.println("### Wio Setup completed.");
  
  km_reboot();
  connect();
  set_mqtt_id_and_topics();
  mqtt_connect_and_subscribe();

  interval_trigger_flag = false;
  Timer2.setOverflow(PING_INTERVAL_MS * 1000, MICROSEC_FORMAT);
  Timer2.attachInterrupt(timer2_callback);
  Timer2.resume();
}

// Required global instances: PubSubClient MqttClient, SerialMon
void connection_check_and_reconnect() {
  if (!MqttClient.connected()) {
    SerialMon.println("MqttClient.connected() is false.");
    SerialMon.print("MqttClient.state(): ");
    SerialMon.println(MqttClient.state());
    mcu_restart();
  }
}

void loop() {
  if (interval_trigger_flag) {
    reportMotorMeasurement();
    interval_trigger_flag = false;
  }
  
  connection_check_and_reconnect();
  
  unsigned long next = millis();
  while (millis() < next + LOOP_INTERVAL_MS) MqttClient.loop();
}