Buenas noches.
Estoy trabajando en el control de un motor, y para ello he creado una librería (la llamaremos Motor) con algunas funciones básicas. Entre ellas, me gustaría poder asignar un control PID a cada motor. Para ello, pensaba usar la libreria que ofrece Arduino "por defecto". Dicha librería crea un objeto del tipo PID.
Mi intención es que, dentro de la librería MOTOR, se cree un objeto del tipo PID. Para empezar, dentro del archivo "Motor.h" he definido el PID.
#ifndef MOTOR_H
#define MOTOR_H
#include <Arduino.h>
#include <PID_v1.h>
class Motor {
public:
Motor (double KP, double KD, double KI); //Constructor
. . .
PID pid;
. . .
};
#endif
Después, lo he inicializado en "Motor.cpp" mediante "Initialiser Lists". Os dejo el fragmento de código a continuación.
#include "Motor.h"
#include "PID_v1.h"
Motor::Motor (double KP, double KD, double KI) : pid(&omega_s, &POWER, &consigna, KP, KI, KD, DIRECT)
{
. . .
}
Las variables consigna, POWER y omega_s están definidas en "Motor.h", y son del tipo double. En el programa principal, puedo acceder (una vez creado el objeto Motor1) a algunas variables del PID así (y funciona).
double KP=Motor1.pid.GetKp();
double KD=Motor1.pid.GetKd();
double KI=Motor1.pid.GetKi();
Sin embargo, el PID no se ejecuta (devuelve 0). Es como si las variables POWER, consigna y omega_s no se pasaran correctamente. Al compilar no salta ningún error. Soy novato en clases y librerías, y probablemente mi explicación sea vaga e imprecisa, pero alguien puede entrever el fallo? Muchas gracias.
EDITO: Subo los archivos, la clase y un ejemplo de implementación. El sketch_feb20a es un ejemplo de implementación de código, con un array de 1 solo elemento del tipo Motor.
Archivo .cpp
#include "Motor.h"
#include "PID_v1.h"
//Constructor
Motor::Motor (int P, int A, int B, int E1, int E2, double KP, double KD, double KI, double C) : pid(&omega_s, &POWER, &consigna, KP, KI, KD, DIRECT)
{
PWM = P;
PIN_A = A;
PIN_B = B;
ENC1 = E1;
ENC2 = E2;
consigna = C;
pid.SetOutputLimits(60, 255);
pid.SetSampleTime(38);
pid.SetMode(AUTOMATIC);
}
void Motor::SetForward() {
digitalWrite(PIN_A, 1);
digitalWrite(PIN_B, 0);
}
void Motor::SetBackward() {
digitalWrite(PIN_A, 0);
digitalWrite(PIN_B, 1);
}
void Motor::SetSpeed(double w) {
pid.SetMode(AUTOMATIC);
consigna = w;
}
void Motor::SetDuty(int w) {
if (w >= 0 && w < 256) {
pid.SetMode(MANUAL);
analogWrite(PWM, w);
}
}
void Motor::ENCODER() {
S = (digitalRead(ENC1)^digitalRead(ENC2));
if (S != S_ant) {
count++;
S_ant = S;
}
n++;
READ();
}
void Motor::READ() {
if (n > 125) { //40 lectures per segon. Si augmentam les lectures, disminuex la precisió a baixes RMP'S
omega = (C / (float(n))) * float(count);
count = 0; //Comptador igual a zero
omega_s = digitalSmooth((int)omega, Array1); //digitalSmooth está definida, pero es muy engorrosa y no pinta nada aquí
n = 0;
pid.Compute();
//analogWrite(PWM, POWER);
}
}
Archivo .h
#ifndef MOTOR_H
#define MOTOR_H
#include <Arduino.h>
#include <PID_v1.h>
class Motor {
public:
// DEFINICIÓ DE PINS PWM DELS MOTORS
int PWM;
// DEFINICIÓ DELS PINS LÒGICS DELS MOTORS
int PIN_A, PIN_B, ENC1, ENC2;
//Constants PID
double consigna; //Consigna de RPM donada pel LIDAR
//Variables Encoder
volatile int n = 0;
volatile int count = 0;
volatile bool S = 0;
volatile bool S_ant = 0;
volatile float omega = 0;
double omega_s = 0;
//Constants PID
double POWER = 0;
const int t = 300; //Temps interrupció lectura 0.3 ms
const float t_s = (float(t) / 1000000); //Temps en segons
const float C = 0.03485 / t_s; //Constant del motor
int Array1 [10]; //array
PID pid;
Motor (int P, int A, int B, int E1, int E2, double KP, double KD, double KI, double C);
void ENCODER();
void SetForward();
void SetBackward();
void SetDuty(int w);
void SetSpeed(double w);
void READ();
int digitalSmooth(int rawIn, int *sensSmoothArray);
private:
};
#endif
Ejemplo.
#include <TimerOne.h>
#include <Motor.h>
Motor M[1] = {
Motor(4, 32, 33, 50,51, 4, 28, 0.3, 28)
};
void setup() {
Serial.begin(9600);
for (int i = 0; i < 1; i++) { //Posa cap envant tots els motors
M[i].SetForward();
}
Timer1.initialize (300);
Timer1.attachInterrupt(ENCODER);
}
void loop() {
Serial.println(M[0].POWER);
}
void ENCODER() {
for (int i = 0; i < 1; i++) {
M[i].ENCODER();
}
}
Motor.cpp (2.69 KB)
Motor.h (1.05 KB)
sketch_feb20a.ino (448 Bytes)