From 16cd3e3bd15481c2038cdf0fe45379dfbe275c3f Mon Sep 17 00:00:00 2001 From: Miguel Angel de Heras Date: Mon, 3 Mar 2025 10:42:59 +0100 Subject: [PATCH] Adquirido actuador lineal con potenciometro y visualizacion de numero de frames capturados --- CraterLab_camera/.gitignore | 2 +- CraterLab_camera/include/constants.h | 2 +- CraterLab_camera/include/interface.html | 27 +++++++--- CraterLab_camera/include/main_m4.cpp | 68 ++++++++++++++----------- CraterLab_camera/include/main_m7.cpp | 23 +++------ CraterLab_camera/lib/README | 46 ----------------- CraterLab_camera/platformio.ini | 3 +- CraterLab_camera/test/README | 11 ---- 8 files changed, 69 insertions(+), 113 deletions(-) delete mode 100644 CraterLab_camera/lib/README delete mode 100644 CraterLab_camera/test/README diff --git a/CraterLab_camera/.gitignore b/CraterLab_camera/.gitignore index b9f3806..3b8da3a 100644 --- a/CraterLab_camera/.gitignore +++ b/CraterLab_camera/.gitignore @@ -1,2 +1,2 @@ .pio -.vscode +.vscode \ No newline at end of file diff --git a/CraterLab_camera/include/constants.h b/CraterLab_camera/include/constants.h index ccab64e..2dca6da 100644 --- a/CraterLab_camera/include/constants.h +++ b/CraterLab_camera/include/constants.h @@ -38,5 +38,5 @@ #define sensor_lineal_motor A0 //Definicion de los limites del sensor lineal -#define LINEAL_MAX 478 +#define LINEAL_MAX 4*478 #define LINEAL_MIN 0 \ No newline at end of file diff --git a/CraterLab_camera/include/interface.html b/CraterLab_camera/include/interface.html index ec74d9a..521a883 100644 --- a/CraterLab_camera/include/interface.html +++ b/CraterLab_camera/include/interface.html @@ -129,7 +129,8 @@ const char *htmlTemplate = R"rawliteral(

Motor Cámara

- +

Counter

+

Speed

@@ -158,11 +159,11 @@ const char *htmlTemplate = R"rawliteral(

Shutter

Fade In/Out Frames:

-

% apertura inicial: 50

- +

% apertura inicial: 0

+ -

Número de frames: 50

- +

Número de frames: 10

+

Fade Activación:

Activar Fade In @@ -403,7 +404,7 @@ const char *htmlTemplate = R"rawliteral( document.getElementById('zoomSlider').addEventListener('input', updateDisplay); document.getElementById('focusSlider').addEventListener('input', updateDisplay); - // pide motorSpeed al M7 + //pide motorSpeed al M7 function updateMotorSpeed() { fetch('/motorSpeed') .then(response => response.json()) @@ -413,6 +414,15 @@ const char *htmlTemplate = R"rawliteral( .catch(error => console.error('Error al obtener motorSpeed:', error)); } + function updateFramesCount() { + fetch('/framesCount') + .then(response => response.json()) + .then(data => { + document.getElementById('frames').value = data.framesCountRead; + }) + .catch(error => console.error('Error al obtener numero de frames:', error)); + } + function updatefadePosition() { fetch('/fadePercent') .then(response => response.json()) @@ -435,6 +445,9 @@ const char *htmlTemplate = R"rawliteral( if (sensorValues.length >= 2) { document.getElementById('fade').value = sensorValues[1]; } + if (sensorValues.length >= 3) { + document.getElementById('frames').value = sensorValues[2]; + } } else { console.error('Estructura inesperada:', data); } @@ -443,7 +456,7 @@ const char *htmlTemplate = R"rawliteral( } // Llama a updateSensors cada 500ms - setInterval(updateSensors, 500); + setInterval(updateSensors, 100); diff --git a/CraterLab_camera/include/main_m4.cpp b/CraterLab_camera/include/main_m4.cpp index a8861a3..949f45e 100644 --- a/CraterLab_camera/include/main_m4.cpp +++ b/CraterLab_camera/include/main_m4.cpp @@ -32,6 +32,10 @@ bool syncWithIntervalOptics = false; bool closed=false; bool isAction=false; +//Sensores +volatile float motorSpeedRead = 0; +int FramesCount = 0; + #if PID_ENABLE // ************************************************ Variables PID ***************************************************************** double Setpoint = 0.0, Input = 0.0, Output = 0.0; // Setpoint=Posición designada; Input=Posición del motor; Output=Tensión de salida para el motor. @@ -107,7 +111,7 @@ float time_move_for_move_position_motor[6] = {0, 0, 0, 0, 0, 0}; unsigned long time_refresh_position_motor[6] = { millis(), millis(), millis(), millis(), millis(), millis()}; float position_motor[6] = { 0, 0, 0, 0, 0, 0}; float position_motor_objective[6] = { 0, 0, 0, 0, 0, 0}; -bool moving_position_motor[6] = { false, false, false, false, false, false}; +int moving_position_motor[6] = { 0, 0, 0, 0, 0, 0}; void config_position_motor(int motor, float position) //Configuracion posicion en % motor { @@ -115,10 +119,17 @@ void config_position_motor(int motor, float position) //Configuracion posicion e { if(motor==0) { - if(position>position_motor[motor]) forward(motor,255); - else backward(motor,255); + if(position>position_motor[motor]) + { + forward(motor,32); + moving_position_motor[motor] = 1; + } + else + { + backward(motor,32); + moving_position_motor[motor] = -1; + } position_motor_objective[motor] = position; - moving_position_motor[motor] = true; } else { @@ -146,7 +157,7 @@ void config_speed_motor(int motor, int direction, int speed) //Configuracion vel } -volatile float fps_read = 0; + volatile float fps_temp = 0; unsigned long time_sec= millis(); unsigned long time_fps = micros(); @@ -157,18 +168,22 @@ static volatile bool force_stop = false; //Tiempo del rebote void fps_count_state() { if((digitalRead(sensor_fps)&&(micros()-lastTimeDebounce>= 100))&&(!closed)) { + if(motorDirection==FORWARD) FramesCount++; + else FramesCount--; lastTimeDebounce = micros(); fps_temp = 1000000./(float)(micros()-time_fps); - if(fps_temp<70) fps_read = fps_temp; + if(fps_temp<70) motorSpeedRead = fps_temp; time_fps = micros(); if((millis()-time_sec)>=1000) { - //RPC.println(fps_read); + //RPC.println(motorSpeedRead); time_sec = millis(); } } else if((digitalRead(sensor_fps)&&(micros()-lastTimeDebounce >= 150))&&(closed)) { + if(motorDirection==FORWARD) FramesCount++; + else FramesCount--; lastTimeDebounce = micros(); closed = false; force_stop = true; @@ -194,23 +209,12 @@ float inc_position_fade = 0; int fade_direction = 0; void config_automatic_fade(int shutterFadeFrames, int shutterSyncWithInterval, int shutterFadeInActive, int shutterFadeOutActive) //Configuracion fade { - - if((shutterFadeInActive)&&(!shutterFadeOutActive)) - { - fade_direction=IN; - inc_position_fade = (100-position_motor[0])/(float)shutterFadeFrames; - } - else if((!shutterFadeInActive)&&(shutterFadeOutActive)) - { - fade_direction=OUT; - inc_position_fade = (position_motor[0])/(float)shutterFadeFrames; - } - else if((shutterFadeInActive)&&(shutterFadeOutActive)) - { - fade_direction=IN_OUT; - inc_position_fade = (100)/(float)shutterFadeFrames; - } + if((shutterFadeInActive)&&(!shutterFadeOutActive)) fade_direction=IN; + else if((!shutterFadeInActive)&&(shutterFadeOutActive)) fade_direction=OUT; + else if((shutterFadeInActive)&&(shutterFadeOutActive)) fade_direction=IN_OUT; else fade_direction=STOP; + if(fade_direction!=IN_OUT) inc_position_fade = (100)/((float)shutterFadeFrames+1); + else inc_position_fade = 2*(100)/((float)shutterFadeFrames+1); } void config_optics(int zoomValue, int focusValue, int diaphragmValue, int syncWithIntervalOptics) //Configuracion de las opticas @@ -248,9 +252,10 @@ int read_lineal_motor() void ini_motors() { //forward(0,255); - while (read_lineal_motor()>LINEAL_MAX) forward(0,255); + unsigned long time_protection = millis(); + while ((read_lineal_motor()>LINEAL_MAX)&&((millis()-time_protection)<10000)) forward(0,255); stop(0); - while (read_lineal_motor()=position_motor_objective[0])&&(moving_position_motor[0]==1))||((position_motor[0]<=position_motor_objective[0])&&(moving_position_motor[0]==-1))) { stop(0); - moving_position_motor[0] = false; + moving_position_motor[0] = 0; } for(int i=1; i<4; i++) { @@ -330,7 +335,7 @@ unsigned long time_adjust = millis(); Functions on the M4 that returns FPS */ int fpsRead() { - int result = fps_read; + int result = motorSpeedRead; return result; } @@ -469,9 +474,9 @@ void RPCRead() for(int i=0; i<1; i++) RPC.println(value[i]); #endif if(value[0]) - RPC.println("/sensors:" + String(fps_read) + "," + String(analogRead(sensor_lineal_motor)) + "," + String(position_motor[1]) + "," + String(position_motor[2]) + "," + String(position_motor[3])); - //RPC.println("/sensors:" + String(fps_read) + "," + String(position_motor[0]) + "," + String(position_motor[1]) + "," + String(position_motor[2]) + "," + String(position_motor[3])); - //RPC.println(fps_read); + //RPC.println("/sensors:" + String(motorSpeedRead) + "," + String(analogRead(sensor_lineal_motor)) + "," + String(position_motor[1]) + "," + String(position_motor[2]) + "," + String(position_motor[3])); + RPC.println("/sensors:" + String(motorSpeedRead) + "," + String(position_motor[0]) + "," + String(position_motor[1]) + "," + String(position_motor[2]) + "," + String(position_motor[3]) + "," + String(FramesCount)); + //RPC.println(motorSpeedRead); } inputString_rpc = String(); digitalWrite(LED_BUILTIN, HIGH); @@ -490,6 +495,7 @@ void setup() { pinMode(EN_M4, OUTPUT); digitalWrite(EN_M4, HIGH); analogWriteResolution(8); + analogReadResolution(12); digitalWrite(Enable, LOW); sr.setAllLow(); // set all pins LOW ini_motors(); diff --git a/CraterLab_camera/include/main_m7.cpp b/CraterLab_camera/include/main_m7.cpp index c4504f5..2a78492 100644 --- a/CraterLab_camera/include/main_m7.cpp +++ b/CraterLab_camera/include/main_m7.cpp @@ -53,24 +53,14 @@ int focusValue = 50; float diaphragmValue = 1.8; bool syncWithIntervalOptics = false; -// Dispositivo 360 -int x0Degrees = 0; -int x0Duration = 0; -int x1Degrees = 0; -int x1Duration = 0; -int y0Degrees = 0; -int y0Duration = 0; -bool syncWithInterval360 = false; - //Sensores float motorSpeedRead = 0; float FadePercentRead = 0; int zoomValueRead = 0; int focusValueRead = 0; float diaphragmValueRead = 0; -int x0DegreesRead = 0; -int x1DegreesRead = 0; -int y0DegreesRead = 0; +int FramesCount = 0; + int read_value[10]; int* decode_values(String inputString, int num_dec) @@ -100,7 +90,7 @@ void RPCRead() { if (inputString_rpc.startsWith("/sensors:")) //Speed Motor { - int* value = decode_values(inputString_rpc, 5); + int* value = decode_values(inputString_rpc, 6); #if DEBUG_M4 RPC.print("Recibido Speed M4: "); RPC.println(inputString_rpc); @@ -111,6 +101,7 @@ void RPCRead() zoomValueRead = value[2]; focusValueRead = value[3]; diaphragmValueRead = value[4]; + FramesCount = value[5]; } else if(inputString_rpc.startsWith("/debug:")) { @@ -179,7 +170,9 @@ bool flag_send[10] = {false ,false ,false ,false ,false ,false ,false ,false ,fa void handlePostRequest(String body) { // Parseamos el cuerpo del JSON - StaticJsonDocument<1000> doc; + //StaticJsonDocument<1000> doc; + JsonDocument doc; + doc.add(1000); deserializeJson(doc, body); int save = true; //DeserializationError error = deserializeJson(doc, body); @@ -324,7 +317,7 @@ void loop() { client.println("HTTP/1.1 200 OK"); client.println("Content-Type: application/json"); client.println(); - String response = "{\"sensors\":[" + String(motorSpeedRead) + "," + String(FadePercentRead) + "]}"; + String response = "{\"sensors\":[" + String(motorSpeedRead) + "," + String(FadePercentRead) + "," + String(FramesCount) + "]}"; client.print(response); } else { // Respuesta para servir la interfaz HTML diff --git a/CraterLab_camera/lib/README b/CraterLab_camera/lib/README deleted file mode 100644 index 2593a33..0000000 --- a/CraterLab_camera/lib/README +++ /dev/null @@ -1,46 +0,0 @@ - -This directory is intended for project specific (private) libraries. -PlatformIO will compile them to static libraries and link into executable file. - -The source code of each library should be placed in an own separate directory -("lib/your_library_name/[here are source files]"). - -For example, see a structure of the following two libraries `Foo` and `Bar`: - -|--lib -| | -| |--Bar -| | |--docs -| | |--examples -| | |--src -| | |- Bar.c -| | |- Bar.h -| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html -| | -| |--Foo -| | |- Foo.c -| | |- Foo.h -| | -| |- README --> THIS FILE -| -|- platformio.ini -|--src - |- main.c - -and a contents of `src/main.c`: -``` -#include -#include - -int main (void) -{ - ... -} - -``` - -PlatformIO Library Dependency Finder will find automatically dependent -libraries scanning project source files. - -More information about PlatformIO Library Dependency Finder -- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/CraterLab_camera/platformio.ini b/CraterLab_camera/platformio.ini index 6c27b7a..ff8de64 100644 --- a/CraterLab_camera/platformio.ini +++ b/CraterLab_camera/platformio.ini @@ -16,7 +16,8 @@ monitor_speed = 1000000 ;upload_port = COM12 board_build.arduino.flash_layout = 75_25 ;50_50, 75_25, 100_0 lib_deps = https://github.com/br3ttb/Arduino-PID-Library - bblanchon/ArduinoJson@^7.2.0 + https://github.com/bblanchon/ArduinoJson + ;bblanchon/ArduinoJson@^7.2.0 [env:ARDUINO_GIGA_M4] diff --git a/CraterLab_camera/test/README b/CraterLab_camera/test/README deleted file mode 100644 index 9b1e87b..0000000 --- a/CraterLab_camera/test/README +++ /dev/null @@ -1,11 +0,0 @@ - -This directory is intended for PlatformIO Test Runner and project tests. - -Unit Testing is a software testing method by which individual units of -source code, sets of one or more MCU program modules together with associated -control data, usage procedures, and operating procedures, are tested to -determine whether they are fit for use. Unit testing finds problems early -in the development cycle. - -More information about PlatformIO Unit Testing: -- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html