From a7e45b6cb7924fd880c0aeb58a9397277a5717f8 Mon Sep 17 00:00:00 2001 From: Miguel Angel de Heras Date: Mon, 3 Mar 2025 16:45:06 +0100 Subject: [PATCH] Adquirido PID en el control del actuador Lineal --- CraterLab_camera/include/main_m4.cpp | 136 +++++++++++++++------------ CraterLab_camera/platformio.ini | 6 +- 2 files changed, 81 insertions(+), 61 deletions(-) diff --git a/CraterLab_camera/include/main_m4.cpp b/CraterLab_camera/include/main_m4.cpp index 949f45e..afb2014 100644 --- a/CraterLab_camera/include/main_m4.cpp +++ b/CraterLab_camera/include/main_m4.cpp @@ -1,12 +1,13 @@ #include #include -//#include +#include //#include #include #include "constants.h" #define DEBUG_M4 false #define MIN_SPEED 55 +#define PID_ENABLE true // Motor int motorSpeedValue = 24; @@ -42,14 +43,15 @@ int FramesCount = 0; double kp = 0.0, ki = 0.0, kd = 0.0; // Constante proporcional, integral y derivativa. double outMax = 0.0, outMin = 0.0; // Límites para no sobrepasar la resolución del PWM. // **************************************************** Otras Variables *********************************************************** - volatile long contador = 0; // En esta variable se guardará los pulsos del encoder y que interpreremos como distancia (o ángulo si ese fuese el caso). - byte ant = 0, act = 0; // Sólo se utiliza los dos primeros bits de estas variables y servirán para decodificar el encoder. (ant=anterior, act=actual.) - byte cmd = 0; // Un byte que utilizamos para la comunicación serie. (cmd=comando.) - unsigned int tmp = 0; // Variable que utilizaremos para poner el tiempo de muestreo. - const byte ledok = 13; // El pin 13 de los Arduinos tienen un led que utilizo para mostrar que el motor ya ha llegado a la posición designada. - // ******************************************************************************************************************************** + + // ************************************************ Variables PID ***************************************************************** + double Setpoint_Linear = 0.0, Input_Linear = 0.0, Output_Linear = 0.0; // Setpoint=Posición designada; Input=Posición del motor; Output=Tensión de salida para el motor. + double kp_Linear = 0.0, ki_Linear = 0.0, kd_Linear = 0.0; // Constante proporcional, integral y derivativa. + double outMax_Linear = 0.0, outMin_Linear = 0.0; // Límites para no sobrepasar la resolución del PWM. + // **************************************************** Otras Variables *********************************************************** PID myPID(&Input, &Output, &Setpoint, 0.0, 0.0, 0.0, DIRECT); // Parámetros y configuración para invocar la librería. + PID PID_LINEAR(&Input_Linear, &Output_Linear, &Setpoint_Linear, 0.0, 0.0, 0.0, DIRECT); // Parámetros y configuración para invocar la librería. #endif // parameters: (data pin, clock pin, latch pin) @@ -107,6 +109,13 @@ void stop(int motor) } } +void linear_motor_control(int motor, int SPEED) + { + if(SPEED>0) forward(motor, SPEED); + else if(SPEED<0) backward(motor, abs(SPEED)); + else stop(motor); + } + 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}; @@ -119,17 +128,8 @@ void config_position_motor(int motor, float position) //Configuracion posicion e { if(motor==0) { - 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; + Setpoint_Linear = position; } else { @@ -206,15 +206,22 @@ void config_interval_motor(int direction, int Frames, int Seconds) //Configuraci } float inc_position_fade = 0; +float fade_position_interval = 0; int fade_direction = 0; -void config_automatic_fade(int shutterFadeFrames, int shutterSyncWithInterval, int shutterFadeInActive, int shutterFadeOutActive) //Configuracion fade +void config_automatic_fade(int shutterFadeFrames, int shutterSyncWithInterval, int shutterFadeInActive, int shutterFadeOutActive) //Configuracion del fade cuando esta sincronizado con el intervalometro { 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); + + if(fade_direction!=IN_OUT) inc_position_fade = (100)/((float)shutterFadeFrames); //Si es fade in o out se divide el incremeto entre el numero de frames + else inc_position_fade = 2*(100)/((float)shutterFadeFrames); //Si es fade in y out se divide el incremeto entre los dos + + //Proteccion para valores fuera de rango + if (position_motor[0]<0) fade_position_interval = 0; + else if (position_motor[0]>100) fade_position_interval = 100; + else fade_position_interval = position_motor[0]; } void config_optics(int zoomValue, int focusValue, int diaphragmValue, int syncWithIntervalOptics) //Configuracion de las opticas @@ -252,11 +259,11 @@ int read_lineal_motor() void ini_motors() { //forward(0,255); - unsigned long time_protection = millis(); + /*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))) + position_motor[0] = map(read_lineal_motor(),LINEAL_MAX,LINEAL_MIN,0,100000)/1000; + #if PID_ENABLE + Input_Linear = position_motor[0]; + PID_LINEAR.Compute(); + linear_motor_control(0, Output_Linear); + /*Input = (double)fps_temp; // Lectura del encoder óptico. El valor del contador se incrementa/decrementa a través de las interrupciones extrenas (pines 2 y 3). + if(myPID.Compute()) + analogWrite(EN_MOTOR[4], abs(Output)); // Por el primer pin sale la señal PWM. + */ + #endif + /*if(((position_motor[0]>=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] = 0; - } + }*/ for(int i=1; i<4; i++) { if(((millis()-time_refresh_position_motor[i])>(unsigned long)time_move_for_move_position_motor[i])&&(moving_position_motor[i])) @@ -299,29 +315,28 @@ void refresh_interval_motors() if(frames_count[FPS_MOTOR]>=interval_frames[FPS_MOTOR]) isAction=false; if (shutterSyncWithInterval) { - float value_position=0; if(fade_direction==IN) { - value_position = position_motor[0] + inc_position_fade; - if (value_position>100) value_position=100; - config_position_motor(0, value_position); + fade_position_interval += inc_position_fade; + if (fade_position_interval>100) fade_position_interval=100; + config_position_motor(0, fade_position_interval); } else if(fade_direction==OUT) { - value_position = position_motor[0] - inc_position_fade; - if (value_position<0) value_position=0; - config_position_motor(0, value_position); + fade_position_interval -= inc_position_fade; + if (fade_position_interval<0) fade_position_interval=0; + config_position_motor(0, fade_position_interval); } else if(fade_direction==IN_OUT) { - if (((position_motor[0] + inc_position_fade)>100)&&(change_direction_inout==1)) change_direction_inout=-1; - else if (((position_motor[0] - inc_position_fade)<0)&&(change_direction_inout==-1)) change_direction_inout=1; - value_position = position_motor[0] + change_direction_inout*inc_position_fade; - RPC.println("/fade:" + String(position_motor[0]) + "," + String(change_direction_inout) + "," + String(inc_position_fade) + "," + String(0) + "," + String(shutterFadeOutActive)); - config_position_motor(0, value_position); + if (((position_motor_objective[0] + inc_position_fade)>100)&&(change_direction_inout==1)) change_direction_inout=-1; + else if (((position_motor_objective[0] - inc_position_fade)<0)&&(change_direction_inout==-1)) change_direction_inout=1; + fade_position_interval += change_direction_inout*inc_position_fade; + config_position_motor(0, fade_position_interval); } //else value_position = position_motor[0]; - + //RPC.println("/debug:" + String(position_motor[0]) + "," + String(fade_position_interval)+ "," + String(inc_position_fade)); + } next_frame(interval_direction[FPS_MOTOR]); } @@ -506,17 +521,28 @@ void setup() { outMax = 255.0; // Límite máximo del controlador PID. outMin = 128.0; // Límite mínimo del controlador PID. - tmp = 5; // Tiempo de muestreo en milisegundos. - kp = 10.0; // Constantes PID iniciales. Los valores son los adecuados para un encoder de 334 ppr, ki = 3.5; // pero como el lector de encoder está diseñado como x4 equivale a uno de 1336 ppr. (ppr = pulsos por revolución.) kd = 30.0; - myPID.SetSampleTime(tmp); // Envía a la librería el tiempo de muestreo. + outMax_Linear = 255.0; // Límite máximo del controlador PID. + outMin_Linear = -255.0; // Límite mínimo del controlador PID. + + kp_Linear = 10.0; // Constantes PID iniciales. Los valores son los adecuados para un encoder de 334 ppr, + ki_Linear = 0.0; // pero como el lector de encoder está diseñado como x4 equivale a uno de 1336 ppr. (ppr = pulsos por revolución.) + kd_Linear = 10.0; + + myPID.SetSampleTime(5); // Envía a la librería el tiempo de muestreo en milisegundos. myPID.SetOutputLimits(outMin, outMax);// Límites máximo y mínimo; corresponde a Max.: 0=0V hasta 255=5V (PWMA), y Min.: 0=0V hasta -255=5V (PWMB). Ambos PWM se convertirán a la salida en valores absolutos, nunca negativos. myPID.SetTunings(kp, ki, kd); // Constantes de sintonización. myPID.SetMode(AUTOMATIC); // Habilita el control PID (por defecto). - Setpoint = 24; // Para evitar que haga cosas extrañas al inciarse, igualamos los dos valores para que comience estando el motor parado. + Setpoint = 100; // Para evitar que haga cosas extrañas al inciarse, igualamos los dos valores para que comience estando el motor parado. + + PID_LINEAR.SetSampleTime(1); // Envía a la librería el tiempo de muestreo en milisegundos. + PID_LINEAR.SetOutputLimits(outMin_Linear, outMax_Linear);// Límites máximo y mínimo; corresponde a Max.: 0=0V hasta 255=5V (PWMA), y Min.: 0=0V hasta -255=5V (PWMB). Ambos PWM se convertirán a la salida en valores absolutos, nunca negativos. + PID_LINEAR.SetTunings(kp_Linear, ki_Linear, kd_Linear); // Constantes de sintonización. + PID_LINEAR.SetMode(AUTOMATIC); // Habilita el control PID (por defecto). + Setpoint_Linear = 0.0; // Para evitar que haga cosas extrañas al inciarse, igualamos los dos valores para que comience estando el motor parado. #endif #if DEBUG_M4 @@ -525,21 +551,13 @@ void setup() { } void loop() -{ - #if PID_ENABLE - /*Input = (double)fps_temp; // Lectura del encoder óptico. El valor del contador se incrementa/decrementa a través de las interrupciones extrenas (pines 2 y 3). - if(myPID.Compute()) - analogWrite(EN_MOTOR[4], abs(Output)); // Por el primer pin sale la señal PWM. - */ - #endif - refresh_position_motors(); - refresh_interval_motors(); - RPCRead(); - if(force_stop) { - //config_speed_motor(FPS_MOTOR, !motorDirection, MIN_SPEED); - //delay(20); - stop(FPS_MOTOR); - force_stop = false; - } -} \ No newline at end of file + refresh_position_motors(); + refresh_interval_motors(); + RPCRead(); + if(force_stop) + { + stop(FPS_MOTOR); + force_stop = false; + } + } \ No newline at end of file diff --git a/CraterLab_camera/platformio.ini b/CraterLab_camera/platformio.ini index ff8de64..fab21a9 100644 --- a/CraterLab_camera/platformio.ini +++ b/CraterLab_camera/platformio.ini @@ -17,6 +17,7 @@ monitor_speed = 1000000 board_build.arduino.flash_layout = 75_25 ;50_50, 75_25, 100_0 lib_deps = https://github.com/br3ttb/Arduino-PID-Library https://github.com/bblanchon/ArduinoJson + https://github.com/Simsso/ShiftRegister74HC595 ;bblanchon/ArduinoJson@^7.2.0 @@ -27,5 +28,6 @@ framework = arduino monitor_speed = 1000000 ;upload_port = COM12 board_build.arduino.flash_layout = 75_25 ;50_50, 75_25, 100_0 -lib_deps = https://github.com/Simsso/ShiftRegister74HC595 - https://github.com/br3ttb/Arduino-PID-Library \ No newline at end of file +lib_deps = https://github.com/br3ttb/Arduino-PID-Library + https://github.com/bblanchon/ArduinoJson + https://github.com/Simsso/ShiftRegister74HC595 \ No newline at end of file