563 lines
22 KiB
C++
563 lines
22 KiB
C++
#include <Arduino.h>
|
|
#include <ShiftRegister74HC595.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;
|
|
bool motorIsSpeedActive = false;
|
|
int motorIntervalFrames = 1;
|
|
int motorIntervalSeconds = 1;
|
|
bool motorIsIntervalActive = false;
|
|
bool motorDirection = 1;
|
|
|
|
// Shutter
|
|
int shutterFadePercent = 0;
|
|
int shutterFadeFrames = 50;
|
|
bool shutterSyncWithInterval = false;
|
|
bool shutterFadeInActive = false;
|
|
bool shutterFadeOutActive = false;
|
|
|
|
// Óptica
|
|
int zoomValue = 50;
|
|
int focusValue = 50;
|
|
float diaphragmValue = 1.8;
|
|
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.
|
|
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 ***********************************************************
|
|
|
|
// ************************************************ 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)
|
|
ShiftRegister74HC595<1> sr(Data_pin, Clock_pin, Latch_pin);
|
|
const int EN_MOTOR[4] = {EN0, EN1, EN2, EN3};
|
|
const int MA_MOTOR[4] = {M0A, M1A, M2A, M3A};
|
|
const int MB_MOTOR[4] = {M0B, M1B, M2B, M3B};
|
|
const long TIME_LINEAL_MAX[6] = {4150, 670, 4250, 6200, 4000, 4000 }; //ms
|
|
|
|
void backward(int motor, int SPEED)
|
|
{
|
|
if(motor<4)
|
|
{
|
|
sr.set(MA_MOTOR[motor], HIGH);
|
|
sr.set(MB_MOTOR[motor], LOW);
|
|
analogWrite(EN_MOTOR[motor], SPEED);
|
|
}
|
|
else
|
|
{
|
|
analogWrite(L_M4, SPEED);
|
|
analogWrite(R_M4, 0);
|
|
}
|
|
|
|
}
|
|
|
|
void forward(int motor, int SPEED)
|
|
{
|
|
if(motor<4)
|
|
{
|
|
sr.set(MA_MOTOR[motor], LOW);
|
|
sr.set(MB_MOTOR[motor], HIGH);
|
|
analogWrite(EN_MOTOR[motor], SPEED);
|
|
}
|
|
else
|
|
{
|
|
analogWrite(L_M4, 0);
|
|
analogWrite(R_M4, SPEED);
|
|
}
|
|
|
|
}
|
|
|
|
void stop(int motor)
|
|
{
|
|
|
|
if(motor<4)
|
|
{
|
|
analogWrite(EN_MOTOR[motor], 0);
|
|
sr.set(MA_MOTOR[motor], LOW);
|
|
sr.set(MB_MOTOR[motor], LOW);
|
|
}
|
|
else
|
|
{
|
|
analogWrite(L_M4, 0);
|
|
analogWrite(R_M4, 0);
|
|
}
|
|
}
|
|
|
|
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};
|
|
float position_motor_objective[6] = { 0, 0, 0, 0, 0, 0};
|
|
int moving_position_motor[6] = { 0, 0, 0, 0, 0, 0};
|
|
|
|
void config_position_motor(int motor, float position) //Configuracion posicion en % motor
|
|
{
|
|
if((position!=position_motor[motor])&&(!moving_position_motor[motor]))
|
|
{
|
|
if(motor==0)
|
|
{
|
|
position_motor_objective[motor] = position;
|
|
Setpoint_Linear = position;
|
|
}
|
|
else
|
|
{
|
|
time_move_for_move_position_motor[motor] = (abs(position_motor[motor]-position)*TIME_LINEAL_MAX[motor])/100.;
|
|
if(position>position_motor[motor]) forward(motor,255);
|
|
else backward(motor,255);
|
|
time_refresh_position_motor[motor] = millis();
|
|
position_motor_objective[motor] = position;
|
|
moving_position_motor[motor] = true;
|
|
//RPC.println("/debug:" + String(position_motor[motor]) + "," + String(position) + "," + String(millis()-time_refresh_position_motor[motor]) + "," + String(time_move_for_move_position_motor[motor]) + "," + "INI");
|
|
//Serial.println(time_move_for_move_position_motor[motor]);
|
|
}
|
|
}
|
|
}
|
|
|
|
bool state_smotor[6] = { false, false, false, false, false, false};
|
|
|
|
void config_speed_motor(int motor, int direction, int speed) //Configuracion velocidad motor
|
|
{
|
|
closed = false;
|
|
if(direction==FORWARD) forward(motor,speed);
|
|
else if(direction==BACKWARD) backward(motor,speed);
|
|
if(speed>0) state_smotor[motor] = true;
|
|
else state_smotor[motor] = false;
|
|
}
|
|
|
|
|
|
|
|
volatile float fps_temp = 0;
|
|
unsigned long time_sec= millis();
|
|
unsigned long time_fps = micros();
|
|
|
|
static volatile unsigned long lastTimeDebounce = 0; //Tiempo del rebote
|
|
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) motorSpeedRead = fps_temp;
|
|
time_fps = micros();
|
|
if((millis()-time_sec)>=1000)
|
|
{
|
|
//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;
|
|
}
|
|
}
|
|
|
|
int interval_direction[6] = {0, 0, 0, 0, 0, 0};
|
|
int interval_frames[6] = {0, 0, 0, 0, 0, 0};
|
|
unsigned long interval_time[6] = {0, 0, 0, 0, 0, 0};
|
|
int frames_count[6] = {0, 0, 0, 0, 0, 0};
|
|
unsigned long time_interval_count[6] = {0, 0, 0, 0, 0, 0};
|
|
void config_interval_motor(int direction, int Frames, int Seconds) //Configuracion velocidad motor
|
|
{
|
|
closed = false;
|
|
interval_direction[FPS_MOTOR] = direction;
|
|
interval_frames[FPS_MOTOR] = Frames;
|
|
interval_time[FPS_MOTOR] = Seconds*1000;
|
|
frames_count[FPS_MOTOR] = 0;
|
|
time_interval_count[FPS_MOTOR] = millis();
|
|
}
|
|
|
|
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 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); //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
|
|
{
|
|
config_position_motor(1, zoomValue);
|
|
config_position_motor(2, focusValue);
|
|
config_position_motor(3, map(int(diaphragmValue*10), 19, 220, 0, 100));
|
|
}
|
|
|
|
void secure_stop(int direction)
|
|
{
|
|
config_speed_motor(FPS_MOTOR, direction, 0);
|
|
delay(150);
|
|
if (!digitalRead(sensor_fps))
|
|
{
|
|
config_speed_motor(FPS_MOTOR, direction, MIN_SPEED);
|
|
closed = true;
|
|
}
|
|
}
|
|
|
|
void next_frame(int direction)
|
|
{
|
|
config_speed_motor(FPS_MOTOR, motorDirection, MIN_SPEED);
|
|
closed = true;
|
|
}
|
|
|
|
#define NUM 20.
|
|
int read_lineal_motor()
|
|
{
|
|
int value = 0;
|
|
for (int i=0; i<10; i++) value += analogRead(sensor_lineal_motor);
|
|
return value/10;
|
|
}
|
|
|
|
void ini_motors()
|
|
{
|
|
//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()<LINEAL_MAX)&&((millis()-time_protection)<10000)) backward(0,255);
|
|
stop(0);*/
|
|
|
|
/*backward(1,255);
|
|
backward(2,255);
|
|
backward(3,255);
|
|
delay(TIME_LINEAL_MAX[3]);
|
|
stop(0);
|
|
stop(1);
|
|
stop(2);
|
|
stop(3);*/
|
|
}
|
|
|
|
void refresh_position_motors()
|
|
{
|
|
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]))
|
|
{
|
|
stop(i);
|
|
position_motor[i] = position_motor_objective[i];
|
|
moving_position_motor[i] = false;
|
|
//RPC.println("/debug:" + String(position_motor[i]) + "," + String(position_motor_objective[i]) + "," + String(millis()-time_refresh_position_motor[i]) + "," + String(time_move_for_move_position_motor[i]) + "," + "END");
|
|
}
|
|
}
|
|
}
|
|
|
|
int change_direction_inout = 1;
|
|
|
|
void refresh_interval_motors()
|
|
{
|
|
if((((millis()-time_interval_count[FPS_MOTOR])>=interval_time[FPS_MOTOR])&&(interval_frames[FPS_MOTOR]!=frames_count[FPS_MOTOR]))&&(isAction))
|
|
{
|
|
frames_count[FPS_MOTOR]++;
|
|
time_interval_count[FPS_MOTOR] = millis();
|
|
if(frames_count[FPS_MOTOR]>=interval_frames[FPS_MOTOR]) isAction=false;
|
|
if (shutterSyncWithInterval)
|
|
{
|
|
if(fade_direction==IN)
|
|
{
|
|
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)
|
|
{
|
|
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_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]);
|
|
}
|
|
}
|
|
|
|
float fps_request = 24;
|
|
uint8_t speed_request = 0;
|
|
unsigned long time_adjust = millis();
|
|
|
|
/*
|
|
Functions on the M4 that returns FPS
|
|
*/
|
|
int fpsRead() {
|
|
int result = motorSpeedRead;
|
|
return result;
|
|
}
|
|
|
|
int read_value[10];
|
|
int* decode_values(String inputString_rpc, int num_dec)
|
|
{
|
|
String str = inputString_rpc.substring(inputString_rpc.lastIndexOf(":") + 1);
|
|
read_value[0] = str.substring(0, str.indexOf(',')).toInt();
|
|
for(int i=1; i<num_dec; i++)
|
|
{
|
|
str = str.substring(str.indexOf(',')+1);
|
|
read_value[i] = str.substring(0, str.indexOf(',')).toInt();
|
|
}
|
|
return read_value;
|
|
}
|
|
|
|
char c_rpc;
|
|
String inputString_rpc;
|
|
unsigned long time_led = millis();
|
|
|
|
void RPCRead()
|
|
{
|
|
////////////////////////////////////////////////////////////
|
|
/////// RUTINA TRATAMIENTO DE STRINGS DEL UART ///////////
|
|
////////////////////////////////////////////////////////////
|
|
if (RPC.available())
|
|
{
|
|
c_rpc = RPC.read();
|
|
if ((c_rpc == '\r') || (c_rpc == '\n'))
|
|
{
|
|
digitalWrite(LED_BUILTIN, LOW);
|
|
if (inputString_rpc.startsWith("/s_motor:")) //Speed Motor
|
|
{
|
|
int* value = decode_values(inputString_rpc, 4);
|
|
#if DEBUG_M4
|
|
RPC.print("Recibido Speed M4: ");
|
|
RPC.println(inputString_rpc);
|
|
for(int i=0; i<4; i++) RPC.println(value[i]);
|
|
#endif
|
|
motorDirection = value[1];
|
|
motorSpeedValue = value[2];
|
|
motorIsSpeedActive = true;
|
|
motorIsIntervalActive = false;
|
|
//if (motorSpeedValue>0) motorSpeedValue = map(motorSpeedValue, 24, 48, 146, 192); //Fake fps
|
|
if (motorSpeedValue>0) motorSpeedValue = map(motorSpeedValue, 0, 48, 0, 255); //Fake fps
|
|
//if (motorSpeedValue>0) motorSpeedValue = 255;
|
|
if (value[3]==false) //Si es falso, se ejecuta, sino se guarda sin ejecutar
|
|
{
|
|
if (motorSpeedValue==0)
|
|
{
|
|
isAction = false;
|
|
secure_stop(motorDirection);
|
|
}
|
|
else config_speed_motor(value[0], motorDirection, motorSpeedValue);
|
|
}
|
|
}
|
|
else if (inputString_rpc.startsWith("/i_motor:")) //Interval Motor
|
|
{
|
|
int* value = decode_values(inputString_rpc, 5);
|
|
#if DEBUG_M4
|
|
RPC.print("Recibido Interval M4: ");
|
|
RPC.println(inputString_rpc);
|
|
for(int i=0; i<5; i++) RPC.println(value[i]);
|
|
#endif
|
|
motorDirection = value[1];
|
|
motorIntervalFrames = value[2];
|
|
motorIntervalSeconds = value[3];
|
|
motorIsIntervalActive = true;
|
|
motorIsSpeedActive = false;
|
|
if (value[4]==false) //Si es falso, se ejecuta, sino se guarda sin ejecutar
|
|
{
|
|
isAction = true;
|
|
config_interval_motor(motorDirection, motorIntervalFrames, motorIntervalSeconds);
|
|
}
|
|
|
|
}
|
|
else if (inputString_rpc.startsWith("/fade:")) //Fade configuration
|
|
{
|
|
int* value = decode_values(inputString_rpc, 6);
|
|
#if DEBUG_M4
|
|
RPC.print("Recibido fade M4: ");
|
|
RPC.println(inputString_rpc);
|
|
for(int i=0; i<6; i++) RPC.println(value[i]);
|
|
#endif
|
|
shutterFadePercent = value[0];
|
|
shutterFadeFrames = value[1];
|
|
shutterSyncWithInterval = value[2];
|
|
shutterFadeInActive = value[3];
|
|
shutterFadeOutActive = value[4];
|
|
//RPC.println("/fade:" + String(shutterFadePercent) + "," + String(shutterFadeFrames) + "," + String(shutterSyncWithInterval) + "," + String(shutterFadeInActive) + "," + String(shutterFadeOutActive));
|
|
config_position_motor(0, shutterFadePercent);
|
|
}
|
|
else if (inputString_rpc.startsWith("/optics:")) //Optics configuration
|
|
{
|
|
int* value = decode_values(inputString_rpc, 5);
|
|
#if DEBUG_M4
|
|
RPC.print("Recibido optics M4: ");
|
|
RPC.println(inputString_rpc);
|
|
for(int i=0; i<5; i++) RPC.println(value[i]);
|
|
#endif
|
|
zoomValue = value[0];
|
|
focusValue = value[1];
|
|
diaphragmValue = value[2];
|
|
syncWithIntervalOptics = value[3];
|
|
if (value[4]==false) //Si es falso, se ejecuta, sino se guarda sin ejecutar
|
|
{
|
|
config_optics(zoomValue, focusValue, diaphragmValue, syncWithIntervalOptics);
|
|
}
|
|
}
|
|
else if (inputString_rpc.startsWith("/action:")) //Optics configuration
|
|
{
|
|
int* value = decode_values(inputString_rpc, 1);
|
|
#if DEBUG_M4
|
|
RPC.print("Recibido action M4: ");
|
|
RPC.println(inputString_rpc);
|
|
for(int i=0; i<1; i++) RPC.println(value[i]);
|
|
#endif
|
|
isAction=value[0];
|
|
if (isAction)
|
|
{
|
|
if (motorIsSpeedActive) config_speed_motor(4, motorDirection, motorSpeedValue);
|
|
else config_interval_motor(motorDirection, motorIntervalFrames, motorIntervalSeconds);
|
|
config_automatic_fade(shutterFadeFrames, shutterSyncWithInterval, shutterFadeInActive, shutterFadeOutActive);
|
|
}
|
|
else
|
|
{
|
|
secure_stop(motorDirection);
|
|
}
|
|
}
|
|
else if (inputString_rpc.startsWith("/sensors:")) //Optics configuration
|
|
{
|
|
int* value = decode_values(inputString_rpc, 1);
|
|
#if DEBUG_M4
|
|
RPC.print("Recibido fps M4: ");
|
|
RPC.println(inputString_rpc);
|
|
for(int i=0; i<1; i++) RPC.println(value[i]);
|
|
#endif
|
|
if(value[0])
|
|
//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);
|
|
}
|
|
else
|
|
inputString_rpc += c_rpc;
|
|
}
|
|
}
|
|
|
|
void setup() {
|
|
//SCB_CleanDCache();
|
|
RPC.begin();
|
|
//delay(5000);
|
|
pinMode(sensor_fps, INPUT_PULLUP);
|
|
pinMode(Enable, OUTPUT);
|
|
pinMode(EN_M4, OUTPUT);
|
|
digitalWrite(EN_M4, HIGH);
|
|
analogWriteResolution(8);
|
|
analogReadResolution(12);
|
|
digitalWrite(Enable, LOW);
|
|
sr.setAllLow(); // set all pins LOW
|
|
ini_motors();
|
|
secure_stop(FORWARD);
|
|
attachInterrupt(digitalPinToInterrupt(sensor_fps), fps_count_state, RISING);
|
|
|
|
#if PID_ENABLE
|
|
outMax = 255.0; // Límite máximo del controlador PID.
|
|
outMin = 128.0; // Límite mínimo del controlador PID.
|
|
|
|
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;
|
|
|
|
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 = 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
|
|
RPC.println("M4 Inicializado.");
|
|
#endif
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
refresh_position_motors();
|
|
refresh_interval_motors();
|
|
RPCRead();
|
|
if(force_stop)
|
|
{
|
|
stop(FPS_MOTOR);
|
|
force_stop = false;
|
|
}
|
|
} |