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
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");
}
}