323 lines
9.6 KiB
C++
323 lines
9.6 KiB
C++
/**
|
|
* @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);
|
|
}
|
|
}
|
|
|
|
///
|
|
/// Homing
|
|
///
|
|
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++) { // If only part is needed
|
|
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");
|
|
}
|
|
}
|