418 lines
14 KiB
C
418 lines
14 KiB
C
#include "mgos.h"
|
|
#include "WEMOS_Motor.h"
|
|
|
|
#include "mgos_rpc.h"
|
|
#include "common/cs_dbg.h"
|
|
#include "common/json_utils.h"
|
|
#include "common/platform.h"
|
|
#include "common/mg_str.h"
|
|
#include "frozen/frozen.h"
|
|
#include "mgos_app.h"
|
|
#include "mgos_gpio.h"
|
|
#include "mgos_net.h"
|
|
#include "mgos_sys_config.h"
|
|
#include "mgos_timers.h"
|
|
#include "mgos_mqtt.h"
|
|
#include "common/str_util.h"
|
|
#include "mgos_crontab.h"
|
|
|
|
static double pwm = 30.0;
|
|
static double flashLightSpeed = 50.0; /* 0.0 .. 100.0 */
|
|
static double flashLightTargetSpeed = 50.0; // used to rampdown to a specific speed
|
|
static uint16_t motorRampUpTime_msec = 2000;
|
|
static uint16_t motorRampDownTime_msec = 1000;
|
|
static uint16_t motorUpdateTime_msec = 100;
|
|
static double flashLightRampUp_deltaSpeed = 0;
|
|
static double flashLightRampDown_deltaSpeed = 0;
|
|
static uint8_t motorDirection = _CW;
|
|
static const char * clientId = "";
|
|
static char commandTopic[256] = {'\0'};
|
|
|
|
static char pubStatusTopic[256] = {'\0'};
|
|
static bool mqttConnected = false;
|
|
enum MotorStatus { MotorStatus_Off = 0, MotorStatus_RampUp, MotorStatus_RampDown, MotorStatus_On, MotorStatus_DemoMode };
|
|
static uint8_t motorStatus = MotorStatus_DemoMode;
|
|
|
|
// Motor numbers
|
|
//Motor shiled I2C Address: 0x30
|
|
//PWM frequency: 1000Hz(1kHz)
|
|
#define M1 0
|
|
#define M1_addr 0x30
|
|
#define M1_freq 3000
|
|
static uint8_t motorAddress = M1_addr;
|
|
static uint32_t motorFrequency = M1_freq;
|
|
|
|
static void recalcTimings() {
|
|
flashLightRampUp_deltaSpeed = (flashLightSpeed * motorUpdateTime_msec) / motorRampUpTime_msec;
|
|
flashLightRampDown_deltaSpeed = (flashLightSpeed * motorUpdateTime_msec) / motorRampDownTime_msec;
|
|
}
|
|
|
|
static void pubStatus(const char *statusString, double percentage) {
|
|
struct mbuf fb;
|
|
struct json_out out = JSON_OUT_MBUF(&fb);
|
|
|
|
if (!mqttConnected) return;
|
|
|
|
mbuf_init(&fb, 30);
|
|
if (mgos_mqtt_global_connect()) {
|
|
json_printf(&out, "{statusString: %Q, speed: %f}", statusString, percentage);
|
|
mgos_mqtt_pub(pubStatusTopic, fb.buf, fb.len, 0, true);
|
|
}
|
|
|
|
}
|
|
|
|
static void motor_timer_cb(void *arg) {
|
|
static bool stopped = false;
|
|
|
|
switch (motorStatus) {
|
|
case MotorStatus_Off:
|
|
wemos_motor_stop(M1);
|
|
break;
|
|
case MotorStatus_On:
|
|
wemos_motor_setmotor(M1, motorDirection, pwm);
|
|
break;
|
|
case MotorStatus_RampUp:
|
|
pwm += flashLightRampUp_deltaSpeed;
|
|
if (pwm >= flashLightSpeed) {
|
|
LOG(LL_INFO, ("MotorStatus_RampUp: Speed target reached"));
|
|
pwm = flashLightSpeed;
|
|
motorStatus = MotorStatus_On;
|
|
}
|
|
wemos_motor_setmotor(M1, motorDirection, pwm);
|
|
// LOG(LL_INFO, ("M1, dir=%d, pwm=%f", motorDirection, pwm));
|
|
break;
|
|
case MotorStatus_RampDown:
|
|
pwm -= flashLightRampDown_deltaSpeed;
|
|
if (pwm <= flashLightTargetSpeed) {
|
|
if (pwm <= 0.0) {
|
|
motorStatus = MotorStatus_Off;
|
|
pwm = 0.0;
|
|
wemos_motor_stop(M1);
|
|
// change direction for next time, when motor turns on again
|
|
if (motorDirection == _CW) {
|
|
motorDirection = _CCW;
|
|
} else {
|
|
motorDirection = _CW;
|
|
}
|
|
} else {
|
|
pwm = flashLightTargetSpeed;
|
|
LOG(LL_INFO, ("MotorStatus_RampDown: Speed target reached"));
|
|
motorStatus = MotorStatus_On;
|
|
wemos_motor_setmotor(M1, motorDirection, pwm);
|
|
}
|
|
} else {
|
|
wemos_motor_setmotor(M1, motorDirection, pwm);
|
|
// LOG(LL_INFO, ("M1, dir=%d, pwm=%f", motorDirection, pwm));
|
|
}
|
|
break;
|
|
case MotorStatus_DemoMode:
|
|
default:
|
|
if (pwm > flashLightSpeed + 5.0) {
|
|
pwm = 0.0; // start again
|
|
stopped = false;
|
|
} else {
|
|
pwm += 0.1;
|
|
if (!stopped) {
|
|
wemos_motor_setmotor(M1, motorDirection, pwm);
|
|
LOG(LL_INFO, ("M1, dir=%d, pwm=%f", motorDirection, pwm));
|
|
pubStatus("on", pwm);
|
|
|
|
if (pwm > flashLightSpeed) {
|
|
// wemos_motor_setmotor(M1, _STOP, 0.0);
|
|
wemos_motor_setmotor(M1, _STANDBY, pwm);
|
|
stopped = true;
|
|
LOG(LL_INFO, ("Stopped/Standby"));
|
|
pubStatus("off", 0);
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
(void) arg;
|
|
}
|
|
|
|
static void flashLightOn() {
|
|
LOG(LL_INFO, ("FlashLight ON\n"));
|
|
flashLightTargetSpeed = flashLightSpeed;
|
|
motorStatus = MotorStatus_RampUp; // this starts the motor on next timer callback
|
|
pubStatus("on", flashLightSpeed);
|
|
}
|
|
|
|
static void rpc_flashLightOn(struct mg_rpc_request_info *ri, void *cb_arg,
|
|
struct mg_rpc_frame_info *fi, struct mg_str args) {
|
|
struct mbuf fb;
|
|
struct json_out out = JSON_OUT_MBUF(&fb);
|
|
|
|
flashLightOn();
|
|
mbuf_init(&fb, 100);
|
|
json_printf(&out, "{result: 0, resultString: %Q}", "OK");
|
|
mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
|
|
ri = NULL;
|
|
mbuf_free(&fb);
|
|
|
|
(void) cb_arg;
|
|
(void) fi;
|
|
(void) args;
|
|
}
|
|
|
|
static void cron_flashLightOn(struct mg_str action, struct mg_str payload, void *userdata) {
|
|
LOG(LL_INFO, ("Crontab flashLightOn fired"));
|
|
flashLightOn();
|
|
(void) action;
|
|
(void) payload;
|
|
(void) userdata;
|
|
}
|
|
|
|
static void flashLightOff() {
|
|
LOG(LL_INFO, ("FlashLight OFF\n"));
|
|
flashLightTargetSpeed = 0.0;
|
|
motorStatus = MotorStatus_RampDown; // this stops the motor on next timer callback
|
|
pubStatus("off", 0);
|
|
}
|
|
|
|
static void rpc_flashLightOff(struct mg_rpc_request_info *ri, void *cb_arg,
|
|
struct mg_rpc_frame_info *fi, struct mg_str args) {
|
|
struct mbuf fb;
|
|
struct json_out out = JSON_OUT_MBUF(&fb);
|
|
|
|
flashLightOff();
|
|
mbuf_init(&fb, 100);
|
|
json_printf(&out, "{result: 0, resultString: %Q}", "OK");
|
|
mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
|
|
ri = NULL;
|
|
mbuf_free(&fb);
|
|
|
|
(void) cb_arg;
|
|
(void) fi;
|
|
(void) args;
|
|
}
|
|
|
|
static void cron_flashLightOff(struct mg_str action, struct mg_str payload, void *userdata) {
|
|
LOG(LL_INFO, ("Crontab flashLightOff fired"));
|
|
flashLightOff();
|
|
(void) action;
|
|
(void) payload;
|
|
(void) userdata;
|
|
}
|
|
|
|
static void cron_init() {
|
|
mgos_crontab_register_handler(mg_mk_str("FlashLightOn"), cron_flashLightOn, NULL);
|
|
mgos_crontab_register_handler(mg_mk_str("FlashLightOff"), cron_flashLightOff, NULL);
|
|
}
|
|
|
|
static void flashLightSetSpeed(uint16_t newSpeed) {
|
|
flashLightTargetSpeed = (double) newSpeed;
|
|
flashLightSpeed = flashLightTargetSpeed;
|
|
if (pwm < flashLightTargetSpeed) {
|
|
motorStatus = MotorStatus_RampUp;
|
|
} else if (pwm > flashLightTargetSpeed) {
|
|
motorStatus = MotorStatus_RampDown;
|
|
}
|
|
}
|
|
|
|
static void rpc_flashLightSetSpeed(struct mg_rpc_request_info *ri, void *cb_arg,
|
|
struct mg_rpc_frame_info *fi, struct mg_str args) {
|
|
struct mbuf fb;
|
|
struct json_out out = JSON_OUT_MBUF(&fb);
|
|
|
|
mbuf_init(&fb, 100);
|
|
|
|
int speed = 0;
|
|
|
|
if (json_scanf(args.p, args.len, ri->args_fmt, &speed) == 1) {
|
|
printf("FlashLight set speed to %d\n", speed);
|
|
json_printf(&out, "{result: 0, resultString: %Q, speed: %d}", "OK", speed);
|
|
flashLightSetSpeed(speed);
|
|
} else {
|
|
json_printf(&out, "{error: %Q}", "speed is required");
|
|
}
|
|
|
|
mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
|
|
ri = NULL;
|
|
mbuf_free(&fb);
|
|
recalcTimings();
|
|
|
|
(void) cb_arg;
|
|
(void) fi;
|
|
(void) args;
|
|
}
|
|
|
|
static void flashLightSetRampupTime(struct mg_rpc_request_info *ri, void *cb_arg,
|
|
struct mg_rpc_frame_info *fi, struct mg_str args) {
|
|
struct mbuf fb;
|
|
struct json_out out = JSON_OUT_MBUF(&fb);
|
|
|
|
mbuf_init(&fb, 100);
|
|
|
|
uint16_t rampUpTime_msec = 0;
|
|
|
|
if (json_scanf(args.p, args.len, ri->args_fmt, &rampUpTime_msec) == 1) {
|
|
motorRampUpTime_msec = rampUpTime_msec;
|
|
printf("FlashLight set motor ramp up time to %d\n", rampUpTime_msec);
|
|
json_printf(&out, "{result: 0, resultString: %Q, rampUpTime_ms: %d}", "OK", rampUpTime_msec);
|
|
} else {
|
|
json_printf(&out, "{error: %Q}", "rampUpTime_msec is required");
|
|
}
|
|
|
|
mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
|
|
ri = NULL;
|
|
mbuf_free(&fb);
|
|
recalcTimings();
|
|
|
|
(void) cb_arg;
|
|
(void) fi;
|
|
(void) args;
|
|
}
|
|
|
|
static void flashLightSetRampdownTime(struct mg_rpc_request_info *ri, void *cb_arg,
|
|
struct mg_rpc_frame_info *fi, struct mg_str args) {
|
|
struct mbuf fb;
|
|
struct json_out out = JSON_OUT_MBUF(&fb);
|
|
|
|
mbuf_init(&fb, 100);
|
|
|
|
uint16_t rampDownTime_msec = 0;
|
|
|
|
if (json_scanf(args.p, args.len, ri->args_fmt, &rampDownTime_msec) == 1) {
|
|
motorRampDownTime_msec = rampDownTime_msec;
|
|
printf("FlashLight set motor ramp up time to %d\n", rampDownTime_msec);
|
|
json_printf(&out, "{result: 0, resultString: %Q, rampDownTime_ms: %d}", "OK", rampDownTime_msec);
|
|
} else {
|
|
json_printf(&out, "{error: %Q}", "rampDownTime_msec is required");
|
|
}
|
|
|
|
mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
|
|
ri = NULL;
|
|
mbuf_free(&fb);
|
|
recalcTimings();
|
|
|
|
(void) cb_arg;
|
|
(void) fi;
|
|
(void) args;
|
|
}
|
|
|
|
static void flashLightSetMotorUpdateTime(struct mg_rpc_request_info *ri, void *cb_arg,
|
|
struct mg_rpc_frame_info *fi, struct mg_str args) {
|
|
struct mbuf fb;
|
|
struct json_out out = JSON_OUT_MBUF(&fb);
|
|
|
|
mbuf_init(&fb, 100);
|
|
|
|
uint16_t updateTime = 0;
|
|
|
|
if (json_scanf(args.p, args.len, ri->args_fmt, &updateTime) == 1) {
|
|
motorUpdateTime_msec = updateTime;
|
|
printf("FlashLight set updateTime_msec tp %d\n", updateTime);
|
|
json_printf(&out, "{result: 0, resultString: %Q, udateTime_ms: %d}", "OK", updateTime);
|
|
} else {
|
|
json_printf(&out, "{error: %Q}", "motorUpdateTime_msec is required");
|
|
}
|
|
|
|
mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
|
|
ri = NULL;
|
|
mbuf_free(&fb);
|
|
recalcTimings();
|
|
|
|
(void) cb_arg;
|
|
(void) fi;
|
|
(void) args;
|
|
}
|
|
|
|
static void flashLightGetSettings(struct mg_rpc_request_info *ri, void *cb_arg,
|
|
struct mg_rpc_frame_info *fi, struct mg_str args) {
|
|
struct mbuf fb;
|
|
struct json_out out = JSON_OUT_MBUF(&fb);
|
|
|
|
mbuf_init(&fb, 1024);
|
|
|
|
json_printf(&out, "{pwm: %f, speed: %f, rampupTime_ms: %d, rampdownTime_ms: %d, updateTime_ms: %d, rampupDeltaSpeed: %f, rampdownDeltaSpeed: %f, motorDirection: %d, clientId: %Q, commandTopic: %Q, statusTopic: %Q, mqttConnected: %Q, motorStatus: %d}",
|
|
pwm, flashLightSpeed, motorRampUpTime_msec, motorRampDownTime_msec, motorUpdateTime_msec, flashLightRampUp_deltaSpeed, flashLightRampDown_deltaSpeed,
|
|
motorDirection, clientId, commandTopic, pubStatusTopic, mqttConnected ? "true" : "false", motorStatus);
|
|
|
|
mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
|
|
ri = NULL;
|
|
mbuf_free(&fb);
|
|
|
|
(void) cb_arg;
|
|
(void) fi;
|
|
(void) args;
|
|
}
|
|
|
|
void net_changed(int ev, void *evd, void *arg) {
|
|
if (ev != MGOS_NET_EV_IP_ACQUIRED) return;
|
|
// call_peer();
|
|
(void) evd;
|
|
(void) arg;
|
|
}
|
|
|
|
static void mqttCommandHandler(struct mg_connection *c, const char *topic, int topic_len,
|
|
const char *msg, int msg_len, void *userdata) {
|
|
LOG(LL_INFO, ("Got message on topic %.*s", topic_len, topic));
|
|
|
|
(void) c;
|
|
(void) topic;
|
|
(void) topic_len;
|
|
(void) msg;
|
|
(void) msg_len;
|
|
(void) userdata;
|
|
}
|
|
|
|
// void onMqttConnection(struct mg_connection *c, const char *client_id, struct mg_send_mqtt_handshake_opts *opts, void *fn_arg) {
|
|
void onMqttConnection(struct mg_connection *c, const char *client_id, struct mg_send_mqtt_handshake_opts *opts, void *fn_arg) {
|
|
// add MQTT cmd subscription
|
|
LOG(LL_INFO, ("onMqttConnection handler called with clientId=%s", client_id));
|
|
#if 0
|
|
#endif
|
|
mgos_mqtt_sub(commandTopic, mqttCommandHandler, NULL);
|
|
mqttConnected = true;
|
|
|
|
(void) c;
|
|
(void) client_id;
|
|
(void) opts;
|
|
(void) fn_arg;
|
|
}
|
|
|
|
enum mgos_app_init_result mgos_app_init(void) {
|
|
struct mg_rpc *c = mgos_rpc_get_global();
|
|
mg_rpc_add_handler(c, "FlashLight.On", NULL, rpc_flashLightOn, NULL);
|
|
mg_rpc_add_handler(c, "FlashLight.Off", NULL, rpc_flashLightOff, NULL);
|
|
mg_rpc_add_handler(c, "FlashLight.Speed", "{speed: %d}", rpc_flashLightSetSpeed, NULL);
|
|
mg_rpc_add_handler(c, "FlashLight.RampUpTime_msec", "{rampUpTime_ms: %d}", flashLightSetRampupTime, NULL);
|
|
mg_rpc_add_handler(c, "FlashLight.RampDownTime_msec", "{rampDownTime_ms: %d}", flashLightSetRampdownTime, NULL);
|
|
mg_rpc_add_handler(c, "FlashLight.MotorUpdateTime_msec", "{uptdateTime_ms: %d}", flashLightSetMotorUpdateTime, NULL);
|
|
mg_rpc_add_handler(c, "FlashLight.GetSettings", NULL, flashLightGetSettings, NULL);
|
|
mgos_event_add_group_handler(MGOS_EVENT_GRP_NET, net_changed, NULL);
|
|
|
|
// enable crontab
|
|
cron_init();
|
|
|
|
// add MQTT cmd subscription
|
|
LOG(LL_INFO, ("Initializing MQTT"));
|
|
clientId = mgos_sys_config_get_mqtt_client_id();
|
|
clientId = clientId ? clientId : mgos_sys_config_get_device_id();
|
|
LOG(LL_INFO, ("clientId=%s", clientId));
|
|
LOG(LL_INFO, ("cmdTopic=%s", mgos_sys_config_get_flashLight_mqttCtrlTopic()));
|
|
LOG(LL_INFO, ("pubStatusTopic=%s", mgos_sys_config_get_flashLight_mqttStatusTopic()));
|
|
c_snprintf(commandTopic, sizeof(commandTopic), mgos_sys_config_get_flashLight_mqttCtrlTopic(), clientId);
|
|
c_snprintf(pubStatusTopic, sizeof(pubStatusTopic), mgos_sys_config_get_flashLight_mqttStatusTopic(), clientId);
|
|
LOG(LL_INFO, ("cmdTopic=%s", commandTopic));
|
|
LOG(LL_INFO, ("pubStatusTopic=%s", pubStatusTopic));
|
|
mgos_mqtt_set_connect_fn(onMqttConnection, NULL);
|
|
|
|
motorFrequency = mgos_sys_config_get_flashLight_motorFrequency();
|
|
LOG(LL_INFO, ("motorFrequency=%d Hz", motorFrequency));
|
|
motorAddress = mgos_sys_config_get_flashLight_address();
|
|
LOG(LL_INFO, ("motorAddress=%d", motorAddress));
|
|
LOG(LL_INFO, ("Initializing motor controller"));
|
|
recalcTimings();
|
|
wemos_motor_init();
|
|
LOG(LL_INFO, ("Initializing motor M1"));
|
|
wemos_motor_initMotor(M1, motorAddress, motorFrequency);
|
|
LOG(LL_INFO, ("Setting up timer"));
|
|
motorUpdateTime_msec = mgos_sys_config_get_flashLight_motorUpdateTime();
|
|
mgos_set_timer(motorUpdateTime_msec, MGOS_TIMER_REPEAT, motor_timer_cb, NULL);
|
|
LOG(LL_INFO, ("Initialization done"));
|
|
return MGOS_APP_INIT_SUCCESS;
|
|
}
|
|
|