exodus/Leo-ElixirLights
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

motor control changed to infinit pipeline with continuous buffer

Browse Source
This commit is contained in:
Jakob Gegeniger
2024-12-22 17:01:06 +01:00
parent eb0eb436ed
commit dd9f59e418
8 changed files with 593 additions and 593 deletions
Download Patch File Download Diff File Expand all files Collapse all files
main/watchMain.cpp
+160 -250
View File
@@ -8,10 +8,6 @@
#include "watchMain.hpp"
static const char *TAG = "Watch";
static esp_eth_handle_t s_eth_handle = NULL;
static bool s_ethernet_is_connected = false;
extern "C" { // This switch allows the ROS C-implementation to find this main
void app_main(void);
}
@@ -25,11 +21,9 @@ struct DeviceState {
Color color{16, 16, 16};
};
DeviceState activeState;
enum class MoveType {
Step,
AllPictograms,
Arbitrary,
Demo
};
struct MoveCommand {
MoveType type;
@@ -39,12 +33,18 @@ struct MoveCommand {
};
static fos::Queue<MoveCommand> commandQueue(8);
DeviceState activeState;
static const char *TAG = "Watch";
static bool EthernetIsConnected = false;
std::unique_ptr<Cli> shellPtr;
/// @brief Callback for update start
/// @param args callback arguments
void onCommand(std::string name, Cli::CArgs args) {
ESP_LOGI(TAG, "Invoking: %s", name.c_str());
}
void startUpdate(Cli::CArgs args) {
ESP_LOGI(TAG, "Invoking Update");
if (args.size() != 1) {
shellPtr->write("Need URL to application'\n");
@@ -53,24 +53,13 @@ void startUpdate(Cli::CArgs args) {
// @ToDo: Start Update here
}
/// @brief Callback for commands that are not directly wired
/// @param name command name
/// @param args callback arguments
void onCommand(std::string name, Cli::CArgs args) {
ESP_LOGI(TAG, "Invoking: %s", name.c_str());
}
void showLogServerConfig(Cli::CArgs args) {
ESP_LOGI(TAG, "on showLogServerConfig:");
// auto &store = NvsApi::getInstance();
// auto logger = store.getValue("syslogDst");
// shellPtr->write(std::string{"syslog destination = '"} + logger + "'\n");
}
void setLogServerConfig(Cli::CArgs args) {
ESP_LOGI(TAG, "on showLogServerConfig:");
if (args.size() != 1) {
shellPtr->write("Need IP address to syslog server'\n");
// auto &store = NvsApi::getInstance();
// auto logger = store.getValue("syslogDst");
// shellPtr->write(std::string{"syslog destination = '"} + logger + "'\n");
return;
}
// auto &store = NvsApi::getInstance();
@@ -78,6 +67,46 @@ void setLogServerConfig(Cli::CArgs args) {
// store.setValue("syslogDst", args[0]);
}
void setDynamics(Cli::CArgs args) {
ESP_LOGI(TAG, "on setDynamics:");
if (args.size() != 2) {
shellPtr->write("need float values for velocity and acceleration'\n");
return;
}
try {
float v = std::stof(args[0]);
float a = std::stof(args[1]);
activeState.accel = a;
activeState.speed = v;
} catch (const std::exception &e) {
ESP_LOGI(TAG, "Failed to parse %s and %s", args[0].c_str(), args[1].c_str());
}
}
void triggerMovement(Cli::CArgs args) {
ESP_LOGI(TAG, "on triggerMovement:");
if (args.size() != 2) {
shellPtr->write("need profile index and steps'\n");
return;
}
try {
int p = std::stoi(args[0]);
int j = std::stoi(args[1]);
activeState.profile = p;
activeState.jumps = j;
commandQueue.enqueue(std::make_unique<MoveCommand>(MoveType::Step, j, p, activeState.color));
} catch (const std::exception &e) {
ESP_LOGI(TAG, "Failed to parse %s and %s", args[0].c_str(), args[1].c_str());
}
}
void startDemoMovement(Cli::CArgs args) {
ESP_LOGI(TAG, "on startDemoMovement:");
commandQueue.enqueue(std::make_unique<MoveCommand>(MoveType::Demo));
}
/**
* @brief Telnet socket via TCP/IP
*
@@ -94,12 +123,15 @@ static void telnetSocketTask(int addrFamily) {
shellPtr->addBinding(
{"update", "Start OTA-Update-Process",
[](Cli::CArgs a) { startUpdate(a); }});
shellPtr->addBinding(
{"logserver_show", "Shows log server config ip",
[](Cli::CArgs a) { showLogServerConfig(a); }});
shellPtr->addBinding(
{"logserver_set", "Set log server config ip",
[](Cli::CArgs a) { setLogServerConfig(a); }});
shellPtr->addBinding(
{"dyn_set", "set dynamics of movement (velocity,accelerations) as floats",
[](Cli::CArgs a) { setDynamics(a); }});
shellPtr->addBinding(
{"move", "start movement (profile-idx,steps) as integers ",
[](Cli::CArgs a) { triggerMovement(a); }});
},
[](TcpSession &session) {
shellPtr->process();
@@ -108,11 +140,9 @@ static void telnetSocketTask(int addrFamily) {
} catch (const std::exception &e) {
ESP_LOGE("TCP", "%s", e.what());
}
vTaskDelete(NULL);
}
constexpr uint32_t DmxUniverse = 0;
constexpr int DmxChannelCount = 9;
@@ -134,11 +164,11 @@ static void UniverseCallback(const uint8_t *data, const uint16_t size) {
data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8]);
float intens = (float)data[3] / UINT8_MAX;
// activeState.color = Color{
// scaledRgbChannel(data[0], intens),
// scaledRgbChannel(data[1], intens),
// scaledRgbChannel(data[2], intens),
// 0};
activeState.color = Color{
scaledRgbChannel(data[0], intens),
scaledRgbChannel(data[1], intens),
scaledRgbChannel(data[2], intens),
0};
static uint8_t lastTrig = 0;
uint8_t thisTrig = data[4];
@@ -225,21 +255,12 @@ void startServerTasks() {
static fos::TaskT<int> telnetSocket(telnetSocketTask, ipVersion, "tcpServer", 4096);
static fos::TaskT<int> dmxNode(dmxSocketTask, ipVersion, "dmxNode", 4096);
// static Task otaUpdate(SystemFunctions::OtaTaskEntry, "otaTask", 8192);
// initialise_mdns();
// netbiosns_init();
// netbiosns_set_name(CONFIG_EXAMPLE_MDNS_HOST_NAME);
// auto &store = NvsApi::getInstance();
// auto logger = store.getValue("syslogDst");
// if (logger != "") {
// Syslog::setupTask(IpAdr{logger, 0});
// }
}
/** Event handler for IP_EVENT_ETH_GOT_IP */
static void got_ip_event_handler(void *arg, esp_event_base_t event_base,
int32_t event_id, void *event_data)
{
ip_event_got_ip_t *event = (ip_event_got_ip_t *) event_data;
int32_t event_id, void *event_data) {
ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data;
const esp_netif_ip_info_t *ip_info = &event->ip_info;
ESP_LOGI(TAG, "Ethernet Got IP Address");
@@ -253,20 +274,19 @@ static void got_ip_event_handler(void *arg, esp_event_base_t event_base,
// Event handler for Ethernet
static void eth_event_handler(void *arg, esp_event_base_t event_base,
int32_t event_id, void *event_data)
{
int32_t event_id, void *event_data) {
uint8_t mac_addr[6] = {0};
switch (event_id) {
case ETHERNET_EVENT_CONNECTED:
ESP_LOGI(TAG, "Ethernet Link Up");
s_ethernet_is_connected = true;
EthernetIsConnected = true;
ESP_LOGI(TAG, "Ethernet Link Up");
ESP_LOGI(TAG, "Ethernet HW Addr %02x:%02x:%02x:%02x:%02x:%02x",
mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
break;
case ETHERNET_EVENT_DISCONNECTED:
ESP_LOGI(TAG, "Ethernet Link Down");
s_ethernet_is_connected = false;
EthernetIsConnected = false;
break;
case ETHERNET_EVENT_START:
ESP_LOGI(TAG, "Ethernet Started");
@@ -279,8 +299,7 @@ static void eth_event_handler(void *arg, esp_event_base_t event_base,
}
}
static void initialize_ethernet(void)
{
static void initialize_ethernet(void) {
uint8_t eth_port_cnt = 0;
esp_eth_handle_t *eth_handles;
ESP_ERROR_CHECK(example_eth_init(&eth_handles, &eth_port_cnt));
@@ -288,6 +307,7 @@ static void initialize_ethernet(void)
ESP_LOGW(TAG, "multiple Ethernet devices detected, the first initialized is to be used!");
}
static esp_eth_handle_t s_eth_handle = NULL;
s_eth_handle = eth_handles[0];
free(eth_handles);
@@ -336,98 +356,60 @@ void LightingTask() {
ESP_LOGI(TAG, "Start Lighting");
static I2cMaster master(I2C_NUM_0,
/*sda=*/ GPIO_NUM_10,
/*scl=*/ GPIO_NUM_11);
/*sda=*/GPIO_NUM_10,
/*scl=*/GPIO_NUM_11);
// static PmwExpanderPca9685 pca9685(master);
static PmwExpanderPca9685 ledDriver1(master, /*iverted=*/ false);
static PmwExpanderPca9685 ledDriver2(master, /*iverted=*/ false, 0x41);
static PmwExpanderPca9685 ledDriver1(master, /*iverted=*/false);
static PmwExpanderPca9685 ledDriver2(master, /*iverted=*/false, 0x41);
PmwExpanderPca9685::GrayValues values[2];
OutputPort pwmEnable(GPIO_NUM_5, GpioPort::Logic::Inverted);
OutputPort pwmEnable(GPIO_NUM_1, GpioPort::Logic::Inverted);
pwmEnable.write(true);
int idx = 0;
while (true) {
values[0].fill(0);
values[1].fill(0);
// values[x][4] = 0x200; // Actually first
values[0][idx] = 0x200;
ledDriver1.sendImage(values[0]);
// values[1][4] = 0x100; // Actually first
values[1][idx] = 0x400;
ledDriver2.sendImage(values[1]);
if (++idx >= 16 ) {
idx = 0;
}
fos::Task::delayMs(100);
}
// ESP_LOGI(TAG, "Start Lighting");
// static I2cMaster master(I2C_NUM_0, GPIO_NUM_10, GPIO_NUM_11);
// static PmwExpanderPca9685 pca9685(master);
// PmwExpanderPca9685::GrayValues values;
// AddressableLeRtz ledBar(GPIO_NUM_15, AddressableLeRtz::Type::Rgb, 6);
// std::vector<Color> pictoSpots(6);
// for (auto &&led : pictoSpots) {
// led.set(0, 0, 0);
// }
// auto const &colorSource = activeState.color;
// std::array<Effect::EffectProcessor, 6> processors{
// Effect::EffectProcessor(8),
// Effect::EffectProcessor(8),
// Effect::EffectProcessor(8),
// Effect::EffectProcessor(8),
// Effect::EffectProcessor(8),
// Effect::EffectProcessor(8)};
// processors[0].setEffect(Effect::macDemoStart, &colorSource, 16);
// processors[1].setEffect(Effect::macDemoStart, &colorSource, 32);
// processors[2].setEffect(Effect::macDemoPulse, &colorSource, 48);
// processors[3].setEffect(Effect::macDemoPulse, &colorSource, 64);
// processors[4].setEffect(Effect::macStartFlicker, &colorSource, 80);
// processors[5].setEffect(Effect::macStartIdle, &colorSource, 96);
// int idx = 0;
// values.fill(0);
// while (true) {
// values[0] = activeState.color.getRed() * 257;
// values[1] = activeState.color.getGreen() * 257;
// values[2] = activeState.color.getBlue() * 257;
// pca9685.sendImage(values);
// for (size_t i = 0; i < 600; i++) {
// Task::delayMs(20);
// for (size_t i = 0; i < pictoSpots.size(); i++) {
// pictoSpots[i] = processors[i].tick();
// }
// ledBar.send(pictoSpots);
// }
auto const &colorSource = activeState.color;
std::array<Effect::EffectProcessor, 6> processors{
Effect::EffectProcessor(8),
Effect::EffectProcessor(8),
Effect::EffectProcessor(8),
Effect::EffectProcessor(8),
Effect::EffectProcessor(8),
Effect::EffectProcessor(8)};
processors[0].setEffect(Effect::macDemoStart, &colorSource, 16);
processors[1].setEffect(Effect::macDemoStart, &colorSource, 32);
processors[2].setEffect(Effect::macDemoPulse, &colorSource, 48);
processors[3].setEffect(Effect::macDemoPulse, &colorSource, 64);
processors[4].setEffect(Effect::macStartFlicker, &colorSource, 80);
processors[5].setEffect(Effect::macStartIdle, &colorSource, 96);
// processors[1].setEffect(Effect::macDark, &colorSource, 32);
// processors[2].setEffect(Effect::macDark, &colorSource, 48);
// processors[3].setEffect(Effect::macDark, &colorSource, 64);
// processors[4].setEffect(Effect::macDark, &colorSource, 80);
// for (size_t i = 0; i < 50; i++) {
// Task::delayMs(20);
// for (size_t i = 0; i < pictoSpots.size(); i++) {
// pictoSpots[i] = processors[i].tick();
// }
// ledBar.send(pictoSpots);
// }
// processors[1].setEffect(Effect::macDemoStart, &colorSource, 32);
// processors[2].setEffect(Effect::macDemoPulse, &colorSource, 48);
// processors[3].setEffect(Effect::macDemoPulse, &colorSource, 64);
// processors[4].setEffect(Effect::macStartFlicker, &colorSource, 80);
// }
while (true) {
values[0].fill(0);
values[1].fill(0);
// values[x][4] = 0x200; // Actually first
values[0][4] = activeState.color.getRed() * 257;
values[0][5] = activeState.color.getGreen() * 257;
values[0][6] = activeState.color.getBlue() * 257;
// values[1][4] = 0x100; // Actually first
values[1][4] = activeState.color.getRed() * 257;
values[1][5] = activeState.color.getGreen() * 257;
values[1][6] = activeState.color.getBlue() * 257;
ledDriver1.sendImage(values[0]);
ledDriver2.sendImage(values[1]);
fos::Task::delayMs(40);
}
}
void MovementTask() {
@@ -438,26 +420,19 @@ void MovementTask() {
{LEDC_CHANNEL_0, LEDC_CHANNEL_1, LEDC_CHANNEL_2, LEDC_CHANNEL_3},
{GPIO_NUM_23, GPIO_NUM_22, GPIO_NUM_21, GPIO_NUM_20},
};
StepperDriverDrv8833 stepperDriver(stepperPins, MicroSteps);
StepperDriverDrv8833 stepperDriver(stepperPins, MicroSteps, 0.25F);
stepperDriver.setPos(0);
StepperMotor::Config stepperCfg{1.8F, MicroSteps};
StepperConfig stepperCfg{1.8F, MicroSteps};
InputPort initSens(GPIO_NUM_18);
InputPort motorFault(GPIO_NUM_8);
motorFault.setLogic(GpioPort::Logic::Inverted);
OutputPort motorSleep(GPIO_NUM_15, GpioPort::Logic::Inverted);
motorSleep.write(false);
StepperMotor stepper(stepperCfg, stepperDriver, initSens);
int i = 0;
while (i++ < 80) {
stepperDriver.step(1);
stepperDriver.getSubPos();
fos::Task::delayMs(100);
}
StepperMotor stepper(stepperCfg, stepperDriver, initSens);
ESP_LOGI("Stepper", "Find Home");
stepper.findRef();
@@ -465,114 +440,50 @@ void MovementTask() {
fos::Task::delayMs(1000);
while (true) {
fos::Task::delayMs(100);
ESP_LOGD(TAG, "Waiting for command");
auto cmd = commandQueue.dequeue();
if (cmd->type == MoveType::Step) {
if (stepper.isIdle()) {
auto delta = cmd->deltaPic;
auto profile = cmd->profile;
// activeState.pictogram = ledPos;
// ledRing.send(ledStrip);
delta = std::min<uint8_t>(delta, 128);
Syslog::logv(Syslog::Severity::Notice,
"Running Stepper by %i steps with prof=%i a=%.2f w=%.2f",
delta, profile, activeState.accel, activeState.speed);
stepper.moveBy(delta * std::numbers::pi / 3,
profile, activeState.accel, activeState.speed);
} else {
fos::Task::delayMs(100);
}
} else if (cmd->type == MoveType::Demo) {
int i = 0;
while (i++ < 80) {
stepperDriver.step(1);
stepperDriver.getSubPos();
fos::Task::delayMs(100);
}
ESP_LOGI("Stepper", "Find Home");
stepper.findRef();
activeState.pictogram = -1;
stepper.findRef();
fos::Task::delayMs(1000);
for (size_t i = 0; i < 6; i++) {
stepper.moveBy(std::numbers::pi / 3);
while (!stepper.isIdle()) {
fos::Task::delayMs(100);
}
ESP_LOGI(TAG, "Position is %f.2", stepper.getScaledPos() * 6);
}
} else {
fos::Task::delayMs(100);
}
}
// ESP_LOGI(TAG, "Start Stepper");
// StepperDriverTb6612::Pins stepperPins{
// LEDC_TIMER_0,
// LEDC_CHANNEL_0,
// GPIO_NUM_23,
// {GPIO_NUM_22, GPIO_NUM_21},
// LEDC_CHANNEL_1,
// GPIO_NUM_20,
// {GPIO_NUM_19, GPIO_NUM_18},
// };
// constexpr int MicroSteps = 32;
// StepperDriverTb6612 stepperDriver(stepperPins, MicroSteps);
// stepperDriver.setPos(0);
// StepperMotor::Config stepperCfg{1.8F, MicroSteps};
// InputPort initSens(GPIO_NUM_3);
// initSens.setLogic(GpioPort::Logic::Inverted);
// StepperMotor stepper(stepperCfg, stepperDriver, initSens);
// ESP_LOGI(TAG, "Find Home");
// stepper.findRef();
// ESP_LOGI(TAG, "Fist Position is %f.2", stepper.getScaledPos() * 6);
// Task::delayMs(1000);
// AddressableLeRtz ledRing(GPIO_NUM_9, AddressableLeRtz::Type::Rgb, 12);
// std::vector<Color> ledStrip(12);
// for (auto &&led : ledStrip) {
// led.set(0, 0, 0);
// }
// uint32_t ledPos = 0;
// ledStrip[0].set(
// activeState.color.getRed(),
// activeState.color.getGreen(),
// activeState.color.getBlue(),
// 0);
// ledRing.send(ledStrip);
// while (true) {
// ESP_LOGD(TAG, "Waiting for command");
// auto cmd = commandQueue.dequeue();
// if (cmd->type == MoveType::Step) {
// if (stepper.isIdle()) {
// auto delta = cmd->deltaPic;
// auto profile = cmd->profile;
// for (auto &&led : ledStrip) {
// led.set(0, 0, 0);
// }
// ledPos = (ledPos + delta) % 6;
// activeState.pictogram = ledPos;
// ledStrip[ledPos * 2].set(
// cmd->color.getRed(),
// cmd->color.getGreen(),
// cmd->color.getBlue(),
// 0);
// ledRing.send(ledStrip);
// MqttClient::publishPosition(ledPos);
// delta = std::min<uint8_t>(delta, 7);
// Syslog::logv(Syslog::Severity::Notice, "Running Stepper by %i steps with prof=%i a=%.2f w=%.2f", delta, profile, activeState.accel, activeState.speed);
// stepper.moveBy(delta * std::numbers::pi / 3,
// profile, activeState.accel, activeState.speed);
// }
// } else if (cmd->type == MoveType::AllPictograms) {
// activeState.pictogram = -1;
// stepper.findRef();
// Task::delayMs(1000);
// for (size_t i = 0; i < 6; i++) {
// for (auto &&led : ledStrip) {
// led.set(0, 0, 0);
// }
// ledStrip[i * 2].set(0, 0, 16);
// ledRing.send(ledStrip);
// stepper.moveBy(std::numbers::pi / 3);
// while (!stepper.isIdle()) {
// Task::delayMs(100);
// }
// ESP_LOGI(TAG, "Position is %f.2", stepper.getScaledPos() * 6);
// }
// } else if (cmd->type == MoveType::Arbitrary) {
// activeState.pictogram = -1;
// ESP_LOGI(TAG, "Full Turn");
// stepper.demoFullTurn();
// Task::delayMs(4000);
// while (true) {
// if (stepper.isIdle()) {
// ESP_LOGI(TAG, "Position is %f.2", stepper.getScaledPos() * 6);
// Task::delayMs(500);
// stepper.createDemoProfile();
// }
// Task::delayMs(20);
// }
// }
// }
}
void IoTest()
{
void IoTest() {
ESP_LOGI(TAG, "Start IO");
InputPort in2(GPIO_NUM_19);
OutputPort nReset(GPIO_NUM_9);
@@ -580,11 +491,10 @@ void IoTest()
while (true) {
nReset.write(in2.read());
fos::Task::delayMs(100);
}
}
}
void app_main(void)
{
void app_main(void) {
esp_err_t ret = nvs_flash_init();
if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
ESP_ERROR_CHECK(nvs_flash_erase());
@@ -592,7 +502,7 @@ void app_main(void)
}
ESP_ERROR_CHECK(ret);
initialize_ethernet();
static fos::Task lightingTask(LightingTask, "Lights", 0x4000uL);
// static fos::Task lightingTask(LightingTask, "Lights", 0x4000uL);
static fos::Task movementTask(MovementTask, "Motor", 0x4000uL);
static fos::Task ioTask(IoTest, "IO", 0x4000uL);
// static fos::Task ioTask(IoTest, "IO", 0x4000uL);
}
main/watchMain.hpp
+1 -16
View File
@@ -10,9 +10,6 @@
#include <string.h>
#include <stdlib.h>
#include "sdkconfig.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "esp_event.h"
#include "esp_log.h"
#include "esp_eth_driver.h"
@@ -55,29 +52,17 @@
// #include "freertos/task.h"
// #include "gpTimer.hpp"
// #include "gpioPort.hpp"
// #include "i2cPort.hpp"
// #include "lwip/apps/netbiosns.h"
// #include "mdns.h"
// #include "mqttClient.hpp"
// #include "nlohmann/json.hpp"
// #include "nvsFunctions.hpp"
// #include "pwmExpanderPca9685.hpp"
// #include "pwmPort.hpp"
// #include "rmtPort.hpp"
// #include "sdkconfig.h"
// #include "soc/soc_caps.h"
// #include "stepperMotor.hpp"
// #include "stepperTB6612.hpp"
// #include "stepperDrv8833.hpp"
// #include "string.h"
// #include "systemFunctions.hpp"
// #include "tcpSockets.hpp"
// #include "uartPort.hpp"
// #include "udpSockets.hpp"
// #include "webServer.hpp"
// #include "wifiFunctions.hpp"
// #include <inttypes.h>
// #include <string.h>
// #include <cstring>
// #include <sys/param.h>
// #include <unistd.h>
// #include <cmath>