exodus/1NiM-WatingRoom-WatchController
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: Debugging motor movements

Browse Source
This commit is contained in:
Jakob Gegeniger
2025-04-02 15:06:25 +02:00
parent 4106bcc67c
commit 5ca595eddc
11 changed files with 2584 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
components/watchFace/CMakeLists.txt
+4
View File
@@ -0,0 +1,4 @@
idf_component_register(
INCLUDE_DIRS "."
REQUIRES devHal
)
components/watchFace/watchFace.hpp
+47
View File
@@ -0,0 +1,47 @@
#pragma once
#include "StepperDrv8833.hpp"
#include "stepperMotor.hpp"
#include <cmath>
class WatchMotor {
static constexpr uint8_t Positions = 6;
public:
WatchMotor(StepperMotor &stepper, OutputPort &sleep)
: _stepper(stepper),
_motorSleep(sleep) {
_motorSleep.write(false);
}
void moveTo(uint8_t sign, uint8_t profile = 0) {
sign = sign % 6;
float delta = 2.0F * std::numbers::pi;
float corr = 0.0F;
float currentPos = _stepper.getScaledPos() * 6;
if ((_currentIndex > 4) && (currentPos < 2)) {
corr = 6 - _currentIndex - currentPos;
} else if ((_currentIndex < 2) && (currentPos > 2)) {
corr = _currentIndex + 6 - currentPos;
} else {
corr = _currentIndex - currentPos;
}
// stepper.getScaledPos() * 6
if (_currentIndex < sign) {
delta = delta / 6 * (sign - _currentIndex);
} else if (_currentIndex > sign) {
delta = delta / 6 * (sign + 6 - _currentIndex);
}
ESP_LOGD("MV", "Moving to %d by %.2f + %.2f", sign, delta, corr);
_stepper.moveBy(delta + corr, profile);
_currentIndex = sign;
}
private:
StepperMotor &_stepper;
OutputPort &_motorSleep;
uint8_t _currentIndex{};
};
main/watchMain.hpp
+4 -2
View File
@@ -7,8 +7,9 @@
#pragma once
#include <string.h>
#include <stdlib.h>
#include <cstring>
#include <cstdlib>
#include <cmath>
#include "sdkconfig.h"
#include "esp_event.h"
#include "esp_log.h"
@@ -36,6 +37,7 @@
#include "Processor.hpp"
#include "nvsFunctions.hpp"
#include "mqttClient.hpp"
#include "watchFace.hpp"
// #include "esp_chip_info.h"
// #include "esp_flash.h"
test/integration/CMakeLists.txt
+12
View File
@@ -0,0 +1,12 @@
cmake_minimum_required(VERSION 3.16)
set(EXTRA_COMPONENT_DIRS
"../../core/components"
"../../core/managed_components"
"../../components"
# "../../managed_components"
)
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
project(INTEGRATION_TESTS_FW)
test/integration/dependencies.lock
+9
View File
@@ -0,0 +1,9 @@
dependencies:
idf:
component_hash: null
source:
type: idf
version: 5.3.0
manifest_hash: c4f4ec05313d427eb56c633f78ce213e553379725bf2af3c511665f930571beb
target: esp32c6
version: 1.0.0
test/integration/main/CMakeLists.txt
+2
View File
@@ -0,0 +1,2 @@
idf_component_register(SRCS "movementTests.cpp"
INCLUDE_DIRS ".")
test/integration/main/movementTests.cpp
+322
View File
@@ -0,0 +1,322 @@
/**
* @file watchMain.cpp
* @author Gustice
* @brief ChaosWatch Application
*
* @copyright Copyright (c) 2024
*/
/// NOTE: Create IDF shell navigate to directory ("test/integration")
// and run "idf.py -p COM9 flash monitor"
#include "Processor.hpp"
#include "PwmExpanderPca9685.hpp"
#include "StepperDrv8833.hpp"
#include "Task.hpp"
#include "esp_event.h"
#include "esp_log.h"
#include "sdkconfig.h"
#include "stepperMotor.hpp"
#include "watchFace.hpp"
#include <array>
#include <cmath>
#include <cstdlib>
#include <cstring>
/* Watchface Indicies
6/ 5/
5 / 1 4 / 0
4 2 3 1
3 2
*/
extern "C" { // This switch allows the ROS C-implementation to find this main
void app_main(void);
}
static const char *TAG = "IT";
struct DeviceState {
uint8_t minJumps{6};
uint8_t maxJumps{32};
uint8_t profile{};
uint8_t restTime{};
float speed = 1.0; // Rad/sec
float accel = 1.0; // Rad/sec^2
Color color{0x80, 0x80, 0x80};
uint8_t pictogram{}; // 0..5 active Pictogramm
};
class IndexIllumination {
public:
IndexIllumination()
: master(I2C_NUM_0, /*sda=*/GPIO_NUM_10, /*scl=*/GPIO_NUM_11),
ledDriver1(master, /*iverted=*/false),
ledDriver2(master, /*iverted=*/false, 0x41),
pwmEnable(GPIO_NUM_1, GpioPort::Logic::Inverted),
RED{CRed.getColor<Color::Rgbw>()},
GREEN{CGreen.getColor<Color::Rgbw>()},
BLUE{CBlue.getColor<Color::Rgbw>()} {
pwmEnable.write(true);
clearColorData();
syncToDevice();
}
void clearColorData() {
for (size_t i = 0; i < 2; i++) {
for (size_t j = 0; j < PmwExpanderPca9685::PwmChannels; j++) {
Values[i][j] = 0;
}
}
}
void setColorOfIndexTo(uint8_t idx, Color::Rgbw color) {
if (idx >= 6) {
return;
}
auto sp = LedStartPoints[idx];
sp[0] = (uint16_t)(color.white) << 5;
sp[1] = (uint16_t)(color.blue) << 5;
sp[2] = (uint16_t)(color.green) << 5;
sp[3] = (uint16_t)(color.red) << 5;
}
void syncToDevice() {
ledDriver1.sendImage(Values[0]);
ledDriver2.sendImage(Values[1]);
}
void flashAll(Color::Rgbw color, int duration = 200) {
clearColorData();
for (size_t i = 0; i < 6; i++) {
setColorOfIndexTo(i, color);
}
syncToDevice();
fos::Task::delayMs(duration);
clearColorData();
syncToDevice();
}
private:
PmwExpanderPca9685::GrayValues Values[2]{};
std::array<uint16_t *, 6> LedStartPoints{
&(Values[0][4]), // 1
&(Values[1][12]),
&(Values[1][8]),
&(Values[1][4]),
&(Values[0][12]),
&(Values[0][8]), // 6
};
I2cMaster master;
PmwExpanderPca9685 ledDriver1;
PmwExpanderPca9685 ledDriver2;
OutputPort pwmEnable;
const Color::Rgbw RED;
const Color::Rgbw GREEN;
const Color::Rgbw BLUE;
};
DeviceState activeState;
void IndexColorSweep(IndexIllumination &illum) {
const auto RED = CRed.getColor<Color::Rgbw>();
const auto GREEN = CGreen.getColor<Color::Rgbw>();
const auto BLUE = CBlue.getColor<Color::Rgbw>();
illum.flashAll(RED);
fos::Task::delayMs(200);
illum.flashAll(GREEN);
fos::Task::delayMs(200);
illum.flashAll(BLUE);
fos::Task::delayMs(200);
illum.clearColorData();
illum.setColorOfIndexTo(0, GREEN);
illum.syncToDevice();
fos::Task::delayMs(500);
illum.clearColorData();
illum.setColorOfIndexTo(0, RED);
illum.setColorOfIndexTo(1, GREEN);
illum.syncToDevice();
fos::Task::delayMs(500);
illum.clearColorData();
illum.setColorOfIndexTo(1, RED);
illum.setColorOfIndexTo(2, GREEN);
illum.syncToDevice();
fos::Task::delayMs(500);
illum.clearColorData();
illum.setColorOfIndexTo(2, RED);
illum.setColorOfIndexTo(3, GREEN);
illum.syncToDevice();
fos::Task::delayMs(500);
illum.clearColorData();
illum.setColorOfIndexTo(3, RED);
illum.setColorOfIndexTo(4, GREEN);
illum.syncToDevice();
fos::Task::delayMs(500);
illum.clearColorData();
illum.setColorOfIndexTo(4, RED);
illum.setColorOfIndexTo(5, GREEN);
illum.syncToDevice();
fos::Task::delayMs(500);
illum.clearColorData();
illum.setColorOfIndexTo(5, RED);
illum.syncToDevice();
fos::Task::delayMs(500);
illum.clearColorData();
illum.setColorOfIndexTo(0, GREEN);
illum.syncToDevice();
}
constexpr bool DoFlushAtBeginning = false;
constexpr bool DoFullTurns = false;
constexpr bool DoTurnSteps = false;
constexpr bool DoTurnProfiles = true;
void testMovementTask() {
esp_log_level_set(TAG, ESP_LOG_DEBUG);
esp_log_level_set("STP", ESP_LOG_DEBUG);
ESP_LOGD(TAG, "Init Lights for signaling");
IndexIllumination illum;
if constexpr (DoFlushAtBeginning) {
IndexColorSweep(illum);
}
const auto BLUE = CBlue.getColor<Color::Rgbw>();
ESP_LOGD(TAG, "Init Stepper");
constexpr int MicroSteps = 32;
constexpr float Intensity = 0.25;
constexpr float StepWidth = 1.8;
StepperDriverDrv8833::Pins stepperPins({LEDC_TIMER_0,
{LEDC_CHANNEL_0, LEDC_CHANNEL_1, LEDC_CHANNEL_2, LEDC_CHANNEL_3},
{GPIO_NUM_21, GPIO_NUM_20, GPIO_NUM_23, GPIO_NUM_22}});
StepperDriverDrv8833 stepperDriver(stepperPins, MicroSteps, Intensity);
StepperConfig stepperCfg(StepWidth, MicroSteps);
InputPort initSens(GPIO_NUM_18);
OutputPort motorSleep(GPIO_NUM_15, GpioPort::Logic::Inverted);
StepperMotor stepper(stepperCfg, stepperDriver, initSens);
WatchMotor watch(stepper, motorSleep);
///
/// Full Turns with full steps
///
if constexpr (DoFullTurns) {
ESP_LOGD(TAG, "Find Home");
stepper.findRef();
ESP_LOGD(TAG, "Fist Position is %3d / %.2f / Micro %d", stepper.getQuanted(), stepper.getScaledPos() * 6, stepper.getSubPos());
fos::Task::delayMs(2000);
ESP_LOGD(TAG, "Make one turns with full steps");
// stepper.setPos(0);
illum.flashAll(BLUE);
int i = 0;
ESP_LOGD(TAG, "Position at begin is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
while (i++ < 200) {
stepper.fullStep(1);
fos::Task::delayMs(100);
ESP_LOGD(TAG, "Position is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
}
fos::Task::delayMs(2000);
int spB = stepper.getSubPos();
stepper.demoFullTurn();
int spE = stepper.getSubPos();
ESP_LOGD(TAG, "Fullsteps SubPos begin %d / end %d", spB, spE);
fos::Task::delayMs(2000);
ESP_LOGD(TAG, "Position at begin is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
i = 0;
while (i++ < 64) {
stepper.step(1);
fos::Task::delayMs(100);
ESP_LOGD(TAG, "Position is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
}
}
///
///
///
ESP_LOGD(TAG, "Find Home");
stepper.findRef();
ESP_LOGD(TAG, "Fist Position is %3d / %.2f / Micro %d", stepper.getQuanted(), stepper.getScaledPos() * 6, stepper.getSubPos());
for (size_t i = 0; i < 7; i++) {
stepper.fullStep(1);
fos::Task::delayMs(40);
}
illum.flashAll(BLUE);
fos::Task::delayMs(1000);
auto ref = stepper.zeroPosition();
ESP_LOGD(TAG, "Reference saved at %.2f ", ref);
ESP_LOGD(TAG, "Correct Home");
///
/// Multiple Turns continuous
///
if constexpr (DoTurnSteps) {
ESP_LOGD(TAG, "Fist Position is %3d / %.2f / Micro %d", stepper.getQuanted(), stepper.getScaledPos() * 6, stepper.getSubPos());
fos::Task::delayMs(1000);
ESP_LOGI(TAG, "Move to each index multiple times");
illum.flashAll(BLUE);
for (size_t i = 1; i <= 2 * 3; i++) {
watch.moveTo(3 * i);
ESP_LOGD(TAG, "Position is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
}
ESP_LOGI(TAG, "Move to each index multiple times");
illum.flashAll(BLUE);
for (size_t i = 1; i <= 6 * 3; i++) {
watch.moveTo(i);
ESP_LOGD(TAG, "Position is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
}
}
///
/// All other Profiles
///
if constexpr (DoTurnProfiles) {
// for (size_t i = ProfileGenerator::Smooth; i < ProfileGenerator::NumProfiles; i++) {
for (size_t i = ProfileGenerator::JumpIn; i < ProfileGenerator::NumProfiles; i++) {
ESP_LOGI(TAG, "Running Profile %d ", i);
ESP_LOGD(TAG, "Fist Position is %3d / %.2f / Micro %d", stepper.getQuanted(), stepper.getScaledPos() * 6, stepper.getSubPos());
fos::Task::delayMs(1000);
illum.flashAll(BLUE);
int steps[3]{4, 2, 0};
for (size_t j = 0; j < 3; j++) {
watch.moveTo(steps[j], i);
ESP_LOGI(TAG, "Last position is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
fos::Task::delayMs(1000);
}
}
}
while (true) {
ESP_LOGI(TAG, "Finish");
fos::Task::delayMs(60000);
}
}
void app_main(void) {
ESP_LOGI(TAG, "Start Movement Integration Tests");
static fos::Task movementTest(testMovementTask, "Move", fos::Task::DefaultStackSize * 8);
while (true) {
fos::Task::delayMs(5000);
// ESP_LOGI(TAG, "Finish");
}
}
test/integration/sdkconfig
+2159
View File
File diff suppressed because it is too large Load Diff
test/integration/sdkconfig.defaults
+18
View File
@@ -0,0 +1,18 @@
#
# Compiler options
#
CONFIG_COMPILER_CXX_EXCEPTIONS=y
#
# Serial flasher config
#
CONFIG_ESPTOOLPY_FLASHSIZE_8MB=y
CONFIG_ESPTOOLPY_FLASHSIZE="8MB"
#
# Log output
#
CONFIG_LOG_DEFAULT_LEVEL_INFO=y
CONFIG_LOG_DEFAULT_LEVEL=3
CONFIG_LOG_MAXIMUM_LEVEL_DEBUG=y
CONFIG_LOG_MAXIMUM_LEVEL=4
test/unitOnHost/.vscode/settings.json
Vendored
+3
View File
@@ -0,0 +1,3 @@
{
"cmake.cmakePath": "C:/msys64/usr/bin/cmake.exe"
}
test/unitOnTarget/CMakeLists.txt
+4 -1
View File
@@ -3,7 +3,10 @@ cmake_minimum_required(VERSION 3.16)
# Include the components directory of the main application:
#
set(EXTRA_COMPONENT_DIRS "../../components ../../managed_components")
set(EXTRA_COMPONENT_DIRS
"../../components"
# "../../managed_components"
)
# Set the components to include the tests for.
# This can be overriden from CMake cache: