miguel/Analogue_Hyperlapse_Camera
1
0
Fork 0
Code Issues Pull requests Actions Packages Projects Releases Wiki Activity

Adquirido PID en el control del actuador Lineal

Browse source
This commit is contained in:
Miguel Angel de Heras 2025-03-03 16:45:06 +01:00
parent 16cd3e3bd1
commit a7e45b6cb7
2 changed files with 81 additions and 61 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

136
CraterLab_camera/include/main_m4.cpp
View file

@ -1,12 +1,13 @@
#include <Arduino.h> #include <Arduino.h>
#include <ShiftRegister74HC595.h> #include <ShiftRegister74HC595.h>
//#include <PID_v1.h> #include <PID_v1.h>
//#include <SPI.h> //#include <SPI.h>
#include <RPC.h> #include <RPC.h>
#include "constants.h" #include "constants.h"
#define DEBUG_M4 false #define DEBUG_M4 false
#define MIN_SPEED 55 #define MIN_SPEED 55
#define PID_ENABLE true
// Motor // Motor
int motorSpeedValue = 24; 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 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. double outMax = 0.0, outMin = 0.0; // Límites para no sobrepasar la resolución del PWM.
// **************************************************** Otras Variables *********************************************************** // **************************************************** 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.) // ************************************************ Variables PID *****************************************************************
byte cmd = 0; // Un byte que utilizamos para la comunicación serie. (cmd=comando.) 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.
unsigned int tmp = 0; // Variable que utilizaremos para poner el tiempo de muestreo. double kp_Linear = 0.0, ki_Linear = 0.0, kd_Linear = 0.0; // Constante proporcional, integral y derivativa.
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. 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 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 #endif
// parameters: <number of shift registers> (data pin, clock pin, latch pin) // parameters: <number of shift registers> (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}; 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()}; 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[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(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; position_motor_objective[motor] = position;
Setpoint_Linear = position;
} }
else else
{ {
@ -206,15 +206,22 @@ void config_interval_motor(int direction, int Frames, int Seconds) //Configuraci
} }
float inc_position_fade = 0; float inc_position_fade = 0;
float fade_position_interval = 0;
int fade_direction = 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; if((shutterFadeInActive)&&(!shutterFadeOutActive)) fade_direction=IN;
else if((!shutterFadeInActive)&&(shutterFadeOutActive)) fade_direction=OUT; else if((!shutterFadeInActive)&&(shutterFadeOutActive)) fade_direction=OUT;
else if((shutterFadeInActive)&&(shutterFadeOutActive)) fade_direction=IN_OUT; else if((shutterFadeInActive)&&(shutterFadeOutActive)) fade_direction=IN_OUT;
else fade_direction=STOP; 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 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() void ini_motors()
{ {
//forward(0,255); //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); while ((read_lineal_motor()>LINEAL_MAX)&&((millis()-time_protection)<10000)) forward(0,255);
stop(0); stop(0);
while ((read_lineal_motor()<LINEAL_MAX)&&((millis()-time_protection)<10000)) backward(0,255); while ((read_lineal_motor()<LINEAL_MAX)&&((millis()-time_protection)<10000)) backward(0,255);
stop(0); stop(0);*/
/*backward(1,255); /*backward(1,255);
backward(2,255); backward(2,255);
@ -270,12 +277,21 @@ void ini_motors()
void refresh_position_motors() void refresh_position_motors()
{ {
position_motor[0] = map(read_lineal_motor(),LINEAL_MAX,LINEAL_MIN,0,100); position_motor[0] = map(read_lineal_motor(),LINEAL_MAX,LINEAL_MIN,0,100000)/1000;
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))) #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); stop(0);
moving_position_motor[0] = 0; moving_position_motor[0] = 0;
} }*/
for(int i=1; i<4; i++) 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])) 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(frames_count[FPS_MOTOR]>=interval_frames[FPS_MOTOR]) isAction=false;
if (shutterSyncWithInterval) if (shutterSyncWithInterval)
{ {
float value_position=0;
if(fade_direction==IN) if(fade_direction==IN)
{ {
value_position = position_motor[0] + inc_position_fade; fade_position_interval += inc_position_fade;
if (value_position>100) value_position=100; if (fade_position_interval>100) fade_position_interval=100;
config_position_motor(0, value_position); config_position_motor(0, fade_position_interval);
} }
else if(fade_direction==OUT) else if(fade_direction==OUT)
{ {
value_position = position_motor[0] - inc_position_fade; fade_position_interval -= inc_position_fade;
if (value_position<0) value_position=0; if (fade_position_interval<0) fade_position_interval=0;
config_position_motor(0, value_position); config_position_motor(0, fade_position_interval);
} }
else if(fade_direction==IN_OUT) else if(fade_direction==IN_OUT)
{ {
if (((position_motor[0] + inc_position_fade)>100)&&(change_direction_inout==1)) change_direction_inout=-1; if (((position_motor_objective[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; else if (((position_motor_objective[0] - inc_position_fade)<0)&&(change_direction_inout==-1)) change_direction_inout=1;
value_position = position_motor[0] + change_direction_inout*inc_position_fade; fade_position_interval += 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, fade_position_interval);
config_position_motor(0, value_position);
} }
//else value_position = position_motor[0]; //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]); next_frame(interval_direction[FPS_MOTOR]);
} }
@ -506,17 +521,28 @@ void setup() {
outMax = 255.0; // Límite máximo del controlador PID. outMax = 255.0; // Límite máximo del controlador PID.
outMin = 128.0; // Límite mínimo 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, 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.) 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; 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.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.SetTunings(kp, ki, kd); // Constantes de sintonización.
myPID.SetMode(AUTOMATIC); // Habilita el control PID (por defecto). 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 #endif
#if DEBUG_M4 #if DEBUG_M4
@ -525,21 +551,13 @@ void setup() {
} }
void loop() 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); refresh_position_motors();
//delay(20); refresh_interval_motors();
stop(FPS_MOTOR); RPCRead();
force_stop = false; if(force_stop)
} {
} stop(FPS_MOTOR);
force_stop = false;
}
}

6
CraterLab_camera/platformio.ini
View file

@ -17,6 +17,7 @@ monitor_speed = 1000000
board_build.arduino.flash_layout = 75_25 ;50_50, 75_25, 100_0 board_build.arduino.flash_layout = 75_25 ;50_50, 75_25, 100_0
lib_deps = https://github.com/br3ttb/Arduino-PID-Library lib_deps = https://github.com/br3ttb/Arduino-PID-Library
https://github.com/bblanchon/ArduinoJson https://github.com/bblanchon/ArduinoJson
https://github.com/Simsso/ShiftRegister74HC595
;bblanchon/ArduinoJson@^7.2.0 ;bblanchon/ArduinoJson@^7.2.0
@ -27,5 +28,6 @@ framework = arduino
monitor_speed = 1000000 monitor_speed = 1000000
;upload_port = COM12 ;upload_port = COM12
board_build.arduino.flash_layout = 75_25 ;50_50, 75_25, 100_0 board_build.arduino.flash_layout = 75_25 ;50_50, 75_25, 100_0
lib_deps = https://github.com/Simsso/ShiftRegister74HC595 lib_deps = https://github.com/br3ttb/Arduino-PID-Library
https://github.com/br3ttb/Arduino-PID-Library https://github.com/bblanchon/ArduinoJson
https://github.com/Simsso/ShiftRegister74HC595