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 <ShiftRegister74HC595.h>
//#include <PID_v1.h>
#include <PID_v1.h>
//#include <SPI.h>
#include <RPC.h>
#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: <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};
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()<LINEAL_MAX)&&((millis()-time_protection)<10000)) backward(0,255);
stop(0);
stop(0);*/
/*backward(1,255);
backward(2,255);
@ -270,12 +277,21 @@ void ini_motors()
void refresh_position_motors()
{
position_motor[0] = map(read_lineal_motor(),LINEAL_MAX,LINEAL_MIN,0,100);
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)))
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;
}
}
refresh_position_motors();
refresh_interval_motors();
RPCRead();
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
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
lib_deps = https://github.com/br3ttb/Arduino-PID-Library
https://github.com/bblanchon/ArduinoJson
https://github.com/Simsso/ShiftRegister74HC595