diff --git a/.DS_Store b/.DS_Store index 2aef324..50bfc48 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/DOCUMENTATION/QID_TRACKING_DOCUMENTATION.md b/DOCUMENTATION/QID_TRACKING_DOCUMENTATION.md new file mode 100644 index 0000000..0f6bf25 --- /dev/null +++ b/DOCUMENTATION/QID_TRACKING_DOCUMENTATION.md @@ -0,0 +1,252 @@ +# QID-Based Done/Busy Tracking & Pause/Resume + +## Overview + +The QID (Query ID) tracking system provides reliable command completion tracking for the UC2 firmware and Python API. Instead of counting serial responses, the firmware's **QidRegistry** tracks internal sub-actions and automatically emits a single `{"qid":X,"state":"done"}` message when all sub-actions for a command complete. + +### Key Benefits +- **Reliable**: No more counting `nResponses` — firmware knows when actions are truly done +- **Multi-axis aware**: A 3-axis motor command sends one "done" when all 3 axes finish +- **Pause/Resume**: Motor movements can be paused mid-motion and resumed later +- **CAN-transparent**: Works identically for local motors and CAN-bus satellite motors +- **Backward compatible**: Legacy `nResponses` mode remains the default + +--- + +## Architecture + +``` +Python API ESP32 Master CAN Slave +───────── ──────────── ───────── + +sendMessage(cmd, qid=5) → SerialProcess receives JSON + MotorJsonParser.act() extracts qid + QidRegistry::registerQid(5, 3) ← 3 axes + serialize(qid) → ACK + ← {"qid":5,"success":1} + + FocusMotor starts 3 axes + Axis 0 (local): stepper runs... + Axis 1 (CAN): ──────────────→ Motor runs + Axis 2 (CAN): ──────────────→ Motor runs + + Axis 0 stops: reportActionDone(5) → 1/3 + Axis 1 CAN response: ← MotorState{qid=5} + reportActionDone(5) → 2/3 + Axis 2 CAN response: ← MotorState{qid=5} + reportActionDone(5) → 3/3 = DONE! + ← {"qid":5,"state":"done"} + +event.wait() returns +``` + +--- + +## Firmware API + +### QID States + +| State | Description | +|-----------|-------------| +| `busy` | Action is in progress | +| `done` | All sub-actions completed successfully | +| `paused` | Motors paused, can be resumed | +| `timeout` | No completion within timeout (default 30s) | +| `error` | Error occurred (e.g. hard limit triggered) | + +### Endpoints + +#### Query QID State +```json +{"task": "/qid_state", "qid": 5} +``` +Response: +```json +{"qid": 5, "state": "done"} +``` + +#### Pause a Running QID +```json +{"task": "/qid_pause", "qid": 5} +``` +- Stops all motors associated with QID 5 +- Saves remaining steps for each axis +- For CAN motors: sends stop, waits 500ms for position update +- Response: `{"qid": 5, "state": "paused"}` + +#### Resume a Paused QID +```json +{"task": "/qid_resume", "qid": 5} +``` +- Restarts paused motors with remaining distance +- Uses relative positioning for remaining steps +- Response: `{"qid": 5, "state": "done"}` (when resumed motion completes) + +### Automatic Notifications + +The firmware automatically sends these JSON messages over serial: + +```json +{"qid": 5, "state": "done"} // All actions completed +{"qid": 5, "state": "timeout"} // Timeout after 30s (configurable) +{"qid": 5, "state": "error"} // Hard limit or other error +{"qid": 5, "state": "paused"} // Paused via /qid_pause +``` + +### Supported Controllers + +| Controller | Type | QID Behavior | +|-----------|------|-------------| +| Motor | Async | Register with N axes, done when all stop | +| Laser | Sync | Register + done immediately in act() | +| LED | Sync | Register + done immediately in act() | +| Galvo | Async | (Future: register in scanner task) | + +--- + +## Python API + +### Enabling QID-Done Mode + +```python +from uc2rest import UC2Client + +# Legacy mode (default) - uses nResponses counting +uc2 = UC2Client(serialport="/dev/ttyUSB0") + +# QID-done mode - waits for firmware "done" event +uc2.serial.use_qid_done = True +``` + +### Motor Movement + +```python +# With use_qid_done=True: +# - Sends motor command with QID +# - Blocks until firmware sends {"qid":X,"state":"done"} +# - No nResponses counting needed +uc2.motor.move_stepper(steps=(1000, 2000, 0), speed=(20000, 20000, 0)) + +# With use_qid_done=False (default, legacy): +# - nResponses = len(steppers) + 1 +# - Blocks until that many serial responses arrive +uc2.motor.move_stepper(steps=(1000, 2000, 0), speed=(20000, 20000, 0)) +``` + +### Pause/Resume from Python + +```python +# Start a long movement +uc2.serial.use_qid_done = True +# (in a thread or non-blocking) +uc2.motor.move_stepper(steps=(100000, 0, 0), speed=(5000, 0, 0)) + +# Pause mid-motion +uc2.serial.post_json("/qid_pause", {"qid": last_qid}) + +# Resume later +uc2.serial.post_json("/qid_resume", {"qid": last_qid}) +``` + +### Query State + +```python +response = uc2.serial.post_json("/qid_state", {"qid": 5}) +# Returns: [{"qid": 5, "state": "done"}] +``` + +--- + +## Implementation Details + +### QidRegistry (Firmware) + +- Located in `main/src/qid/QidRegistry.h` and `.cpp` +- Circular buffer of 32 entries (configurable via `QID_REGISTRY_SIZE`) +- Thread-safe: protected by FreeRTOS mutex +- Eviction policy: when full, evicts oldest DONE/TIMEOUT/ERROR entry +- Default timeout: 30 seconds (`QID_DEFAULT_TIMEOUT_MS`) +- Timeout check runs in `FocusMotor::loop()` via `tickTimeout()` + +### CAN Protocol Change + +The `MotorState` struct (packed, sent over CAN) now includes a `qid` field: + +```cpp +#pragma pack(push,1) +struct MotorState { + int32_t currentPosition = 0; + bool isRunning = 0; + uint8_t axis = 0; + int16_t qid = -1; // NEW: QID for tracking +}; +#pragma pack(pop) +``` + +**IMPORTANT**: This changes the CAN message size. Both master and slave firmware must be updated together. + +### Files Modified + +| File | Changes | +|------|---------| +| `main/src/qid/QidRegistry.h` | New - Registry header | +| `main/src/qid/QidRegistry.cpp` | New - Registry implementation | +| `main/src/motor/MotorTypes.h` | Added `qid` to `MotorState` | +| `main/src/motor/FocusMotor.cpp` | reportActionDone in stopStepper, tickTimeout in loop, reportActionError in startStepper | +| `main/src/motor/MotorJsonParser.cpp` | registerQid in parseMotorDriveJson | +| `main/src/laser/LaserController.cpp` | register+reportDone in act() | +| `main/src/led/LedController.cpp` | register+reportDone in act() | +| `main/src/can/can_controller.cpp` | QID propagation in MotorState, reportActionDone on slave response | +| `main/src/serial/SerialProcess.cpp` | New endpoint handlers, QidRegistry::setup() | +| `main/src/wifi/Endpoints.h` | New endpoint constants | +| `uc2rest/mserial.py` | use_qid_done flag, event-based blocking | +| `uc2rest/motor.py` | Conditional nResponses | + +--- + +## Testing + +### Basic Motor Test +```json +{"task": "/motor_act", "motor": {"steppers": [{"stepperid": 1, "position": 10000, "speed": 20000, "isabs": 0}]}, "qid": 5} +``` +Expected: +1. Immediate: `{"qid":5,"success":1}` +2. Motor position updates during motion +3. On completion: `{"qid":5,"state":"done"}` + +### Multi-Axis Test +```json +{"task": "/motor_act", "motor": {"steppers": [{"stepperid": 0, "position": 5000, "speed": 10000}, {"stepperid": 1, "position": 10000, "speed": 20000}, {"stepperid": 2, "position": 3000, "speed": 15000}]}, "qid": 7} +``` +Expected: Single `{"qid":7,"state":"done"}` after ALL 3 axes finish. + +### Laser Test (Synchronous) +```json +{"task": "/laser_act", "LASERid": 0, "LASERval": 512, "qid": 10} +``` +Expected: Immediate `{"qid":10,"state":"done"}` (laser is synchronous). + +### Pause/Resume Test +```json +{"task": "/motor_act", "motor": {"steppers": [{"stepperid": 1, "position": 100000, "speed": 5000}]}, "qid": 15} +// Wait 2 seconds... +{"task": "/qid_pause", "qid": 15} +// Response: {"qid":15,"state":"paused"} +// Wait... +{"task": "/qid_resume", "qid": 15} +// Motor completes remaining distance +// Response: {"qid":15,"state":"done"} +``` + +### Timeout Test +Send motor command, physically block motor for >30 seconds: +```json +{"qid":X,"state":"timeout"} +``` + +### State Query +```json +{"task": "/qid_state", "qid": 15} +// Response: {"qid":15,"state":"busy"} or "done", "paused", etc. +``` diff --git a/ESP32/AS5311-AB-LIB-QUEUE/AS5311-AB-LIB-QUEUE.ino b/ESP32/AS5311-AB-LIB-QUEUE/AS5311-AB-LIB-QUEUE.ino deleted file mode 100644 index f8aa26a..0000000 --- a/ESP32/AS5311-AB-LIB-QUEUE/AS5311-AB-LIB-QUEUE.ino +++ /dev/null @@ -1,13 +0,0 @@ -#include "AS5311AB.h" - -AS5311AB encoder(33, 32); - -void setup() { - Serial.begin(115200); - encoder.begin(); -} - -void loop() { - Serial.println(encoder.getPosition()); - delay(1000); -} diff --git a/ESP32/AS5311-AB-LIB-QUEUE/AS5311AB.cpp b/ESP32/AS5311-AB-LIB-QUEUE/AS5311AB.cpp deleted file mode 100644 index fc905a8..0000000 --- a/ESP32/AS5311-AB-LIB-QUEUE/AS5311AB.cpp +++ /dev/null @@ -1,57 +0,0 @@ -#include "AS5311AB.h" - -volatile int AS5311AB::_encoderPos = 0; -QueueHandle_t AS5311AB::_encoderQueue = nullptr; -int AS5311AB::_pinA = 0; -int AS5311AB::_pinB = 0; - -AS5311AB::AS5311AB(int pinA, int pinB) - { - AS5311AB::_pinA = pinA; - AS5311AB::_pinB = pinB; - } - -void AS5311AB::begin() { - pinMode(_pinA, INPUT_PULLUP); - pinMode(_pinB, INPUT_PULLUP); - - attachInterrupt(digitalPinToInterrupt(_pinA), _handleAChange, CHANGE); - attachInterrupt(digitalPinToInterrupt(_pinB), _handleBChange, CHANGE); - - _encoderQueue = xQueueCreate(10, sizeof(int)); - xTaskCreate(_processEncoderData, "ProcessEncoder", 2048, NULL, 2, NULL); -} - -int AS5311AB::getPosition() { - int pos; - xQueueReceive(_encoderQueue, &pos, 0); // Non-blocking read - return pos; -} - -void IRAM_ATTR AS5311AB::_handleAChange() -{ - if (digitalRead(digitalPinToInterrupt(AS5311AB::_pinA)) == digitalRead(digitalPinToInterrupt(AS5311AB::_pinB))) { - AS5311AB::_encoderPos++; - } else { - AS5311AB::_encoderPos--; - } -} - -void IRAM_ATTR AS5311AB::_handleBChange() -{ - if (digitalRead(digitalPinToInterrupt(AS5311AB::_pinB)) == digitalRead(digitalPinToInterrupt(AS5311AB::_pinA))) { - AS5311AB::_encoderPos--; - } else { - AS5311AB::_encoderPos++; - } -} - -void AS5311AB::_processEncoderData(void* parameter) { - int position; - for (;;) { - if (xQueueReceive(_encoderQueue, &position, portMAX_DELAY)) { - // Handle your data here, ensure you are not in an ISR when using certain functions (like Serial). - Serial.println(position); - } - } -} diff --git a/ESP32/AS5311-AB-LIB-QUEUE/AS5311AB.h b/ESP32/AS5311-AB-LIB-QUEUE/AS5311AB.h deleted file mode 100644 index a7305df..0000000 --- a/ESP32/AS5311-AB-LIB-QUEUE/AS5311AB.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef AS5311AB_H -#define AS5311AB_H - -#include "Arduino.h" -#include "freertos/queue.h" - -class AS5311AB { -public: - AS5311AB(int pinA, int pinB); - void begin(); - int getPosition(); - -private: - static int _pinA; - static int _pinB; - static volatile int _encoderPos; - static QueueHandle_t _encoderQueue; - - - static void IRAM_ATTR _handleAChange(); - static void IRAM_ATTR _handleBChange(); - static void _processEncoderData(void* parameter); -}; - -#endif diff --git a/ESP32/AS5311-AB-LIB/AS5311-AB-LIB.ino b/ESP32/AS5311-AB-LIB/AS5311-AB-LIB.ino deleted file mode 100644 index 202d12c..0000000 --- a/ESP32/AS5311-AB-LIB/AS5311-AB-LIB.ino +++ /dev/null @@ -1,18 +0,0 @@ -#include "AS5311.h" - -AS5311 sensor(GPIO_NUM_26, GPIO_NUM_27); - -void setup() { - Serial.begin(115200); - sensor.begin(); -} - -void loop() { - float position = sensor.readPosition(); - int edgeCount = sensor.readEdgeCounter(); - if (position != -1.0f) { - Serial.println(position); - Serial.println(edgeCount); - } - delay(10); -} diff --git a/ESP32/AS5311-AB-LIB/AS5311.cpp b/ESP32/AS5311-AB-LIB/AS5311.cpp deleted file mode 100644 index 3b36bcf..0000000 --- a/ESP32/AS5311-AB-LIB/AS5311.cpp +++ /dev/null @@ -1,147 +0,0 @@ -#include "AS5311.h" -#include "freertos/queue.h" - -volatile uint32_t AS5311::_pos_edg_0 = 0; -volatile uint32_t AS5311::_pos_edg_1 = 0; -volatile uint32_t AS5311::_neg_edg_0 = 0; -volatile AS5311::PWM_Params AS5311::_pwm = {0, 0, 0}; -volatile int AS5311::_edgeCounter = 0; -volatile float AS5311::_position = 0.f; -volatile float AS5311::_offset = 0.f; -QueueHandle_t AS5311::dataQueue = nullptr; // Define the Queue handle globally. - -int AS5311::_pwmPin = 0; -int AS5311::_interruptPin = 0; - -AS5311::AS5311(int pwmPin, int interruptPin) { - AS5311::_pwmPin = pwmPin; - _interruptPin = interruptPin; -} - -void AS5311::begin() { - pinMode(_pwmPin, INPUT_PULLDOWN); - pinMode(_interruptPin, INPUT_PULLDOWN); - - attachInterrupt(digitalPinToInterrupt(_pwmPin), _Ext_PWM_ISR_handler, CHANGE); - attachInterrupt(digitalPinToInterrupt(_interruptPin), _handleRisingEdge, RISING); - - // Create a queue to handle PWM and EdgeCounter data in a safe context - dataQueue = xQueueCreate(10, sizeof(PWM_Params)); - - // Task creation should be added here - xTaskCreate( - handleDataTask, /* Task function. */ - "HandleData", /* String with name of task. */ - 10000, /* Stack size in bytes. */ - NULL, /* Parameter passed as input to the task */ - 1, /* Priority at which the task is created. */ - NULL); /* Task handle. */ -} - -float AS5311::readPWM() { - return _position; -} - -int AS5311::readEdgeCounter() { - return _edgeCounter; -} - -float AS5311::readPosition() { - return 2000.*(_edgeCounter+_position)+_offset; -} - -void AS5311::setOffset(float offset) { - _offset = offset; -} - -float AS5311::getOffset() { - return _offset; -} - -void IRAM_ATTR AS5311::_handleRisingEdge() { - PWM_Params localData; - localData.period = 0; - localData.duty_cycle = 0; - localData.edgeCounter = 1; - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xQueueSendFromISR(dataQueue, &localData, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken) { - portYIELD_FROM_ISR(); - } -} - -void IRAM_ATTR AS5311::_Ext_PWM_ISR_handler() { - uint32_t current_time = micros(); - PWM_Params localData; - localData.period = 0; - localData.duty_cycle = 0; - localData.edgeCounter = 0; - - if (digitalRead(_pwmPin) == HIGH) { - _pos_edg_1 = current_time; - localData.period = _pos_edg_1 - _pos_edg_0; - _pos_edg_0 = _pos_edg_1; - } else { - _neg_edg_0 = current_time; - localData.duty_cycle = _neg_edg_0 - _pos_edg_0; - } - - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xQueueSendFromISR(dataQueue, &localData, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken) { - portYIELD_FROM_ISR(); - } -} - -void AS5311::handleDataTask(void *parameter) { - PWM_Params receivedData; - PWM_Params localData; - localData = {0, 0, 0}; - float position = 0.f; - for (;;) { - if (xQueueReceive(dataQueue, &receivedData, portMAX_DELAY)) { - if (receivedData.edgeCounter != 0) { - if (_position>.5){ - _edgeCounter = _edgeCounter +1; - } - else{ - _edgeCounter = _edgeCounter -1; - } - //Serial.print("Edge Counter: "); - //Serial.println(_edgeCounter); - } - - if(receivedData.period != 0) { - //Serial.print("Period: "); - //Serial.println(receivedData.period); - localData.period = receivedData.period; - } - if (receivedData.duty_cycle != 0) { - //Serial.print("Duty Cycle: "); - //Serial.println(receivedData.duty_cycle); - localData.duty_cycle = receivedData.duty_cycle; - } - - // Calculate position - if (localData.period !=0 and localData.duty_cycle <= localData.period){ - // At the 0/1 change the duty cycle gets noisy, so we need to average it over multiple senses - _position = calculateRollingAverage((float) localData.duty_cycle / (float) localData.period); - } - - } - } -} - -float AS5311::calculateRollingAverage(float newVal) { - static float values[3] = {0.0, 0.0, 0.0}; - static int insertIndex = 0; - static float sum = 0.0; - - sum -= values[insertIndex]; - values[insertIndex] = newVal; - sum += newVal; - - insertIndex = (insertIndex + 1) % 3; - - return sum / 3.0; -} diff --git a/ESP32/AS5311-AB-LIB/AS5311.h b/ESP32/AS5311-AB-LIB/AS5311.h deleted file mode 100644 index d57c8dd..0000000 --- a/ESP32/AS5311-AB-LIB/AS5311.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef AS5311_H -#define AS5311_H - -#include - -class AS5311 { - public: - AS5311(int pwmPin, int interruptPin); - - void begin(); - float readPWM(); - int readEdgeCounter(); - float readPosition(); - static void setOffset(float offset); - static float getOffset(); - - private: - static int _pwmPin, _interruptPin; - static bool writing_counter; - static volatile uint32_t _pos_edg_0, _pos_edg_1, _neg_edg_0; - static volatile int _edgeCounter; - static volatile float _position; - static volatile float _offset; - static volatile bool _time_2_print; - static QueueHandle_t dataQueue; // Define the Queue handle globally. - static void handleDataTask(void *parameter); - static float calculateRollingAverage(float newVal); - typedef struct { - uint32_t period; - uint32_t duty_cycle; - int edgeCounter; - } PWM_Params; - - static volatile PWM_Params _pwm; - - static void IRAM_ATTR _handleRisingEdge(); - static void IRAM_ATTR _Ext_PWM_ISR_handler(); - static void IRAM_ATTR _print_adcpwm(); -}; - -#endif diff --git a/ESP32/AS5311-AB/AS5311-AB.ino b/ESP32/AS5311-AB/AS5311-AB.ino deleted file mode 100644 index 5009c3e..0000000 --- a/ESP32/AS5311-AB/AS5311-AB.ino +++ /dev/null @@ -1,52 +0,0 @@ -#include "freertos/queue.h" - -#define PIN_A 33 -#define PIN_B 32 - -volatile int encoder_pos = 0; -QueueHandle_t encoderQueue; - -void IRAM_ATTR handleAChange() { - if(digitalRead(PIN_A) == digitalRead(PIN_B)) { - encoder_pos++; - } else { - encoder_pos--; - } - xQueueSendFromISR(encoderQueue, (void *) &encoder_pos, NULL); -} - -void IRAM_ATTR handleBChange() { - if(digitalRead(PIN_A) != digitalRead(PIN_B)) { - encoder_pos++; - } else { - encoder_pos--; - } - xQueueSendFromISR(encoderQueue, (void *) &encoder_pos, NULL); -} - -void setup() { - Serial.begin(115200); - - pinMode(PIN_A, INPUT_PULLUP); - pinMode(PIN_B, INPUT_PULLUP); - - attachInterrupt(digitalPinToInterrupt(PIN_A), handleAChange, CHANGE); - attachInterrupt(digitalPinToInterrupt(PIN_B), handleBChange, CHANGE); - - encoderQueue = xQueueCreate(10, sizeof(int)); - xTaskCreate(processEncoderData, "ProcessEncoder", 2048, NULL, 2, NULL); -} - -void loop() { - // Your main code here -} - -void processEncoderData(void * parameter) { - int position; - for(;;) { - if(xQueueReceive(encoderQueue, &position, portMAX_DELAY)) { - Serial.println(position); - // Additional data processing can be performed here - } - } -} diff --git a/ESP32/AS5311-Forum/AS5311-Forum.ino b/ESP32/AS5311-Forum/AS5311-Forum.ino deleted file mode 100644 index 06681d3..0000000 --- a/ESP32/AS5311-Forum/AS5311-Forum.ino +++ /dev/null @@ -1,76 +0,0 @@ -#include - -const int PWM_PIN = GPIO_NUM_26; - -volatile uint32_t pos_edg_0 = 0; -volatile uint32_t pos_edg_1 = 0; -volatile uint32_t neg_edg_0 = 0; -volatile uint8_t time_2_print = 0; - -const int INTERRUPT_PIN = 27; // New interrupt pin -volatile uint32_t edgeCounter = 0; // Counter for rising edges -float currentPosition = 0; - - -void IRAM_ATTR handleRisingEdge() { // ISR for the new interrupt - if(currentPosition > 1000){ // - edgeCounter++; - } - else{ - edgeCounter--; - } -} - - -typedef struct { - uint32_t period; - uint32_t duty_cycle; -} PWM_Params; - -volatile PWM_Params pwm = {0, 0}; -hw_timer_t * timer = NULL; - -void IRAM_ATTR Ext_PWM_ISR_handler() { - uint32_t current_time = micros(); - - if (digitalRead(PWM_PIN) == HIGH) { // Rising edge - pos_edg_1 = current_time; - pwm.period = pos_edg_1 - pos_edg_0; - pos_edg_0 = pos_edg_1; - } else { // Falling edge - neg_edg_0 = current_time; - pwm.duty_cycle = neg_edg_0 - pos_edg_0; - } -} - -void IRAM_ATTR print_adcpwm() { - time_2_print = 1; -} - -void setup() { - Serial.begin(115200); - - pinMode(PWM_PIN, INPUT_PULLDOWN); - attachInterrupt(digitalPinToInterrupt(PWM_PIN), Ext_PWM_ISR_handler, CHANGE); - - // Setup for the new interrupt - pinMode(INTERRUPT_PIN, INPUT_PULLDOWN); - attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), handleRisingEdge, CHANGE); - - // Setup timer - timer = timerBegin(0, 80, true); // Use timer 0, with a prescaler of 80 - timerAttachInterrupt(timer, &print_adcpwm, false); - timerAlarmWrite(timer, 100000, true); // Set the timer to fire every 3 seconds - timerAlarmEnable(timer); -} - -void loop() { - if (time_2_print) { - currentPosition = (float)pwm.duty_cycle/(float)pwm.period*2000+2000*edgeCounter; - Serial.println(currentPosition); - time_2_print = 0; - Serial.printf("Rising Edge Counter: %u\n", edgeCounter); // Print the new counter - - } - delay(10); // Small delay to prevent tight loop. -} diff --git a/ESP32/AS5311-PWM-LIB-QUEUE/AS5311-PWM-LIB-QUEUE.ino b/ESP32/AS5311-PWM-LIB-QUEUE/AS5311-PWM-LIB-QUEUE.ino deleted file mode 100644 index 8c2c08f..0000000 --- a/ESP32/AS5311-PWM-LIB-QUEUE/AS5311-PWM-LIB-QUEUE.ino +++ /dev/null @@ -1,19 +0,0 @@ -#include "AS5311.h" - -AS5311 sensor(GPIO_NUM_32, GPIO_NUM_33); - -void setup() { - Serial.begin(115200); - sensor.begin(); -} - -void loop() { - - float position = sensor.readPosition(); - int edgeCount = sensor.readEdgeCounter(); - if (position != -1.0f) { - Serial.println((edgeCount+position)); - Serial.println(edgeCount); - } - delay(10); -} diff --git a/ESP32/AS5311-PWM-LIB-QUEUE/AS5311.cpp b/ESP32/AS5311-PWM-LIB-QUEUE/AS5311.cpp deleted file mode 100644 index 2502ae4..0000000 --- a/ESP32/AS5311-PWM-LIB-QUEUE/AS5311.cpp +++ /dev/null @@ -1,193 +0,0 @@ -#include "AS5311.h" -#include "freertos/queue.h" - -volatile uint32_t AS5311::_pos_edg_0 = 0; -volatile uint32_t AS5311::_pos_edg_1 = 0; -volatile uint32_t AS5311::_neg_edg_0 = 0; -volatile AS5311::PWM_Params AS5311::_pwm = {0, 0, 0}; -volatile int AS5311::_edgeCounter = 0; -volatile float AS5311::_position = 0.f; -volatile float AS5311::_offset = 0.f; -QueueHandle_t AS5311::dataQueue = nullptr; // Define the Queue handle globally. - -int AS5311::_pwmPin = 0; -int AS5311::_interruptPin = 0; -bool AS5311::_is_isr_service_installed = false; - -AS5311::AS5311(int pwmPin, int interruptPin, bool is_isr_service_installed = false) -{ - AS5311::_pwmPin = pwmPin; - _interruptPin = interruptPin; - _is_isr_service_installed = is_isr_service_installed; -} - -void AS5311::begin() -{ - if (!_is_isr_service_installed) - { - gpio_install_isr_service(0); - } - gpio_config_t io_conf_pwm = { - .pin_bit_mask = (1ULL << _pwmPin), - .mode = GPIO_MODE_INPUT, - .pull_up_en = GPIO_PULLUP_DISABLE, - .pull_down_en = GPIO_PULLDOWN_ENABLE, - .intr_type = GPIO_INTR_ANYEDGE, // To handle CHANGE interrupt type - }; - gpio_config(&io_conf_pwm); - - gpio_config_t io_conf_interrupt = { - .pin_bit_mask = (1ULL << _interruptPin), - .mode = GPIO_MODE_INPUT, - .pull_up_en = GPIO_PULLUP_DISABLE, - .pull_down_en = GPIO_PULLDOWN_ENABLE, - .intr_type = GPIO_INTR_POSEDGE, // To handle RISING interrupt type - }; - gpio_config(&io_conf_interrupt); - - - gpio_isr_handler_add((gpio_num_t)_pwmPin, _Ext_PWM_ISR_handler, (void *)_pwmPin); - gpio_isr_handler_add((gpio_num_t)_interruptPin, _handleRisingEdge, (void *)_interruptPin); - - // Create a queue to handle PWM and EdgeCounter data in a safe context - dataQueue = xQueueCreate(10, sizeof(PWM_Params)); - - // Task creation should be added here - xTaskCreate( - handleDataTask, /* Task function. */ - "HandleData", /* String with name of task. */ - 10000, /* Stack size in bytes. */ - NULL, /* Parameter passed as input to the task */ - 1, /* Priority at which the task is created. */ - NULL); /* Task handle. */ -} - -float AS5311::readPWM() -{ - return _position; -} - -int AS5311::readEdgeCounter() -{ - return _edgeCounter; -} - -float AS5311::readPosition() -{ - return 2000. * (_edgeCounter + _position) + _offset; -} - -void AS5311::setOffset(float offset) -{ - _offset = offset; -} - -float AS5311::getOffset() -{ - return _offset; -} - -void IRAM_ATTR AS5311::_handleRisingEdge(void* arg) -{ - log_i("Rising edge detected on pin %d", (uint32_t)arg); - PWM_Params localData; - localData.period = 0; - localData.duty_cycle = 0; - localData.edgeCounter = 1; - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xQueueSendFromISR(dataQueue, &localData, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken) - { - portYIELD_FROM_ISR(); - } -} - -void IRAM_ATTR AS5311::_Ext_PWM_ISR_handler(void* arg) -{ - uint32_t current_time = micros(); - PWM_Params localData; - localData.period = 0; - localData.duty_cycle = 0; - localData.edgeCounter = 0; - - if (digitalRead(_pwmPin) == HIGH) - { - _pos_edg_1 = current_time; - localData.period = _pos_edg_1 - _pos_edg_0; - _pos_edg_0 = _pos_edg_1; - } - else - { - _neg_edg_0 = current_time; - localData.duty_cycle = _neg_edg_0 - _pos_edg_0; - } - - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xQueueSendFromISR(dataQueue, &localData, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken) - { - portYIELD_FROM_ISR(); - } -} - -void AS5311::handleDataTask(void *parameter) -{ - PWM_Params receivedData; - PWM_Params localData; - localData = {0, 0, 0}; - float position = 0.f; - for (;;) - { - if (xQueueReceive(dataQueue, &receivedData, portMAX_DELAY)) - { - if (receivedData.edgeCounter != 0) - { - if (_position > .5) - { - _edgeCounter = _edgeCounter + 1; - } - else - { - _edgeCounter = _edgeCounter - 1; - } - // Serial.print("Edge Counter: "); - // Serial.println(_edgeCounter); - } - - if (receivedData.period != 0) - { - // Serial.print("Period: "); - // Serial.println(receivedData.period); - localData.period = receivedData.period; - } - if (receivedData.duty_cycle != 0) - { - // Serial.print("Duty Cycle: "); - // Serial.println(receivedData.duty_cycle); - localData.duty_cycle = receivedData.duty_cycle; - } - - // Calculate position - if (localData.period != 0 and localData.duty_cycle <= localData.period) - { - // At the 0/1 change the duty cycle gets noisy, so we need to average it over multiple senses - _position = calculateRollingAverage((float)localData.duty_cycle / (float)localData.period); - } - } - } -} - -float AS5311::calculateRollingAverage(float newVal) -{ - static float values[3] = {0.0, 0.0, 0.0}; - static int insertIndex = 0; - static float sum = 0.0; - - sum -= values[insertIndex]; - values[insertIndex] = newVal; - sum += newVal; - - insertIndex = (insertIndex + 1) % 3; - - return sum / 3.0; -} diff --git a/ESP32/AS5311-PWM-LIB-QUEUE/AS5311.h b/ESP32/AS5311-PWM-LIB-QUEUE/AS5311.h deleted file mode 100644 index 5c124bf..0000000 --- a/ESP32/AS5311-PWM-LIB-QUEUE/AS5311.h +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef AS5311_H -#define AS5311_H - -#include - -class AS5311 { - public: - AS5311(int pwmPin, int interruptPin, bool is_isr_service_installed); - - void begin(); - float readPWM(); - int readEdgeCounter(); - float readPosition(); - static void setOffset(float offset); - static float getOffset(); - - private: - static int _pwmPin, _interruptPin; - static bool writing_counter; - static volatile uint32_t _pos_edg_0, _pos_edg_1, _neg_edg_0; - static volatile int _edgeCounter; - static volatile float _position; - static volatile float _offset; - static volatile bool _time_2_print; - static QueueHandle_t dataQueue; // Define the Queue handle globally. - static void handleDataTask(void *parameter); - static float calculateRollingAverage(float newVal); - static bool _is_isr_service_installed; - - typedef struct { - uint32_t period; - uint32_t duty_cycle; - int edgeCounter; - } PWM_Params; - - static volatile PWM_Params _pwm; - - static void IRAM_ATTR _handleRisingEdge(void* arg); - static void IRAM_ATTR _Ext_PWM_ISR_handler(void* arg); - static void IRAM_ATTR _print_adcpwm(); -}; - -#endif diff --git a/ESP32/AS5311-PWM-LIB/AS5311-PWM-LIB.ino b/ESP32/AS5311-PWM-LIB/AS5311-PWM-LIB.ino deleted file mode 100644 index 202d12c..0000000 --- a/ESP32/AS5311-PWM-LIB/AS5311-PWM-LIB.ino +++ /dev/null @@ -1,18 +0,0 @@ -#include "AS5311.h" - -AS5311 sensor(GPIO_NUM_26, GPIO_NUM_27); - -void setup() { - Serial.begin(115200); - sensor.begin(); -} - -void loop() { - float position = sensor.readPosition(); - int edgeCount = sensor.readEdgeCounter(); - if (position != -1.0f) { - Serial.println(position); - Serial.println(edgeCount); - } - delay(10); -} diff --git a/ESP32/AS5311-PWM-LIB/AS5311.cpp b/ESP32/AS5311-PWM-LIB/AS5311.cpp deleted file mode 100644 index 3b36bcf..0000000 --- a/ESP32/AS5311-PWM-LIB/AS5311.cpp +++ /dev/null @@ -1,147 +0,0 @@ -#include "AS5311.h" -#include "freertos/queue.h" - -volatile uint32_t AS5311::_pos_edg_0 = 0; -volatile uint32_t AS5311::_pos_edg_1 = 0; -volatile uint32_t AS5311::_neg_edg_0 = 0; -volatile AS5311::PWM_Params AS5311::_pwm = {0, 0, 0}; -volatile int AS5311::_edgeCounter = 0; -volatile float AS5311::_position = 0.f; -volatile float AS5311::_offset = 0.f; -QueueHandle_t AS5311::dataQueue = nullptr; // Define the Queue handle globally. - -int AS5311::_pwmPin = 0; -int AS5311::_interruptPin = 0; - -AS5311::AS5311(int pwmPin, int interruptPin) { - AS5311::_pwmPin = pwmPin; - _interruptPin = interruptPin; -} - -void AS5311::begin() { - pinMode(_pwmPin, INPUT_PULLDOWN); - pinMode(_interruptPin, INPUT_PULLDOWN); - - attachInterrupt(digitalPinToInterrupt(_pwmPin), _Ext_PWM_ISR_handler, CHANGE); - attachInterrupt(digitalPinToInterrupt(_interruptPin), _handleRisingEdge, RISING); - - // Create a queue to handle PWM and EdgeCounter data in a safe context - dataQueue = xQueueCreate(10, sizeof(PWM_Params)); - - // Task creation should be added here - xTaskCreate( - handleDataTask, /* Task function. */ - "HandleData", /* String with name of task. */ - 10000, /* Stack size in bytes. */ - NULL, /* Parameter passed as input to the task */ - 1, /* Priority at which the task is created. */ - NULL); /* Task handle. */ -} - -float AS5311::readPWM() { - return _position; -} - -int AS5311::readEdgeCounter() { - return _edgeCounter; -} - -float AS5311::readPosition() { - return 2000.*(_edgeCounter+_position)+_offset; -} - -void AS5311::setOffset(float offset) { - _offset = offset; -} - -float AS5311::getOffset() { - return _offset; -} - -void IRAM_ATTR AS5311::_handleRisingEdge() { - PWM_Params localData; - localData.period = 0; - localData.duty_cycle = 0; - localData.edgeCounter = 1; - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xQueueSendFromISR(dataQueue, &localData, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken) { - portYIELD_FROM_ISR(); - } -} - -void IRAM_ATTR AS5311::_Ext_PWM_ISR_handler() { - uint32_t current_time = micros(); - PWM_Params localData; - localData.period = 0; - localData.duty_cycle = 0; - localData.edgeCounter = 0; - - if (digitalRead(_pwmPin) == HIGH) { - _pos_edg_1 = current_time; - localData.period = _pos_edg_1 - _pos_edg_0; - _pos_edg_0 = _pos_edg_1; - } else { - _neg_edg_0 = current_time; - localData.duty_cycle = _neg_edg_0 - _pos_edg_0; - } - - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xQueueSendFromISR(dataQueue, &localData, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken) { - portYIELD_FROM_ISR(); - } -} - -void AS5311::handleDataTask(void *parameter) { - PWM_Params receivedData; - PWM_Params localData; - localData = {0, 0, 0}; - float position = 0.f; - for (;;) { - if (xQueueReceive(dataQueue, &receivedData, portMAX_DELAY)) { - if (receivedData.edgeCounter != 0) { - if (_position>.5){ - _edgeCounter = _edgeCounter +1; - } - else{ - _edgeCounter = _edgeCounter -1; - } - //Serial.print("Edge Counter: "); - //Serial.println(_edgeCounter); - } - - if(receivedData.period != 0) { - //Serial.print("Period: "); - //Serial.println(receivedData.period); - localData.period = receivedData.period; - } - if (receivedData.duty_cycle != 0) { - //Serial.print("Duty Cycle: "); - //Serial.println(receivedData.duty_cycle); - localData.duty_cycle = receivedData.duty_cycle; - } - - // Calculate position - if (localData.period !=0 and localData.duty_cycle <= localData.period){ - // At the 0/1 change the duty cycle gets noisy, so we need to average it over multiple senses - _position = calculateRollingAverage((float) localData.duty_cycle / (float) localData.period); - } - - } - } -} - -float AS5311::calculateRollingAverage(float newVal) { - static float values[3] = {0.0, 0.0, 0.0}; - static int insertIndex = 0; - static float sum = 0.0; - - sum -= values[insertIndex]; - values[insertIndex] = newVal; - sum += newVal; - - insertIndex = (insertIndex + 1) % 3; - - return sum / 3.0; -} diff --git a/ESP32/AS5311-PWM-LIB/AS5311.h b/ESP32/AS5311-PWM-LIB/AS5311.h deleted file mode 100644 index d57c8dd..0000000 --- a/ESP32/AS5311-PWM-LIB/AS5311.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef AS5311_H -#define AS5311_H - -#include - -class AS5311 { - public: - AS5311(int pwmPin, int interruptPin); - - void begin(); - float readPWM(); - int readEdgeCounter(); - float readPosition(); - static void setOffset(float offset); - static float getOffset(); - - private: - static int _pwmPin, _interruptPin; - static bool writing_counter; - static volatile uint32_t _pos_edg_0, _pos_edg_1, _neg_edg_0; - static volatile int _edgeCounter; - static volatile float _position; - static volatile float _offset; - static volatile bool _time_2_print; - static QueueHandle_t dataQueue; // Define the Queue handle globally. - static void handleDataTask(void *parameter); - static float calculateRollingAverage(float newVal); - typedef struct { - uint32_t period; - uint32_t duty_cycle; - int edgeCounter; - } PWM_Params; - - static volatile PWM_Params _pwm; - - static void IRAM_ATTR _handleRisingEdge(); - static void IRAM_ATTR _Ext_PWM_ISR_handler(); - static void IRAM_ATTR _print_adcpwm(); -}; - -#endif diff --git a/ESP32/AS5311-PWM/AS5311-PWM.ino b/ESP32/AS5311-PWM/AS5311-PWM.ino deleted file mode 100644 index c9c52e6..0000000 --- a/ESP32/AS5311-PWM/AS5311-PWM.ino +++ /dev/null @@ -1,106 +0,0 @@ -#include "Arduino.h" -#include "driver/mcpwm.h" -#include "driver/gpio.h" - -#define GPIO_CAP0_IN GPIO_NUM_26 -#define GPIO_CAP1_IN GPIO_NUM_27 -#define GPIO_TRIGGER GPIO_NUM_14 // New GPIO pin for triggering the counter - -struct CaptureData { - uint32_t time_rising_edge; - uint32_t time_falling_edge; - uint32_t period; - bool last_was_rising; -}; - -#define ISR_MIN_INTERVAL 5 // for 1ms, adjust as needed -static volatile uint32_t lastISRTime = 0; - - -CaptureData cap_data[2]; -volatile float latest_duty_cycle[2] = {0.0f, 0.0f}; // Updated by the MCPWM capture ISR -volatile int counter = 0; - -void IRAM_ATTR capture_handler(void *arg) { - uint32_t currentISRTime = micros(); - if ((currentISRTime - lastISRTime) < ISR_MIN_INTERVAL) { - return; // Exit ISR if called too soon - } - lastISRTime = currentISRTime; - uint32_t cap_unit = (uint32_t)arg; - uint32_t curr_time = mcpwm_capture_signal_get_value(MCPWM_UNIT_0, (mcpwm_capture_signal_t)cap_unit); - - if (mcpwm_capture_signal_get_edge(MCPWM_UNIT_0, (mcpwm_capture_signal_t)cap_unit) == MCPWM_POS_EDGE) { - if (cap_data[cap_unit].last_was_rising) { - cap_data[cap_unit].period = curr_time - cap_data[cap_unit].time_rising_edge; - } - cap_data[cap_unit].time_rising_edge = curr_time; - cap_data[cap_unit].last_was_rising = true; - } else { - cap_data[cap_unit].time_falling_edge = curr_time; - cap_data[cap_unit].last_was_rising = false; - } - - - if (cap_data[cap_unit].period != 0) { - uint32_t high_time = cap_data[cap_unit].time_falling_edge - cap_data[cap_unit].time_rising_edge; - if ((float)cap_data[cap_unit].period > 0){ - /*Serial.print((float)high_time ); - Serial.print("/"); - Serial.println((float)cap_data[cap_unit].period);*/ - latest_duty_cycle[cap_unit] = ((float)high_time / (float)cap_data[cap_unit].period) ; - - } - } -} - -void IRAM_ATTR trigger_handler(void* arg) { - for (int i = 0; i < 2; i++) { - if (latest_duty_cycle[i] > 50.0f) { - counter++; - } else { - counter--; - } - } - Serial.printf("Counter (triggered): %d\n", counter); -} - -void setup() { - Serial.begin(115200); - Serial.println("Testing MCPWM capture on two pins..."); - - // Initialize MCPWM capture - mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM_CAP_0, GPIO_CAP0_IN); - mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM_CAP_1, GPIO_CAP1_IN); - - mcpwm_capture_enable(MCPWM_UNIT_0, MCPWM_SELECT_CAP0, MCPWM_BOTH_EDGE, 0); - mcpwm_capture_enable(MCPWM_UNIT_0, MCPWM_SELECT_CAP1, MCPWM_BOTH_EDGE, 0); - - intr_handle_t cap0_intr_handle = NULL; - intr_handle_t cap1_intr_handle = NULL; - - mcpwm_isr_register(MCPWM_UNIT_0, capture_handler, (void*)MCPWM_SELECT_CAP0, ESP_INTR_FLAG_IRAM, &cap0_intr_handle); - mcpwm_isr_register(MCPWM_UNIT_0, capture_handler, (void*)MCPWM_SELECT_CAP1, ESP_INTR_FLAG_IRAM, &cap1_intr_handle); - - // Initialize the GPIO_TRIGGER pin for external interrupt - /* - gpio_config_t io_conf; - io_conf.intr_type = GPIO_INTR_POSEDGE; // Trigger on rising edge - io_conf.mode = GPIO_MODE_INPUT; - io_conf.pin_bit_mask = (1ULL << GPIO_TRIGGER); - io_conf.pull_down_en = GPIO_PULLDOWN_ENABLE; - io_conf.pull_up_en = GPIO_PULLUP_DISABLE; - gpio_config(&io_conf); - */ - - // Register ISR for the trigger pin - gpio_install_isr_service(ESP_INTR_FLAG_IRAM); - //gpio_isr_handler_add(GPIO_TRIGGER, trigger_handler, NULL); - - // Clear capture data - memset(cap_data, 0, sizeof(cap_data)); -} - -void loop() { - Serial.println(latest_duty_cycle[0] ); -} diff --git a/ESP32/AS5311/AS5311.ino b/ESP32/AS5311/AS5311.ino deleted file mode 100644 index dc6dd92..0000000 --- a/ESP32/AS5311/AS5311.ino +++ /dev/null @@ -1,58 +0,0 @@ -#include "AS5311_BD.h" - -AS5311 myAS5311(5,18,19); // data, clock, chip select - -const int pwmInputPin = 34; // Replace with your chosen analog input pin -const int maxPWMCount = 4095; // Maximum PWM count for AS5311 -int currentPosition1 = 0; // Initialize the absolute position counter - - -const int interruptPin = 34; -volatile int counter = 0; - -long value = 0; -void incrementCounter() { - - - // comming from high to low value - if (value>2048) - counter++; - else - counter--; -} - - -void setup() -{ - Serial.begin(115200); - pinMode(interruptPin, INPUT_PULLUP); - attachInterrupt(digitalPinToInterrupt(interruptPin), incrementCounter, RISING); -} - -int posLast = 0; - -void loop() -{ - value = myAS5311.encoder_value(); - - Serial.print("measured value: "); - Serial.println(value+counter*4096); - - if(0){ - int pwmValue = analogRead(pwmInputPin); - Serial.print("AS5311 RElative Position: "); - Serial.println(pwmValue); - - // Calculate the change in position from the previous reading - int positionChange = pwmValue - posLast; // Assuming the midpoint is 50% duty cycle - Serial.print("last Position: "); - Serial.println(posLast); - currentPosition1 = currentPosition1 + positionChange; - Serial.print("AS5311 Absolute Position: "); - Serial.println(currentPosition1); - - posLast = pwmValue; - - - } -} diff --git a/ESP32/AS5311/AS5311_BD.cpp b/ESP32/AS5311/AS5311_BD.cpp deleted file mode 100644 index 9277477..0000000 --- a/ESP32/AS5311/AS5311_BD.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#include "Arduino.h" -#include "AS5311_BD.h" - -AS5311::AS5311(uint16_t DataPin, uint16_t ClockPin, uint16_t ChipSelectPin) - : _data(DataPin), _clock(ClockPin), _cs(ChipSelectPin) -{ - pinMode(_data, INPUT); - pinMode(_clock, OUTPUT); - pinMode(_cs, OUTPUT); -} - -uint32_t AS5311::encoder_position() -{ - return ((encoder_value() * 2) / 4096); -} - -uint32_t AS5311::encoder_value() -{ - return (read_chip() >> 6); -} - -uint32_t AS5311::encoder_error() -{ - uint16_t error_code; - uint32_t raw_value; - raw_value = read_chip(); - error_code = raw_value & 0b000000000000111111; - err_value.DECn = error_code & 2; - err_value.INCn = error_code & 4; - err_value.LIN = error_code & 8; - err_value.COF = error_code & 16; - err_value.OCF = error_code & 32; - return error_code; -} - -uint32_t AS5311::read_chip() -{ - uint32_t raw_value = 0; - uint16_t inputstream = 0; - uint16_t c; - - digitalWrite(_cs, HIGH); - digitalWrite(_clock, HIGH); - delayMicroseconds(1); // Reduced delay to 1 microsecond - digitalWrite(_cs, LOW); - delayMicroseconds(1); // Reduced delay to 1 microsecond - digitalWrite(_clock, LOW); - delayMicroseconds(1); // Reduced delay to 1 microsecond - - for (c = 0; c < 18; c++) - { - digitalWrite(_clock, HIGH); - delayMicroseconds(1); // Reduced delay to 1 microsecond - inputstream = digitalRead(_data); - raw_value = ((raw_value << 1) + inputstream); - digitalWrite(_clock, LOW); - delayMicroseconds(1); // Reduced delay to 1 microsecond - } - - return raw_value; -} diff --git a/ESP32/AS5311/AS5311_BD.h b/ESP32/AS5311/AS5311_BD.h deleted file mode 100644 index d2ca717..0000000 --- a/ESP32/AS5311/AS5311_BD.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef AS5311_h -#define AS5311_h - -#include "Arduino.h" - -class AS5311 -{ - public: - AS5311(uint16_t DataPin, uint16_t ClockPin, uint16_t ChipSelectPin); - uint32_t encoder_position(void); - uint32_t encoder_value(void); - uint32_t encoder_error(void); - struct err_struct{ - bool DECn; - bool INCn; - bool OCF; - bool COF; - bool LIN; } err_value; - private: - uint32_t read_chip(void); - const uint16_t _clock; // clock pin: output from arduino to as5311 - const uint16_t _cs; // chip select: output - const uint16_t _data; // data pin: input - -}; - -#endif diff --git a/ESP32/digital-calliper/esp32-digital-caliper-WithInterrupts/esp32-digital-caliper-WithInterrupts.ino b/ESP32/digital-calliper/esp32-digital-caliper-WithInterrupts/esp32-digital-caliper-WithInterrupts.ino deleted file mode 100644 index 686e7d8..0000000 --- a/ESP32/digital-calliper/esp32-digital-caliper-WithInterrupts/esp32-digital-caliper-WithInterrupts.ino +++ /dev/null @@ -1,64 +0,0 @@ -// IR Decoder Example using Interrupts on ESP32 - -#include - -// Variables -volatile int i; -volatile int sign; -volatile long value; -volatile float result; -const int clockpin = 33; -const int datapin = 32; -volatile unsigned long tempmicros; -volatile bool decoding = false; - -// Function Declarations -void IRAM_ATTR handleClockInterrupt(); -void IRAM_ATTR handleDataInterrupt(); -void decode(); - -void setup() { - Serial.begin(115200); - pinMode(clockpin, INPUT_PULLUP); // Ensure pull-up for clockpin as the interrupt will trigger on falling edge - pinMode(datapin, INPUT_PULLUP); // Ensure pull-up for datapin as the interrupt will trigger on falling edge - - attachInterrupt(digitalPinToInterrupt(clockpin), handleClockInterrupt, FALLING); // Attach clockpin interrupt - attachInterrupt(digitalPinToInterrupt(datapin), handleDataInterrupt, FALLING); // Attach datapin interrupt -} - -void loop() { - // Do nothing here, the decoding is handled by interrupts -} - -void IRAM_ATTR handleClockInterrupt() { - if (decoding == false) { - tempmicros = micros(); - } else { - if ((micros() - tempmicros) > 500) { - decoding = false; - decode(); - } - } - decoding = !decoding; -} - -void IRAM_ATTR handleDataInterrupt() { - if (decoding) { - if (i < 20) { - value |= 1 << i; - } - if (i == 20) { - sign = -1; - } - i++; - } -} - -void decode() { - value = 0; - sign = 1; - i = 0; - result = (value * sign) / 100.00; - Serial.println(result, 2); - delay(50); -} diff --git a/ESP32/digital-calliper/esp32-digital-calliper/esp32-digital-calliper.ino b/ESP32/digital-calliper/esp32-digital-calliper/esp32-digital-calliper.ino deleted file mode 100644 index 0780cd5..0000000 --- a/ESP32/digital-calliper/esp32-digital-calliper/esp32-digital-calliper.ino +++ /dev/null @@ -1,61 +0,0 @@ -// IR Decoder Example - -// Libraries -#include - -// Variables -int i; -int sign; -long value; -float result; -int clockpin = 33; -int datapin = 32; -unsigned long tempmicros; - -// Function Declarations -void setup(); -void loop(); -void decode(); - -// Setup Function - Runs once at the start -void setup() { - Serial.begin(115200); // Initialize Serial communication at 115200 baud rate - pinMode(clockpin, INPUT); // Set clockpin as an input - pinMode(datapin, INPUT); // Set datapin as an input -} - -// Loop Function - Runs repeatedly -void loop() { - while (digitalRead(clockpin) == HIGH) {} // If clock is HIGH, wait until it turns to LOW - tempmicros = micros(); // Record the current micros() value - - while (digitalRead(clockpin) == LOW) {} // Wait for the end of the HIGH pulse - - if ((micros() - tempmicros) > 500) { // If the HIGH pulse was longer than 500 microseconds, we are at the start of a new bit sequence - decode(); // Decode the bit sequence - } -} - -// Decode Function - Decodes the received IR signal -void decode() { - sign = 1; // Initialize sign to positive - value = 0; // Initialize value to zero - - for (i = 0; i < 23; i++) { - while (digitalRead(clockpin) == HIGH) {} // Wait until clock returns to HIGH (the first bit is not needed) - while (digitalRead(clockpin) == LOW) {} // Wait until clock returns to LOW - - if (digitalRead(datapin) == LOW) { - if (i < 20) { - value |= 1 << i; // Set the bit in 'value' at position 'i' to 1 - } - if (i == 20) { - sign = -1; // Set sign to negative - } - } - } - - result = (value * sign) / 100.00; // Calculate the result (value with sign and two decimal places) - Serial.println(result); // Print the result with 2 decimal places - delay(50); // Delay for a short time to avoid continuous decoding -} diff --git a/ESP32/ds1822-temperaturecontrol/ds1822-temperaturecontrol.ino b/ESP32/ds1822-temperaturecontrol/ds1822-temperaturecontrol.ino deleted file mode 100644 index 94874f8..0000000 --- a/ESP32/ds1822-temperaturecontrol/ds1822-temperaturecontrol.ino +++ /dev/null @@ -1,74 +0,0 @@ -#include -#include -#include - -// GPIO where the DS18B20 is connected to -const int oneWireBus = GPIO_NUM_25; - - -// Setup a oneWire instance to communicate with any OneWire devices -OneWire oneWire(oneWireBus); - -// Pass our oneWire reference to Dallas Temperature sensor -DallasTemperature sensors(&oneWire); - -// PID constants -const float Kp = 2.0; -const float Ki = 0.5; -const float Kd = 1.0; - -// PID variables -float previousError = 0; -float integral = 0; - -// Desired temperature -const float setpoint = 36.0; // for example, 50°C - -// LEDC variables -const int freq = 5000; -const int ledChannel = 0; -const int resolution = 8; -const int heaterPin = GPIO_NUM_12; // PWM pin connected to the heater - -// Get the current temperature -// Here you'll want to replace this with your actual temperature reading logic -float getTemperature() { - // Dummy implementation, replace with your sensor's logic - sensors.requestTemperatures(); - return sensors.getTempCByIndex(0); -} - -void setup() { - Serial.begin(115200); - sensors.begin(); - // Setup LEDC for heater control - ledcSetup(ledChannel, freq, resolution); - ledcAttachPin(heaterPin, ledChannel); -} - -void loop() { - - float currentTemperature = getTemperature(); - float error = setpoint - currentTemperature; - - integral += error; - float derivative = error - previousError; - - // Calculate PID output - float output = Kp * error + Ki * integral + Kd * derivative; - - // Ensure output is within the range [0, 255] for the 8-bit PWM resolution - output = constrain(output, 0, 255); - - // Set the PWM signal - ledcWrite(ledChannel, (int)output); - - // Update the previous error - previousError = error; - - // Print values for debugging (optional) - Serial.print("Current Temperature: "); Serial.print(currentTemperature); - Serial.print(" °C | PWM Output: "); Serial.println(output); - - delay(1000); -} diff --git a/ESP32/main/control_DAC.ino b/ESP32/main/control_DAC.ino deleted file mode 100644 index 8131e5d..0000000 --- a/ESP32/main/control_DAC.ino +++ /dev/null @@ -1,158 +0,0 @@ - -#if defined(IS_DAC) || defined(IS_DAC_FAKE) - -// Custom function accessible by the API -void dac_act_fct() { - // here you can do something - - Serial.println("dac_act_fct"); - - // apply default parameters - // DAC Channel - dac_channel = DAC_CHANNEL_1; - if (jsonDocument.containsKey("dac_channel")) { - dac_channel = jsonDocument["dac_channel"]; - } - - // DAC Frequency - frequency = 1000; - if (jsonDocument.containsKey("frequency")) { - frequency = jsonDocument["frequency"]; - } - - // DAC offset - int offset = 0; - if (jsonDocument.containsKey("offset")) { - int offset = jsonDocument["offset"]; - } - - // DAC amplitude - int amplitude = 0; - if (jsonDocument.containsKey("amplitude")) { - int amplitude = jsonDocument["amplitude"]; - } - - // DAC clk_div - int clk_div = 0; - if (jsonDocument.containsKey("clk_div")) { - int clk_div = jsonDocument["clk_div"]; - } - - if (jsonDocument["dac_channel"] == 1) - dac_channel = DAC_CHANNEL_1; - else if (jsonDocument["dac_channel"] == 2) - dac_channel = DAC_CHANNEL_2; - - //Scale output of a DAC channel using two bit pattern: - if (amplitude == 0) scale = 0; - else if (amplitude == 1) scale = 01; - else if (amplitude == 2) scale = 10; - else if (amplitude == 3) scale = 11; - - - if (DEBUG) { - Serial.print("dac_channel "); Serial.println(dac_channel); - Serial.print("frequency "); Serial.println(frequency); - Serial.print("offset "); Serial.println(offset); - } - - #ifdef IS_DAC - - if (dac_is_running) - if (frequency == 0) { - dac_is_running = false; - dac->Stop(dac_channel); - dacWrite(dac_channel, offset); - } - else { - dac->Stop(dac_channel); - dac->Setup(dac_channel, clk_div, frequency, scale, phase, invert); - dac_is_running = true; - } - else { - dac->Setup(dac_channel, clk_div, frequency, scale, phase, invert); - dac->dac_offset_set(dac_channel, offset); - } - #endif - - jsonDocument.clear(); - jsonDocument["return"] = 1; -} - -void dac_set_fct() { - // here you can set parameters - int value = jsonDocument["value"]; - - if (DEBUG) { - Serial.print("value "); Serial.println(value); - } - - int dac_set = jsonDocument["dac_set"]; - - if (dac_set != NULL) { - if (DEBUG) Serial.print("dac_set "); Serial.println(dac_set); - // SET SOMETHING - } - - jsonDocument.clear(); - jsonDocument["return"] = 1; - -} - - - - - -// Custom function accessible by the API -void dac_get_fct() { - // GET SOME PARAMETERS HERE - int dac_variable = 12343; - - jsonDocument.clear(); - jsonDocument["dac_variable"] = dac_variable; -} - - -/* - wrapper for HTTP requests -*/ -void dac_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - dac_act_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void dac_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - dac_get_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - - -// wrapper for HTTP requests -void dac_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - dac_set_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - - - -void drive_galvo(void * parameter){ - while(true){ // infinite loop - digitalWrite(DAC_FAKE_PIN_1, HIGH); - digitalWrite(DAC_FAKE_PIN_2, HIGH); - vTaskDelay(frequency/portTICK_PERIOD_MS); // pause 1ms - digitalWrite(DAC_FAKE_PIN_1, LOW); - digitalWrite(DAC_FAKE_PIN_2, LOW); - vTaskDelay(frequency/portTICK_PERIOD_MS); // pause 1ms - } -} -#endif diff --git a/ESP32/main/control_LEDarray.ino b/ESP32/main/control_LEDarray.ino deleted file mode 100644 index fc57e8c..0000000 --- a/ESP32/main/control_LEDarray.ino +++ /dev/null @@ -1,231 +0,0 @@ - - - -// Custom function accessible by the API -void ledarr_act_fct() { - - // here you can do something - if (DEBUG) Serial.println("ledarr_act_fct"); - isBusy = true; - - - const char* LEDArrMode = jsonDocument["LEDArrMode"]; // "array", "full", "full", "single", "off", "left", "right", "top", "bottom", - - // individual pattern gets adressed - // PYTHON: send_LEDMatrix_array(self, led_pattern, timeout=1) - if (strcmp(LEDArrMode, "array") == 0) { - if (DEBUG) Serial.println("pattern"); - for (int iled = 0; iled < LED_ARRAY_NUM; iled++) { //Iterate through results - int red = jsonDocument["red"][iled]; //Implicit cast - int green = jsonDocument["green"][iled]; //Implicit cast - int blue = jsonDocument["blue"][iled]; //Implicit cast - set_led_RGB(iled, red, green, blue); - } - } - // only if a single led will be updated, all others stay the same - // PYTHON: send_LEDMatrix_single(self, indexled=0, intensity=(255,255,255), timeout=1) - else if (strcmp(LEDArrMode, "single") == 0) { - if (DEBUG) Serial.println("single"); - int indexled = jsonDocument["indexled"]; - int red = jsonDocument["red"]; //Implicit cast - int green = jsonDocument["green"]; //Implicit cast - int blue = jsonDocument["blue"]; //Implicit cast - //if (DEBUG) Serial.print(red); Serial.print(green); Serial.println(blue); - set_led_RGB(indexled, red, green, blue); - } - // only few leds will be updated, all others stay the same - // PYTHON: send_LEDMatrix_multi(self, indexled=(0), intensity=((255,255,255)), Nleds=8*8, timeout=1) - else if (strcmp(LEDArrMode, "multi") == 0) { - if (DEBUG) Serial.println("multi"); - int Nleds = jsonDocument["Nleds"]; - for (int i = 0; i < Nleds; i++) { //Iterate through results - int indexled = jsonDocument["indexled"][i]; - int red = jsonDocument["red"][indexled]; //Implicit cast - int green = jsonDocument["green"][indexled]; //Implicit cast - int blue = jsonDocument["blue"][indexled]; //Implicit cast - //if (DEBUG) Serial.print(red); Serial.print(green); Serial.println(blue); - set_led_RGB(indexled, red, green, blue); } - } - // turn on all LEDs - // PYTHON: send_LEDMatrix_full(self, intensity = (255,255,255),timeout=1) - else if (strcmp(LEDArrMode, "full") == 0) { - if (DEBUG) Serial.println("full"); - int red = jsonDocument["red"]; - int green = jsonDocument["green"]; - int blue = jsonDocument["blue"]; - set_all(red, green, blue); - } - // turn off all LEDs - else if (strcmp(LEDArrMode, "left") == 0) { - if (DEBUG) Serial.println("left"); - int red = jsonDocument["red"]; - int green = jsonDocument["green"]; - int blue = jsonDocument["blue"]; - int NLeds = jsonDocument["NLeds"]; - set_left(NLeds, red, green, blue); - } - // turn off all LEDs - else if (strcmp(LEDArrMode, "right") == 0) { - if (DEBUG) Serial.println("right"); - int red = jsonDocument["red"]; - int green = jsonDocument["green"]; - int blue = jsonDocument["blue"]; - int NLeds = jsonDocument["NLeds"]; - set_right(NLeds, red, green, blue); - } - // turn off all LEDs - else if (strcmp(LEDArrMode, "top") == 0) { - if (DEBUG) Serial.println("top"); - int red = jsonDocument["red"]; - int green = jsonDocument["green"]; - int blue = jsonDocument["blue"]; - int NLeds = jsonDocument["NLeds"]; - set_top(NLeds, red, green, blue); - } - // turn off all LEDs - else if (strcmp(LEDArrMode, "bottom") == 0) { - if (DEBUG) Serial.println("bottom"); - int red = jsonDocument["red"]; - int green = jsonDocument["green"]; - int blue = jsonDocument["blue"]; - int NLeds = jsonDocument["NLeds"]; - set_bottom(NLeds, red, green, blue); - } - jsonDocument.clear(); - jsonDocument["return"] = 1; - jsonDocument["LEDArrMode"] = LEDArrMode; - isBusy = false; - -} - -void ledarr_set_fct() { - - Serial.println("Updating Hardware config of LED Array"); - - jsonDocument.clear(); - jsonDocument["return"] = 1; -} - - - -// Custom function accessible by the API -void ledarr_get_fct() { - jsonDocument.clear(); - jsonDocument["LED_ARRAY_PIN"] = LED_ARRAY_PIN; - jsonDocument["LED_ARRAY_NUM"] = LED_ARRAY_NUM; - - -} - - - -/***************************************************************************************************/ -/******************************************* LED Array *******************************************/ -/***************************************************************************************************/ -/*******************************FROM OCTOPI ********************************************************/ - -void set_led_RGB(int iLed, int R, int G, int B) { - matrix->setPixelColor(iLed, matrix->Color(R, G, B)); // Set pixel's color (in RAM) - matrix->show(); // Update strip to match -} - -void setup_matrix() { - // LED Matrix - if(DEBUG) Serial.println("Setting up LED array"); - if(DEBUG) Serial.println("LED_ARRAY_PIN: " + String(LED_ARRAY_PIN)); - matrix = new Adafruit_NeoPixel(LED_ARRAY_NUM, LED_ARRAY_PIN, NEO_GRB + NEO_KHZ800); - matrix->begin(); - matrix->setBrightness(255); - set_all(0,0,0); - delay(100); - set_all(100,100,100); -} - -void set_all(int R, int G, int B) -{ - for (int i = 0; i < (LED_ARRAY_NUM); i++) { - set_led_RGB(i, R, G, B); - } -} - -void set_left(int NLed, int R, int G, int B){ -if(NLed == NLED4x4){ - for (int i = 0; i < (NLED4x4); i++) { - set_led_RGB(i, LED_PATTERN_DPC_LEFT_4x4[i]*R, LED_PATTERN_DPC_LEFT_4x4[i]*G, LED_PATTERN_DPC_LEFT_4x4[i]*B); -}} -if(NLed == NLED8x8){ - for (int i = 0; i < (NLED8x8); i++) { - set_led_RGB(i, LED_PATTERN_DPC_LEFT_8x8[i]*R, LED_PATTERN_DPC_LEFT_8x8[i]*G, LED_PATTERN_DPC_LEFT_8x8[i]*B); -}} -} - -void set_right(int NLed, int R, int G, int B){ -if(NLed == NLED4x4){ - for (int i = 0; i < (NLED4x4); i++) { - set_led_RGB(i, (1-LED_PATTERN_DPC_LEFT_4x4[i])*R, (1-LED_PATTERN_DPC_LEFT_4x4[i])*G, (1-LED_PATTERN_DPC_LEFT_4x4[i])*B); -}} -if(NLed == NLED8x8){ - for (int i = 0; i < (NLED8x8); i++) { - set_led_RGB(i, (1-LED_PATTERN_DPC_LEFT_8x8[i])*R, (1-LED_PATTERN_DPC_LEFT_8x8[i])*G, (1-LED_PATTERN_DPC_LEFT_8x8[i])*B); -}} -} - -void set_top(int NLed, int R, int G, int B){ -if(NLed == NLED4x4){ - for (int i = 0; i < (NLED4x4); i++) { - set_led_RGB(i, (LED_PATTERN_DPC_TOP_4x4[i])*R, (LED_PATTERN_DPC_TOP_4x4[i])*G, (LED_PATTERN_DPC_TOP_4x4[i])*B); -}} -if(NLed == NLED8x8){ - for (int i = 0; i < (NLED8x8); i++) { - set_led_RGB(i, (LED_PATTERN_DPC_TOP_8x8[i])*R, (LED_PATTERN_DPC_TOP_8x8[i])*G, (LED_PATTERN_DPC_TOP_8x8[i])*B); -}} -} - -void set_bottom(int NLed, int R, int G, int B){ -if(NLed == NLED4x4){ - for (int i = 0; i < (NLED4x4); i++) { - set_led_RGB(i, (1-LED_PATTERN_DPC_TOP_4x4[i])*R, (1-LED_PATTERN_DPC_TOP_4x4[i])*G, (1-LED_PATTERN_DPC_TOP_4x4[i])*B); -}} -if(NLed == NLED8x8){ - for (int i = 0; i < (NLED8x8); i++) { - set_led_RGB(i, (1-LED_PATTERN_DPC_TOP_8x8[i])*R, (1-LED_PATTERN_DPC_TOP_8x8[i])*G, (1-LED_PATTERN_DPC_TOP_8x8[i])*B); -}} -} - -void set_center(int R, int G, int B) -{ - /* - matrix->fillScreen(matrix->Color(0, 0, 0)); - matrix->drawPixel(4, 4, matrix->Color(R, G, B)); - matrix->show(); - */ -} - -/* - wrapper for HTTP requests -*/ -void ledarr_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - ledarr_act_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void ledarr_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - ledarr_get_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void ledarr_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - ledarr_set_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} diff --git a/ESP32/main/control_PID.ino b/ESP32/main/control_PID.ino deleted file mode 100644 index 465decd..0000000 --- a/ESP32/main/control_PID.ino +++ /dev/null @@ -1,178 +0,0 @@ -#ifdef IS_PID - -// Custom function accessible by the API -void PID_act_fct() { - - // here you can do something - if (DEBUG) Serial.println("PID_act_fct"); - - - if (jsonDocument.containsKey("PIDactive")) - PID_active = (int)jsonDocument["PIDactive"]; - if (jsonDocument.containsKey("Kp")) - PID_Kp = jsonDocument["Kp"]; - if (jsonDocument.containsKey("Ki")) - PID_Ki = jsonDocument["Ki"]; - if (jsonDocument.containsKey("Kd")) - PID_Kd = jsonDocument["Kd"]; - if (jsonDocument.containsKey("target")) - PID_target = jsonDocument["target"]; - if (jsonDocument.containsKey("PID_updaterate")) - PID_updaterate = (int)jsonDocument["PID_updaterate"]; - - if (not PID_active) { - // force shutdown the motor - mspeed1 = 0; - stepper_X.setSpeed(mspeed1); - stepper_X.setMaxSpeed(mspeed1); - stepper_X.runSpeed(); - } - - - - jsonDocument.clear(); - jsonDocument["Kp"] = PID_Kp; - jsonDocument["Ki"] = PID_Ki; - jsonDocument["Kd"] = PID_Kd; - jsonDocument["PID_updaterate"] = PID_updaterate; - jsonDocument["PID"] = PID_active; - jsonDocument["target"] = PID_target; - -} - -void PID_background() { - // hardcoded for now: - int N_sensor_avg = 50; - int sensorpin = ADC_pin_0; - - // get rid of noise? - float sensorValueAvg = 0; - for (int imeas = 0; imeas < N_sensor_avg; imeas++) { - sensorValueAvg += analogRead(sensorpin); - } - - sensorValueAvg = (float)sensorValueAvg / (float)N_sensor_avg; - long motorValue = returnControlValue(PID_target, sensorValueAvg, PID_Kp, PID_Ki, PID_Kd); - isforever = 1; // run motor at certain speed - mspeed1 = motorValue; - setEnableMotor(true); - stepper_X.setSpeed(mspeed1); - stepper_X.setMaxSpeed(mspeed1); - -} - - -long returnControlValue(float controlTarget, float sensorValue, float Kp, float Ki, float Kd) { - float sensorOffset = 0.; - float maxError = 1.; - float error = (controlTarget - (sensorValue - sensorOffset)) / maxError; - float cP = Kp * error; - float cI = Ki * errorRunSum; - float cD = Kd * (error - previousError); - float PID = cP + cI + cD; - long stepperOut = (long)PID; - - if (stepperOut > stepperMaxValue) { - stepperOut = stepperMaxValue; - } - - if (stepperOut < -stepperMaxValue) { - stepperOut = -stepperMaxValue; - } - - - errorRunSum = errorRunSum + error; - previousError = error; - - if (DEBUG) Serial.println("sensorValue: " + String(sensorValue) + ", P: " + String(cP) + ", I: " + String(cI) + ", D: " + String(cD) + ", errorRunSum: " + String(errorRunSum) + ", previousError: " + String(previousError) + ", stepperOut: " + String(stepperOut)); - return stepperOut; -} - - - -void PID_set_fct() { - if (DEBUG) Serial.println("PID_set_fct"); - int PIDID = (int)jsonDocument["PIDID"]; - int PIDPIN = (int)jsonDocument["PIDPIN"]; - if (jsonDocument.containsKey("N_sensor_avg")) - N_sensor_avg = (int)jsonDocument["N_sensor_avg"]; - - switch (PIDID) { - case 0: - ADC_pin_0 = PIDPIN; - break; - case 1: - ADC_pin_1 = PIDPIN; - break; - case 2: - ADC_pin_2 = PIDPIN; - break; - } - - - jsonDocument.clear(); - jsonDocument["PIDPIN"] = PIDPIN; - jsonDocument["PIDID"] = PIDID; -} - - - -// Custom function accessible by the API -void PID_get_fct() { - if (DEBUG) Serial.println("PID_get_fct"); - int PIDID = (int)jsonDocument["PIDID"]; - int PIDPIN = 0; - switch (PIDID) { - case 0: - PIDPIN = ADC_pin_0; - break; - case 1: - PIDPIN = ADC_pin_1; - break; - case 2: - PIDPIN = ADC_pin_2; - break; - } - - jsonDocument.clear(); - jsonDocument["N_sensor_avg"] = N_sensor_avg; - jsonDocument["PIDPIN"] = PIDPIN; - jsonDocument["PIDID"] = PIDID; -} - - -void setup_PID() { - if (DEBUG) Serial.println("Setting up sensors..."); -} - - - -/* - wrapper for HTTP requests -*/ -void PID_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - PID_act_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void PID_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - PID_get_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void PID_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - PID_set_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} -#endif diff --git a/ESP32/main/control_PS3.ino b/ESP32/main/control_PS3.ino deleted file mode 100644 index 1ae7253..0000000 --- a/ESP32/main/control_PS3.ino +++ /dev/null @@ -1,271 +0,0 @@ -#ifdef IS_PS3 -bool IS_PS3_CONTROLER_LEDARRAY = false; -void onConnectPS3() { - if (DEBUG) Serial.println("PS3 Controller Connected."); - IS_PSCONTROLER_ACTIVE = true; - setEnableMotor(true); -} - -void onAttachPS3() { - Ps3.attach(activate_PS3); -} - - -void onDisConnectPS3() { - if (DEBUG) Serial.println("PS3 Controller Connected."); - setEnableMotor(false); -} - - -void activate_PS3() { - // callback for events - if (Ps3.event.button_down.select) { - IS_PSCONTROLER_ACTIVE = !IS_PSCONTROLER_ACTIVE; - if (DEBUG) Serial.print("Setting manual mode to: "); - if (DEBUG) Serial.println(IS_PSCONTROLER_ACTIVE); - setEnableMotor(IS_PSCONTROLER_ACTIVE); - delay(1000); //Debounce? - } - if (Ps3.event.button_down.cross) { - IS_PS3_CONTROLER_LEDARRAY = !IS_PS3_CONTROLER_LEDARRAY; - if (DEBUG) Serial.print("Turning LED Matrix to: "); - if (DEBUG) Serial.println(IS_PS3_CONTROLER_LEDARRAY); - set_all(255 * IS_PS3_CONTROLER_LEDARRAY, 255 * IS_PS3_CONTROLER_LEDARRAY, 255 * IS_PS3_CONTROLER_LEDARRAY); - delay(1000); //Debounce? - } - - - // LASER 1 - if (Ps3.event.button_down.up) { - if (DEBUG) Serial.print("Turning on LAser 10000"); - ledcWrite(PWM_CHANNEL_LASER_2, 20000); - delay(100); //Debounce? - } - if (Ps3.event.button_down.down) { - if (DEBUG) Serial.print("Turning off LAser "); - ledcWrite(PWM_CHANNEL_LASER_2, 0); - delay(100); //Debounce? - } - - // LASER 2 - if (Ps3.event.button_down.right) { - if (DEBUG) Serial.print("Turning on LAser 10000"); - ledcWrite(PWM_CHANNEL_LASER_1, 20000); - delay(100); //Debounce? - } - if (Ps3.event.button_down.left) { - if (DEBUG) Serial.print("Turning off LAser "); - ledcWrite(PWM_CHANNEL_LASER_1, 0); - delay(100); //Debounce? - } - - -} - -void control_PS3() { - if (Ps3.isConnected() and IS_PSCONTROLER_ACTIVE) { - // Y-Direction - if ( abs(Ps3.data.analog.stick.ly) > offset_val) { - // move_z - stick_ly = Ps3.data.analog.stick.ly; - stick_ly = stick_ly - sgn(stick_ly) * offset_val; - if (abs(stick_ly) > 100) - stick_ly *= 2; - - mspeed3 = stick_ly * 5 * global_speed; - if (not getEnableMotor()) - setEnableMotor(true); - } - else if (mspeed3 != 0) { - mspeed3 = 0; - stepper_Y.setSpeed(mspeed3); // set motor off only once to not affect other modes - } - - // Z-Direction - if ( (abs(Ps3.data.analog.stick.rx) > offset_val)) { - // move_x - stick_rx = Ps3.data.analog.stick.rx; - stick_rx = stick_rx - sgn(stick_rx) * offset_val; - - if (abs(stick_rx) > 100) - stick_rx *= 2; - - mspeed2 = stick_rx * 5 * global_speed; - if (not getEnableMotor()) - setEnableMotor(true); - } - else if (mspeed2 != 0) { - mspeed2 = 0; - stepper_X.setSpeed(mspeed2); // set motor off only once to not affect other modes - } - - // X-direction - if ( (abs(Ps3.data.analog.stick.ry) > offset_val)) { - // move_y - stick_ry = Ps3.data.analog.stick.ry; - stick_ry = stick_ry - sgn(stick_ry) * offset_val; - if (abs(stick_ry) > 100) - stick_ry *= 2; - mspeed1 = stick_ry * 5 * global_speed; - if (not getEnableMotor()) - setEnableMotor(true); - } - else if (mspeed1 != 0) { - mspeed1 = 0; - stepper_Z.setSpeed(mspeed1); // set motor off only once to not affect other modes - } - - /* - // fine control for Z using buttons - if ( Ps3.data.button.down) { - // fine focus + - //run_motor(0, 0, 10); - delay(100); - } - if ( Ps3.data.button.up) { - // fine focus - - //run_motor(0, 0, -10); - delay(100); - } - */ - - -#ifdef IS_ANALOG - /* - Keypad left - */ - if ( Ps3.data.button.left) { - // fine lens - - analog_val_1 -= 1; - delay(100); - ledcWrite(PWM_CHANNEL_analog_1, analog_val_1); - } - if ( Ps3.data.button.right) { - // fine lens + - analog_val_1 += 1; - delay(100); - ledcWrite(PWM_CHANNEL_analog_1, analog_val_1); - } - if ( Ps3.data.button.start) { - // reset - analog_val_1 = 0; - ledcWrite(PWM_CHANNEL_analog_1, analog_val_1); - } - - int offset_val_shoulder = 5; - if ( abs(Ps3.data.analog.button.r2) > offset_val_shoulder) { - // analog_val_1++ coarse - if ((analog_val_1 + 1000 < pwm_max)) { - analog_val_1 += 1000; - ledcWrite(PWM_CHANNEL_analog_1, analog_val_1); - } - if (DEBUG) Serial.println(analog_val_1); - delay(100); - } - - if ( abs(Ps3.data.analog.button.l2) > offset_val_shoulder) { - // analog_val_1-- coarse - if ((analog_val_1 - 1000 > 0)) { - analog_val_1 -= 1000; - ledcWrite(PWM_CHANNEL_analog_1, analog_val_1); - } - if (DEBUG) Serial.println(analog_val_1); - delay(100); - } - - - if ( abs(Ps3.data.analog.button.r1) > offset_val_shoulder) { - // analog_val_1 + semi coarse - if ((analog_val_1 + 100 < pwm_max)) { - analog_val_1 += 100; - ledcWrite(PWM_CHANNEL_analog_1, analog_val_1); - delay(100); - } - } - if ( abs(Ps3.data.analog.button.l1) > offset_val_shoulder) { - // analog_val_1 - semi coarse - if ((analog_val_1 - 100 > 0)) { - analog_val_1 -= 100; - ledcWrite(PWM_CHANNEL_analog_1, analog_val_1); - delay(50); - } - } - -#endif - - // run all motors simultaneously - stepper_Z.setSpeed(mspeed1); - stepper_Y.setSpeed(mspeed3); - stepper_X.setSpeed(mspeed2); - - if (mspeed1 or mspeed3 or mspeed2) { - isforever = true; - } - else { - isforever = false; - } - } - -} - - -/* unused for now - -*/ - - -// -// if ( abs(Ps3.data.analog.stick.lx) > offset_val) { -// // LED Stip -// stick_lx = Ps3.data.analog.stick.lx; -// //stick_lx = stick_lx + sgn(stick_lx) * offset_val; -// if ((colour_led += sgn(stick_lx) * 5) > 0 and (colour_led += sgn(stick_lx) * 5) < 255) -// colour_led += sgn(stick_lx) * 5; -// if (colour_led < 0) -// colour_led = 0; -// strip.setPixelColor(0, strip.Color(colour_led, colour_led, colour_led)); -// strip.show(); -// delay(100); -// } -// -// - -// if ( Ps3.data.button.circle ) { -// //if(not is_laser_red){ -// if(DEBUG) Serial.println("Laser on"); -// is_laser_red = true; -// laserval += 200; -// run_laser(laserval); -// delay(100); -// //} -// -// } -// -// if ( Ps3.data.button.cross ) { -// if (is_laser_red) { -// if(DEBUG) Serial.println("Laser off"); -// is_laser_red = false; -// laserval = 0; -// run_laser(0); -// } -// -// } -// -// if ( Ps3.data.button.triangle) { -// if (not is_sofi) { -// if(DEBUG) Serial.println("SOFI on"); -// is_sofi = true; -// glob_amplitude_sofi = 300; -// } -// } -// -// if ( Ps3.data.button.square ) { -// if (is_sofi) { -// is_sofi = false; -// if(DEBUG) Serial.println("SOFI off"); -// glob_amplitude_sofi = 0; -// } -// -// } - -#endif diff --git a/ESP32/main/control_PS4.ino b/ESP32/main/control_PS4.ino deleted file mode 100644 index f998ffb..0000000 --- a/ESP32/main/control_PS4.ino +++ /dev/null @@ -1,309 +0,0 @@ -#ifndef IS_PS3 -bool IS_PS4_CONTROLER_LEDARRAY = false; -void onConnectPS4() { - if (DEBUG) Serial.println("PS4 Controller Connected."); - IS_PSCONTROLER_ACTIVE = true; - setEnableMotor(IS_PSCONTROLER_ACTIVE); - - if (PS4.Charging()) - { - Serial.println("The controller is charging"); - } - - Serial.printf("Battery Level : %d\n", PS4.Battery()); - Serial.println(); -} - -void onAttachPS4() { - PS4.attach(activate_PS4); -} - -void onDisConnectPS4() { - if (DEBUG) Serial.println("PS4 Controller disconnected."); - setEnableMotor(false); -} - - -void activate_PS4() { - // callback for events - if (PS4.event.button_down.share) { - IS_PSCONTROLER_ACTIVE = !IS_PSCONTROLER_ACTIVE; - if (DEBUG) Serial.print("Setting manual mode to: "); - if (DEBUG) Serial.println(IS_PSCONTROLER_ACTIVE); - setEnableMotor(IS_PSCONTROLER_ACTIVE); - delay(1000); //Debounce? - } - if (PS4.event.button_down.cross) { - IS_PS4_CONTROLER_LEDARRAY = !IS_PS4_CONTROLER_LEDARRAY; - if (DEBUG) Serial.print("Turning LED Matrix to (cross): "); - if (DEBUG) Serial.println(IS_PS4_CONTROLER_LEDARRAY); - - set_all(255*IS_PS4_CONTROLER_LEDARRAY,255*IS_PS4_CONTROLER_LEDARRAY,255*IS_PS4_CONTROLER_LEDARRAY); - delay(1000); //Debounce? - } - if (PS4.event.button_down.circle) { - IS_PS4_CONTROLER_LEDARRAY = !IS_PS4_CONTROLER_LEDARRAY; - if (DEBUG) Serial.print("Turning LED Matrix to (circle): "); - if (DEBUG) Serial.println(IS_PS4_CONTROLER_LEDARRAY); - - set_center(255*IS_PS4_CONTROLER_LEDARRAY,255*IS_PS4_CONTROLER_LEDARRAY,255*IS_PS4_CONTROLER_LEDARRAY); - delay(1000); //Debounce? - - } - // LASER 3 - if (PS4.event.button_down.triangle) { - if (DEBUG) Serial.print("Turning on LAser 10000"); - ledcWrite(PWM_CHANNEL_LASER_3, 10000); - delay(100); //Debounce? - } - if (PS4.event.button_down.square) { - if (DEBUG) Serial.print("Turning off LAser "); - ledcWrite(PWM_CHANNEL_LASER_3, 0); - delay(100); //Debounce? - } - -// FOCUS -/* - if (PS4.event.button_down.up) { - if (not getEnableMotor()) - setEnableMotor(true); - POSITION_MOTOR_X = stepper_X.currentPosition(); - stepper_X.move(POSITION_MOTOR_X+2); - delay(100); //Debounce? - } - if (PS4.event.button_down.down) { - if (not getEnableMotor()) - setEnableMotor(true); - POSITION_MOTOR_X = stepper_X.currentPosition(); - stepper_X.move(POSITION_MOTOR_X-2); - delay(100); //Debounce? - } -*/ - - // LASER 1 - if (PS4.event.button_down.up) { - if (DEBUG) Serial.print("Turning on LAser 10000"); - ledcWrite(PWM_CHANNEL_LASER_2, 20000); - delay(100); //Debounce? - } - if (PS4.event.button_down.down) { - if (DEBUG) Serial.print("Turning off LAser "); - ledcWrite(PWM_CHANNEL_LASER_2, 0); - delay(100); //Debounce? - } - - // LASER 2 - if (PS4.event.button_down.right) { - if (DEBUG) Serial.print("Turning on LAser 10000"); - ledcWrite(PWM_CHANNEL_LASER_1, 20000); - delay(100); //Debounce? - } - if (PS4.event.button_down.left) { - if (DEBUG) Serial.print("Turning off LAser "); - ledcWrite(PWM_CHANNEL_LASER_1, 0); - delay(100); //Debounce? - } - - - - - -} - -void control_PS4() { - if (PS4.isConnected() and IS_PSCONTROLER_ACTIVE) { - // Y-Direction - if ( abs(PS4.data.analog.stick.ly) > offset_val) { - // move_z - stick_ly = PS4.data.analog.stick.ly; - stick_ly = stick_ly - sgn(stick_ly) * offset_val; - mspeed2 = stick_ly * 5 * global_speed; - if (not getEnableMotor()) - setEnableMotor(true); - } - else if (mspeed2 != 0) { - mspeed2 = 0; - stepper_Y.setSpeed(mspeed2); // set motor off only once to not affect other modes - } - - // Z-Direction - if ( (abs(PS4.data.analog.stick.rx) > offset_val)) { - // move_x - stick_rx = PS4.data.analog.stick.rx; - stick_rx = stick_rx - sgn(stick_rx) * offset_val; - mspeed3 = stick_rx * 5 * global_speed; - if (not getEnableMotor()) - setEnableMotor(true); - } - else if (mspeed3 != 0) { - mspeed3 = 0; - stepper_Z.setSpeed(mspeed3); // set motor off only once to not affect other modes - } - - // X-direction - if ( (abs(PS4.data.analog.stick.ry) > offset_val)) { - // move_y - stick_ry = PS4.data.analog.stick.ry; - stick_ry = stick_ry - sgn(stick_ry) * offset_val; - mspeed1 = stick_ry * 5 * global_speed; - if (not getEnableMotor()) - setEnableMotor(true); - } - else if (mspeed1 != 0) { - mspeed1 = 0; - stepper_X.setSpeed(mspeed1); // set motor off only once to not affect other modes - } - - /* - // fine control for Z using buttons - if ( PS4.data.button.down) { - // fine focus + - //run_motor(0, 0, 10); - delay(100); - } - if ( PS4.data.button.up) { - // fine focus - - //run_motor(0, 0, -10); - delay(100); - } - */ - - - /* - Keypad left - */ - #ifdef NO - if ( PS4.data.button.left) { - // fine lens - - analog_val_1 -= 1; - delay(100); - ledcWrite(PWM_CHANNEL_LASER_1, analog_val_1); - } - if ( PS4.data.button.right) { - // fine lens + - analog_val_1 += 1; - delay(100); - ledcWrite(PWM_CHANNEL_LASER_1, analog_val_1); - } - if ( PS4.data.button.start) { - // reset - analog_val_1 = 0; - ledcWrite(PWM_CHANNEL_LASER_1, analog_val_1); - } - - int offset_val_shoulder = 5; - if ( abs(PS4.data.analog.button.r2) > offset_val_shoulder) { - // analog_val_1++ coarse - if ((analog_val_1 + 1000 < pwm_max)) { - analog_val_1 += 1000; - ledcWrite(PWM_CHANNEL_LASER_1, analog_val_1); - } - if (DEBUG) Serial.println(analog_val_1); - delay(100); - } - - if ( abs(PS4.data.analog.button.l2) > offset_val_shoulder) { - // analog_val_1-- coarse - if ((analog_val_1 - 1000 > 0)) { - analog_val_1 -= 1000; - ledcWrite(PWM_CHANNEL_LASER_1, analog_val_1); - } - if (DEBUG) Serial.println(analog_val_1); - delay(100); - } - - - if ( abs(PS4.data.analog.button.r1) > offset_val_shoulder) { - // analog_val_1 + semi coarse - if ((analog_val_1 + 100 < pwm_max)) { - analog_val_1 += 100; - ledcWrite(PWM_CHANNEL_LASER_1, analog_val_1); - delay(100); - } - } - if ( abs(PS4.data.analog.button.l1) > offset_val_shoulder) { - // analog_val_1 - semi coarse - if ((analog_val_1 - 100 > 0)) { - analog_val_1 -= 100; - ledcWrite(PWM_CHANNEL_LASER_1, analog_val_1); - delay(50); - } - } - #endif - - // run all motors simultaneously - stepper_X.setSpeed(mspeed1); - stepper_Y.setSpeed(mspeed2); - stepper_Z.setSpeed(mspeed3); - - if (mspeed1 or mspeed2 or mspeed3) { - isforever = true; - } - else { - isforever = false; - } - } - -} - - -/* unused for now - -*/ - - -// -// if ( abs(PS4.data.analog.stick.lx) > offset_val) { -// // LED Stip -// stick_lx = PS4.data.analog.stick.lx; -// //stick_lx = stick_lx + sgn(stick_lx) * offset_val; -// if ((colour_led += sgn(stick_lx) * 5) > 0 and (colour_led += sgn(stick_lx) * 5) < 255) -// colour_led += sgn(stick_lx) * 5; -// if (colour_led < 0) -// colour_led = 0; -// strip.setPixelColor(0, strip.Color(colour_led, colour_led, colour_led)); -// strip.show(); -// delay(100); -// } -// -// - -// if ( PS4.data.button.circle ) { -// //if(not is_laser_red){ -// if(DEBUG) Serial.println("Laser on"); -// is_laser_red = true; -// laserval += 200; -// run_laser(laserval); -// delay(100); -// //} -// -// } -// -// if ( PS4.data.button.cross ) { -// if (is_laser_red) { -// if(DEBUG) Serial.println("Laser off"); -// is_laser_red = false; -// laserval = 0; -// run_laser(0); -// } -// -// } -// -// if ( PS4.data.button.triangle) { -// if (not is_sofi) { -// if(DEBUG) Serial.println("SOFI on"); -// is_sofi = true; -// glob_amplitude_sofi = 300; -// } -// } -// -// if ( PS4.data.button.square ) { -// if (is_sofi) { -// is_sofi = false; -// if(DEBUG) Serial.println("SOFI off"); -// glob_amplitude_sofi = 0; -// } -// -// } - -#endif diff --git a/ESP32/main/control_analog.ino b/ESP32/main/control_analog.ino deleted file mode 100644 index 9094260..0000000 --- a/ESP32/main/control_analog.ino +++ /dev/null @@ -1,152 +0,0 @@ -#ifdef IS_ANALOG - -// Custom function accessible by the API -void analog_act_fct() { - // here you can do something - Serial.println("analog_act_fct"); - - int analogid = jsonDocument["analogid"]; - int analogval = jsonDocument["analogval"]; - - if (DEBUG) { - Serial.print("analogid "); Serial.println(analogid); - Serial.print("analogval "); Serial.println(analogval); - } - - if (analogid == 1) { - analog_val_1 = analogval; - ledcWrite(PWM_CHANNEL_analog_1, analogval); - } - else if (analogid == 2) { - analog_val_2 = analogval; - ledcWrite(PWM_CHANNEL_analog_2, analogval); - } - else if (analogid == 3) { - analog_val_3 = analogval; - ledcWrite(PWM_CHANNEL_analog_3, analogval); - } - jsonDocument.clear(); - jsonDocument["return"] = 1; -} - -void analog_set_fct() { - // here you can set parameters - - if (jsonDocument.containsKey("analogid")) { - int analogid = jsonDocument["analogid"]; - } - else{ - int analogid = 0; - } - - if (jsonDocument.containsKey("analogpin")) { - int analogpin = jsonDocument["analogpin"]; - } - else{ - int analogpin = 0; - } - - if (DEBUG) Serial.print("analogid "); Serial.println(analogid); - if (DEBUG) Serial.print("analogpin "); Serial.println(analogpin); - - - - if (analogid == 1) { - ANALOG_PIN_1 = analogpin; - pinMode(ANALOG_PIN_1, OUTPUT); - digitalWrite(ANALOG_PIN_1, LOW); - - /* setup the PWM ports and reset them to 0*/ - ledcSetup(PWM_CHANNEL_analog_1, pwm_frequency, pwm_resolution); - ledcAttachPin(ANALOG_PIN_1, PWM_CHANNEL_analog_1); - ledcWrite(PWM_CHANNEL_analog_1, 0); - - } - else if (analogid == 2) { - ANALOG_PIN_2 = analogpin; - pinMode(ANALOG_PIN_2, OUTPUT); - digitalWrite(ANALOG_PIN_2, LOW); - - /* setup the PWM ports and reset them to 0*/ - ledcSetup(PWM_CHANNEL_analog_2, pwm_frequency, pwm_resolution); - ledcAttachPin(ANALOG_PIN_2, PWM_CHANNEL_analog_2); - ledcWrite(PWM_CHANNEL_analog_2, 0); - } - else if (analogid == 3) { - ANALOG_PIN_3 = analogpin; - pinMode(ANALOG_PIN_3, OUTPUT); - digitalWrite(ANALOG_PIN_3, LOW); - - /* setup the PWM ports and reset them to 0*/ - ledcSetup(PWM_CHANNEL_analog_3, pwm_frequency, pwm_resolution); - ledcAttachPin(ANALOG_PIN_3, PWM_CHANNEL_analog_3); - ledcWrite(PWM_CHANNEL_analog_3, 0); - -} - -jsonDocument.clear(); -jsonDocument["return"] = 1; - -} - -// Custom function accessible by the API -void analog_get_fct() { - // GET SOME PARAMETERS HERE - int analogid = jsonDocument["analogid"]; - int analogpin = 0; - int analogval = 0; - - if (analogid == 1) { - if (DEBUG) Serial.println("analog 1"); - analogpin = ANALOG_PIN_1; - analogval = analog_val_1; - } - else if (analogid == 2) { - if (DEBUG) Serial.println("AXIS 2"); - if (DEBUG) Serial.println("analog 2"); - analogpin = ANALOG_PIN_2; - analogval = analog_val_2; - } - else if (analogid == 3) { - if (DEBUG) Serial.println("AXIS 3"); - if (DEBUG) Serial.println("analog 1"); - analogpin = ANALOG_PIN_3; - analogval = analog_val_3; - } - - jsonDocument.clear(); - jsonDocument["analogid"] = analogid; - jsonDocument["analogval"] = analogval; - jsonDocument["analogpin"] = analogpin; -} - - -/* - wrapper for HTTP requests -*/ -void analog_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - analog_act_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void analog_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - analog_get_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void analog_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - analog_set_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} -#endif diff --git a/ESP32/main/control_config.ino b/ESP32/main/control_config.ino deleted file mode 100644 index 1747093..0000000 --- a/ESP32/main/control_config.ino +++ /dev/null @@ -1,276 +0,0 @@ - - -void config_act_fct() { - - if (jsonDocument.containsKey("resetConfig")) { - if (DEBUG) Serial.println("resetConfig"); - resetConfigurations(); - } - if (jsonDocument.containsKey("applyConfig")) { - if (DEBUG) Serial.println("applyConfig"); - if (DEBUG) Serial.println("Restarting.."); - ESP.restart(); - } - if (jsonDocument.containsKey("printConfig")) { - if (DEBUG) Serial.println("printConfig"); - printConfig(); - } - - jsonDocument.clear(); - jsonDocument["return"] = 1; -} - -void config_set_fct() { - setConfigurations(); -} - -void config_get_fct() { - jsonDocument.clear(); - loadConfiguration(); -} - -bool resetConfigurations() { - Serial.println("Resetting Hardware configuration!!!"); - preferences.begin(prefNamespace , false); - preferences.clear(); - preferences.end(); - return true; -} - -bool setConfigurations() { - /* - This function sets the preferences for the parameters based on a JSON document. - */ - if (DEBUG) Serial.println("Setting Hardware Configuration"); - - preferences.begin(prefNamespace , false); - - // MOTOR X - if (jsonDocument.containsKey(keyMotXStepPin)) - preferences.putUInt(keyMotXStepPin, jsonDocument[keyMotXStepPin]); - if (jsonDocument.containsKey(keyMotXDirPin)) - preferences.putUInt(keyMotXDirPin, jsonDocument[keyMotXDirPin]); - - // MOTOR Y - if (jsonDocument.containsKey(keyMotYStepPin)) - preferences.putUInt(keyMotYStepPin, jsonDocument[keyMotYStepPin]); - if (jsonDocument.containsKey(keyMotYDirPin)) - preferences.putUInt(keyMotYDirPin, jsonDocument[keyMotYDirPin]); - - // MOTOR Z - if (jsonDocument.containsKey(keyMotZStepPin)) - preferences.putUInt(keyMotZStepPin, jsonDocument[keyMotZStepPin]); - if (jsonDocument.containsKey(keyMotZDirPin)) - preferences.putUInt(keyMotZDirPin, jsonDocument[keyMotZDirPin]); - - // MOTOR A - if (jsonDocument.containsKey(keyMotAStepPin)) - preferences.putUInt(keyMotAStepPin, jsonDocument[keyMotAStepPin]); - if (jsonDocument.containsKey(keyMotADirPin)) - preferences.putUInt(keyMotADirPin, jsonDocument[keyMotADirPin]); - - // MOTOR ENABLE - if (jsonDocument.containsKey(keyMotEnable)) - preferences.putUInt(keyMotEnable, jsonDocument[keyMotEnable]); - - // LEDARRAY - if (jsonDocument.containsKey(keyLEDArray)) - preferences.putUInt(keyLEDArray, jsonDocument[keyLEDArray]); - if (jsonDocument.containsKey(keyLEDNumLEDArray)) - preferences.putUInt(keyLEDNumLEDArray, jsonDocument[keyLEDNumLEDArray]); - - // DIGITAL PINN - if (jsonDocument.containsKey(keyDigital1Pin)) - preferences.putUInt(keyDigital1Pin, jsonDocument[keyDigital1Pin]); - if (jsonDocument.containsKey(keyDigital2Pin)) - preferences.putUInt(keyDigital2Pin, jsonDocument[keyDigital2Pin]); - - // ANALOG PIN - if (jsonDocument.containsKey(keyAnalog1Pin)) - preferences.putUInt(keyAnalog1Pin, jsonDocument[keyAnalog1Pin]); - if (jsonDocument.containsKey(keyAnalog2Pin)) - preferences.putUInt(keyAnalog2Pin, jsonDocument[keyAnalog2Pin]); - if (jsonDocument.containsKey(keyAnalog3Pin)) - preferences.putUInt(keyAnalog3Pin, jsonDocument[keyAnalog3Pin]); - - // LASER PIN - if (jsonDocument.containsKey(keyLaser1Pin)) - preferences.putUInt(keyLaser1Pin, jsonDocument[keyLaser1Pin]); - if (jsonDocument.containsKey(keyLaser2Pin)) - preferences.putUInt(keyLaser2Pin, jsonDocument[keyLaser2Pin]); - if (jsonDocument.containsKey(keyLaser3Pin)) - preferences.putUInt(keyLaser3Pin, jsonDocument[keyLaser3Pin]); - - // DAC FAKE PIN - if (jsonDocument.containsKey(keyDACfake1Pin)) - preferences.putUInt(keyDACfake1Pin, jsonDocument[keyDACfake1Pin]); - if (jsonDocument.containsKey(keyDACfake2Pin)) - preferences.putUInt(keyDACfake2Pin, jsonDocument[keyDACfake2Pin]); - - // IDENTIFIER - if (jsonDocument.containsKey(keyIdentifier)) { - String Identifier = jsonDocument[keyIdentifier]; - preferences.putString(keyIdentifier, Identifier); - } - - // WIFI - if (jsonDocument.containsKey(keyWifiSSID)) { - String WifiSSID = jsonDocument[keyWifiSSID]; - preferences.putString(keyWifiSSID, WifiSSID); - } - if (jsonDocument.containsKey(keyWifiPW)) { - String WifiPW = jsonDocument[keyWifiPW]; - preferences.putString(keyWifiPW, WifiPW); - } - - // Playstation - if (jsonDocument.containsKey(keyPS3Mac)) { - String PS3MacTmp = jsonDocument[keyPS3Mac]; - preferences.putString(keyPS3Mac, PS3MacTmp); - } - if (jsonDocument.containsKey(keyPS4Mac)) { - String PS4MacTmp = jsonDocument[keyPS4Mac]; - preferences.putString(keyPS4Mac, PS4MacTmp); - } - preferences.end(); - - - // indicate that new config is applied - preferences.begin("setup" , false); - preferences.putBool("isNewConfig", true); - preferences.end(); - - - - - return true; -} - - -bool loadConfiguration() { - /* This function gets the preferences for the parameters - and returns a JSON document. */ - - preferences.begin(prefNamespace, false); - STEP_PIN_X = preferences.getUInt(keyMotXStepPin, STEP_PIN_X); - DIR_PIN_X = preferences.getUInt(keyMotXDirPin, DIR_PIN_X); - - STEP_PIN_Y = preferences.getUInt(keyMotYStepPin, STEP_PIN_Y); - DIR_PIN_Y = preferences.getUInt(keyMotYDirPin, DIR_PIN_Y); - - STEP_PIN_Z = preferences.getUInt(keyMotZStepPin, STEP_PIN_Z); - DIR_PIN_Z = preferences.getUInt(keyMotZDirPin, DIR_PIN_Z); - - STEP_PIN_A = preferences.getUInt(keyMotAStepPin, STEP_PIN_A); - DIR_PIN_A = preferences.getUInt(keyMotADirPin, DIR_PIN_A); - - ENABLE_PIN = preferences.getUInt(keyMotEnable, ENABLE_PIN); - - LED_ARRAY_PIN = preferences.getUInt(keyLEDArray , LED_ARRAY_PIN); - LED_ARRAY_NUM = preferences.getUInt(keyLEDNumLEDArray, LED_ARRAY_NUM); - - DIGITAL_PIN_1 = preferences.getUInt(keyDigital1Pin, DIGITAL_PIN_1); - DIGITAL_PIN_2 = preferences.getUInt(keyDigital2Pin, DIGITAL_PIN_2); - - ANALOG_PIN_1 = preferences.getUInt(keyAnalog1Pin, ANALOG_PIN_1); - ANALOG_PIN_2 = preferences.getUInt(keyAnalog2Pin, ANALOG_PIN_2); - ANALOG_PIN_3 = preferences.getUInt(keyAnalog3Pin, ANALOG_PIN_3); - - LASER_PIN_1 = preferences.getUInt(keyLaser1Pin, LASER_PIN_1); - LASER_PIN_2 = preferences.getUInt(keyLaser2Pin, LASER_PIN_2); - LASER_PIN_3 = preferences.getUInt(keyLaser3Pin, LASER_PIN_3); - - DAC_FAKE_PIN_1 = preferences.getUInt(keyDACfake1Pin, DAC_FAKE_PIN_1); - DAC_FAKE_PIN_2 = preferences.getUInt(keyDACfake2Pin, DAC_FAKE_PIN_2); - - IDENTIFIER_NAME = preferences.getString(keyDACfake1Pin, IDENTIFIER_NAME); - WifiSSID = preferences.getString(keyWifiSSID, WifiSSID).c_str(); - WifiPW = preferences.getString(keyWifiPW, WifiPW).c_str(); - PS3Mac = preferences.getString(keyPS3Mac, PS3Mac).c_str(); - PS4Mac = preferences.getString(keyPS4Mac, PS4Mac).c_str(); - - preferences.end(); - - // return preferences as json document - config2json(); - - return true; -} - -void printConfig() { - // Send JSON information back - Serial.println("++"); - config2json(); - serializeJsonPretty(jsonDocument, Serial); - Serial.println(); - Serial.println("--"); - jsonDocument.clear(); - jsonDocument.garbageCollect(); - -} - -void config2json() { - // Assign to JSON jsonDocumentument - jsonDocument.clear(); - - jsonDocument[keyMotXStepPin] = STEP_PIN_X; - jsonDocument[keyMotXDirPin] = DIR_PIN_X; - jsonDocument[keyMotYStepPin] = STEP_PIN_Y; - jsonDocument[keyMotYDirPin] = DIR_PIN_Y; - jsonDocument[keyMotZStepPin] = STEP_PIN_Z; - jsonDocument[keyMotZDirPin] = DIR_PIN_Z; - jsonDocument[keyMotAStepPin] = STEP_PIN_A; - jsonDocument[keyMotADirPin] = DIR_PIN_A; - jsonDocument[keyMotEnable] = ENABLE_PIN; - jsonDocument[keyLEDArray] = LED_ARRAY_PIN; - jsonDocument[keyLEDNumLEDArray] = LED_ARRAY_NUM; - jsonDocument[keyDigital1Pin] = DIGITAL_PIN_1; - jsonDocument[keyDigital2Pin] = DIGITAL_PIN_2; - jsonDocument[keyAnalog1Pin] = ANALOG_PIN_1; - jsonDocument[keyAnalog2Pin] = ANALOG_PIN_2; - jsonDocument[keyAnalog3Pin] = ANALOG_PIN_3; - jsonDocument[keyLaser1Pin] = LASER_PIN_1; - jsonDocument[keyLaser2Pin] = LASER_PIN_2; - jsonDocument[keyLaser3Pin] = LASER_PIN_3; - jsonDocument[keyDACfake1Pin] = DAC_FAKE_PIN_1; - jsonDocument[keyDACfake2Pin] = DAC_FAKE_PIN_2; - jsonDocument[keyIdentifier] = IDENTIFIER_NAME; - jsonDocument[keyWifiSSID] = WifiSSID; - jsonDocument[keyWifiPW] = WifiPW; - jsonDocument[keyPS3Mac] = PS3Mac; - jsonDocument[keyPS4Mac] = PS4Mac; - - if (DEBUG) Serial.println("Current pin definitions:"); - if (DEBUG) serializeJsonPretty(jsonDocument, Serial); - -} - - -/* - wrapper for HTTP requests -*/ -void config_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - config_act_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void config_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - config_get_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void config_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - config_set_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} diff --git a/ESP32/main/control_digitalout.ino b/ESP32/main/control_digitalout.ino deleted file mode 100644 index 8e5a32d..0000000 --- a/ESP32/main/control_digitalout.ino +++ /dev/null @@ -1,173 +0,0 @@ -#ifdef IS_DIGITAL - -// Custom function accessible by the API -void digital_act_fct() { - // here you can do something - Serial.println("digital_act_fct"); - isBusy = true; - - int digitalid = jsonDocument["digitalid"]; - int digitalval = jsonDocument["digitalval"]; - int triggerdelay=10; - - if (DEBUG) { - Serial.print("digitalid "); Serial.println(digitalid); - Serial.print("digitalval "); Serial.println(digitalval); - } - - if (digitalid == 1) { - digital_val_1 = digitalval; - if (digitalval == -1) { - // perform trigger - digitalWrite(DIGITAL_PIN_1, HIGH); - delay(triggerdelay); - digitalWrite(DIGITAL_PIN_1, LOW); - } - else { - digitalWrite(DIGITAL_PIN_1, digital_val_1); - Serial.print("digital_PIN "); Serial.println(DIGITAL_PIN_1); - } - } - else if (digitalid == 2) { - digital_val_2 = digitalval; - if (digitalval == -1) { - // perform trigger - digitalWrite(DIGITAL_PIN_2, HIGH); - delay(triggerdelay); - digitalWrite(DIGITAL_PIN_2, LOW); - } - else { - digitalWrite(DIGITAL_PIN_2, digital_val_2); - Serial.print("digital_PIN "); Serial.println(DIGITAL_PIN_2); - } - } - else if (digitalid == 3) { - digital_val_3 = digitalval; - if (digitalval == -1) { - // perform trigger - digitalWrite(DIGITAL_PIN_3, HIGH); - delay(triggerdelay); - digitalWrite(DIGITAL_PIN_3, LOW); - } - else { - digitalWrite(DIGITAL_PIN_3, digital_val_3); - Serial.print("digital_PIN "); Serial.println(DIGITAL_PIN_3); - } - } - jsonDocument.clear(); - jsonDocument["return"] = 1; - isBusy = false; -} - -void digital_set_fct() { - // here you can set parameters - int digitalid = jsonDocument["digitalid"]; - int digitalpin = jsonDocument["digitalpin"]; - - if (DEBUG) Serial.print("digitalid "); Serial.println(digitalid); - if (DEBUG) Serial.print("digitalpin "); Serial.println(digitalpin); - - if (digitalid != NULL and digitalpin != NULL) { - if (digitalid == 1) { - DIGITAL_PIN_1 = digitalpin; - pinMode(DIGITAL_PIN_1, OUTPUT); - digitalWrite(DIGITAL_PIN_1, LOW); - } - else if (digitalid == 2) { - DIGITAL_PIN_2 = digitalpin; - pinMode(DIGITAL_PIN_2, OUTPUT); - digitalWrite(DIGITAL_PIN_2, LOW); - } - else if (digitalid == 3) { - DIGITAL_PIN_3 = digitalpin; - pinMode(DIGITAL_PIN_3, OUTPUT); - digitalWrite(DIGITAL_PIN_3, LOW); - } - } - - jsonDocument.clear(); - jsonDocument["return"] = 1; - isBusy = false; - -} - -// Custom function accessible by the API -void digital_get_fct() { - // GET SOME PARAMETERS HERE - int digitalid = jsonDocument["digitalid"]; - int digitalpin = 0; - int digitalval = 0; - - if (digitalid == 1) { - if (DEBUG) Serial.println("digital 1"); - digitalpin = DIGITAL_PIN_1; - digitalval = digital_val_1; - } - else if (digitalid == 2) { - if (DEBUG) Serial.println("AXIS 2"); - if (DEBUG) Serial.println("digital 2"); - digitalpin = DIGITAL_PIN_2; - digitalval = digital_val_2; - } - else if (digitalid == 3) { - if (DEBUG) Serial.println("AXIS 3"); - if (DEBUG) Serial.println("digital 1"); - digitalpin = DIGITAL_PIN_3; - digitalval = digital_val_3; - } - - jsonDocument.clear(); - jsonDocument["digitalid"] = digitalid; - jsonDocument["digitalval"] = digitalval; - jsonDocument["digitalpin"] = digitalpin; -} - -void setupDigital() { - Serial.println("Setting Up digital"); - /* setup the output nodes and reset them to 0*/ - pinMode(DIGITAL_PIN_1, OUTPUT); - - digitalWrite(DIGITAL_PIN_1, HIGH); - delay(50); - digitalWrite(DIGITAL_PIN_1, LOW); - - pinMode(DIGITAL_PIN_2, OUTPUT); - digitalWrite(DIGITAL_PIN_2, HIGH); - delay(50); - digitalWrite(DIGITAL_PIN_2, LOW); - - pinMode(DIGITAL_PIN_3, OUTPUT); - digitalWrite(DIGITAL_PIN_3, HIGH); - delay(50); - digitalWrite(DIGITAL_PIN_3, LOW); - -} -/* - wrapper for HTTP requests -*/ -void digital_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - digital_act_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void digital_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - digital_get_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void digital_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - digital_set_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} -#endif diff --git a/ESP32/main/control_dummy.ino b/ESP32/main/control_dummy.ino deleted file mode 100644 index d9c8a6b..0000000 --- a/ESP32/main/control_dummy.ino +++ /dev/null @@ -1,82 +0,0 @@ -/* - - // Custom function accessible by the API -DynamicJsonDocument dummy_act_fct(JsonDocument& Values) { - // here you can do something - Serial.println("dummy_act_fct"); - - int value = Values["value"]; - - if (DEBUG) { - Serial.print("value "); Serial.println(value); - } - - Values.clear(); - Values["return"] = 1; - - return Values ; -} - -DynamicJsonDocument dummy_set_fct(JsonDocument& Values) { - // here you can set parameters - int value = Values["value"]; - - if (DEBUG) { - Serial.print("value "); Serial.println(value); - } - - int dummy_set = jsonDocument["dummy_set"]; - - if (dummy_set != NULL) { - if (DEBUG) Serial.print("dummy_set "); Serial.println(dummy_set); - // SET SOMETHING - } - - Values.clear(); - Values["return"] = 1; - - return Values ; -} - -// Custom function accessible by the API -DynamicJsonDocument dummy_get_fct(JsonDocument& Values) { - // GET SOME PARAMETERS HERE - int dummy_variable = 12343; - - jsonDocument.clear(); - jsonDocument["dummy_variable"] = dummy_variable; - return jsonDocument; -} - - -* - wrapper for HTTP requests -* -#ifdef IS_WIFI -void dummy_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - jsonDocument = dummy_act_fct(jsonDocument); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void dummy_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - jsonDocument = dummy_get_fct(jsonDocument); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void dummy_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - jsonDocument = dummy_set_fct(jsonDocument); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} -#endif -*/ diff --git a/ESP32/main/control_laser.ino b/ESP32/main/control_laser.ino deleted file mode 100644 index 5f559e0..0000000 --- a/ESP32/main/control_laser.ino +++ /dev/null @@ -1,228 +0,0 @@ -void LASER_despeckle(int LASERdespeckle, int LASERid, int LASERperiod) { - - if ( not isBusy) { - - int LASER_val_wiggle = 0; - int PWM_CHANNEL_LASER = 0; - if (LASERid == 1) { - LASER_val_wiggle = LASER_val_1; - PWM_CHANNEL_LASER = PWM_CHANNEL_LASER_1; - } - else if (LASERid == 2) { - LASER_val_wiggle = LASER_val_2; - PWM_CHANNEL_LASER = PWM_CHANNEL_LASER_2; - } - else if (LASERid == 3) { - LASER_val_wiggle = LASER_val_3; - PWM_CHANNEL_LASER = PWM_CHANNEL_LASER_3; - } - // add random number to current value to let it oscliate - long laserwiggle = random(-LASERdespeckle, LASERdespeckle); - LASER_val_wiggle += laserwiggle; - if (LASER_val_wiggle > pwm_max) - LASER_val_wiggle -= (2 * abs(laserwiggle)); - if (LASER_val_wiggle < 0) - LASER_val_wiggle += (2 * abs(laserwiggle)); - - if (DEBUG) Serial.println(LASERid); - if (DEBUG) Serial.println(LASER_val_wiggle); - - ledcWrite(PWM_CHANNEL_LASER, LASER_val_wiggle); - - delay(LASERperiod); - - } -} - - -// Custom function accessible by the API -void LASER_act_fct() { - // here you can do something - Serial.println("LASER_act_fct"); - - isBusy = true; - - int LASERid = jsonDocument["LASERid"]; - int LASERval = jsonDocument["LASERval"]; - int LASERdespeckle = jsonDocument["LASERdespeckle"]; - int LASERdespecklePeriod = 20; - if (jsonDocument.containsKey("LASERdespecklePeriod")) { - LASERdespecklePeriod = jsonDocument["LASERdespecklePeriod"]; - } - - if (DEBUG) { - Serial.print("LASERid "); Serial.println(LASERid); - Serial.print("LASERval "); Serial.println(LASERval); - Serial.print("LASERdespeckle "); Serial.println(LASERdespeckle); - Serial.print("LASERdespecklePeriod "); Serial.println(LASERdespecklePeriod); - } - - if (LASERid == 1) { - LASER_val_1 = LASERval; - LASER_despeckle_1 = LASERdespeckle; - LASER_despeckle_period_1 = LASERdespecklePeriod; - if (DEBUG) { - Serial.print("LaserPIN "); - Serial.println(LASER_PIN_1); - } - ledcWrite(PWM_CHANNEL_LASER_1, LASERval); - } - else if (LASERid == 2) { - LASER_val_2 = LASERval; - LASER_despeckle_2 = LASERdespeckle; - LASER_despeckle_period_2 = LASERdespecklePeriod; - if (DEBUG) { - Serial.print("LaserPIN "); - Serial.println(LASER_PIN_2); - } - ledcWrite(PWM_CHANNEL_LASER_2, LASERval); - } - else if (LASERid == 3) { - LASER_val_3 = LASERval; - LASER_despeckle_3 = LASERdespeckle; - LASER_despeckle_period_3 = LASERdespecklePeriod; - if (DEBUG) { - Serial.print("LaserPIN "); - Serial.println(LASER_PIN_3); - } - ledcWrite(PWM_CHANNEL_LASER_3, LASERval); - } - - jsonDocument.clear(); - jsonDocument["return"] = 1; - - isBusy = false; -} - -void LASER_set_fct() { - // here you can set parameters - int LASERid = jsonDocument["LASERid"]; - int LASERpin = jsonDocument["LASERpin"]; - - if (DEBUG) Serial.print("LASERid "); Serial.println(LASERid); - if (DEBUG) Serial.print("LASERpin "); Serial.println(LASERpin); - - if (LASERid != NULL and LASERpin != NULL) { - if (LASERid == 1) { - LASER_PIN_1 = LASERpin; - pinMode(LASER_PIN_1, OUTPUT); - digitalWrite(LASER_PIN_1, LOW); - /* setup the PWM ports and reset them to 0*/ - ledcSetup(PWM_CHANNEL_LASER_1, pwm_frequency, pwm_resolution); - ledcAttachPin(LASER_PIN_1, PWM_CHANNEL_LASER_1); - ledcWrite(PWM_CHANNEL_LASER_1, 0); - } - else if (LASERid == 2) { - LASER_PIN_2 = LASERpin; - pinMode(LASER_PIN_2, OUTPUT); - digitalWrite(LASER_PIN_2, LOW); - /* setup the PWM ports and reset them to 0*/ - ledcSetup(PWM_CHANNEL_LASER_2, pwm_frequency, pwm_resolution); - ledcAttachPin(LASER_PIN_2, PWM_CHANNEL_LASER_2); - ledcWrite(PWM_CHANNEL_LASER_2, 0); - } - else if (LASERid == 3) { - LASER_PIN_3 = LASERpin; - pinMode(LASER_PIN_3, OUTPUT); - digitalWrite(LASER_PIN_3, LOW); - /* setup the PWM ports and reset them to 0*/ - ledcSetup(PWM_CHANNEL_LASER_3, pwm_frequency, pwm_resolution); - ledcAttachPin(LASER_PIN_3, PWM_CHANNEL_LASER_3); - ledcWrite(PWM_CHANNEL_LASER_3, 0); - } - } - - jsonDocument.clear(); - jsonDocument["return"] = 1; -} - -// Custom function accessible by the API -void LASER_get_fct() { - // GET SOME PARAMETERS HERE - int LASERid = jsonDocument["LASERid"]; - int LASERpin = 0; - int LASERval = 0; - - if (LASERid == 1) { - if (DEBUG) Serial.println("LASER 1"); - LASERpin = LASER_PIN_1; - LASERval = LASER_val_1; - } - else if (LASERid == 2) { - if (DEBUG) Serial.println("AXIS 2"); - if (DEBUG) Serial.println("LASER 2"); - LASERpin = LASER_PIN_2; - LASERval = LASER_val_2; - } - else if (LASERid == 3) { - if (DEBUG) Serial.println("AXIS 3"); - if (DEBUG) Serial.println("LASER 1"); - LASERpin = LASER_PIN_3; - LASERval = LASER_val_3; - } - - jsonDocument.clear(); - jsonDocument["LASERid"] = LASERid; - jsonDocument["LASERval"] = LASERval; - jsonDocument["LASERpin"] = LASERpin; -} - -void setup_laser() { - Serial.println("Setting Up LASERs"); - - // switch of the LASER directly - pinMode(LASER_PIN_1, OUTPUT); - pinMode(LASER_PIN_2, OUTPUT); - pinMode(LASER_PIN_3, OUTPUT); - digitalWrite(LASER_PIN_1, LOW); - digitalWrite(LASER_PIN_2, LOW); - digitalWrite(LASER_PIN_3, LOW); - - /* setup the PWM ports and reset them to 0*/ - ledcSetup(PWM_CHANNEL_LASER_1, pwm_frequency, pwm_resolution); - ledcAttachPin(LASER_PIN_1, PWM_CHANNEL_LASER_1); - ledcWrite(PWM_CHANNEL_LASER_1, 10000); - delay(500); - ledcWrite(PWM_CHANNEL_LASER_1, 0); - - ledcSetup(PWM_CHANNEL_LASER_2, pwm_frequency, pwm_resolution); - ledcAttachPin(LASER_PIN_2, PWM_CHANNEL_LASER_2); - ledcWrite(PWM_CHANNEL_LASER_2, 10000); - delay(500); - ledcWrite(PWM_CHANNEL_LASER_2, 0); - - ledcSetup(PWM_CHANNEL_LASER_3, pwm_frequency, pwm_resolution); - ledcAttachPin(LASER_PIN_3, PWM_CHANNEL_LASER_3); - ledcWrite(PWM_CHANNEL_LASER_3, 10000); - delay(500); - ledcWrite(PWM_CHANNEL_LASER_3, 0); -} - -/* - wrapper for HTTP requests -*/ -void LASER_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - LASER_act_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void LASER_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - LASER_get_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void LASER_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - LASER_set_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} diff --git a/ESP32/main/control_motor.ino b/ESP32/main/control_motor.ino deleted file mode 100644 index a1e7da1..0000000 --- a/ESP32/main/control_motor.ino +++ /dev/null @@ -1,641 +0,0 @@ -// Custom function accessible by the API -void motor_act_fct() { - if (DEBUG) Serial.println("motor_act_fct"); - - - // assign default values to thhe variables - if (jsonDocument.containsKey("speed0")) { - mspeed0 = jsonDocument["speed0"]; - } - else if (jsonDocument.containsKey("speed")) { - mspeed0 = jsonDocument["speed"]; - } - if (jsonDocument.containsKey("speed1")) { - mspeed1 = jsonDocument["speed1"]; - } - else if (jsonDocument.containsKey("speed")) { - mspeed1 = jsonDocument["speed"]; - } - if (jsonDocument.containsKey("speed2")) { - mspeed2 = jsonDocument["speed2"]; - } - else if (jsonDocument.containsKey("speed")) { - mspeed2 = jsonDocument["speed"]; - } - if (jsonDocument.containsKey("speed3")) { - mspeed3 = jsonDocument["speed3"]; - } - else if (jsonDocument.containsKey("speed")) { - mspeed3 = jsonDocument["speed"]; - } - - if (jsonDocument.containsKey("pos0")) { - mposition0 = jsonDocument["pos0"]; - } - else { - mposition0 = 0; - } - if (jsonDocument.containsKey("pos1")) { - mposition1 = jsonDocument["pos1"]; - } - else { - mposition1 = 0; - } - - if (jsonDocument.containsKey("pos2")) { - mposition2 = jsonDocument["pos2"]; - } - else { - mposition2 = 0; - } - - if (jsonDocument.containsKey("pos3")) { - mposition3 = jsonDocument["pos3"]; - } - else { - mposition3 = 0; - } - - if (jsonDocument.containsKey("isabs")) { - isabs = jsonDocument["isabs"]; - } - else { - isabs = 0; - } - - if (jsonDocument.containsKey("isstop")) { - isstop = jsonDocument["isstop"]; - } - else { - isstop = 0; - } - - if (jsonDocument.containsKey("isaccel")) { - isaccel = jsonDocument["isaccel"]; - } - else { - isaccel = 1; - } - - if (jsonDocument.containsKey("isen")) { - isen = jsonDocument["isen"]; - } - else { - isen = 0; - } - - if (jsonDocument.containsKey("isforever")) { - isforever = jsonDocument["isforever"]; - } - else { - isforever = 0; - } - - // check if homing is expected - if (jsonDocument.containsKey("ishomeX")) { - ishomeX = jsonDocument["ishomeX "]; - } - else { - ishomeX = 0; - } - // check if homing is expected - if (jsonDocument.containsKey("ishomeY")) { - ishomeY = jsonDocument["ishomeY"]; - } - else { - ishomeY = 0; - } - // check if homing is expected - if (jsonDocument.containsKey("ishomeZ")) { - ishomeZ = jsonDocument["ishomeZ"]; - } - else { - ishomeZ = 0; - } - - // check if homing is expected - if (jsonDocument.containsKey("homePinX")) { - homePinX = jsonDocument["homePinX"]; - pinMode(homePinX, INPUT_PULLUP); - } - if (jsonDocument.containsKey("homePinY")) { - homePinY = jsonDocument["homePinY"]; - pinMode(homePinY, INPUT_PULLUP); - } - if (jsonDocument.containsKey("homePinZ")) { - homePinZ = jsonDocument["homePinZ"]; - pinMode(homePinZ, INPUT_PULLUP); - } - if (jsonDocument.containsKey("homePinA")) { - homePinA = jsonDocument["homePinA"]; - pinMode(homePinA, INPUT_PULLUP); - } - - - - jsonDocument.clear(); - - if (DEBUG) { - Serial.print("speed0 "); Serial.println(mspeed0); - Serial.print("speed1 "); Serial.println(mspeed1); - Serial.print("speed2 "); Serial.println(mspeed2); - Serial.print("speed3 "); Serial.println(mspeed3); - Serial.print("position0 "); Serial.println(mposition0); - Serial.print("position1 "); Serial.println(mposition1); - Serial.print("position2 "); Serial.println(mposition2); - Serial.print("position3 "); Serial.println(mposition3); - Serial.print("isabs "); Serial.println(isabs); - Serial.print("isen "); Serial.println(isen); - Serial.print("isstop "); Serial.println(isstop); - Serial.print("isaccel "); Serial.println(isaccel); - Serial.print("isforever "); Serial.println(isforever); - Serial.print("ishomeX "); Serial.println(ishomeX); - Serial.print("ishomeY "); Serial.println(ishomeY); - Serial.print("ishomeZ "); Serial.println(ishomeZ); - } - - if (isstop) { - // Immediately stop the motor - stepper_A.stop(); - stepper_X.stop(); - stepper_Y.stop(); - stepper_Z.stop(); - isforever = 0; - setEnableMotor(false); - - POSITION_MOTOR_A = stepper_A.currentPosition(); - POSITION_MOTOR_X = stepper_X.currentPosition(); - POSITION_MOTOR_Y = stepper_Y.currentPosition(); - POSITION_MOTOR_Z = stepper_Z.currentPosition(); - - jsonDocument["POSA"] = POSITION_MOTOR_A; - jsonDocument["POSX"] = POSITION_MOTOR_X; - jsonDocument["POSY"] = POSITION_MOTOR_Y; - jsonDocument["POSZ"] = POSITION_MOTOR_Z; - return; - } - - if (abs(ishomeX)) { - doHoming("X", ishomeX); - } - if (abs(ishomeY)) { - doHoming("Y", ishomeY); - } - if (abs(ishomeZ)) { - doHoming("Z", ishomeZ); - } - - if (IS_PSCONTROLER_ACTIVE) - IS_PSCONTROLER_ACTIVE = false; // override PS controller settings #TODO: Somehow reset it later? - - // prepare motor to run - setEnableMotor(true); - stepper_A.setSpeed(mspeed0); - stepper_X.setSpeed(mspeed1); - stepper_Y.setSpeed(mspeed2); - stepper_Z.setSpeed(mspeed3); - stepper_A.setMaxSpeed(mspeed0); - stepper_X.setMaxSpeed(mspeed1); - stepper_Y.setMaxSpeed(mspeed2); - stepper_Z.setMaxSpeed(mspeed3); - - - if (not isforever) { - if (isabs) { - // absolute position coordinates - stepper_A.moveTo(SIGN_A * mposition0); - stepper_X.moveTo(SIGN_X * mposition1); - stepper_Y.moveTo(SIGN_Y * mposition2); - stepper_Z.moveTo(SIGN_Z * mposition3); - } - else { - // relative position coordinates - stepper_A.move(SIGN_A * mposition0); - stepper_X.move(SIGN_X * mposition1); - stepper_Y.move(SIGN_Y * mposition2); - stepper_Z.move(SIGN_Z * mposition3); - } - } - - if (DEBUG) Serial.println("Start rotation in background"); - - POSITION_MOTOR_A = stepper_A.currentPosition(); - POSITION_MOTOR_X = stepper_X.currentPosition(); - POSITION_MOTOR_Y = stepper_Y.currentPosition(); - POSITION_MOTOR_Z = stepper_Z.currentPosition(); - - jsonDocument["POSA"] = POSITION_MOTOR_A; - jsonDocument["POSX"] = POSITION_MOTOR_X; - jsonDocument["POSY"] = POSITION_MOTOR_Y; - jsonDocument["POSZ"] = POSITION_MOTOR_Z; -} - -void setEnableMotor(bool enable) { - //isBusy = enable; - digitalWrite(ENABLE_PIN, !enable); - motor_enable = enable; -} - -bool getEnableMotor() { - return motor_enable; -} - - -void doHoming(String axis, int signHomging) { - // FIXME: Move motor until switch is hit... - long tStart = millis(); - if(DEBUG) Serial.println("Start homing"); - if (axis == "A") { - stepper_A.setSpeed(signHomging*mspeed0); - while (millis() - tStart < 1000 or !digitalRead(homePinA)) { // Here we want to check a button - stepper_A.runSpeed(); - } - stepper_A.setCurrentPosition(0); - } - if (axis == "X") { - stepper_X.setSpeed(signHomging*mspeed1); - while (millis() - tStart < 1000 or !digitalRead(homePinX)) { // Here we want to check a button - stepper_X.runSpeed(); - } - stepper_X.setCurrentPosition(0); - } - if (axis == "Y") { - stepper_Y.setSpeed(signHomging*mspeed0); - while (millis() - tStart < 1000 or !digitalRead(homePinY)) { // Here we want to check a button - stepper_Y.runSpeed(); - } - stepper_A.setCurrentPosition(0); - } - if (axis == "Z") { - stepper_Z.setSpeed(signHomging*mspeed0); - while (millis() - tStart < 1000 or !digitalRead(homePinZ)) { // Here we want to check a button - stepper_Z.runSpeed(); - } - stepper_Z.setCurrentPosition(0); - } - if(DEBUG) Serial.println("Finishing homing"); -} - -void motor_set_fct() { - - - // default value handling - int axis = -1; - if (jsonDocument.containsKey("axis")) { - int axis = jsonDocument["axis"]; - } - - int currentposition = NULL; - if (jsonDocument.containsKey("currentposition")) { - int currentposition = jsonDocument["currentposition"]; - } - - int maxspeed = NULL; - if (jsonDocument.containsKey("maxspeed")) { - int maxspeed = jsonDocument["maxspeed"]; - } - - int accel = NULL; - if (jsonDocument.containsKey("accel")) { - int accel = jsonDocument["accel"]; - } - - int pinstep = -1; - if (jsonDocument.containsKey("pinstep")) { - int pinstep = jsonDocument["pinstep"]; - } - - int pindir = -1; - if (jsonDocument.containsKey("pindir")) { - int pindir = jsonDocument["pindir"]; - } - - int isen = -1; - if (jsonDocument.containsKey("isen")) { - int isen = jsonDocument["isen"]; - } - - int sign = NULL; - if (jsonDocument.containsKey("sign")) { - int sign = jsonDocument["sign"]; - } - - // DEBUG printing - if (DEBUG) { - Serial.print("axis "); Serial.println(axis); - Serial.print("currentposition "); Serial.println(currentposition); - Serial.print("maxspeed "); Serial.println(maxspeed); - Serial.print("pinstep "); Serial.println(pinstep); - Serial.print("pindir "); Serial.println(pindir); - Serial.print("isen "); Serial.println(isen); - Serial.print("accel "); Serial.println(accel); - Serial.print("isen: "); Serial.println(isen); - } - - - if (accel >= 0) { - if (DEBUG) Serial.print("accel "); Serial.println(accel); - switch (axis) { - case 0: - MAX_ACCELERATION_A = accel; - stepper_A.setAcceleration(MAX_ACCELERATION_A); - break; - case 1: - MAX_ACCELERATION_X = accel; - stepper_X.setAcceleration(MAX_ACCELERATION_X); - break; - case 2: - MAX_ACCELERATION_Y = accel; - stepper_Y.setAcceleration(MAX_ACCELERATION_Y); - break; - case 3: - MAX_ACCELERATION_Z = accel; - stepper_Z.setAcceleration(MAX_ACCELERATION_Z); - break; - } - } - - if (sign != NULL) { - if (DEBUG) Serial.print("sign "); Serial.println(sign); - switch (axis) { - case 0: - SIGN_A = sign; - break; - case 1: - SIGN_X = sign; - break; - case 2: - SIGN_Y = sign; - break; - case 3: - SIGN_Z = sign; - break; - } - } - - - if (currentposition != NULL) { - if (DEBUG) Serial.print("currentposition "); Serial.println(currentposition); - switch (axis) { - case 0: - POSITION_MOTOR_A = currentposition; - stepper_A.setCurrentPosition(currentposition); - break; - case 1: - POSITION_MOTOR_X = currentposition; - stepper_X.setCurrentPosition(currentposition); - break; - case 2: - POSITION_MOTOR_Y = currentposition; - stepper_Y.setCurrentPosition(currentposition); - case 3: - POSITION_MOTOR_Z = currentposition; - stepper_Z.setCurrentPosition(currentposition); - break; - } - } - if (maxspeed != 0) { - switch (axis) { - case 1: - stepper_X.setMaxSpeed(maxspeed); - break; - case 2: - stepper_Y.setMaxSpeed(maxspeed); - break; - case 3: - stepper_Z.setMaxSpeed(maxspeed); - break; - } - } - - - //if (DEBUG) Serial.print("isen "); Serial.println(isen); - if (isen != 0 and isen) { - digitalWrite(ENABLE_PIN, 0); - } - else if (isen != 0 and not isen) { - digitalWrite(ENABLE_PIN, 1); - } - jsonDocument.clear(); - jsonDocument["return"] = 1; -} - - - -// Custom function accessible by the API -void motor_get_fct() { - int axis = jsonDocument["axis"]; - if (DEBUG) Serial.println("motor_get_fct"); - if (DEBUG) Serial.println(axis); - - int mmaxspeed = 0; - int mposition = 0; - int pinstep = 0; - int pindir = 0; - int sign = 0; - int mspeed = 0; - - if (axis == -1) { - //then we ask for position of all axis - jsonDocument.clear(); - jsonDocument["positionX"] = stepper_X.currentPosition(); - jsonDocument["positionY"] = stepper_Y.currentPosition(); - jsonDocument["positionZ"] = stepper_Z.currentPosition(); - jsonDocument["positionA"] = stepper_A.currentPosition(); - } - else { - if (axis == 0) { - if (DEBUG) Serial.println("AXIS 0"); - mmaxspeed = stepper_A.maxSpeed(); - mspeed = stepper_A.speed(); - POSITION_MOTOR_A = stepper_A.currentPosition(); - mposition = POSITION_MOTOR_A; - pinstep = STEP_PIN_A; - pindir = DIR_PIN_A; - sign = SIGN_A; - } - if (axis == 1) { - if (DEBUG) Serial.println("AXIS 1"); - mmaxspeed = stepper_X.maxSpeed(); - mspeed = stepper_X.speed(); - POSITION_MOTOR_X = stepper_X.currentPosition(); - mposition = POSITION_MOTOR_X; - pinstep = STEP_PIN_X; - pindir = DIR_PIN_X; - sign = SIGN_X; - } - if (axis == 2) { - if (DEBUG) Serial.println("AXIS 2"); - mmaxspeed = stepper_Y.maxSpeed(); - mspeed = stepper_Y.speed(); - POSITION_MOTOR_Y = stepper_Y.currentPosition(); - mposition = POSITION_MOTOR_Y; - pinstep = STEP_PIN_Y; - pindir = DIR_PIN_Y; - sign = SIGN_Y; - } - if (axis == 3) { - if (DEBUG) Serial.println("AXIS 3"); - mmaxspeed = stepper_Z.maxSpeed(); - mspeed = stepper_Z.speed(); - POSITION_MOTOR_Z = stepper_Z.currentPosition(); - mposition = POSITION_MOTOR_Z; - pinstep = STEP_PIN_Z; - pindir = DIR_PIN_Z; - sign = SIGN_Z; - } - - - jsonDocument.clear(); - jsonDocument["position"] = mposition; - jsonDocument["speed"] = mspeed; - jsonDocument["maxspeed"] = mmaxspeed; - jsonDocument["pinstep"] = pinstep; - jsonDocument["pindir"] = pindir; - jsonDocument["sign"] = sign; - } -} - - -void setup_motor() { - - /* - Motor related settings - */ - if (DEBUG) Serial.println("Setting Up Motors"); - pinMode(ENABLE_PIN, OUTPUT); - setEnableMotor(true); - if (DEBUG) Serial.println("Setting Up Motor A"); - if (DEBUG) Serial.println("PIN A"); Serial.println(STEP_PIN_A); Serial.println(DIR_PIN_A); - stepper_A = AccelStepper(AccelStepper::DRIVER, STEP_PIN_A, DIR_PIN_A); - stepper_A.setMaxSpeed(MAX_VELOCITY_A); - stepper_A.setAcceleration(MAX_ACCELERATION_A); - stepper_A.enableOutputs(); - stepper_A.runToNewPosition(-100); - stepper_A.runToNewPosition(100); - stepper_A.setCurrentPosition(0); - - if (DEBUG) Serial.println("Setting Up Motor X"); - if (DEBUG) Serial.println("PIN X"); Serial.println(STEP_PIN_X); Serial.println(DIR_PIN_X); - stepper_X = AccelStepper(AccelStepper::DRIVER, STEP_PIN_X, DIR_PIN_X); - stepper_X.setMaxSpeed(MAX_VELOCITY_X); - stepper_X.setAcceleration(MAX_ACCELERATION_X); - stepper_X.enableOutputs(); - stepper_X.runToNewPosition(-100); - stepper_X.runToNewPosition(100); - stepper_X.setCurrentPosition(0); - - if (DEBUG) Serial.println("Setting Up Motor Y"); - if (DEBUG) Serial.println("PIN Y"); Serial.println(STEP_PIN_Y); Serial.println(DIR_PIN_Y); - stepper_Y = AccelStepper(AccelStepper::DRIVER, STEP_PIN_Y, DIR_PIN_Y); - stepper_Y.setMaxSpeed(MAX_VELOCITY_Y); - stepper_Y.setAcceleration(MAX_ACCELERATION_Y); - stepper_Y.enableOutputs(); - stepper_Y.runToNewPosition(-100); - stepper_Y.runToNewPosition(100); - stepper_Y.setCurrentPosition(0); - - if (DEBUG) Serial.println("Setting Up Motor Z"); - if (DEBUG) Serial.println("PIN Z"); Serial.println(STEP_PIN_Z); Serial.println(DIR_PIN_Z); - stepper_Z = AccelStepper(AccelStepper::DRIVER, STEP_PIN_Z, DIR_PIN_Z); - stepper_Z.setMaxSpeed(MAX_VELOCITY_Z); - stepper_Z.setAcceleration(MAX_ACCELERATION_Z); - stepper_Z.enableOutputs(); - stepper_Z.runToNewPosition(-100); - stepper_Z.runToNewPosition(100); - stepper_Z.setCurrentPosition(0); - - if (DEBUG) Serial.println("Done Setting Up Motor Z"); - setEnableMotor(false); -} - - - -bool drive_motor_background() { - - // update motor positions - POSITION_MOTOR_A = stepper_A.currentPosition(); - POSITION_MOTOR_X = stepper_X.currentPosition(); - POSITION_MOTOR_Y = stepper_Y.currentPosition(); - POSITION_MOTOR_Z = stepper_Z.currentPosition(); - - // this function is called during every loop cycle - if (isforever) { - // run forever - // is this a bug? Otherwise the speed won't be set properly - seems like it is accelerating eventhough it shouldnt - stepper_A.setSpeed(mspeed0); - stepper_X.setSpeed(mspeed1); - stepper_Y.setSpeed(mspeed2); - stepper_Z.setSpeed(mspeed3); - // we have to at least set this. It will be recomputed or something?! - stepper_A.setMaxSpeed(mspeed1); - stepper_X.setMaxSpeed(mspeed1); - stepper_Y.setMaxSpeed(mspeed2); - stepper_Z.setMaxSpeed(mspeed3); - - stepper_A.runSpeed(); - stepper_X.runSpeed(); - stepper_Y.runSpeed(); - stepper_Z.runSpeed(); - } - else { - // run at constant speed - if (isaccel) { - stepper_A.run(); - stepper_X.run(); - stepper_Y.run(); - stepper_Z.run(); - } - else { - stepper_A.runSpeedToPosition(); - stepper_X.runSpeedToPosition(); - stepper_Y.runSpeedToPosition(); - stepper_Z.runSpeedToPosition(); - } - } - - // PROBLEM; is running wont work here! - if ((stepper_A.distanceToGo() == 0) and (stepper_X.distanceToGo() == 0) and (stepper_Y.distanceToGo() == 0) and (stepper_Z.distanceToGo() == 0 ) and not isforever) { - if (not isen) { - setEnableMotor(false); - } - isBusy = false; - return true; - } - isBusy = true; - return false; //never reached, but keeps compiler happy? -} - - -/* - wrapper for HTTP requests -*/ - -void motor_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - if (DEBUG) serializeJsonPretty(jsonDocument, Serial); - motor_act_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void motor_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - if (DEBUG) serializeJsonPretty(jsonDocument, Serial); - motor_get_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void motor_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - if (DEBUG) serializeJsonPretty(jsonDocument, Serial); - motor_set_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} diff --git a/ESP32/main/control_scanner.ino b/ESP32/main/control_scanner.ino deleted file mode 100644 index f56f7ea..0000000 --- a/ESP32/main/control_scanner.ino +++ /dev/null @@ -1,297 +0,0 @@ -#ifdef IS_SCANNER -#include "parameters_scanner.h" -//#include -#include "soc/timer_group_struct.h" -#include "soc/timer_group_reg.h" - -void controlGalvoTask( void * parameter ) { - Serial.println("Starting Scanner Thread"); - - while (1) { - // loop forever - - if (isScanRunning or scannernFrames > 0) { - runScanner(); - } - else { - vTaskDelay(100); - } - } - vTaskDelete(NULL); -} - - -void runScanner() { - if (DEBUG) Serial.println("Start FrameStack"); - int roundTripCounter = 0; - /* - {"task": "/scanner_act", - "scannernFrames":100, - "scannerMode":"classic", - "scannerXFrameMin":0, - "scannerXFrameMax":255, - "scannerYFrameMin":0, - "scannerYFrameMax":255, - "scannerEnable":0, - "scannerXFrameMin":1, - "scannerXFrameMax":1, - "scannerYFrameMin":1, - "scannerYFrameMax":1, - "scannerXStep":15, - "scannerYStep":15, - "scannerLaserVal":32000, - "scannerExposure":10, - "scannerDelay":1000} - */ - - for (int iFrame = 0; iFrame <= scannernFrames; iFrame++) { - // shifting phase in x - for (int idelayX = scannerXFrameMin; idelayX <= scannerXFrameMax; idelayX++) { - // shifting phase in y - for (int idelayY = scannerYFrameMin; idelayY <= scannerYFrameMax; idelayY++) { - // iteratinv over all pixels in x - for (int ix = scannerxMin; ix < scannerxMax - scannerXStep; ix += scannerXStep) { - // move X-mirror - dacWrite(scannerPinX, ix + idelayX); - //Serial.print("X");Serial.print(ix + idelayX); - - for (int iy = scanneryMin; iy < scanneryMax - scannerYStep; iy += scannerYStep) { - // move Y-mirror triangle - int scannerPosY = 0; - if ((roundTripCounter % 2) == 0 ) { - scannerPosY = iy + idelayY; - } - else { - scannerPosY = (scanneryMax - scannerYStep) - (iy + idelayY); - } - roundTripCounter++; - dacWrite(scannerPinY, scannerPosY); - //Serial.print("Y");Serial.println(scannerPosY); - delayMicroseconds(scannerDelay); - // expose Laser - ledcWrite(PWM_CHANNEL_LASER_1, scannerLaserVal); //digitalWrite(LASER_PIN_1, HIGH); // - delayMicroseconds(scannerExposure); - ledcWrite(PWM_CHANNEL_LASER_1, 0); // digitalWrite(LASER_PIN_1, LOW); // - delayMicroseconds(scannerDelay); - } - } - } - } - } - scannernFrames = 0; - if (DEBUG) Serial.println("Ending FrameStack"); -} - - - -// Custom function accessible by the API -void scanner_act_fct() { - - // here you can do something - if (DEBUG) Serial.println("scanner_act_fct"); - - // select scanning mode - const char* scannerMode = jsonDocument["scannerMode"]; - - - if (strcmp(scannerMode, "pattern") == 0) { - if (DEBUG) Serial.println("pattern"); - // individual pattern gets adressed - int arraySize = 0; - scannerExposure = 0; - scannerLaserVal = 32000; - scannernFrames = 1; - scannerDelay = 0; - - if (jsonDocument.containsKey("scannerExposure")) { - scannerExposure = jsonDocument["scannerExposure"]; - } - if (jsonDocument.containsKey("scannerLaserVal")) { - scannerLaserVal = jsonDocument["scannerLaserVal"]; - } - if (jsonDocument.containsKey("scannerDelay")) { - scannerDelay = jsonDocument["scannerDelay"]; - } - if (jsonDocument.containsKey("scannernFrames")) { - scannernFrames = jsonDocument["scannernFrames"]; - } - if (jsonDocument.containsKey("arraySize")) { - arraySize = jsonDocument["arraySize"]; - } - - for (int iFrame = 0; iFrame < scannernFrames; iFrame++) { - for (int i = 0; i < arraySize; i++) { //Iterate through results - int scannerIndex = jsonDocument["i"][i]; //Implicit cast - int scannerPosY = scannerIndex % 255; - int scannerPosX = scannerIndex / 255; - dacWrite(scannerPinY, scannerPosY); - dacWrite(scannerPinX, scannerPosX); - - /* - * Serial.print(scannerPosY); - Serial.print("/"); - Serial.println(scannerPosX); - */ - - //Serial.print("Y");Serial.println(scannerPosY); - delayMicroseconds(scannerDelay); - // expose Laser - ledcWrite(PWM_CHANNEL_LASER_1, scannerLaserVal); //digitalWrite(LASER_PIN_1, HIGH); // - delayMicroseconds(scannerExposure); - ledcWrite(PWM_CHANNEL_LASER_1, 0); // digitalWrite(LASER_PIN_1, LOW); // - delayMicroseconds(scannerDelay); - } - } - } - - else if (strcmp(scannerMode, "classic") == 0) { - if (DEBUG) Serial.println("classic"); - - // assert values - scannerxMin = 0; - scanneryMin = 0; - scannerxMax = 255; - scanneryMax = 255; - scannerExposure = 0; - scannerEnable = 0; - scannerLaserVal = 32000; - - scannerXFrameMax = 5; - scannerXFrameMin = 0; - scannerYFrameMax = 5; - scannerYFrameMin = 0; - scannerXStep = 5; - scannerYStep = 5; - scannernFrames = 1; - - scannerDelay = 0; - - - - if (jsonDocument.containsKey("scannernFrames")) { - scannernFrames = jsonDocument["scannernFrames"]; - } - if (jsonDocument.containsKey("scannerXFrameMax")) { - scannerXFrameMax = jsonDocument["scannerXFrameMax"]; - } - if (jsonDocument.containsKey("scannerXFrameMin")) { - scannerXFrameMin = jsonDocument["scannerXFrameMin"]; - } - if (jsonDocument.containsKey("scannerYFrameMax")) { - scannerYFrameMax = jsonDocument["scannerYFrameMax"]; - } - if (jsonDocument.containsKey("scannerYFrameMin")) { - scannerYFrameMin = jsonDocument["scannerYFrameMin"]; - } - if (jsonDocument.containsKey("scannerXStep")) { - scannerXStep = jsonDocument["scannerXStep"]; - } - if (jsonDocument.containsKey("scannerYStep")) { - scannerYStep = jsonDocument["scannerYStep"]; - } - if (jsonDocument.containsKey("scannerxMin")) { - scannerxMin = jsonDocument["scannerxMin"]; - } - if (jsonDocument.containsKey("scannerLaserVal")) { - scannerLaserVal = jsonDocument["scannerLaserVal"]; - } - if (jsonDocument.containsKey("scanneryMin")) { - scanneryMin = jsonDocument["scanneryMin"]; - } - if (jsonDocument.containsKey("scannerxMax")) { - scannerxMax = jsonDocument["scannerxMax"]; - } - if (jsonDocument.containsKey("scanneryMax")) { - scanneryMax = jsonDocument["scanneryMax"]; - } - if (jsonDocument.containsKey("scannerExposure")) { - scannerExposure = jsonDocument["scannerExposure"]; - } - if (jsonDocument.containsKey("scannerEnable")) { - scannerEnable = jsonDocument["scannerEnable"]; - } - if (jsonDocument.containsKey("scannerDelay")) { - scannerDelay = jsonDocument["scannerDelay"]; - } - - if (DEBUG) Serial.print("scannerxMin "); Serial.println(scannerxMin); - if (DEBUG) Serial.print("scanneryMin "); Serial.println(scanneryMin ); - if (DEBUG) Serial.print("scannerxMax "); Serial.println(scannerxMax ); - if (DEBUG) Serial.print("scanneryMax "); Serial.println(scanneryMax ); - if (DEBUG) Serial.print("scannerExposure "); Serial.println(scannerExposure ); - if (DEBUG) Serial.print("scannerEnable "); Serial.println(scannerEnable); - if (DEBUG) Serial.print("scannerLaserVal "); Serial.println(scannerLaserVal ); - if (DEBUG) Serial.print("scannerXFrameMax "); Serial.println(scannerXFrameMax); - if (DEBUG) Serial.print("scannerXFrameMin "); Serial.println(scannerXFrameMin); - if (DEBUG) Serial.print("scannerYFrameMax "); Serial.println(scannerYFrameMax); - if (DEBUG) Serial.print("scannerYFrameMin "); Serial.println(scannerYFrameMin); - if (DEBUG) Serial.print("scannerXStep "); Serial.println(scannerXStep ); - if (DEBUG) Serial.print("scannerYStep "); Serial.println(scannerYStep); - if (DEBUG) Serial.print("scannernFrames "); Serial.println(scannernFrames); - if (DEBUG) Serial.print("scannerDelay "); Serial.println(scannerDelay); - - jsonDocument.clear(); - if (DEBUG) Serial.println("Start controlGalvoTask"); - isScanRunning = scannerEnable; // Trigger a frame acquisition - if (DEBUG) Serial.println("Done with setting up Tasks"); - jsonDocument["return"] = 1; - - } -} - -void scanner_set_fct() { - jsonDocument.clear(); - jsonDocument["return"] = 1; -} - - - -// Custom function accessible by the API -void scanner_get_fct() { - jsonDocument.clear(); - jsonDocument["return"] = 0; -} - - - -/***************************************************************************************************/ -/******************************************* SCANNER *******************************************/ -/***************************************************************************************************/ -/****************************************** ********************************************************/ - - -void setup_scanner() { - runScanner(); // run not as a task - disableCore0WDT(); - xTaskCreate(controlGalvoTask, "controlGalvoTask", 10000, NULL, 1, NULL); -} - -/* - wrapper for HTTP requests -*/ -void scanner_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - scanner_act_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void scanner_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - scanner_get_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void scanner_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - scanner_set_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} -#endif diff --git a/ESP32/main/control_slm.ino b/ESP32/main/control_slm.ino deleted file mode 100644 index 161ad0e..0000000 --- a/ESP32/main/control_slm.ino +++ /dev/null @@ -1,368 +0,0 @@ -#ifdef IS_SLM - -#include "parameters_slm.h" - - - -Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS, TFT_DC, TFT_MOSI, TFT_CLK, TFT_RST); -int NX = tft.height(); -int NY = tft.width(); - - -// Custom function accessible by the API -void slm_act_fct() { - - // here you can do something - if (DEBUG) Serial.println("slm_act_fct"); - isBusy = true; - - const char* slmMode = jsonDocument["slmMode"]; // "ring", "clear" - - // individual pattern gets adressed - // PYTHON: send_LEDMatrix_array(self, led_pattern, timeout=1) - if (strcmp(slmMode, "circle") == 0) { - if (DEBUG) Serial.println("circle"); - int posX = 0; - int posY = 0; - int radius = 0; - uint16_t color = 0; - - if (jsonDocument.containsKey("posX")) { - posX = jsonDocument["posX"]; - } - if (jsonDocument.containsKey("posY")) { - posY = jsonDocument["posY"]; - } - if (jsonDocument.containsKey("radius")) { - radius = jsonDocument["radius"]; - } - if (jsonDocument.containsKey("color")) { - color = jsonDocument["color"]; - } - tft.fillCircle(posX, posY, radius, color); - - } - // PYTHON: send_LEDMatrix_array(self, led_pattern, timeout=1) - if (strcmp(slmMode, "rect") == 0) { - if (DEBUG) Serial.println("rect"); - int posX = 0; - int posY = 0; - int nX = 0; - int nY = 0; - uint16_t color = 0; - - if (jsonDocument.containsKey("posX")) { - posX = jsonDocument["posX"]; - } - if (jsonDocument.containsKey("posY")) { - posY = jsonDocument["posY"]; - } - if (jsonDocument.containsKey("nX")) { - nX = jsonDocument["nX"]; - } - if (jsonDocument.containsKey("nY")) { - nY = jsonDocument["nY"]; - } - if (jsonDocument.containsKey("color")) { - color = jsonDocument["color"]; - } - tft.drawRect(posX, posY, nX, nY, color); - - } - - if (strcmp(slmMode, "full") == 0) { - if (DEBUG) Serial.println("full"); - uint16_t color = jsonDocument["color"]; - tft.fillScreen(color); - } - if (strcmp(slmMode, "clear") == 0) { - if (DEBUG) Serial.println("clear"); - tft.fillScreen(ST77XX_BLACK); - } - // only if a single led will be updated, all others stay the same - // PYTHON: send_LEDMatrix_single(self, indexled=0, intensity=(255,255,255), timeout=1) - else if (strcmp(slmMode, "image") == 0) { - if (DEBUG) Serial.println("image"); - int startX = jsonDocument["startX"]; - int startY = jsonDocument["startY"]; - int endX = jsonDocument["endX"]; - int endY = jsonDocument["endY"]; - - int nX = endX-startX; - int nY = endY-startY; - for (int ix = 0; ix < nX; ix++) { - for (int iy = 0; iy < nY; iy++) { - uint16_t color = jsonDocument["color"][ix*(nX)+iy]; //Implicit cast - Serial.println(color); - tft.drawPixel(iy+nY, ix+nX, color); - } - } - } - - jsonDocument.clear(); - jsonDocument["return"] = 1; - isBusy = false; -} - -void slm_set_fct() { - /* - if (jsonDocument["LED_ARRAY_PIN"] != 0) { - //if (DEBUG) Serial.print("LED_ARRAY_PIN "); Serial.println(jsonDocument["LED_ARRAY_PIN"]); - LED_ARRAY_PIN = jsonDocument["LED_ARRAY_PIN"]; - } - - if (jsonDocument["LED_N_X"] != 0) { - //if (DEBUG) Serial.print("LED_N_X "); Serial.println(jsonDocument["LED_N_X"]); - LED_N_X = jsonDocument["LED_N_X"]; - } - - if (jsonDocument["LED_N_Y"] != 0) { - //if (DEBUG) Serial.print("LED_N_Y "); Serial.println(jsonDocument["LED_N_Y"]); - LED_N_Y = jsonDocument["LED_N_Y"]; - } - */ - jsonDocument.clear(); - jsonDocument["return"] = 1; -} - - - -// Custom function accessible by the API -void slm_get_fct() { - /* - jsonDocument.clear(); - jsonDocument["LED_ARRAY_PIN"] = LED_ARRAY_PIN; - jsonDocument["LED_N_X"] = LED_N_X; - jsonDocument["LED_N_Y"] = LED_N_Y; - */ -} - - - -/***************************************************************************************************/ -/******************************************* LED Array *******************************************/ -/***************************************************************************************************/ -/*******************************FROM OCTOPI ********************************************************/ - -/* - wrapper for HTTP requests -*/ -void slm_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - slm_act_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void slm_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - slm_get_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void slm_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - slm_set_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -void setup_slm() { - if(DEBUG) Serial.println("Initializing SLM"); - tft.initR(INITR_BLACKTAB); // Init ST7735S chip, black tab - tft.fillScreen(ST77XX_WHITE); - delay(500); - tft.fillScreen(ST77XX_BLACK); - if(DEBUG) Serial.println("Done Initializing SLM"); -} - -/* - Auxilary functions -*/ - -// https://github.com/fatur04/arduino/blob/master/ESP32_WEBSOCKET_SERVER_WITH_TFT_JPEG_V1/ESP32_WEBSOCKET_SERVER_WITH_TFT_JPEG_V1.ino - -//==================================================================================== -// Opens the image file and prime the Jpeg decoder -//==================================================================================== -void drawJpeg(String filename, int xpos, int ypos) { - - Serial.println("==========================="); - Serial.print("Drawing file: "); Serial.println(filename); - Serial.println("==========================="); - - // Open the named file (the Jpeg decoder library will close it after rendering image) - fs::File jpegFile = SPIFFS.open( filename, "r"); // File handle reference for SPIFFS - // File jpegFile = SD.open( filename, FILE_READ); // or, file handle reference for SD library - - if ( !jpegFile ) { - Serial.print("ERROR: File \""); Serial.print(filename); Serial.println ("\" not found!"); - return; - } - - // Use one of the three following methods to initialise the decoder: - //boolean decoded = JpegDec.decodeFsFile(jpegFile); // Pass a SPIFFS file handle to the decoder, - //boolean decoded = JpegDec.decodeSdFile(jpegFile); // or pass the SD file handle to the decoder, - boolean decoded = JpegDec.decodeFsFile(filename); // or pass the filename (leading / distinguishes SPIFFS files) - // Note: the filename can be a String or character array type - if (decoded) { - // print information about the image to the serial port - jpegInfo(); - - // render the image onto the screen at given coordinates - jpegRender(xpos, ypos); - } - else { - Serial.println("Jpeg file format not supported!"); - } -} - -//==================================================================================== -// Decode and render the Jpeg image onto the TFT screen -//==================================================================================== -void jpegRender(int xpos, int ypos) { - - // retrieve infomration about the image - uint16_t *pImg; - uint16_t mcu_w = JpegDec.MCUWidth; - uint16_t mcu_h = JpegDec.MCUHeight; - uint32_t max_x = JpegDec.width; - uint32_t max_y = JpegDec.height; - - // Jpeg images are draw as a set of image block (tiles) called Minimum Coding Units (MCUs) - // Typically these MCUs are 16x16 pixel blocks - // Determine the width and height of the right and bottom edge image blocks - uint32_t min_w = minimum(mcu_w, max_x % mcu_w); - uint32_t min_h = minimum(mcu_h, max_y % mcu_h); - - // save the current image block size - uint32_t win_w = mcu_w; - uint32_t win_h = mcu_h; - - // record the current time so we can measure how long it takes to draw an image - uint32_t drawTime = millis(); - - // save the coordinate of the right and bottom edges to assist image cropping - // to the screen size - max_x += xpos; - max_y += ypos; - - // read each MCU block until there are no more - while ( JpegDec.read()) { - - // save a pointer to the image block - pImg = JpegDec.pImage; - - // calculate where the image block should be drawn on the screen - int mcu_x = JpegDec.MCUx * mcu_w + xpos; - int mcu_y = JpegDec.MCUy * mcu_h + ypos; - - // check if the image block size needs to be changed for the right edge - if (mcu_x + mcu_w <= max_x) win_w = mcu_w; - else win_w = min_w; - - // check if the image block size needs to be changed for the bottom edge - if (mcu_y + mcu_h <= max_y) win_h = mcu_h; - else win_h = min_h; - - // copy pixels into a contiguous block - if (win_w != mcu_w) - { - for (int h = 1; h < win_h - 1; h++) - { - memcpy(pImg + h * win_w, pImg + (h + 1) * mcu_w, win_w << 1); - } - } - - // draw image MCU block only if it will fit on the screen - if ( ( mcu_x + win_w) <= tft.width() && ( mcu_y + win_h) <= tft.height()) - { - tft.drawRGBBitmap(mcu_x, mcu_y, pImg, win_w, win_h); - } - - else if ( ( mcu_y + win_h) >= tft.height()) JpegDec.abort(); - - } - - // calculate how long it took to draw the image - drawTime = millis() - drawTime; // Calculate the time it took - - // print the results to the serial port - Serial.print ("Total render time was : "); Serial.print(drawTime); Serial.println(" ms"); - Serial.println("====================================="); - -} - -//==================================================================================== -// Print information decoded from the Jpeg image -//==================================================================================== -void jpegInfo() { - - Serial.println("==============="); - Serial.println("JPEG image info"); - Serial.println("==============="); - Serial.print ("Width :"); Serial.println(JpegDec.width); - Serial.print ("Height :"); Serial.println(JpegDec.height); - Serial.print ("Components :"); Serial.println(JpegDec.comps); - Serial.print ("MCU / row :"); Serial.println(JpegDec.MCUSPerRow); - Serial.print ("MCU / col :"); Serial.println(JpegDec.MCUSPerCol); - Serial.print ("Scan type :"); Serial.println(JpegDec.scanType); - Serial.print ("MCU width :"); Serial.println(JpegDec.MCUWidth); - Serial.print ("MCU height :"); Serial.println(JpegDec.MCUHeight); - Serial.println("==============="); - Serial.println(""); -} - -//==================================================================================== -// Open a Jpeg file and send it to the Serial port in a C array compatible format -//==================================================================================== -void createArray(const char *filename) { - - // Open the named file - fs::File jpgFile = SPIFFS.open( filename, "r"); // File handle reference for SPIFFS - // File jpgFile = SD.open( filename, FILE_READ); // or, file handle reference for SD library - - if ( !jpgFile ) { - Serial.print("ERROR: File \""); Serial.print(filename); Serial.println ("\" not found!"); - return; - } - - uint8_t data; - byte line_len = 0; - Serial.println(""); - Serial.println("// Generated by a JPEGDecoder library example sketch:"); - Serial.println("// https://github.com/Bodmer/JPEGDecoder"); - Serial.println(""); - Serial.println("#if defined(__AVR__)"); - Serial.println(" #include "); - Serial.println("#endif"); - Serial.println(""); - Serial.print ("const uint8_t "); - while (*filename != '.') Serial.print(*filename++); - Serial.println("[] PROGMEM = {"); // PROGMEM added for AVR processors, it is ignored by Due - - while ( jpgFile.available()) { - - data = jpgFile.read(); - Serial.print("0x"); if (abs(data) < 16) Serial.print("0"); - Serial.print(data, HEX); Serial.print(",");// Add value and comma - line_len++; - if ( line_len >= 32) { - line_len = 0; - Serial.println(); - } - - } - - Serial.println("};\r\n"); - jpgFile.close(); -} - - -#endif diff --git a/ESP32/main/control_state.ino b/ESP32/main/control_state.ino deleted file mode 100644 index 9245c1a..0000000 --- a/ESP32/main/control_state.ino +++ /dev/null @@ -1,180 +0,0 @@ - - -static inline int8_t sgn(int val) { - if (val < 0) return -1; - if (val == 0) return 0; - return 1; -} - -// Custom function accessible by the API -void state_act_fct() { - // here you can do something - if (DEBUG) Serial.println("state_act_fct"); - - // assign default values to thhe variables - if (jsonDocument.containsKey("restart")) { - ESP.restart(); - } - // assign default values to thhe variables - if (jsonDocument.containsKey("delay")) { - int mdelayms = jsonDocument["delay"]; - delay(mdelayms); - } - if (jsonDocument.containsKey("isBusy")) { - isBusy = jsonDocument["isBusy"]; - } - - if (jsonDocument.containsKey("pscontroller")) { - IS_PSCONTROLER_ACTIVE = jsonDocument["pscontroller"]; - } - jsonDocument.clear(); - jsonDocument["return"] = 1; -} - -void state_set_fct() { - // here you can set parameters - - int isdebug = jsonDocument["isdebug"]; - DEBUG = isdebug; - jsonDocument.clear(); - jsonDocument["return"] = 1; - -} - -// Custom function accessible by the API -void state_get_fct() { - // GET SOME PARAMETERS HERE - if (jsonDocument.containsKey("isBusy")) { - jsonDocument.clear(); - jsonDocument["isBusy"] = isBusy; // returns state of function that takes longer to finalize (e.g. motor) - } - else if (jsonDocument.containsKey("pscontroller")) { - jsonDocument.clear(); - jsonDocument["pscontroller"] = IS_PSCONTROLER_ACTIVE; // returns state of function that takes longer to finalize (e.g. motor) - } - else { - jsonDocument.clear(); - jsonDocument["identifier_name"] = identifier_name; - jsonDocument["identifier_id"] = identifier_id; - jsonDocument["identifier_date"] = identifier_date; - jsonDocument["identifier_author"] = identifier_author; - jsonDocument["IDENTIFIER_NAME"] = IDENTIFIER_NAME; - } -} - -void printInfo() { - if (DEBUG) Serial.println("You can use this software by sending JSON strings, A full documentation can be found here:"); - if (DEBUG) Serial.println("https://github.com/openUC2/UC2-REST/"); - //Serial.println("A first try can be: \{\"task\": \"/state_get\""); -} - - - - -// Check if Bluetoothdevice exists? -//https://github.com/espressif/arduino-esp32/blob/master/libraries/BluetoothSerial/examples/bt_remove_paired_devices/bt_remove_paired_devices.ino -//https://github.com/aed3/PS4-esp32/issues/40 -#define PAIR_MAX_DEVICES 20 -uint8_t pairedDeviceBtAddr[PAIR_MAX_DEVICES][6]; -char bda_str[18]; -#define REMOVE_BONDED_DEVICES 1 // <- Set to 0 to view all bonded devices addresses, set to 1 to remove - -char *bda2str(const uint8_t* bda, char *str, size_t size) -{ - if (bda == NULL || str == NULL || size < 18) { - return NULL; - } - sprintf(str, "%02x:%02x:%02x:%02x:%02x:%02x", - bda[0], bda[1], bda[2], bda[3], bda[4], bda[5]); - return str; -} -void clearBlueetoothDevice() { - Serial.print("ESP32 bluetooth address: "); Serial.println(bda2str(esp_bt_dev_get_address(), bda_str, 18)); - // Get the numbers of bonded/paired devices in the BT module - int count = esp_bt_gap_get_bond_device_num(); - if (!count) { - Serial.println("No bonded device found."); - } else { - Serial.print("Bonded device count: "); Serial.println(count); - if (PAIR_MAX_DEVICES < count) { - count = PAIR_MAX_DEVICES; - Serial.print("Reset bonded device count: "); Serial.println(count); - } - esp_err_t tError = esp_bt_gap_get_bond_device_list(&count, pairedDeviceBtAddr); - if (ESP_OK == tError) { - for (int i = 0; i < count; i++) { - Serial.print("Found bonded device # "); Serial.print(i); Serial.print(" -> "); - Serial.println(bda2str(pairedDeviceBtAddr[i], bda_str, 18)); - if (REMOVE_BONDED_DEVICES) { - esp_err_t tError = esp_bt_gap_remove_bond_device(pairedDeviceBtAddr[i]); - if (ESP_OK == tError) { - Serial.print("Removed bonded device # "); - } else { - Serial.print("Failed to remove bonded device # "); - } - Serial.println(i); - } - } - } - } -} - - -bool isFirstRun() { - // define preference name - const char* prefName = "firstRun"; - preferences.begin(prefName, false); - static const char dateKey[] = "date"; - const char *compiled_date = __DATE__ " " __TIME__; - String stored_date = preferences.getString(dateKey, ""); // FIXME - - Serial.println("Stored date:"); - Serial.println(stored_date); - Serial.println("Compiled date:"); - Serial.println(compiled_date); - - Serial.print("First run? "); - if (!stored_date.equals(compiled_date)) { - Serial.println("yes"); - } else { - Serial.println("no"); - } - - preferences.putString(dateKey, compiled_date); // FIXME? - preferences.end(); - return !stored_date.equals(compiled_date); -} - -void getIdentity() { - if(DEBUG) Serial.println("Get Identity"); - server.send(200, "application/json", identifier_name); -} - -/* - wrapper for HTTP requests -*/ -void state_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - state_act_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void state_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - state_get_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void state_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - state_set_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} diff --git a/ESP32/main/control_wifi.ino b/ESP32/main/control_wifi.ino deleted file mode 100644 index 39abe32..0000000 --- a/ESP32/main/control_wifi.ino +++ /dev/null @@ -1,278 +0,0 @@ -#include -#include -#include - - -void init_Spiffs() -{ - if (!SPIFFS.begin()) /* Démarrage du gestionnaire de fichiers SPIFFS */ - { - Serial.println("Erreur SPIFFS..."); - return; - } - - /* Détection des fichiers présents sur l'Esp32 */ - File root = SPIFFS.open("/"); /* Ouverture de la racine */ - File file = root.openNextFile(); /* Ouverture du 1er fichier */ - while (file) /* Boucle de test de présence des fichiers - Si plus de fichiers la boucle s'arrête*/ - - { - Serial.print("File: "); - Serial.println(file.name()); - file.close(); - file = root.openNextFile(); /* Lecture du fichier suivant */ - } -} - -void initWifiAP(const char *ssid) { - Serial.print("Network SSID (AP): "); - Serial.println(ssid); - - WiFi.softAP(ssid); - Serial.print("AP IP address: "); - Serial.println(WiFi.softAPIP()); -} - -void joinWifi(char *ssid, char *password) { - Serial.print("Connecting to "); - Serial.println(ssid); - - WiFi.begin(ssid, password); - - int nConnectTrials = 0; - while (WiFi.status() != WL_CONNECTED) { - Serial.print("."); - delay(500); - nConnectTrials += 1; - if (nConnectTrials > 10) - ESP.restart(); - // we can even make the ESP32 to sleep - } - - Serial.print("Connected. IP: "); - Serial.println(WiFi.localIP()); -} - - - -void autoconnectWifi(boolean isResetWifiSettings) { - WiFi.mode(WIFI_STA); // explicitly set mode, esp defaults to STA+AP - // it is a good practice to make sure your code sets wifi mode how you want it. - - // reset settings - wipe stored credentials for testing - // these are stored by the esp library - if (isResetWifiSettings) { - Serial.println("First run => resetting Wifi Settings"); - wm.resetSettings(); - } - wm.setHostname(hostname); - wm.setConfigPortalBlocking(false); - //wm.setConfigPortalBlocking(false); - //wm.setConfigPortalTimeout(90); // auto close configportal after n seconds - wm.setConnectTimeout(10); - - // Automatically connect using saved credentials, - // if connection fails, it starts an access point with the specified name ( "AutoConnectAP"), - // if empty will auto generate SSID, if password is blank it will be anonymous AP (wm.autoConnect()) - // then goes into a blocking loop awaiting configuration and will return success result - bool res; - // res = wm.autoConnect(); // auto generated AP name from chipid - // res = wm.autoConnect("AutoConnectAP"); // anonymous ap - res = wm.autoConnect(WifiSSIDAP); // password protected ap - - if (!res) { - Serial.println("Failed to connect"); - initWifiAP(WifiSSIDAP); - } - else { - //if you get here you have connected to the WiFi - Serial.println("connected..."); - } - - Serial.print("Connected. IP: "); - Serial.println(WiFi.localIP()); - -} - -//https://www.gabrielcsapo.com/arduino-web-server-esp-32/ -bool loadFromSPIFFS(String path) { - String dataType = "text/html"; - - Serial.print("Requested page -> "); - Serial.println(path); - if (SPIFFS.exists(path)){ - File dataFile = SPIFFS.open(path, "r"); - if (!dataFile) { - handleNotFound(); - return false; - } - - if (server.streamFile(dataFile, dataType) != dataFile.size()) { - Serial.println("Sent less data than expected!"); - }else{ - Serial.println("Page served!"); - } - - dataFile.close(); - }else{ - handleNotFound(); - return false; - } - return true; -} - -void handleNotFound() { - String message = "File Not Found\n\n"; - message += "URI: "; - message += server.uri(); - message += "\nMethod: "; - message += (server.method() == HTTP_GET) ? "GET" : "POST"; - message += "\nArguments: "; - message += server.args(); - message += "\n"; - - for (uint8_t i = 0; i < server.args(); i++) { - message += " " + server.argName(i) + ": " + server.arg(i) + "\n"; - } - - server.send(404, "text/plain", message); -} - - -void handleSwaggerYaml() { //Handler for the body path - loadFromSPIFFS("/openapi.yaml"); -} - -void handleSwaggerUI() { //Handler for the body path - loadFromSPIFFS("/index.html"); -} - - -void handlestandalone() { - loadFromSPIFFS("/swagger_standalone.js"); -} - -void handleswaggerbundle() { - loadFromSPIFFS("/swagger-ui-bundle.js"); -} - -void handleswaggercss() { - loadFromSPIFFS("/swagger-ui.css"); -} - - -/* - Define Endpoints for HTTP REST API -*/ - - -void setupRouting() { - // GET - // server.on("/temperature", getTemperature); - //server.on("/env", getEnv); - // https://www.survivingwithandroid.com/esp32-rest-api-esp32-api-server/ - Serial.println("Setting up HTTP Routing"); - server.on(state_act_endpoint, HTTP_POST, state_act_fct_http); - server.on(state_get_endpoint, HTTP_POST, state_get_fct_http); - server.on(state_set_endpoint, HTTP_POST, state_set_fct_http); - - server.on("/identity", getIdentity); - - server.on("/ota", HTTP_GET, []() { - server.sendHeader("Connection", "close"); - server.send(200, "text/html", otaindex); - }); - /*handling uploading firmware file */ - server.on("/update", HTTP_POST, []() { - server.sendHeader("Connection", "close"); - server.send(200, "text/plain", (Update.hasError()) ? "FAIL" : "OK"); - ESP.restart(); - }, []() { - HTTPUpload& upload = server.upload(); - if (upload.status == UPLOAD_FILE_START) { - Serial.printf("Update: %s\n", upload.filename.c_str()); - if (!Update.begin(UPDATE_SIZE_UNKNOWN)) { //start with max available size - Update.printError(Serial); - } - } else if (upload.status == UPLOAD_FILE_WRITE) { - /* flashing firmware to ESP*/ - if (Update.write(upload.buf, upload.currentSize) != upload.currentSize) { - Update.printError(Serial); - } - } else if (upload.status == UPLOAD_FILE_END) { - if (Update.end(true)) { //true to set the size to the current progress - Serial.printf("Update Success: %u\nRebooting...\n", upload.totalSize); - } else { - Update.printError(Serial); - } - } - }); - - - // Website - server.on("/openapi.yaml", handleSwaggerYaml); - server.on("/index.html", handleSwaggerUI); - server.on("/swagger_standalone.js", handlestandalone); - server.on("/swagger-ui-bundle.js", handleswaggerbundle); - server.on("/swagger-ui.css", handleswaggercss); - - - /* - * server.on("/backbone-min.js", handlebackbone); - server.on("/handlebars.min.js", handlehandlebars); - server.on("/highlight.min.js", handlehighlight); - server.on("/images/throbber.gif", handlethrobber); - server.on("/jquery-1.8.0.min.js", handlejquery); - server.on("/jquery.ba-bbq.min.js", handlejquerybbq); - server.on("/json.min.js", handlejson); - server.on("/jsoneditor.min.js", handlejsoneditor); - server.on("/lodash.min.js", handlelodash); - server.on("/logo_small.png", handlelogo_small); - server.on("/marked.min.js", handlemarked); - server.on("/reset.min.css", handlereset); - server.on("/screen.css", handlescreen); - server.on("/swagger-ui.min.js", handleswaggerui); - */ - - - // POST - server.on(motor_act_endpoint, HTTP_POST, motor_act_fct_http); - server.on(motor_get_endpoint, HTTP_POST, motor_get_fct_http); - server.on(motor_set_endpoint, HTTP_POST, motor_set_fct_http); - -#ifdef IS_DAC - server.on(dac_act_endpoint, HTTP_POST, dac_act_fct_http); - server.on(dac_get_endpoint, HTTP_POST, dac_get_fct_http); - server.on(dac_set_endpoint, HTTP_POST, dac_set_fct_http); -#endif - - server.on(laser_act_endpoint, HTTP_POST, LASER_act_fct_http); - server.on(laser_get_endpoint, HTTP_POST, LASER_get_fct_http); - server.on(laser_set_endpoint, HTTP_POST, LASER_set_fct_http); - -#ifdef IS_ANALOG - server.on(analog_act_endpoint, HTTP_POST, analog_act_fct_http); - server.on(analog_get_endpoint, HTTP_POST, analog_get_fct_http); - server.on(analog_set_endpoint, HTTP_POST, analog_set_fct_http); -#endif - -#ifdef IS_DIGITAL - server.on(digital_act_endpoint, HTTP_POST, digital_act_fct_http); - server.on(digital_get_endpoint, HTTP_POST, digital_get_fct_http); - server.on(digital_set_endpoint, HTTP_POST, digital_set_fct_http); -#endif - -server.on(ledarr_act_endpoint, HTTP_POST, ledarr_act_fct_http); -server.on(ledarr_get_endpoint, HTTP_POST, ledarr_get_fct_http); -server.on(ledarr_set_endpoint, HTTP_POST, ledarr_set_fct_http); - -server.on(config_act_endpoint, HTTP_POST, config_act_fct_http); -server.on(config_get_endpoint, HTTP_POST, config_get_fct_http); -server.on(config_set_endpoint, HTTP_POST, config_set_fct_http); - -} - -void startServer() { - Serial.println("Starting Server"); - server.begin(); -} diff --git a/ESP32/main/docs.cpp b/ESP32/main/docs.cpp deleted file mode 100644 index f3cb24f..0000000 --- a/ESP32/main/docs.cpp +++ /dev/null @@ -1,143 +0,0 @@ -/* - - Serial protocol looks like so: - - {"task": "/state_get"} - returns: - - ++ - {"identifier_name":"UC2_Feather","identifier_id":"V0.1","identifier_date":"2022-02-04","identifier_author":"BD"} - -- - - {"task": "/state_set", "isdebug":0} - {"task": "/state_get", "active":1} - - - retrieve sensor value - {"task": "/readsensor_act", "readsensorID":0, "N_sensor_avg":100} - {"task": "/readsensor_get", "readsensorID":0} - {"task": "/readsensor_set", "readsensorID":0, "readsensorPIN":34, "N_sensor_avg":10} - - setup PID controller - {"task": "/PID_act", "PIDactive":1, "target": 500} - {"task": "/PID_act", "PIDactive":1, "Kp":5, "Ki":.1, "Kd":.1, "target": 500, "PID_updaterate":200} - {"task": "/readsensor_get", "readsensorID":0} - {"task": "/readsensor_set", "readsensorID":0, "readsensorPIN":34, "N_sensor_avg":10} - - - - turn on the laser: - {"task": "/laser_act", "LASERid":1, "LASERval":10000, "LASERdespeckle":100} - - move the motor - {"task": "/motor_act", "speed":1000, "pos1":4000, "pos2":4000, "pos3":4000, "isabs":1, "isen":1} - {"task": "/motor_act", "speed":1000, "pos1":4000, "pos2":4000, "pos3":4000, "isabs":1, "isen":1} // move in the background - {"task": "/motor_act", "speed1":1000,"speed2":100,"speed3":5000, "pos1":4000, "pos2":4000, "pos3":4000, "isabs":1, "isen":1} - {"task": "/motor_act", "isstop":1} - {'task': '/motor_set', 'axis': 1, 'currentposition': 1} - {'task': '/motor_set', 'axis': 1, 'sign': 1} // 1 or -1 - {'task': '/motor_set', 'axis': 1, 'sign': 1} // 1 or -1 - {'task': '/motor_get', 'axis': 1} - {"task": "/motor_act", "speed":30, "pos1":400, "pos2":0, "pos3":0, "isabs":0, "isblock":0, "isen":1} - - - operate the analog out - {"task": "/analog_act", "analogid": 1, "analogval":1000} - - operate the digital out - {"task": "/digital_act", "digitalid": 1, "digitalval":1} - - operate the dac (e.g. lightsheet) - {"task": "/dac_act", "dac_channel": 1, "frequency":1, "offset":0, "amplitude":0, "clk_div": 1000} - amplitude: 0,1,2,3 - - # SLM display - {"task": "/slm_act","slmMode": "ring", "posX":100, "posY": 100, "radius":100, "color": 10000} - {"task": "/slm_act","slmMode": "full", "color": 10000} - {"task": "/slm_act","slmMode": "clear"} - - operate ledmatrix - // "pattern", "individual", "full", "off", "left", "right", "top", "bottom", - {"task": "/ledarr_act","LEDArrMode": "full", "red":100, "green": 100, "blue":100} - {"task": "/ledarr_act","LEDArrMode": "full", "red":0, "green": 0, "blue":0} - {'task': '/ledarr_act', 'red': 193, 'green': 193, 'blue': 193, 'NLeds':16, 'LEDArrMode': 'top'} - {'task': '/ledarr_act', 'red': 193, 'green': 193, 'blue': 193, 'NLeds':16, 'LEDArrMode': 'bottom'} - {'task': '/ledarr_act', 'red': 193, 'green': 193, 'blue': 193, 'NLeds':16, 'LEDArrMode': 'left'} - {'task': '/ledarr_act', 'red': 193, 'green': 193, 'blue': 193, 'NLeds':16, 'LEDArrMode': 'right'} - {'task': '/ledarr_act', 'red': 193, 'green': 193, 'blue': 193, 'LEDArrMode': 'full'} - - - - {'task': '/scanner_act', 'scannernFrames': 1, 'scannerMode': 'pattern', 'arraySize': 9, 'i': [0, 16, 32, 48, 64, 80, 96, 112, 128], 'scannerLaserVal': 32000, 'scannerExposure': 500, 'scannerDelay': 500} - - { "red": [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 244, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 244, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ], "green": [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 244, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 244, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ], "blue": [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 244, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 244, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ], "arraySize": 64, "LEDArrMode": "array", "task": "/ledarr_act" } - - // attempt to have fast triggering - - We want to send a table of tasks: - - move motor, wait, take picture cam 1/2, wait, move motor.. - { - "task": "multitable", - "task_n": 9, - "repeats_n": 5, - "0": {"task": "/motor_act", "speed":1000, "pos1":4000, "pos2":4000, "pos3":4000, "isabs":1, "isblock":1, "isen":1}, - "1": {"task": "/state_act", "delay": 100}, - "2": {"task": "/digital_act", "digitalid": 1, "digitalval":1}, - "3": {"task": "/digital_act", "digitalid": 2, "digitalval":1}, - "4": {"task": "/digital_act", "digitalid": 2, "digitalval":0}, - "5": {"task": "/digital_act", "digitalid": 1, "digitalval":0}, - "6": {"task": "/laser_act", "LASERid":1, "LASERval":10000, "LASERdespeckle":100}, - "7": {"task": "/state_act", "delay": 100}, - "8": {"task": "/laser_act", "LASERid":1, "LASERval":10000, "LASERdespeckle":100} - } - - - - // trigger camera at a rate of 20hz - - {"task": "/motor_act", "speed0":0, "speed1":0,"speed2":40,"speed3":9000, "isforever":1, "isaccel":1} - {"task": "/state_set", "isdebug":0} - {"task": "/state_act", "delay": 100} - { - "task": "multitable", - "task_n": 2, - "repeats_n": 200, - "0": {"task": "/digital_act", "digitalid": 1, "digitalval":-1}, - "1": {"task": "/state_act", "delay": 50} - } - {"task": "/motor_act", "isstop":1} - {"task": "/motor_act", "isenable":0} - {"task": "/s-tate_set", "isdebug":1} - - - -// load config - { - "task":"/config_set", - "motXstp": 2, - "motXdir": 31, - "motYstp": 27, - "motYdir": 16, - "motZstp": 12, - "motZdir": 14, - "motAstp": 22, - "motAdir": 21, - "motEnable": 13, - "ledArrPin": 17, - "ledArrNum": 16, - "digitalPin1":10, - "digitalPin2":11, - "analogPin1":0, - "analogPin2":0, - "analogPin3":0, - "laserPin1":4, - "laserPin2":15, - "laserPin3":0, - "dacFake1":0, - "dacFake2":0, - "identifier": "UC2Standalone", - "ssid": "Blynk", - "PW": "12345678" - } -*/ diff --git a/ESP32/main/main.ino b/ESP32/main/main.ino deleted file mode 100644 index 3b47def..0000000 --- a/ESP32/main/main.ino +++ /dev/null @@ -1,572 +0,0 @@ -//#define IS_PS3 -#define IS_PS3 - -// external headers -#include "soc/soc.h" -#include "soc/rtc_cntl_reg.h" -#ifdef IS_PS3 -#include -#else -#include -#endif -#include -#include "esp_bt_main.h" -#include "esp_bt_device.h" -#include"esp_gap_bt_api.h" -#include "esp_err.h" -#include "Preferences.h" -#include -#include - - -// internal headers -#include "parameters_state.h" -#include "parameters_laser.h" -#include "parameters_motor.h" -#include "parameters_ledarr.h" -#include "pindef.h" // for pin definitions -//#include "pindefUC2Standalone.h" -#include "parameters_ps.h" // playstation parameters -#include "parameters_config.h" - -// We use the strip instead of the matrix to ensure different dimensions; Convesion of the pattern has to be done on the cliet side! -Adafruit_NeoPixel* matrix = NULL; - - - - - -// define permanent flash object -Preferences preferences; - -#if defined(IS_DAC) || defined(IS_DAC_FAKE) -#include "parameters_dac.h" -#endif - -int DEBUG = 1; // if tihs is set to true, the arduino runs into problems during multiple serial prints.. -#define BAUDRATE 115200 - -#include "parameters_wifi.h" -WiFiManager wm; - -#ifdef IS_ANALOG -#include "parameters_analog.h" -#endif -#ifdef IS_DIGITAL -#include "parameters_digital.h" -#endif - -#ifdef IS_READSENSOR -//#include "parameters_readsensor.h" -#endif - -#ifdef IS_PID -#include "parameters_PID.h" -#endif - -DynamicJsonDocument jsonDocument(32784); - -WebServer server(80); -char output[1000]; - -#ifdef IS_DAC -DAC_Module *dac = new DAC_Module(); -#endif - -/* - Register devices -*/ -AccelStepper stepper_A; -AccelStepper stepper_X; -AccelStepper stepper_Y; -AccelStepper stepper_Z; - -#ifdef IS_SLM -#include "parameters_slm.h" -#endif - -/* -------------------------------------------- - Setup - -------------------------------------------- -*/ -void setup() -{ - /* - SETTING UP DEVICES - */ - - - - WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); //disable brownout detector - - // for any timing related puposes.. - startMillis = millis(); - - // Start Serial - Serial.begin(BAUDRATE); - Serial.println("Start"); - - - // if we boot for the first time => reset the preferences! // TODO: Smart? If not, we may have the problem that a wrong pin will block bootup - if (isFirstRun()) { - Serial.println("First Run, resetting config?"); - resetConfigurations(); - } - - // check if setup went through after new config - avoid endless boot-loop - preferences.begin("setup", false); - if (preferences.getBool("setupComplete", true) == false) { - - // make sure next time no bootloop is created - preferences.putBool("setupComplete", false); - preferences.end(); - - Serial.println("Setup not done, resetting config?"); //TODO not working! - resetConfigurations(); - - } - else { - Serial.println("Setup done, continue."); - } - preferences.putBool("setupComplete", false); - preferences.end(); - - // load config - loadConfiguration(); - - // display state - printInfo(); - - // reset jsonDocument - jsonDocument.clear(); - - // connect to wifi if necessary - bool isResetWifiSettings = false; - //autoconnectWifi(isResetWifiSettings); - initWifiAP(WifiSSIDAP); - setupRouting(); - startServer(); - init_Spiffs(); - - Serial.println(state_act_endpoint); - Serial.println(state_get_endpoint); - Serial.println(state_set_endpoint); - -#ifdef IS_SLM - setup_slm(); -#endif - - setup_matrix(); - setup_motor(); - setup_laser(); - - /* - setting up playstation controller - */ - if(isFirstRun()) { - // - clearBlueetoothDevice(); - ESP.restart(); - } - -#ifdef IS_PS3 - Serial.println("Connnecting to the PS3 controller, please please the magic round button in the center.."); - Ps3.attach(onAttachPS3); - Ps3.attachOnConnect(onConnectPS3); - Ps3.attachOnDisconnect(onDisConnectPS3); - const char* PS3_MACADDESS = "1a:2b:3c:01:01:01"; - Serial.println("PS3 controler is set up."); - Ps3.begin("1a:2b:3c:01:01:01"); - Serial.println(PS3_MACADDESS); -#else - //String address = Ps3.getAddress(); // have arbitrary address? - //Serial.println(address); - Serial.println("PS4 controler is set up."); - Serial.println("Connnecting to the PS4 controller, please please the magic round button in the center.."); - const char* PS4_MACADDESS = "1a:2b:3c:01:01:01"; // TODO not working with adaptive address - Serial.println(PS4Mac); - Serial.println(PS4_MACADDESS); - PS4.begin(PS4_MACADDESS); - PS4.attach(onAttachPS4); - PS4.attachOnConnect(onConnectPS4); - PS4.attachOnDisconnect(onDisConnectPS4); - Serial.print("PS4 controler is set up. - UNICAST!"); -#endif - - - -#ifdef IS_DAC - Serial.println("Setting Up DAC"); - //Setup(dac_channel, clk_div, frequency, scale, phase, invert); - dac->Setup(DAC_CHANNEL_1, 1000, 50, 0, 0, 2); - dac->Setup(DAC_CHANNEL_2, 1000, 50, 0, 0, 2); -#endif - - -#ifdef IS_ANALOG - Serial.println("Setting Up analog"); - /* setup the PWM ports and reset them to 0*/ - ledcSetup(PWM_CHANNEL_analog_1, pwm_frequency, pwm_resolution); - ledcAttachPin(ANALOG_PIN_1, PWM_CHANNEL_analog_1); - ledcWrite(PWM_CHANNEL_analog_1, 0); - - ledcSetup(PWM_CHANNEL_analog_2, pwm_frequency, pwm_resolution); - ledcAttachPin(ANALOG_PIN_2, PWM_CHANNEL_analog_2); - ledcWrite(PWM_CHANNEL_analog_2, 0); -#endif - -#ifdef IS_DIGITAL - setupDigital(); -#endif - - // list modules -#ifdef IS_SLM - Serial.println("IS_SLM"); -#endif - -#ifdef IS_DAC - Serial.println(dac_act_endpoint); - Serial.println(dac_get_endpoint); - Serial.println(dac_set_endpoint); -#endif - Serial.println(motor_act_endpoint); - Serial.println(motor_get_endpoint); - Serial.println(motor_set_endpoint); - - Serial.println(laser_act_endpoint); - Serial.println(laser_get_endpoint); - Serial.println(laser_set_endpoint); -#ifdef IS_ANALOG - Serial.println(analog_act_endpoint); - Serial.println(analog_get_endpoint); - Serial.println(analog_set_endpoint); -#endif -#ifdef IS_SLM - Serial.println(slm_act_endpoint); - Serial.println(slm_get_endpoint); - Serial.println(slm_set_endpoint); -#endif -#ifdef IS_DIGITALGOUT - Serial.println(digital_act_endpoint); - Serial.println(digital_get_endpoint); - Serial.println(digital_set_endpoint); -#endif - - Serial.println(ledarr_act_endpoint); - Serial.println(ledarr_get_endpoint); - Serial.println(ledarr_set_endpoint); - - Serial.println(config_act_endpoint); - Serial.println(config_get_endpoint); - Serial.println(config_set_endpoint); - -#ifdef IS_DAC_FAKE - pinMode(DAC_FAKE_PIN_1, OUTPUT); - pinMode(DAC_FAKE_PIN_2, OUTPUT); - frequency = 1; - xTaskCreate( - drive_galvo, // Function that should be called - "drive_galvo", // Name of the task (for debugging) - 1000, // Stack size (bytes) - NULL, // Parameter to pass - 1, // Task priority - NULL // Task handle - ); -#endif - - -#ifdef IS_READSENSOR - setup_sensors(); - Serial.println(readsensor_act_endpoint); - Serial.println(readsensor_set_endpoint); - Serial.println(readsensor_get_endpoint); -#endif - -#ifdef IS_PID - setup_PID(); - Serial.println(PID_act_endpoint); - Serial.println(PID_set_endpoint); - Serial.println(PID_get_endpoint); -#endif - - - // check if setup went through after new config - preferences.begin("setup", false); - preferences.putBool("setupComplete", true); - preferences.end(); -} - -//char *task = strdup(""); -//char* task = ""; - -void loop() { - // handle any http requests - server.handleClient(); - - // for any timing-related purposes - currentMillis = millis(); - - // process incoming serial commands - if (Serial.available()) { - DeserializationError error = deserializeJson(jsonDocument, Serial); - //free(Serial); - if (error) { - Serial.print(F("deserializeJson() failed: ")); - Serial.println(error.f_str()); - return; - } - Serial.flush(); - if (DEBUG) serializeJsonPretty(jsonDocument, Serial); - - - String task_s = jsonDocument["task"]; - char task[50]; - task_s.toCharArray(task, 256); - - - //jsonDocument.garbageCollect(); // memory leak? - /*if (task == "null") return;*/ - if (DEBUG) { - Serial.print("TASK: "); - Serial.println(task); - } - - // do the processing based on the incoming stream - if (strcmp(task, "multitable") == 0) { - // multiple tasks - tableProcessor(); - } - else { - // Process individual tasks - jsonProcessor(task); - } - - } - - /* - continous control during loop - */ - // attempting to despeckle by wiggeling the temperature-dependent modes of the laser? - if (LASER_despeckle_1 > 0 and LASER_val_1 > 0) - LASER_despeckle(LASER_despeckle_1, 1, LASER_despeckle_period_1); - if (LASER_despeckle_2 > 0 and LASER_val_2 > 0) - LASER_despeckle(LASER_despeckle_2, 2, LASER_despeckle_period_2); - if (LASER_despeckle_3 > 0 and LASER_val_3 > 0) - LASER_despeckle(LASER_despeckle_3, 3, LASER_despeckle_period_3); - - -#ifdef IS_PS3 - control_PS3(); // if controller is operating motors, overheating protection is enabled -#else - control_PS4(); -#endif - - /* - continous control during loop - */ - if (not isstop) { - isactive = true; - drive_motor_background(); - } - - -#ifdef IS_PID - if (PID_active and (currentMillis - startMillis >= PID_updaterate)) { - PID_background(); - startMillis = millis(); - } -#endif -} - - -void jsonProcessor(char task[]) { - /* - Return state - */ - if (strcmp(task, state_act_endpoint) == 0) - state_act_fct(); - if (strcmp(task, state_set_endpoint) == 0) - state_set_fct(); - if (strcmp(task, state_get_endpoint) == 0) - state_get_fct(); - - /* - Drive Motors - */ - if (strcmp(task, motor_act_endpoint) == 0) { - motor_act_fct(); - } - if (strcmp(task, motor_set_endpoint) == 0) { - motor_set_fct(); - } - if (strcmp(task, motor_get_endpoint) == 0) { - motor_get_fct(); - } - - - /* - Operate SLM - */ -#ifdef IS_SLM - if (strcmp(task, slm_act_endpoint) == 0) { - slm_act_fct(); - } - if (strcmp(task, slm_set_endpoint) == 0) { - slm_set_fct(); - } - if (strcmp(task, slm_get_endpoint) == 0) { - slm_get_fct(); - } -#endif - - /* - Drive DAC - */ -#ifdef IS_DAC - if (strcmp(task, dac_act_endpoint) == 0) - dac_act_fct(); - if (strcmp(task, dac_set_endpoint) == 0) - dac_set_fct(); - if (strcmp(task, dac_get_endpoint) == 0) - dac_get_fct(); -#endif - - /* - Drive Laser - */ - if (strcmp(task, laser_act_endpoint) == 0) - LASER_act_fct(); - if (strcmp(task, laser_set_endpoint) == 0) - LASER_get_fct(); - if (strcmp(task, laser_get_endpoint) == 0) - LASER_set_fct(); - - /* - Drive analog - */ -#ifdef IS_ANALOG - if (strcmp(task, analog_act_endpoint) == 0) - analog_act_fct(); - if (strcmp(task, analog_set_endpoint) == 0) - analog_set_fct(); - if (strcmp(task, analog_get_endpoint) == 0) - analog_get_fct(); -#endif - - - /* - Drive digital - */ -#ifdef IS_DIGITAL - if (strcmp(task, digital_act_endpoint) == 0) - digital_act_fct(); - if (strcmp(task, digital_set_endpoint) == 0) - digital_set_fct(); - if (strcmp(task, digital_get_endpoint) == 0) - digital_get_fct(); -#endif - - - /* - Drive LED Matrix - */ - - if (strcmp(task, ledarr_act_endpoint) == 0) - ledarr_act_fct(); - if (strcmp(task, ledarr_set_endpoint) == 0) - ledarr_set_fct(); - if (strcmp(task, ledarr_get_endpoint) == 0) - ledarr_get_fct(); - - /* - Change Configuration - */ - if (strcmp(task, config_act_endpoint) == 0) - config_act_fct(); - if (strcmp(task, config_set_endpoint) == 0) - config_set_fct(); - if (strcmp(task, config_get_endpoint) == 0) - config_get_fct(); - - /* - Read the sensor - */ -#ifdef IS_READSENSOR - if (strcmp(task, readsensor_act_endpoint) == 0) - readsensor_act_fct(); - if (strcmp(task, readsensor_set_endpoint) == 0) - readsensor_set_fct(); - if (strcmp(task, readsensor_get_endpoint) == 0) - readsensor_get_fct(); -#endif - - /* - Control PID controller - */ -#ifdef IS_PID - if (strcmp(task, PID_act_endpoint) == 0) - PID_act_fct(); - if (strcmp(task, PID_set_endpoint) == 0) - PID_set_fct(); - if (strcmp(task, PID_get_endpoint) == 0) - PID_get_fct(); -#endif - - - // Send JSON information back - Serial.println("++"); - serializeJson(jsonDocument, Serial); - Serial.println(); - Serial.println("--"); - jsonDocument.clear(); - jsonDocument.garbageCollect(); - - if (DEBUG) Serial.println("JSON processed"); -} - - -void tableProcessor() { - - isactive = true; - // 1. Copy the table - DynamicJsonDocument tmpJsonDoc = jsonDocument; - jsonDocument.clear(); - - // 2. now we need to extract the indidvidual tasks - int N_tasks = tmpJsonDoc["task_n"]; - int N_repeats = tmpJsonDoc["repeats_n"]; - - Serial.println("N_tasks"); - Serial.println(N_tasks); - Serial.println("N_repeats"); - Serial.println(N_repeats); - - - for (int irepeats = 0; irepeats < N_repeats; irepeats++) { - for (int itask = 0; itask < N_tasks; itask++) { - char json_string[256]; - // Hacky, but should work - Serial.println(itask); - serializeJson(tmpJsonDoc[String(itask)], json_string); - Serial.println(json_string); - deserializeJson(jsonDocument, json_string); - - String task_s = jsonDocument["task"]; - char task[50]; - task_s.toCharArray(task, 256); - - //jsonDocument.garbageCollect(); // memory leak? - /*if (task == "null") return;*/ - if (DEBUG) { - Serial.print("TASK: "); - Serial.println(task); - } - - jsonProcessor(task); - - } - } - tmpJsonDoc.clear(); - - isactive = false; -} diff --git a/ESP32/main/parameters_PID.h b/ESP32/main/parameters_PID.h deleted file mode 100644 index 45bb966..0000000 --- a/ESP32/main/parameters_PID.h +++ /dev/null @@ -1,9 +0,0 @@ -float errorRunSum=0; -float previousError=0; -float stepperMaxValue=2500.; -float PID_Kp = 10; -float PID_Ki = 0.1; -float PID_Kd = 0.1; -float PID_target = 500; -float PID_updaterate = 200; // ms -bool PID_active=false; diff --git a/ESP32/main/parameters_analog.h b/ESP32/main/parameters_analog.h deleted file mode 100644 index c42e927..0000000 --- a/ESP32/main/parameters_analog.h +++ /dev/null @@ -1,30 +0,0 @@ - -#include "esp_err.h" -#include "esp_log.h" -#include "driver/ledc.h" -#include "driver/periph_ctrl.h" -#include "soc/ledc_reg.h" - -#define J1772_LEDC_TIMER LEDC_TIMER_0 -#define J1772_LEDC_CHANNEL LEDC_CHANNEL_0 -#define J1772_LEDC_TIMER_RES LEDC_TIMER_9_BIT -#define J1772_DUTY_MAX ((1 << LEDC_TIMER_9_BIT) -1 ) -#define J1772_PWM_FREQUENCY_HZ 1000 -#define J1772_LEDC_SPEEDMODE LEDC_HIGH_SPEED_MODE - -// PWM Stuff - ESP only -int pwm_resolution = 15; -int pwm_frequency = 80000;//19000; //12000 -int pwm_max = (int)pow(2, pwm_resolution); - -int analog_val_1 = 0; -int analog_val_2 = 0; -int analog_val_3 = 0; -int analog_VIBRATE = 0; - -int analog_SOFI_1 = 0; -int analog_SOFI_2 = 0; - -int PWM_CHANNEL_analog_1 = 4; -int PWM_CHANNEL_analog_2 = 5; -int PWM_CHANNEL_analog_3 = 6; diff --git a/ESP32/main/parameters_config.h b/ESP32/main/parameters_config.h deleted file mode 100644 index 024b4fe..0000000 --- a/ESP32/main/parameters_config.h +++ /dev/null @@ -1,181 +0,0 @@ -/* -sample config file for ESP WEMOS D1 E32 + CNC Shield -{ - "motXstp": 26, - "motXdir": 16, - "motYstp": 25, - "motYdir": 27, - "motZstp": 17, - "motZdir": 14, - "motAstp": 0, - "motAdir": 0, - "motEnable": 12, - "ledArrPin": 4, - "ledArrNum": 64, - "digitalPin1": 0, - "digitalPin2": 0, - "analogPin1": 0, - "analogPin2": 0, - "analogPin3": 0, - "laserPin1": 19, - "laserPin2": 18, - "laserPin3": 0, - "dacFake1": 0, - "dacFake2": 0, - "identifier": "", - "ssid": "", - "PW": "" -} - -sample config file for Uc2 Standalone - -led -> LIMX -laser1 -> LIMY -laser2 -> LIMZ - { - "task":"/config_set", - "motXstp": 2, - "motXdir": 33, - "motYstp": 27, - "motYdir": 16, - "motZstp": 12, - "motZdir": 14, - "motAstp": 22, - "motAdir": 21, - "motEnable": 13, - "ledArrPin": 17, - "ledArrNum": 16, - "digitalPin1":10, - "digitalPin2":11, - "analogPin1":0, - "analogPin2":0, - "analogPin3":0, - "laserPin1":4, - "laserPin2":15, - "laserPin3":0, - "dacFake1":0, - "dacFake2":0, - "identifier": "UC2Standalone", - "ssid": "Blynk", - "PW": "12345678", - "PS4Mac": "1a:2b:3c:01:01:01" - } - {"task":"/config_act", "applyConfig":1} - {"task":"/config_get"} - - - -sample config file for Uc2 Standalone 1,1 - - { - "task":"/config_set", - "motXstp": 2, - "motXdir": 33, - "motYstp": 27, - "motYdir": 16, - "motZstp": 12, - "motZdir": 14, - "motAstp": 22, - "motAdir": 21, - "motXLim": -1, - "motYLim": 34, - "motZLim": 35, - "motEnable": 13, - "ledArrPin": 32, - "ledArrNum": 16, - "digitalPin1":0, - "digitalPin2":0, - "analogPin1":0, - "analogPin2":0, - "analogPin3":0, - "laserPin1":17, - "laserPin2":4, - "laserPin3":15, - "dacFake1":0, - "dacFake2":0, - "identifier": "UC2Standalone",{"task": "/motor_act", "motor": {"steppers": [{"stepperid": 3, "isforever": true, "speed": 200}]}} - "ssid": "Blynk", - "PW": "12345678", - "PS4Mac": "1a:2b:3c:01:01:01" - } - {"task":"/config_act", "applyConfig":1} - {"task":"/config_get"} - - -// for cellstorm -led -> LIMX -lens1 -> LIMY -lens2 -> LIMZ -laser -> xlim - - { - "task":"/config_set", - "motXstp": 2, - "motXdir": 33, - "motYstp": 27, - "motYdir": 16, - "motZstp": 12, - "motZdir": 14, - "motAstp": 22, - "motAdir": 21, - "motEnable": 13, - "ledArrPin": 0, - "ledArrNum": 16, - "digitalPin1":10, - "digitalPin2":11, - "analogPin1":0, - "analogPin2":0, - "analogPin3":0, - "laserPin1":4, - "laserPin2":15, - "laserPin3":17, - "dacFake1":0, - "dacFake2":0, - "identifier": "UC2Standalone", - "ssid": "Blynk", - "PW": "12345678", - "PS4Mac": "1a:2b:3c:01:01:01" - } - {"task":"/config_act", "applyConfig":1} - {"task":"/config_get"} - - - -*/ - -const char* prefNamespace = "UC2"; - -const char* keyMotXStepPin = "motXstp"; -const char* keyMotXDirPin = "motXdir"; -const char* keyMotYStepPin = "motYstp"; -const char* keyMotYDirPin = "motYdir"; -const char* keyMotZStepPin = "motZstp"; -const char* keyMotZDirPin = "motZdir"; -const char* keyMotAStepPin = "motAstp"; -const char* keyMotADirPin = "motAdir"; -const char* keyMotEnable = "motEnable"; - -const char* keyLEDArray = "ledArrPin"; -const char* keyLEDNumLEDArray = "ledArrNum"; - -const char* keyDigital1Pin = "digitalPin1"; -const char* keyDigital2Pin = "digitalPin2"; - -const char* keyAnalog1Pin = "analogPin1"; -const char* keyAnalog2Pin = "analogPin2"; -const char* keyAnalog3Pin = "analogPin3"; - -const char* keyLaser1Pin = "laserPin1"; -const char* keyLaser2Pin = "laserPin2"; -const char* keyLaser3Pin = "laserPin3"; - -const char* keyDACfake1Pin = "dacFake1"; -const char* keyDACfake2Pin = "dacFake2"; - -const char* keyIdentifier = "identifier"; - -const char* keyWifiSSID = "ssid"; -const char* keyWifiPW = "PW"; - -const char* keyPS3Mac = "PS3Mac"; -const char* keyPS4Mac = "PS4Mac"; diff --git a/ESP32/main/parameters_dac.h b/ESP32/main/parameters_dac.h deleted file mode 100644 index f4c2c3b..0000000 --- a/ESP32/main/parameters_dac.h +++ /dev/null @@ -1,12 +0,0 @@ -// DAC-specific parameters -#include "DAC_Module.h" - -dac_channel_t dac_channel = DAC_CHANNEL_1; - -uint32_t clk_div = 0; -uint32_t scale = 0; -uint32_t invert = 2; -uint32_t phase = 0; -uint32_t frequency = 1000; - -boolean dac_is_running = false; diff --git a/ESP32/main/parameters_digital.h b/ESP32/main/parameters_digital.h deleted file mode 100644 index b55182f..0000000 --- a/ESP32/main/parameters_digital.h +++ /dev/null @@ -1,3 +0,0 @@ -int digital_val_1 = 0; -int digital_val_2 = 0; -int digital_val_3 = 0; diff --git a/ESP32/main/parameters_laser.h b/ESP32/main/parameters_laser.h deleted file mode 100644 index fe5e69d..0000000 --- a/ESP32/main/parameters_laser.h +++ /dev/null @@ -1,29 +0,0 @@ -int LASER_val_1 = 0; -int LASER_val_2 = 0; -int LASER_val_3 = 0; - -// PWM Stuff - ESP only - -/* -int pwm_resolution = 15; -int pwm_frequency = 800000; //19000; // 12000; -int pwm_max = (int)pow(2,pwm_resolution); -*/ - -int pwm_resolution = 10; //8bit 256, 10bit 1024, 12bit 4096; -int pwm_frequency = 5000;//19000; //12000 -long pwm_max = (int)pow(2, pwm_resolution); - - -int PWM_CHANNEL_LASER_1 = 0; -int PWM_CHANNEL_LASER_2 = 1; -int PWM_CHANNEL_LASER_3 = 2; - -// temperature dependent despeckeling? -int LASER_despeckle_1 = 0; -int LASER_despeckle_2 = 0; -int LASER_despeckle_3 = 0; - -int LASER_despeckle_period_1 = 20; -int LASER_despeckle_period_2 = 20; -int LASER_despeckle_period_3 = 20; diff --git a/ESP32/main/parameters_ledarr.h b/ESP32/main/parameters_ledarr.h deleted file mode 100644 index 68ebc5d..0000000 --- a/ESP32/main/parameters_ledarr.h +++ /dev/null @@ -1,34 +0,0 @@ -#include -#include - - -int NLED4x4=16; -int NLED8x8=64; - -int LED_PATTERN_DPC_TOP_8x8 [] = {1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1, - 0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0}; - -int LED_PATTERN_DPC_LEFT_8x8 []= {1,1,1,1,0,0,0,0, - 0,0,0,0,1,1,1,1, - 1,1,1,1,0,0,0,0, - 0,0,0,0,1,1,1,1, - 1,1,1,1,0,0,0,0, - 0,0,0,0,1,1,1,1, - 1,1,1,1,0,0,0,0, - 0,0,0,0,1,1,1,1}; - -int LED_PATTERN_DPC_TOP_4x4 []= {1,1,1,1, - 1,1,1,1, - 0,0,0,0, - 0,0,0,0}; - -int LED_PATTERN_DPC_LEFT_4x4 []= {1,1,0,0, - 0,0,1,1, - 1,1,0,0, - 0,0,1,1}; diff --git a/ESP32/main/parameters_motor.h b/ESP32/main/parameters_motor.h deleted file mode 100644 index 31dd023..0000000 --- a/ESP32/main/parameters_motor.h +++ /dev/null @@ -1,67 +0,0 @@ -#include -#include "esp_task_wdt.h" - -// for stepper.h -#define MOTOR_STEPS 200 -#define SLEEP 0 -#define MS1 0 -#define MS2 0 -#define MS3 0 -#define RPM 120 - -bool isaccel = false; -bool isforever = false; -bool motor_enable = false; - -// global variables for the motor -long mspeed0 = 1000; -long mspeed1 = 1000; -long mspeed2 = 1000; -long mspeed3 = 1000; -long mposition0 = 0; -long mposition1 = 0; -long mposition2 = 0; -long mposition3 = 0; -boolean isstop = 0; - -long POSITION_MOTOR_A = 0; -long POSITION_MOTOR_X = 0; -long POSITION_MOTOR_Y = 0; -long POSITION_MOTOR_Z = 0; - -int MOTOR_ACCEL = 5000; -int MOTOR_DECEL = 5000; - -int isabs = true; -int isen = false; -bool isactive = false; - -// Homing parameters -int ishomeX=0; -int ishomeY=0; -int ishomeZ=0; -int ishomeA=0; -int homePinA=0; -int homePinX=0; -int homePinY=0; -int homePinZ=0; - - -// direction -int SIGN_A = 1; -int SIGN_X = 1; -int SIGN_Y = 1; -int SIGN_Z = 1; -static const int FULLSTEPS_PER_REV_A = 200; -static const int FULLSTEPS_PER_REV_X = 200; -static const int FULLSTEPS_PER_REV_Y = 200; -static const int FULLSTEPS_PER_REV_Z = 200; - -long MAX_VELOCITY_A = 20000; -long MAX_VELOCITY_X = 20000; -long MAX_VELOCITY_Y = 20000; -long MAX_VELOCITY_Z = 20000; -long MAX_ACCELERATION_A = 100000; -long MAX_ACCELERATION_X = 100000; -long MAX_ACCELERATION_Y = 100000; -long MAX_ACCELERATION_Z = 100000; diff --git a/ESP32/main/parameters_ps.h b/ESP32/main/parameters_ps.h deleted file mode 100644 index d805c4a..0000000 --- a/ESP32/main/parameters_ps.h +++ /dev/null @@ -1,13 +0,0 @@ -int offset_val = 20; // make sure you do not accidentally turn on two directions at the same time -int stick_ly = 0; -int stick_lx = 0; -int stick_rx = 0; -int stick_ry = 0; - -int speed_x = 0; -int speed_y = 0; -int speed_z = 0; - -int IS_PSCONTROLER_ACTIVE = false; - -int global_speed = 2; // multiplier for the speed diff --git a/ESP32/main/parameters_scanner.h b/ESP32/main/parameters_scanner.h deleted file mode 100644 index 36e15e7..0000000 --- a/ESP32/main/parameters_scanner.h +++ /dev/null @@ -1,23 +0,0 @@ -bool isScanRunning = false; -int scannerPinX = 25; -int scannerPinY = 26; -int scannerPinLaser = 4; - -int scannerXFrameMax= 5; -int scannerXFrameMin= 0; -int scannerYFrameMax= 5; -int scannerYFrameMin= 0; -int scannerXStep = 5; -int scannerYStep = 5; - -int scannerxMin = 0; -int scanneryMin = 0; -int scannerxMax = 255; -int scanneryMax = 255; -int scannertDelay = 0; -int scannerEnable = 0; -int scannerExposure = 0; -int scannerLaserVal = 255; -int scannerDelay = 0; - -int scannernFrames = 1; diff --git a/ESP32/main/parameters_slm.h b/ESP32/main/parameters_slm.h deleted file mode 100644 index aa09d81..0000000 --- a/ESP32/main/parameters_slm.h +++ /dev/null @@ -1,38 +0,0 @@ -#include -#include // Hardware-specific library for ST7735 -#include - - -// Return the minimum of two values a and b -#define minimum(a,b) (((a) < (b)) ? (a) : (b)) - -/* -//default values -#define TFT_RST 4 -#define TFT_DC 2 //A0 -#define TFT_CS 15 //CS -#define TFT_MOSI 23 //SDA -#define TFT_CLK 18 //SCK - */ - - -/* -#define ST7735_DRIVER // Definieren Sie weitere Parameter für diese Anzeige -#define ST7735_REDTAB -#define TFT_MISO 19 -#define TFT_SCLK 18 -#define TFT_CS 15// Chip select control pin -#define TFT_DC 2 // Data Command control pin -#define TFT_RST 14 // Reset pin (could connect to RST pin) -#define LOAD_GLCD // Font 1. Original Adafruit 8 pixel font needs ~1820 bytes in FLASH -#define LOAD_FONT2 // Font 2. Small 16 pixel high font, needs ~3534 bytes in FLASH, 96 characters -#define LOAD_FONT4 // Font 4. Medium 26 pixel high font, needs ~5848 bytes in FLASH, 96 characters -#define LOAD_FONT6 // Font 6. Large 48 pixel font, needs ~2666 bytes in FLASH, only characters 1234567890:-.apm -#define LOAD_FONT7 // Font 7. 7 segment 48 pixel font, needs ~2438 bytes in FLASH, only characters 1234567890:-. -#define LOAD_FONT8 // Font 8. Large 75 pixel font needs ~3256 bytes in FLASH, only characters 1234567890:-. -#define LOAD_GFXFF // FreeFonts. Include access to the 48 Adafruit_GFX free fonts FF1 to FF48 and custom fonts -#define SMOOTH_FONT -#define SPI_FREQUENCY 27000000 // Actually sets it to 26.67MHz = 80/3 -#define SPI_READ_FREQUENCY 20000000 -#define SPI_TOUCH_FREQUENCY 2500000 -*/ diff --git a/ESP32/main/parameters_state.h b/ESP32/main/parameters_state.h deleted file mode 100644 index 388dacf..0000000 --- a/ESP32/main/parameters_state.h +++ /dev/null @@ -1,59 +0,0 @@ -const char* identifier_name = "UC2_Feather"; -const char* identifier_id = "V1.2"; -const char* identifier_date = __DATE__ "" __TIME__; -const char* identifier_author = "BD"; -String IDENTIFIER_NAME = ""; - -// timing variables -unsigned long startMillis; -unsigned long currentMillis; -bool isBusy = false; - - -/* -Endpoint definitions -*/ - -const char* state_act_endpoint = "/state_act"; -const char* state_set_endpoint = "/state_set"; -const char* state_get_endpoint = "/state_get"; - -const char* laser_act_endpoint = "/laser_act"; -const char* laser_set_endpoint = "/laser_set"; -const char* laser_get_endpoint = "/laser_get"; - -const char* motor_act_endpoint = "/motor_act"; -const char* motor_set_endpoint = "/motor_set"; -const char* motor_get_endpoint = "/motor_get"; - -const char* dac_act_endpoint = "/dac_act"; -const char* dac_set_endpoint = "/dac_set"; -const char* dac_get_endpoint = "/dac_get"; - -const char* analog_act_endpoint = "/analog_act"; -const char* analog_set_endpoint = "/analog_set"; -const char* analog_get_endpoint = "/analog_get"; - -const char* digital_act_endpoint = "/digital_act"; -const char* digital_set_endpoint = "/digital_set"; -const char* digital_get_endpoint = "/digital_get"; - -const char* ledarr_act_endpoint = "/ledarr_act"; -const char* ledarr_set_endpoint = "/ledarr_set"; -const char* ledarr_get_endpoint = "/ledarr_get"; - -const char* config_act_endpoint = "/config_act"; -const char* config_set_endpoint = "/config_set"; -const char* config_get_endpoint = "/config_get"; - -const char* slm_act_endpoint = "/slm_act"; -const char* slm_set_endpoint = "/slm_set"; -const char* slm_get_endpoint = "/slm_get"; - -const char* readsensor_act_endpoint = "/readsensor_act"; -const char* readsensor_set_endpoint = "/readsensor_set"; -const char* readsensor_get_endpoint = "/readsensor_get"; - -const char* PID_act_endpoint = "/PID_act"; -const char* PID_set_endpoint = "/PID_set"; -const char* PID_get_endpoint = "/PID_get"; diff --git a/ESP32/main/parameters_wifi.h b/ESP32/main/parameters_wifi.h deleted file mode 100644 index 2e9a4fc..0000000 --- a/ESP32/main/parameters_wifi.h +++ /dev/null @@ -1,352 +0,0 @@ -#include "WiFi.h" -#include "Update.h" -#include "WebServer.h" -#include // https://github.com/tzapu/WiFiManager - -// setting up wifi parameters -boolean hostWifiAP = false; // set this variable if you want the ESP32 to be the host -boolean isCaptivePortal = true; // want to autoconnect to wifi networks? - - -static const char PROGMEM otaindex[] = R"rawliteral( - - -

- -

-
-

OTA Updater for the UC2-REST

-

Please go to the Github repository of the UC2-REST and download the latest ".bin" file.  

-

-
-
Progress: 0%
-

- -

- -)rawliteral"; - - -static const char PROGMEM swagger_index[] = R"rawliteral( - - - - - - - - -
- - - - - -)rawliteral"; - - -static const char PROGMEM swagger_openapi[] = R"rawliteral( -openapi: 3.0.1 -info: - title: UC2 REST-API - description: This is a summary of controllable UC2 modules. - version: 1.0.0 -servers: - - url: / -tags: - - name: Temperature - description: Getting temperature measurements -paths: - /motor_act: - post: - tags: - - Motors - summary: Endpoint for controlling the motor - operationId: state_act_fct_http - description: Move the microscope in XYZ - requestBody: - description: Specify the place to move to (as a point) and the thing to move (combining motion target, mount, and model if necessary) - required: true - content: - application/json: - schema: - type: object - required: - - speed - - pos1 - - pos2 - - pos3 - - isabs - - isblock - - isen - properties: - speed: - type: number - pos1: - type: number - pos2: - type: number - pos3: - type: number - isabs: - type: number - isblock: - type: number - isen: - type: number - examples: - moveXYZ: - description: Move the microscope in xyz. - summary: Move in XYZ - value: - speed: 1000 - pos1: 1000 - pos2: 2000 - pos3: 3000 - isabs: 1 - isblock: 0 - isen: 0 - - responses: - '200': - description: Move successfully executed - content: - application/json: - example: - message: 'Move copmlete. New position: (25, 25, 50)' - '400': - description: Invalid request - content: - application/json: - example: - message: 'Invalid target key: ''robot'' (target must be one of ''mount'' or ''pipette'')' - /motor_set: - post: - tags: - - Motors - summary: Endpoint for chhanging Motor settings - operationId: state_set_fct_http - description: Chhange motor settings - requestBody: - description: Specify the place to move to (as a point) and the thing to move (combining motion target, mount, and model if necessary) - required: true - content: - application/json: - schema: - type: object - required: - - axis - properties: - axis: - type: number - maxspeed: - type: number - currentposition: - type: number - acceleration: - type: number - pinstep: - type: number - pindir: - type: number - isen: - type: number - accel: - type: number - deccel: - type: number - examples: - setX: - description: Set some parameters for the motor X - summary: Change settings X - value: - axis: 1 - maxspeed: 10000 - currentposition: 100 - acceleration: 1000 - pinstep: 2 - pindir: 3 - isen: 0 - accel: 1000 - deccel: 1000 - - responses: - '200': - description: Settings successfully updated - content: - application/json: - example: - message: 'Move copmlete. New position: (25, 25, 50)' - '400': - description: Invalid request - content: - application/json: - example: - message: 'Invalid target key: ''robot'' (target must be one of ''mount'' or ''pipette'')' - /motor_get: - get: - tags: - - Motors - summary: Endpoint for getting Motor settings - operationId: state_get_fct_http - description: Get motor settings - requestBody: - description: Specify the place to move to (as a point) and the thing to move (combining motion target, mount, and model if necessary) - required: true - content: - application/json: - schema: - type: object - required: - - axis - properties: - axis: - type: number - examples: - getX: - description: Get some parameters for the motor X - summary: Get settings X - value: - axis: 1 - responses: - '200': - description: Settings successfully updated - content: - application/json: - example: - message: 'Move copmlete. New position: (25, 25, 50)' - '400': - description: Invalid request - content: - application/json: - example: - message: 'Invalid target key: ''robot'' (target must be one of ''mount'' or ''pipette'')' - /ledarray_act: - post: - tags: - - ledarray - summary: Endpoint for controlling the ledarray - operationId: state_act_fct_http - description: Manipulate the microscopes LED matrix - requestBody: - description: Coming soon - required: true - content: - application/json: - schema: - type: object - required: - - LEDArrMode - - arraySize - - red - - green - - blue - - indexled - - Nleds - properties: - LEDArrMode: - type: string - arraySize: - type: number - red: - type: number - green: - type: number - blue: - type: number - indexled: - type: number - Nleds: - type: number - examples: - singleLED: - description: Turn on single LED - summary: "To be entered" - value: - LEDArrMode: "single" - indexled: 45 - red: 22 - green: 55 - blue: 1 - - responses: - '200': - description: LED successfully manipulated - content: - application/json: - example: - message: 'Done' - '400': - description: Invalid request - content: - application/json: - example: - message: '' - -components: - schemas: - act_motor: - type: object -)rawliteral"; - - - -static const char PROGMEM swagger_standalonejs[] = R"rawliteral( -)rawliteral"; - -static const char PROGMEM swagger_ui_bundlejs[] = R"rawliteral( -)rawliteral"; - -static const char PROGMEM swagger_ui_css[] = R"rawliteral( -)rawliteral"; diff --git a/ESP32/main/pindef.h b/ESP32/main/pindef.h deleted file mode 100644 index d584df4..0000000 --- a/ESP32/main/pindef.h +++ /dev/null @@ -1,39 +0,0 @@ -#include "pindef_WEMOS_d1_r32.h" - -int STEP_PIN_X = WEMOS_D1_R32_X_STEP_PIN; -int STEP_PIN_Y = WEMOS_D1_R32_Y_STEP_PIN; -int STEP_PIN_Z = WEMOS_D1_R32_Z_STEP_PIN; -int STEP_PIN_A = 0; - -int DIR_PIN_X = WEMOS_D1_R32_X_DIRECTION_PIN; -int DIR_PIN_Y = WEMOS_D1_R32_Y_DIRECTION_PIN; -int DIR_PIN_Z = WEMOS_D1_R32_Z_DIRECTION_PIN; -int DIR_PIN_A = 0; - -int ENABLE_PIN = WEMOS_D1_R32_STEPPERS_ENABLE_PIN; - -int LED_ARRAY_PIN = WEMOS_D1_R32_FEED_HOLD_PIN; -int LED_ARRAY_NUM = 64; - -int DIGITAL_PIN_1 = 0; -int DIGITAL_PIN_2 = 0; -int DIGITAL_PIN_3 = 0; - -int ANALOG_PIN_1 = 0; -int ANALOG_PIN_2 = 0; -int ANALOG_PIN_3 = 0; - -int LASER_PIN_1 = WEMOS_D1_R32_SPINDLEPWMPIN; -int LASER_PIN_2 = WEMOS_D1_R32_SPINDLE_ENABLE_PIN; -int LASER_PIN_3 = 0; - -int DAC_FAKE_PIN_1 = 0; -int DAC_FAKE_PIN_2 = 0; - -const char* PS3Mac = "01:02:03:04:05:06"; -const char* PS4Mac = "1a:2b:3c:01:01:01"; - -const char* WifiSSID = "BenMur";//"UC2 - F8Team"; //"IPHT - Konf"; // "Blynk"; -const char* WifiPW = "MurBen3128"; //"_lachmannUC2"; //"WIa2!DcJ"; //"12345678"; -const char* WifiSSIDAP = "UC2"; -String hostname = "youseetoo"; diff --git a/ESP32/main/pindefUC2Standalone.h b/ESP32/main/pindefUC2Standalone.h deleted file mode 100644 index 8045379..0000000 --- a/ESP32/main/pindefUC2Standalone.h +++ /dev/null @@ -1,39 +0,0 @@ -#include "pindef_WEMOS_d1_r32.h" - -int STEP_PIN_X = 2; -int STEP_PIN_Y = 27; -int STEP_PIN_Z = 12; -int STEP_PIN_A = 22; - -int DIR_PIN_X = 33; -int DIR_PIN_Y = 16; -int DIR_PIN_Z = 14; -int DIR_PIN_A = 21; - -int ENABLE_PIN = 13; - -int LED_ARRAY_PIN = 32; -int LED_ARRAY_NUM = 25; - -int DIGITAL_PIN_1 = 0; -int DIGITAL_PIN_2 = 0; -int DIGITAL_PIN_3 = 0; - -int ANALOG_PIN_1 = 0; -int ANALOG_PIN_2 = 0; -int ANALOG_PIN_3 = 0; - -int LASER_PIN_1 = 17; -int LASER_PIN_2 = 4; -int LASER_PIN_3 = 15; - -int DAC_FAKE_PIN_1 = 0; -int DAC_FAKE_PIN_2 = 0; - -const char* PS3Mac = "01:02:03:04:05:06"; -const char* PS4Mac = "1a:2b:3c:01:01:01"; - -const char* WifiSSID = "Blynk"; -const char* WifiPW = "12345678"; -const char* WifiSSIDAP = "UC2"; -String hostname = "youseetoo"; diff --git a/ESP32/main/pindef_WEMOS_d1_r32.h b/ESP32/main/pindef_WEMOS_d1_r32.h deleted file mode 100644 index a24cae8..0000000 --- a/ESP32/main/pindef_WEMOS_d1_r32.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - This map is for relatively common ESP32 boards replicating the form factor of Arduino UNO. - This map allows use of such uno-compatible board with very popular - "Protoneer Arduino CNC shield" and is based on its pinout. - This makes perfect match for retrofiting older Arduino+GRBL based machines - with 32b microcontroler capable of running grblHAL and providing few extra IO pins (eg. for modbus). - These boards are sold under several names, for instance: - + ESPDUINO-32 (USB type B) - + Wemos D1 R32 (Micro USB) -*/ - - -#define WEMOS_D1_R32_BOARD_NAME "ESPDUINO-32 Wemos D1 R32" - -// timer definitions -#define WEMOS_D1_R32_STEP_TIMER_GROUP TIMER_GROUP_0 -#define WEMOS_D1_R32_STEP_TIMER_INDEX TIMER_0 - -// Define step pulse output pins. -#define WEMOS_D1_R32_X_STEP_PIN GPIO_NUM_26 -#define WEMOS_D1_R32_Y_STEP_PIN GPIO_NUM_25 -#define WEMOS_D1_R32_Z_STEP_PIN GPIO_NUM_17 -#define WEMOS_D1_R32_A_STEP_PIN GPIO_NUM_19 - -// Define step direction output pins. NOTE: All direction pins must be on the same port. -#define WEMOS_D1_R32_X_DIRECTION_PIN GPIO_NUM_16 -#define WEMOS_D1_R32_Y_DIRECTION_PIN GPIO_NUM_27 -#define WEMOS_D1_R32_Z_DIRECTION_PIN GPIO_NUM_14 -#define WEMOS_D1_R32_A_DIRECTION_PIN GPIO_NUM_18 - -#define WEMOS_D1_R32_X_END_STOP GPIO_NUM_13 // arduino 9 -#define WEMOS_D1_R32_X_END_STOP GPIO_NUM_5 // arduino 10 -#define WEMOS_D1_R32_X_END_STOP GPIO_NUM_23 // arduino 11 - - -// Define stepper driver enable/disable output pin(s). -#define WEMOS_D1_R32_STEPPERS_ENABLE_PIN GPIO_NUM_12 - -// Define homing/hard limit switch input pins and limit interrupt vectors. -#define WEMOS_D1_R32_X_LIMIT_PIN GPIO_NUM_13 -#define WEMOS_D1_R32_Y_LIMIT_PIN GPIO_NUM_5 -#define WEMOS_D1_R32_Z_LIMIT_PIN GPIO_NUM_23 - -// Define spindle enable and spindle direction output pins. -#define WEMOS_D1_R32_SPINDLE_ENABLE_PIN GPIO_NUM_18 -#define WEMOS_D1_R32_SPINDLEPWMPIN GPIO_NUM_19 - -// Define flood enable output pin. -#define WEMOS_D1_R32_COOLANT_FLOOD_PIN GPIO_NUM_32 - -// Define user-control CONTROLs (cycle start, reset, feed hold) input pins. -#define WEMOS_D1_R32_RESET_PIN GPIO_NUM_2 -#define WEMOS_D1_R32_FEED_HOLD_PIN GPIO_NUM_4 -#define WEMOS_D1_R32_CYCLE_START_PIN GPIO_NUM_35 - -// Define probe switch input pin. -#define WEMOS_D1_R32_PROBE_PIN GPIO_NUM_39 - -#define WEMOS_D1_R32_UART2_RX_PIN GPIO_NUM_33 -#define WEMOS_D1_R32_UART2_TX_PIN GPIO_NUM_32 -#define WEMOS_D1_R32_MODBUS_DIRECTION_PIN GPIO_NUM_15 -#define WEMOS_D1_R32_MODBUS_BAUD 19200 - - -// Pin mapping when using SPI mode. -// With this mapping, SD card can be used both in SPI and 1-line SD mode. -// Note that a pull-up on CS line is required in SD mode. -#define WEMOS_D1_R32_PIN_NUM_MISO GPIO_NUM_19 -#define WEMOS_D1_R32_PIN_NUM_MOSI GPIO_NUM_23 -#define WEMOS_D1_R32_PIN_NUM_CLK GPIO_NUM_18 -#define WEMOS_D1_R32_PIN_NUM_CS GPIO_NUM_5 diff --git a/ESP32/main/read_sensor.ino b/ESP32/main/read_sensor.ino deleted file mode 100644 index 8882a01..0000000 --- a/ESP32/main/read_sensor.ino +++ /dev/null @@ -1,127 +0,0 @@ -#ifdef IS_READSENSOR - -// Custom function accessible by the API -void readsensor_act_fct() { - - // here you can do something - if (DEBUG) Serial.println("readsensor_act_fct"); - int readsensorID = (int)jsonDocument["readsensorID"]; - int mN_sensor_avg = N_sensor_avg; - if (jsonDocument.containsKey("N_sensor_avg")) - mN_sensor_avg = (int)jsonDocument["N_sensor_avg"]; - int sensorpin = 0 ; - - if (DEBUG) Serial.print("readsensorID "); Serial.println(readsensorID); - switch (readsensorID) { - case 0: - sensorpin = ADC_pin_0; - break; - case 1: - sensorpin = ADC_pin_1; - break; - case 2: - sensorpin = ADC_pin_2; - break; - } - - float sensorValueAvg = 0; - for (int imeas=0; imeas < N_sensor_avg; imeas++) { - sensorValueAvg += analogRead(sensorpin); - } - float returnValue = (float)sensorValueAvg / (float)N_sensor_avg; - - jsonDocument.clear(); - jsonDocument["sensorValue"] = returnValue; - jsonDocument["sensorpin"] = sensorpin; - jsonDocument["N_sensor_avg"] = N_sensor_avg; - -} - - - -void readsensor_set_fct() { - if (DEBUG) Serial.println("readsensor_set_fct"); - int readsensorID = (int)jsonDocument["readsensorID"]; - int readsensorPIN = (int)jsonDocument["readsensorPIN"]; - if (jsonDocument.containsKey("N_sensor_avg")) - N_sensor_avg = (int)jsonDocument["N_sensor_avg"]; - - switch (readsensorID) { - case 0: - ADC_pin_0 = readsensorPIN; - break; - case 1: - ADC_pin_1 = readsensorPIN; - break; - case 2: - ADC_pin_2 = readsensorPIN; - break; - } - - - jsonDocument.clear(); - jsonDocument["readsensorPIN"] = readsensorPIN; - jsonDocument["readsensorID"] = readsensorID; -} - - - -// Custom function accessible by the API -void readsensor_get_fct() { -if (DEBUG) Serial.println("readsensor_get_fct"); - int readsensorID = (int)jsonDocument["readsensorID"]; - int readsensorPIN = 0; - switch (readsensorID) { - case 0: - readsensorPIN = ADC_pin_0; - break; - case 1: - readsensorPIN = ADC_pin_1; - break; - case 2: - readsensorPIN = ADC_pin_2; - break; - } - - jsonDocument.clear(); - jsonDocument["N_sensor_avg"] = N_sensor_avg; - jsonDocument["readsensorPIN"] = readsensorPIN; - jsonDocument["readsensorID"] = readsensorID; -} - - -void setup_sensors(){ - if(DEBUG) Serial.println("Setting up sensors..."); -} - - - -/* - wrapper for HTTP requests -*/ -void readsensor_act_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - readsensor_act_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void readsensor_get_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - readsensor_get_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} - -// wrapper for HTTP requests -void readsensor_set_fct_http() { - String body = server.arg("plain"); - deserializeJson(jsonDocument, body); - readsensor_set_fct(); - serializeJson(jsonDocument, output); - server.send(200, "application/json", output); -} -#endif diff --git a/ESP32/main/wifi.ino b/ESP32/main/wifi.ino deleted file mode 100644 index 8b13789..0000000 --- a/ESP32/main/wifi.ino +++ /dev/null @@ -1 +0,0 @@ - diff --git a/MATLAB/OLD/main.m b/MATLAB/OLD/main.m deleted file mode 100644 index 3c015a2..0000000 --- a/MATLAB/OLD/main.m +++ /dev/null @@ -1,37 +0,0 @@ - -% Find the serial port connected to the ESP32 device -portInfo = instrhwinfo('serial'); -portName = portInfo.AvailableSerialPorts; - -espPort = ''; -for i = 1:numel(portName) - if contains(portName{i}, 'wchusb') || contains(portName{i}, 'UART') % Modify this condition according to your ESP32's serial port identifier - espPort = portName{i}; - break; - end -end - -if isempty(espPort) - error('ESP32 device not found'); -end - - -% Establish connection with ESP32 -esp = serial(espPort, 'BaudRate', 115200); % Modify the baud rate if necessary - -% Open the serial port -fopen(esp); - -% turn motors -sendMotorAction(esp); - -% Create the LED array structure -ledArray = struct('id', 0, 'red', 0, 'green', 5, 'blue', 0); - -% Send the LED array action JSON message -sendLEDArrayAction(esp, 1, ledArray); - -% Close the serial port -fclose(esp); -delete(esp); -clear esp; \ No newline at end of file diff --git a/MATLAB/OLD/sendJSONMessage.m b/MATLAB/OLD/sendJSONMessage.m deleted file mode 100644 index 5043f92..0000000 --- a/MATLAB/OLD/sendJSONMessage.m +++ /dev/null @@ -1,12 +0,0 @@ -function response = sendJSONMessage(esp, message) - % Send JSON message over the serial connection - - % Convert message to string - jsonString = jsonencode(message); - - % Send the message - fprintf(esp, jsonString); - - % wait for the response - response = fscanf(esp); -end diff --git a/MATLAB/OLD/sendLEDArrayAction.m b/MATLAB/OLD/sendLEDArrayAction.m deleted file mode 100644 index 8957c34..0000000 --- a/MATLAB/OLD/sendLEDArrayAction.m +++ /dev/null @@ -1,10 +0,0 @@ -function returnMessage = sendLEDArrayAction(esp, mode, ledArray) - % Send LED array action JSON message over the serial connection - - % Create the message structure - message = struct('task', '/ledarr_act', 'led', struct('LEDArrMode', mode, 'led_array', ledArray)); - - % Convert message to string - returnMessage = sendJSONMessage(esp, message); - -end \ No newline at end of file diff --git a/MATLAB/OLD/sendMotorAction.m b/MATLAB/OLD/sendMotorAction.m deleted file mode 100644 index f3014a5..0000000 --- a/MATLAB/OLD/sendMotorAction.m +++ /dev/null @@ -1,11 +0,0 @@ -function returnMessage = sendMotorAction(esp) -% Create the JSON message for the motor -message = struct('task', '/motor_act', 'motor', struct('steppers', ... - [struct('stepperid', 0, 'position', 10000, 'speed', 5000, 'isabs', 0, 'isaccel', 0), ... - struct('stepperid', 1, 'position', 10000, 'speed', 5000, 'isabs', 0, 'isaccel', 0), ... - struct('stepperid', 2, 'position', 10000, 'speed', 5000, 'isabs', 0, 'isaccel', 0), ... - struct('stepperid', 3, 'position', 10000, 'speed', 5000, 'isabs', 0, 'isaccel', 0)])); - -% Convert message to string -returnMessage = sendJSONMessage(esp, message); -end diff --git a/MATLAB/README.md b/MATLAB/README.md deleted file mode 100644 index 00b4d4f..0000000 --- a/MATLAB/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# UC2-REST Matlab - -This is the first attempt to use the UC2-ESP32 firmware in matlab. Use as is. no warranty! - -License: MIT \ No newline at end of file diff --git a/MATLAB/TEST_UC2_REST.m b/MATLAB/TEST_UC2_REST.m deleted file mode 100644 index f4763fc..0000000 --- a/MATLAB/TEST_UC2_REST.m +++ /dev/null @@ -1,33 +0,0 @@ -% DIP-Image related settings -rmpath('/Applications/dip/common/dipimage') -savepath - -% Create an instance of the class and specify a serial port, e.g., 'COM4' -mySerialPort = mserial('/dev/tty.SLAB_USBtoUART'); - - -% get state -path = "/state_get" -payload = "{}" - -returnmessage = mySerialPort.post_json(path, payload) - -% Display the return message -disp(returnmessage); - -% driving motors -mMotor = motor(mySerialPort) - -% X,Y,Z,A -steps=[100,100,100,0]; -speed=[15000,15000,1500,0]; -is_absolute = false; -timeout = 100; -is_blocking = true; -is_enabled = true; - - -mMotor.move_stepper(steps, speed, is_absolute, timeout, is_blocking, is_enabled) - -% Close the serial port when done -mySerialPort.close(); \ No newline at end of file diff --git a/MATLAB/mSerial.m b/MATLAB/mSerial.m deleted file mode 100644 index dc6f75b..0000000 --- a/MATLAB/mSerial.m +++ /dev/null @@ -1,146 +0,0 @@ -classdef mserial - properties - port - device - parent - NumberRetryReconnect - MaxNumberRetryReconnect - DEBUG - is_connected - isSafetyBreak - end - methods - function obj = mserial(port) - obj.port = port; - obj.device = serial(port, 'BaudRate',115200); - fopen(obj.device); - pause(3) - obj.is_connected = true; - obj.NumberRetryReconnect = 0; - obj.MaxNumberRetryReconnect = 10; - obj.DEBUG = false; - obj.isSafetyBreak = false; - end - - function close(obj) - fclose(obj.device); - end - - function returnmessage = get_json(obj, path) - message.task = path; - message_json = jsonencode(message); - returnmessage = message_json; - end - - function returnmessage = post_json(obj, path, payload, getReturn, timeout) - if ~isstruct(payload) && payload == "{}" - payload = struct() - end - - if ~isfield(payload, 'task') - payload.task = path; - end - - if isfield(payload, 'isblock') - is_blocking = payload.isblock; - else - is_blocking = true; - end - - if ~exist('getReturn','var') - getReturn = true; - end - if ~exist('timeout','var') - timeout = 1; - end - - - obj.writeSerial(payload); - - if getReturn - returnmessage = obj.readSerial(is_blocking, timeout); - else - returnmessage = false; - end - end - - function writeSerial(obj, payload) - try % reconnection not implemented - if 0 && obj.is_connected == false && obj.NumberRetryReconnect < obj.MaxNumberRetryReconnect - fprintf('Trying to reconnect to serial device: %d\n', obj.NumberRetryReconnect); - obj.NumberRetryReconnect = obj.NumberRetryReconnect + 1; - fopen(obj.device); - end - - flushinput(obj.device); - flushoutput(obj.device); - - catch ME - fprintf('Error: %s\n', ME.message); - obj.is_connected = false; - fopen(obj.device); - end - - if isstruct(payload) - payload = jsonencode(payload); - end - - try - fprintf(obj.device, '%s', payload); - catch ME - fprintf('Error: %s\n', ME.message); - end - end - - function breakCurrentCommunication(obj) - obj.isSafetyBreak = true; - flushinput(obj.device); - flushoutput(obj.device); - end - - function returnmessage = readSerial(obj, is_blocking, timeout) - returnmessage = ''; - rmessage = ''; - t0 = tic; - - if is_blocking - while is_blocking && ~obj.isSafetyBreak - try - rmessage = fgetl(obj.device); - if obj.DEBUG - fprintf('%s\n', rmessage); - end - returnmessage = [returnmessage, rmessage]; - if strncmp(rmessage, "--", 2) - break; - end - catch ME - fprintf('Error: %s\n', ME.message); - end - if toc(t0) > timeout - break; - end - end - - % casting to struct - try - returnmessage = split(returnmessage, "--"); - returnmessage = split(returnmessage{1}, "++"); - returnmessage = strrep(returnmessage{end}, '\r', ''); - returnmessage = strrep(returnmessage, '\n', ''); - returnmessage = strrep(returnmessage, '''', '"'); - if obj.DEBUG - fprintf('%s\n', returnmessage); - end - returnmessage = jsondecode(returnmessage); - catch ME - if obj.DEBUG - fprintf('Casting json string from serial to Matlab struct failed\n'); - end - end - - - end - end - end -end diff --git a/MATLAB/motor.m b/MATLAB/motor.m deleted file mode 100644 index 97d9d9d..0000000 --- a/MATLAB/motor.m +++ /dev/null @@ -1,118 +0,0 @@ -classdef motor < handle - - properties - isRunning - mserial - isCoreXY - nMotors - motorAxisOrder - stepSizeT - stepSizeX - stepSizeY - stepSizeZ - steps_last - end - - methods - function obj = motor(mserial) - obj.isRunning = false; - obj.mserial = mserial; - obj.isCoreXY = false; - obj.stepSizeT = 1 - obj.stepSizeX = 1 - obj.stepSizeY = 1 - obj.stepSizeZ = 1 - obj.steps_last = [0,0,0,0] - obj.motorAxisOrder = [1,2,3,0] - end - - function r = stop(obj, axis) - if nargin < 2 - axisNumberList = 0:obj.nMotors-1; - else - axisNumberList = obj.xyztTo1230(axis); - end - - motorPropList = cell(1,length(axisNumberList)); - for i = 1:length(axisNumberList) - motorProp = struct('stepperid', obj.motorAxisOrder(axisNumberList(i)), 'isstop', true); - motorPropList{i} = motorProp; - end - - path = "/motor_act"; - payload = struct('task', path, 'motor', struct('steppers', motorPropList)); - - if obj.isRunning - obj.mserial.serial.breakCurrentCommunication(); - end - - r = obj.mserial.post_json(path, payload, false, 0); - end - - function r = move_stepper(obj, steps, speed, is_absolute, timeout, is_blocking, is_enabled, backlash, acceleration) - - % set default parameters - if nargin < 2, steps = [0,0,0,0]; end - if nargin < 3, speed = [1000,1000,1000,1000]; end - if nargin < 4, is_absolute = [false,false,false,false]; end - if nargin < 5, timeout = gTIMEOUT; end - if nargin < 6, is_blocking = true; end - if nargin < 7, is_enabled = true; end - if nargin < 8, backlash = [0,0,0,0]; end - if nargin < 9, acceleration = [NaN, NaN, NaN, NaN]; end - - % convert inputs to arrays if necessary - if isscalar(speed), speed = repmat(speed,1,4); end - if isscalar(acceleration), acceleration = repmat(acceleration,1,4); end - if isscalar(is_absolute), is_absolute = repmat(is_absolute,1,4); end - if isscalar(backlash), backlash = repmat(backlash,1,4); end - - % determine the axis to operate - axisToMove = find(abs(speed)>0); - - % detect change in direction - for iMotor = 1:4 - if sign(obj.steps_last(iMotor)) ~= sign(steps(iMotor)) - % we want to overshoot a bit - steps(iMotor) = steps(iMotor) + sign(steps(iMotor))*backlash(iMotor); - end - end - - % convert to physical units - steps(1) = steps(1) / obj.stepSizeT; - steps(2) = steps(2) / obj.stepSizeX; - steps(3) = steps(3) / obj.stepSizeY; - steps(4) = steps(4) / obj.stepSizeZ; - - % only consider those actions that are necessary - motorPropList = {}; - for iMotor = 1:4 - if is_absolute(iMotor) || abs(steps(iMotor))>0 - motorProp = struct('stepperid', obj.motorAxisOrder(iMotor), 'position', steps(iMotor), ... - 'speed', speed(iMotor), 'isabs', is_absolute(iMotor), 'isaccel', 0, 'isen', is_enabled); - if ~isnan(acceleration(iMotor)) - motorProp.accel = acceleration(iMotor); - end - motorPropList{end+1} = motorProp; - end - end - - path = '/motor_act'; - payload = struct('task', path, 'motor', struct('steppers', {motorPropList})); - - % save steps to track direction for backlash compensation - obj.steps_last = steps; - - % drive motor - obj.isRunning = true; - r = obj.mserial.post_json(path, payload, is_blocking, timeout); - - % TODO: implement waiting until job has been done as in Python code - - % reset busy flag - obj.isRunning = false; - - end - - end -end diff --git a/binaries/latest/UC2_3_CAN_HAT_Master_v2.bin b/binaries/latest/UC2_3_CAN_HAT_Master_v2.bin deleted file mode 100644 index 38f79c7..0000000 Binary files a/binaries/latest/UC2_3_CAN_HAT_Master_v2.bin and /dev/null differ diff --git a/binaries/latest/UC2_canopen_master_release.bin b/binaries/latest/UC2_canopen_master_release.bin new file mode 100644 index 0000000..f4f17fc Binary files /dev/null and b/binaries/latest/UC2_canopen_master_release.bin differ diff --git a/binaries/latest/UC2_canopen_slave_led_release.bin b/binaries/latest/UC2_canopen_slave_led_release.bin new file mode 100644 index 0000000..1a82872 Binary files /dev/null and b/binaries/latest/UC2_canopen_slave_led_release.bin differ diff --git a/binaries/latest/UC2_canopen_slave_motor_release.bin b/binaries/latest/UC2_canopen_slave_motor_release.bin new file mode 100644 index 0000000..1a82872 Binary files /dev/null and b/binaries/latest/UC2_canopen_slave_motor_release.bin differ diff --git a/binaries/latest/esp32_UC2_3_CAN_HAT_Master_v2.bin b/binaries/latest/esp32_UC2_3_CAN_HAT_Master_v2.bin deleted file mode 100644 index 09930a1..0000000 Binary files a/binaries/latest/esp32_UC2_3_CAN_HAT_Master_v2.bin and /dev/null differ diff --git a/binaries/latest/esp32_seeed_xiao_esp32s3_can_slave_motor.bin b/binaries/latest/esp32_seeed_xiao_esp32s3_can_slave_motor.bin deleted file mode 100644 index 2ff42d7..0000000 Binary files a/binaries/latest/esp32_seeed_xiao_esp32s3_can_slave_motor.bin and /dev/null differ diff --git a/uc2rest/can.py b/uc2rest/can.py index 3834dbd..1fa37d3 100644 --- a/uc2rest/can.py +++ b/uc2rest/can.py @@ -57,15 +57,29 @@ def register_callback(self, key, callbackfct): """Register a callback function for a specific key.""" self._callbackPerKey[key] = callbackfct + def get_can_ids(self): + """Return CAN node IDs from the latest scan (e.g. for OTA flashing).""" + return [entry.get("canId") for entry in self.scanResults + if entry.get("canId") is not None] + def reboot_remote(self, qid=1, can_address=0, isBlocking=False, timeout=2): """ - Send a reboot signal to the remote CAN device. + Reboot a CAN device. - :param qid: Query ID for the CAN command (default: 1) + - ``can_address == 0`` reboots the master (this ESP32) itself. + - ``can_address in 1..127`` reboots a remote slave by SDO-writing 1 + to OD index 0x2507 sub 0 on the target node. The slave's + CO_tmr_task observes the write and calls ESP.restart() ~200 ms + later. + + :param qid: Query ID for the CAN command (unused by firmware, kept + for API compatibility) + :param can_address: 0 = master, 1..127 = remote slave nodeId :param isBlocking: If True, wait for response :param timeout: Timeout for the command in seconds - :param can_address: Address of the CAN device to reboot (0 is master) - :return: Response from the device + :return: Response from the device, e.g. + ``{"status":"ok","nodeId":11}`` or + ``{"status":"error","error":"SDO write failed","nodeId":11}``. """ path = "/can_act" payload = { @@ -98,7 +112,7 @@ def scan(self, qid=1, timeout=5): "count": 2 } """ - path = "/can_act" + path = "/can_act" # {"task":"/can_act", "scan": true} payload = { "task": path, "scan": True, @@ -109,7 +123,7 @@ def scan(self, qid=1, timeout=5): payload, getReturn=True, timeout=timeout, - nResponses=2 + nResponses=1 ) def get_available_devices(self, timeout=2): diff --git a/uc2rest/canota.py b/uc2rest/canota.py index d4062a7..bbad8c2 100644 --- a/uc2rest/canota.py +++ b/uc2rest/canota.py @@ -1,7 +1,7 @@ import time import json -import struct -import hashlib +import re +import zlib import threading from pathlib import Path @@ -15,39 +15,23 @@ gTIMEOUT = 10 # seconds to wait for a response from the ESP32 # ============================================================================ -# CAN OTA STREAMING PROTOCOL CONSTANTS +# CANopen OTA (serial -> ESP32 master -> CAN) protocol constants +# ---------------------------------------------------------------------------- +# Mirrors tools/ota/canopen_ota_serial.py in uc2-ESP. The master accepts a +# JSON preamble ``/ota_start`` and then expects the raw firmware bytes +# streamed over serial. After each CHUNK_SIZE bytes it answers with +# ``{"ota_rx": }`` until the final ``{"ota_status": "success"}``. # ============================================================================ -# Protocol constants (must match CanOtaStreaming.h) -SYNC_1 = 0xAA -SYNC_2 = 0x55 +STREAMING_BAUD = 921600 # match canopen_ota_serial.py default +CHUNK_SIZE = 4096 # bytes per ACK-paced write +READY_TIMEOUT_S = 10.0 # wait for {"ota_status":"ready"} +FLASH_TIMEOUT_S = 600.0 # SDO domain transfer can take minutes +ACK_TIMEOUT_S = 30.0 # per-chunk wait for {"ota_rx": N} +INTER_CHUNK_DELAY_S = 0.0 # >0 only if host overruns master's UART -# Stream message types -STREAM_START = 0x70 -STREAM_DATA = 0x71 -STREAM_ACK = 0x72 -STREAM_NAK = 0x73 -STREAM_FINISH = 0x74 -STREAM_ABORT = 0x75 -STREAM_STATUS = 0x76 - - -# Protocol parameters -PORT = "/dev/cu.SLAB_USBtoUART" -BAUD = 921600 -CAN_ID = 11 - -# Streaming protocol constants -PAGE_SIZE = 4096 # 4KB pages (flash-aligned) -CHUNK_SIZE = 512 # 512 bytes per chunk within page -CHUNKS_PER_PAGE = PAGE_SIZE // CHUNK_SIZE # 8 chunks per page - -PAGE_ACK_TIMEOUT = 10.0 -MAX_PAGE_RETRIES = 3 -MAX_SESSION_RETRIES = 2 - -# Default baud rate for streaming -STREAMING_BAUD = 921600 +# Match standalone JSON objects on a single line. +_JSON_LINE_RE = re.compile(rb"\{[^\n]*\}") class CANOTA(object): @@ -301,95 +285,78 @@ def get_platformio_upload_command(self, can_id, project_path="."): return f"platformio run -t upload --upload-port {hostname}" # ======================================================================== - # CAN OTA STREAMING (USB-based, no WiFi required) + # CANopen OTA streaming (serial -> ESP32 master -> CAN) + # ---------------------------------------------------------------------- + # Mirrors tools/ota/canopen_ota_serial.py: send the ``/ota_start`` JSON + # preamble, wait for ``{"ota_status":"ready"}``, then stream raw firmware + # bytes in CHUNK_SIZE blocks, blocking after each block until the master + # acknowledges with ``{"ota_rx": }``. Verify CRC32 (not + # MD5) and wait for ``{"ota_status":"success"|"error"}``. # ======================================================================== - - def start_can_streaming_ota(self, can_id: int, firmware_path: str, + + def start_can_streaming_ota(self, can_id: int, firmware_path: str, progress_callback=None, status_callback=None, port: str = None, baud: int = STREAMING_BAUD): """ - Upload firmware to a CAN slave device via USB->CAN streaming protocol. - - This method: - 1. Sends streaming start command via JSON to initialize the slave - 2. Temporarily closes the parent's serial connection - 3. Opens a raw serial connection for binary streaming - 4. Uploads the firmware in 4KB pages with ACK per page - 5. Verifies MD5 checksum - 6. Restores the parent's serial connection - - :param can_id: CAN ID of the target device (e.g., 11 for Motor X) + Upload firmware to a CAN slave via the CANopen OTA path + (laptop -> serial -> ESP32 master -> CAN -> slave). + + :param can_id: Target slave CANopen node ID (e.g., 11 for Motor X) :param firmware_path: Path to the firmware binary (.bin file) - :param progress_callback: Function(page, total_pages, bytes_sent, speed_kbps) + :param progress_callback: Function(chunk_idx, total_chunks, bytes_sent, speed_kbps) :param status_callback: Function(status_str, success_bool) :param port: Serial port (default: use parent's port) - :param baud: Baud rate for streaming (default: 921600) - :return: True if successful, False otherwise - - Example: - def on_progress(page, total, bytes_sent, speed): - print(f"Page {page}/{total} - {speed:.1f} KB/s") - - def on_status(msg, success): - print(f"Status: {msg} ({'OK' if success else 'FAIL'})") - - ESP32.canota.start_can_streaming_ota( - 11, "/path/to/firmware.bin", - progress_callback=on_progress, - status_callback=on_status - ) + :param baud: Baud rate for streaming (default: 115200) + :return: Thread handle; join + read .result for success/failure. """ if not HAS_SERIAL: if status_callback: status_callback("Serial library not available", False) return False - - # Resolve firmware path + firmware_path = Path(firmware_path) if not firmware_path.exists(): if status_callback: status_callback(f"Firmware not found: {firmware_path}", False) return False - - # Get port from parent if not specified + if port is None and self._parent and hasattr(self._parent, "serial"): port = self._parent.serial.serialport - + if not port: if status_callback: status_callback("No serial port specified", False) return False - - # Load firmware + firmware_data = firmware_path.read_bytes() + if not firmware_data: + if status_callback: + status_callback("Firmware file is empty", False) + return False + firmware_size = len(firmware_data) - md5_hex = hashlib.md5(firmware_data).hexdigest() - md5_bytes = bytes.fromhex(md5_hex) - num_pages = (firmware_size + PAGE_SIZE - 1) // PAGE_SIZE - + crc32 = zlib.crc32(firmware_data) & 0xFFFFFFFF + num_chunks = (firmware_size + CHUNK_SIZE - 1) // CHUNK_SIZE + if status_callback: - status_callback(f"Firmware: {firmware_size} bytes, {num_pages} pages, MD5: {md5_hex[:16]}...", True) - - # Run streaming upload in a separate thread to not block + status_callback( + f"Firmware: {firmware_size} bytes, {num_chunks} chunks, " + f"CRC32: 0x{crc32:08X}", True) + upload_thread = threading.Thread( target=self._streaming_upload_worker, - args=(can_id, port, baud, firmware_data, firmware_size, - md5_hex, md5_bytes, num_pages, progress_callback, status_callback) + args=(can_id, port, baud, firmware_data, firmware_size, + crc32, num_chunks, progress_callback, status_callback) ) upload_thread.daemon = True upload_thread.start() - + return upload_thread - + def start_can_streaming_ota_blocking(self, can_id: int, firmware_path: str, progress_callback=None, status_callback=None, port: str = None, baud: int = STREAMING_BAUD): - """ - Blocking version of start_can_streaming_ota. - Waits for upload to complete before returning. - - :return: True if successful, False otherwise - """ + """Blocking variant of :meth:`start_can_streaming_ota`.""" thread = self.start_can_streaming_ota( can_id, firmware_path, progress_callback, status_callback, port, baud ) @@ -397,17 +364,18 @@ def start_can_streaming_ota_blocking(self, can_id: int, firmware_path: str, thread.join() return getattr(thread, 'result', False) return False - + def _streaming_upload_worker(self, can_id, port, baud, firmware_data, firmware_size, - md5_hex, md5_bytes, num_pages, + crc32, num_chunks, progress_callback, status_callback): - """Worker thread for streaming upload.""" + """Worker thread for the ``/ota_start`` streaming upload.""" ser = None parent_serial_was_open = False result = False - + try: - # Step 1: Close parent's serial connection temporarily + # Step 1: Close parent's serial connection temporarily so we can + # take exclusive control of the port. if self._parent and hasattr(self._parent, "serial"): parent_serial = self._parent.serial if parent_serial.serialdevice and parent_serial.serialdevice.isOpen(): @@ -415,137 +383,132 @@ def _streaming_upload_worker(self, can_id, port, baud, firmware_data, firmware_s if status_callback: status_callback("Closing parent serial connection...", True) parent_serial.closeSerial() - for i in range(10): time.sleep(0.1) # wait a bit - - # Step 2: Open raw serial connection + time.sleep(1.0) + + # Step 2: Open raw serial connection. if status_callback: - status_callback(f"Opening streaming connection on {port} at {baud} baud...", True) - if type(port) == serial.tools.list_ports_common.ListPortInfo: + status_callback( + f"Opening streaming connection on {port} at {baud} baud...", True) + if hasattr(serial, "tools") and hasattr(serial.tools, "list_ports_common") \ + and isinstance(port, serial.tools.list_ports_common.ListPortInfo): port = port.device - ser = serial.Serial(port, baud, timeout=0.1)#, write_timeout=1.0) + ser = serial.Serial(port, baud, timeout=0.05) time.sleep(0.5) - self._drain_serial(ser) - # Step 3: Send streaming start command via JSON + ser.reset_input_buffer() + + # Step 3: Send the JSON preamble and wait for ``ready``. + cmd = json.dumps({ + "task": "/ota_start", + "ota": { + "nodeId": can_id, + "size": firmware_size, + "crc32": f"0x{crc32:08X}", + }, + }) if status_callback: - status_callback(f"Initializing streaming session for CAN ID {can_id}...", True) - - start_cmd = { - "task": "/can_ota_stream", - "canid": can_id, - "action": "start", - "firmware_size": firmware_size, - "page_size": PAGE_SIZE, - "chunk_size": CHUNK_SIZE, - "md5": md5_hex - } - print(" Sending STREAM_START command...", start_cmd) - response = self._send_json(ser, start_cmd) - - # Wait for JSON response - if response and str(response).find("success") <= 0: + status_callback(f"Sending /ota_start for node {can_id}...", True) + print(f" TX: {cmd}") + ser.write((cmd + "\n").encode()) + ser.flush() + + ready = self._wait_for_status( + ser, time.time() + READY_TIMEOUT_S, key_values=("ready",), + status_callback=status_callback) + if not ready or ready.get("ota_status") != "ready": if status_callback: - status_callback("STREAM_START command failed", False) - status_callback(f"Response: {response}", False) + status_callback(f"Master did not become ready: {ready}", False) return - if status_callback: - status_callback("Streaming session started, uploading...", True) - - # CRITICAL: Wait for slave to initialize via CAN - time.sleep(1.0) - - # Drain any pending data (logs, old ACKs, etc.) - self._drain_serial(ser, "post-start", verbose=False) - - # Step 4: Stream pages + status_callback(f"Master ready: {ready}", True) + + # Step 4: Stream raw firmware bytes — strictly ACK-paced. + if status_callback: + status_callback( + f"Uploading {firmware_size} bytes " + f"(ACK-paced, {CHUNK_SIZE} B chunks)...", True) + start_time = time.time() - seq = 0 - bytes_sent = 0 - failed_pages = 0 - - for page_idx in range(num_pages): - page_start = page_idx * PAGE_SIZE - page_end = min(page_start + PAGE_SIZE, firmware_size) - page_data = firmware_data[page_start:page_end] - - # Pad last page if necessary - if len(page_data) < PAGE_SIZE: - page_data = page_data + bytes(PAGE_SIZE - len(page_data)) - - # Progress display - progress = (page_idx + 1) / num_pages * 100 - elapsed = time.time() - start_time - speed = bytes_sent / elapsed / 1024 if elapsed > 0 else 0 - print(f"\r Page {page_idx+1}/{num_pages} ({progress:.1f}%) - {speed:.1f} KB/s ", end="") - - - # Send page with retry - # Send page with retry logic - success, seq, acked_page, acked_bytes = self._send_page_with_retry( - ser, page_idx, page_data, seq - ) - bytes_sent += PAGE_SIZE - - if not success: - if status_callback: - status_callback(f"Page {page_idx} failed after retries", False) - return - - # Progress callback + sent = 0 + last_ack = 0 + ack_buf = bytearray() + chunk_no = 0 + + while sent < firmware_size: + end = min(sent + CHUNK_SIZE, firmware_size) + payload = firmware_data[sent:end] + chunk_no += 1 + t_send = time.time() + ser.write(payload) + ser.flush() + sent = end + + # Block until the master ACKs at least ``sent`` bytes. + deadline = time.time() + ACK_TIMEOUT_S + while last_ack < sent: + last_ack, err = self._drain_acks(ser, ack_buf, last_ack) + if err is not None: + if status_callback: + status_callback( + f"Master reported error at chunk #{chunk_no}: {err}", + False) + time.sleep(0.3) + self._drain_acks(ser, ack_buf, last_ack) + return + if last_ack >= sent: + break + if time.time() > deadline: + if status_callback: + status_callback( + f"Timeout waiting for ACK at chunk #{chunk_no} " + f"(sent={sent}, last_ack={last_ack})", False) + time.sleep(0.3) + self._drain_acks(ser, ack_buf, last_ack) + return + time.sleep(0.002) + if progress_callback: elapsed = time.time() - start_time - speed = bytes_sent / elapsed / 1024 if elapsed > 0 else 0 - progress_callback(page_idx + 1, num_pages, bytes_sent, speed) - - # Step 5: Send FINISH command + speed = sent / elapsed / 1024 if elapsed > 0 else 0 + progress_callback(chunk_no, num_chunks, sent, speed) + + if INTER_CHUNK_DELAY_S > 0: + time.sleep(INTER_CHUNK_DELAY_S) + + elapsed = time.time() - start_time + if elapsed > 0 and status_callback: + status_callback( + f"Transfer: {elapsed:.1f}s " + f"({firmware_size / elapsed / 1024:.1f} KB/s)", True) + + # Step 5: Wait for the flashing -> success/error transition. if status_callback: - status_callback("All pages sent, verifying MD5...", True) - - finish_packet = self._build_stream_finish_packet(md5_bytes) - ser.write(finish_packet) - ser.flush() - - # Wait for final verification - success, raw = self._wait_for_final_ack(ser, num_pages - 1, timeout=30.0) - - if success: - elapsed = time.time() - start_time - speed = firmware_size / elapsed / 1024 + status_callback( + "Waiting for slave flash result (may take minutes)...", True) + final = self._wait_for_status( + ser, time.time() + FLASH_TIMEOUT_S, + key_values=("success", "error"), + status_callback=status_callback) + if final is None: if status_callback: - status_callback(f"SUCCESS! {firmware_size} bytes in {elapsed:.1f}s ({speed:.1f} KB/s)", True) + status_callback("Timed out waiting for OTA result", False) + return + if final.get("ota_status") == "success": + if status_callback: + status_callback( + "OTA flash successful — slave will reboot.", True) result = True else: - # Check for reboot indicator - try: - raw_text = raw.decode('utf-8', errors='replace') if raw else "" - if "Rebooting" in raw_text or "OTA COMPLETE" in raw_text: - if status_callback: - status_callback("Device rebooting - OTA successful!", True) - result = True - else: - if status_callback: - status_callback("MD5 verification failed", False) - except: - if status_callback: - status_callback("Final verification failed", False) - + if status_callback: + status_callback(f"OTA flash failed: {final}", False) + except Exception as e: if status_callback: status_callback(f"Error: {str(e)}", False) - - # Try to abort - if ser and ser.isOpen(): - try: - self._send_json(ser, {"task": "/can_ota_stream", "canid": can_id, "action": "abort"}) - except: - pass - + finally: - # Close streaming connection if ser and ser.isOpen(): ser.close() - - # Restore parent's serial connection + if parent_serial_was_open and self._parent and hasattr(self._parent, "serial"): if status_callback: status_callback("Restoring serial connection...", True) @@ -554,248 +517,72 @@ def _streaming_upload_worker(self, can_id, port, baud, firmware_data, firmware_s self._parent.serial.openDevice(port, baud) except Exception as e: if status_callback: - status_callback(f"Warning: Could not restore serial: {e}", False) - - # Store result in thread for blocking version + status_callback( + f"Warning: Could not restore serial: {e}", False) + threading.current_thread().result = result - + # ======================================================================== - # Streaming Protocol Helper Methods + # Streaming protocol helpers (mirror canopen_ota_serial.py) # ======================================================================== - - def _drain_serial(self, ser, label="", verbose=True): - """Read and display any pending data""" - time.sleep(0.05) - data = b'' - while ser.in_waiting: - data += ser.read(ser.in_waiting) - time.sleep(0.01) - if data and verbose: - print(f" [{label}] Drained {len(data)} bytes") - return data - - def _send_json(self, ser, data, wait_response=True, timeout=3.0): - """Send JSON command over serial.""" - json_str = json.dumps(data, separators=(',', ':')) - tx_data = (json_str + '\n').encode() - print(f" TX: {json_str[:80]}{'...' if len(json_str) > 80 else ''}") - ser.write(tx_data) - ser.flush() - - if wait_response: - time.sleep(0.3) - start = time.time() - response = b'' - while time.time() - start < timeout: - if ser.in_waiting: - ser_line = ser.read(ser.in_waiting) - print(ser_line) - response += ser_line - if b'{"success":' in response or b'{"error":' in response: - break - time.sleep(0.05) - print(f" RX: {len(response)} bytes") - return response - return None - - def _build_stream_data_packet(self, page_idx, offset, seq, chunk_data): - # Match can_ota_streaming.py wire format + checksum - packet_body = bytes([SYNC_1, SYNC_2, STREAM_DATA]) + struct.pack( - '>HHHH', page_idx, offset, len(chunk_data), seq - ) + chunk_data - checksum = 0 - for b in packet_body: - checksum ^= b - return packet_body + bytes([checksum]) - - def _build_stream_finish_packet(self, md5_bytes): - packet_body = bytes([SYNC_1, SYNC_2, STREAM_FINISH]) + md5_bytes - checksum = 0 - for b in packet_body: - checksum ^= b - return packet_body + bytes([checksum]) - - def _wait_for_session_start(self, ser, timeout=10.0): - """Wait for streaming session start ACK.""" - start = time.time() - buffer = bytearray() - - while time.time() - start < timeout: - if ser.in_waiting: - buffer.extend(ser.read(ser.in_waiting)) - - # Look for STREAM_ACK - for i in range(len(buffer) - 5): - if buffer[i] == SYNC_1 and buffer[i+1] == SYNC_2: - if buffer[i+2] == STREAM_ACK and buffer[i+3] == 0: # status OK - return True - else: - time.sleep(0.01) - - return False - - def _wait_for_page_ack(self, ser, expected_page, timeout=PAGE_ACK_TIMEOUT): - """ - Wait for STREAM_ACK response. - Returns: (success, last_complete_page, bytes_received, raw_response) - """ - start = time.time() - buffer = bytearray() - - while time.time() - start < timeout: - if ser.in_waiting: - new_data = ser.read(ser.in_waiting) - buffer.extend(new_data) - - # Check for log messages indicating success (for final ACK) - try: - text = bytes(buffer).decode('utf-8', errors='replace') - if "OTA COMPLETE" in text or "Rebooting" in text: - return (True, expected_page, 0, bytes(buffer)) - except: - pass - - # Search for STREAM_ACK response - # Format: [SYNC][CMD][status][canId][lastPage_L][lastPage_H][bytes(4)][nextSeq(2)][reserved(2)] - i = 0 - while i < len(buffer) - 15: - if buffer[i] == SYNC_1 and buffer[i+1] == SYNC_2: - cmd = buffer[i+2] - - if cmd == STREAM_ACK: - status = buffer[i+3] - can_id = buffer[i+4] - last_page = buffer[i+5] | (buffer[i+6] << 8) # Little endian - bytes_recv = struct.unpack(' 0: + del buf[: idx + 1] + else: + time.sleep(0.005) + return None - def wait_for_stream_ack(self, ser, expected_page: int, timeout: float = PAGE_ACK_TIMEOUT): - """ - Wait for STREAM_ACK response. - Returns: (success, last_complete_page, bytes_received, raw_response) - """ - start = time.time() - buffer = bytearray() - - while time.time() - start < timeout: - if ser.in_waiting: - new_data = ser.read(ser.in_waiting) - buffer.extend(new_data) - - # Check for log messages indicating success (for final ACK) + def _drain_acks(self, ser, ack_buf: bytearray, last_ack: int): + """Read pending bytes, return (latest_ack, error_obj_or_None).""" + latest = last_ack + err = None + if ser.in_waiting: + ack_buf.extend(ser.read(ser.in_waiting)) + for match in _JSON_LINE_RE.findall(bytes(ack_buf)): + try: + obj = json.loads(match.decode(errors="ignore")) + except json.JSONDecodeError: + continue + if "ota_rx" in obj: try: - text = bytes(buffer).decode('utf-8', errors='replace') - print("DEBUG TEXT:", text) - if "OTA COMPLETE" in text or "Rebooting" in text: - return (True, expected_page, 0, bytes(buffer)) - except: + latest = max(latest, int(obj["ota_rx"])) + except (TypeError, ValueError): pass - - # Search for STREAM_ACK response - # Format: [SYNC][CMD][status][canId][lastPage_L][lastPage_H][bytes(4)][nextSeq(2)][reserved(2)] - i = 0 - while i < len(buffer) - 15: - if buffer[i] == SYNC_1 and buffer[i+1] == SYNC_2: - cmd = buffer[i+2] - - if cmd == STREAM_ACK: - status = buffer[i+3] - can_id = buffer[i+4] - last_page = buffer[i+5] | (buffer[i+6] << 8) # Little endian - bytes_recv = struct.unpack('= 0: + del ack_buf[: nl + 1] + return latest, err \ No newline at end of file diff --git a/uc2rest/ledmatrix.py b/uc2rest/ledmatrix.py index 7f89c92..7d04dd4 100644 --- a/uc2rest/ledmatrix.py +++ b/uc2rest/ledmatrix.py @@ -309,7 +309,7 @@ def send_LEDMatrix_status(self, status="idle"): Set the status of the LED matrix to "idle" or "busy". JSON: "action":"status", "status": """ - if status not in ["error", "idle", "warn", "success", "busy", "rainbow"]: + if status not in ["error", "idle", "warn", "success", "busy", "rainbow", "on", "off"]: status = "idle" path = "/ledarr_act" payload = { diff --git a/uc2rest/motor.py b/uc2rest/motor.py index 1fffd06..19a2539 100644 --- a/uc2rest/motor.py +++ b/uc2rest/motor.py @@ -632,7 +632,10 @@ def move_stepper(self, steps=(0,0,0,0), speed=(1000,1000,1000,1000), is_absolute self.isRunning = True is_blocking = is_blocking and self._parent.serial.is_connected timeout = timeout if is_blocking else 0 - nResponses = len(payload["motor"]["steppers"]) + 1 + if self._parent.serial.use_qid_done: + nResponses = 1 # QID mode: firmware handles completion tracking + else: + nResponses = len(payload["motor"]["steppers"]) + 1 # if we get a return, we will receive the latest position feedback from the driver by means of the axis that moves the longest r = self._parent.post_json(path, payload, getReturn=is_blocking, timeout=timeout, nResponses=nResponses) diff --git a/uc2rest/mserial.py b/uc2rest/mserial.py index 28e1af1..de0fa0d 100644 --- a/uc2rest/mserial.py +++ b/uc2rest/mserial.py @@ -55,6 +55,11 @@ def __init__(self, port, baudrate=115200, timeout=5, # setup callback list for parent modules self.callBackList = [] # TODO: We should pop items when list is full + # QID-based done tracking (new mode) + self.use_qid_done = False # Default: use legacy nResponses mode + self.qid_done_events = {} # {qid: threading.Event} + self.qid_done_responses = {} # {qid: response_data} + # initialize serial connection self.thread = None if IS_SERIAL: @@ -124,6 +129,7 @@ def openDevice(self, port=None, baud_rate=115200): self._logger.debug(f"Connected to {port.device} at {baud_rate} baud.") ser = self.serialdevice self.is_connected = True + self.manufacturer = "UC2" except Exception as e: self._logger.error("[OpenDevice]: "+str(e)) @@ -289,7 +295,7 @@ def _process_commands(self): try: if self.manufacturer == "UC2Mock": self.running = False - return + break # device not ready yet if self.serialdevice is None: @@ -365,6 +371,14 @@ def _process_commands(self): except Exception as e: self._logger.error("No QID found in the response: "+str(e)) + # Check for QID done/state notification from firmware + if self.use_qid_done and "state" in json_response: + state_val = json_response["state"] + resp_qid = json_response.get("qid", -1) + if state_val in ("done", "error", "timeout", "paused") and resp_qid in self.qid_done_events: + self.qid_done_responses[resp_qid] = json_response.copy() + self.qid_done_events[resp_qid].set() + with self.lock: try: # TODO: THis looks fishy an self.responses[currentIdentifier].append(json_response.copy()) @@ -463,12 +477,37 @@ def sendMessage(self, command, nResponses=1, timeout = 20): if nResponses <= 0 or not self.is_connected or self.manufacturer=="UC2Mock": time.sleep(0.1) return identifier + + # QID-done mode: wait for {"qid":X,"state":"done"} from firmware + if self.use_qid_done and nResponses > 0: + event = threading.Event() + self.qid_done_events[identifier] = event + self.qid_done_responses.pop(identifier, None) + + # Wait for done event or timeout + if event.wait(timeout=timeout): + # Got done notification + response = self.qid_done_responses.pop(identifier, {}) + self.qid_done_events.pop(identifier, None) + # Also collect any queued responses + with self.lock: + extra = self.responses.pop(identifier, []) + return [response] + extra if extra else [response] + else: + # Timeout + self.qid_done_events.pop(identifier, None) + self.qid_done_responses.pop(identifier, None) + if self.DEBUG: + self._logger.debug("QID done timeout for qid: %s", identifier) + return "qid_done timeout: " + str(identifier) + t0 = time.time() maxRetry = 3 iRetry = 0 while self.running: time.sleep(0.002) - if self.resetLastCommand or time.time()-t0>timeout or not self.is_connected: + isTimeout = time.time()-t0>timeout + if self.resetLastCommand or isTimeout or not self.is_connected: self.resetLastCommand = False if self.DEBUG: self._logger.debug("Communication interrupted by timeout or reset for command: "+str(self.commands[identifier])) @@ -483,7 +522,8 @@ def sendMessage(self, command, nResponses=1, timeout = 20): if -identifier in self.responses: self._logger.debug("You have sent the wrong command!") return "Wrong Command" - if time.time()-t0>self.timeReturnReceived and not (identifier in self.responses and len(self.responses[identifier]) > 0): + isTimeout = time.time()-t0>self.timeReturnReceived + if isTimeout and not (identifier in self.responses and len(self.responses[identifier]) > 0): if iRetry > maxRetry: self.resetLastCommand = True return "No response received"