Revision 0:f46e78766b9c, committed 2021-09-03

Comitter:

CCastrop1012

Date:

Fri Sep 03 05:13:46 2021 +0000

Commit message:

Modelo de sistema de control para un motor.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.cpp	Fri Sep 03 05:13:46 2021 +0000
@@ -0,0 +1,113 @@
+#include "mbed.h"
+#include "Motor.h"
+
+
+MotorContinuo::MotorContinuo(PinName _L1, PinName _L2, PinName _speedPin, PinName _encodin, PinName _PosInicial, int _EncodPulses) :
+L1(_L1), L2(_L2), speedPin(_speedPin), encodin(_encodin), PosInicial(_PosInicial), EncodPulses(_EncodPulses)
+{
+    speedPin.period_ms(2);
+    speedPin.write(0);
+    
+};
+
+void MotorContinuo::Forward()   { L1=1;  L2=0;}
+void MotorContinuo::Back()      { L1=0;  L2=1;}
+void MotorContinuo::Stop()      { L1=0;  L2=0;}
+void MotorContinuo::StopT()     { L1=1;  L2=1;}
+void MotorContinuo::SpeedDuty(float v) { speedPin.write(v/float(100));}
+void MotorContinuo::SpinLength_ms(float t)    { t++;}// Duración del giro en ms
+void MotorContinuo::SpinLength(float t)       { t++;};
+    
+
+
+MotorDiscreto::MotorDiscreto(PinName _Dir, PinName _Step, int _NPasos, PinName _encodin, PinName _PosInicial, int _EncodPulses) :
+Dir(_Dir), Step(_Step), NPasos(_NPasos), encodin(_encodin), PosInicial(_PosInicial), EncodPulses(_EncodPulses)
+{
+    
+    
+
+};    
+    
+void MotorDiscreto::moveMotor(void)  
+    {
+        if (StepOnHold != 0) 
+            {
+                Step = !Step; // Se hace elcambio de estado en el pin Step
+                entradas++; // se registra cada paso efectivo el cual es valido cada 2 entradas 
+                            // 1ra entrada: tiempo en Bajo
+                            // 2da entrada: tiempo en Alto
+                            // Es decir cuando Step cumple un periodo.
+                if (entradas >= 2) // cuando se registran 2 entradas se disminuye un paso
+                {
+                    StepOnHold--; // Se elimina 1 paso de los pendientes
+                    entradas = 0; 
+                }
+                
+                Move.attach(callback(this,&MotorDiscreto::moveMotor), (TStep/2) ); // Reiniciamos el tiempo de imterrupcion
+                
+            }
+        else if (StepOnHold == 0)
+            {
+                Move.detach();
+            }
+    }
+
+
+void MotorDiscreto::Forward()   { Dir=1;}
+void MotorDiscreto::Back()      { Dir=0;}
+void MotorDiscreto::Stop()      { EnablePin = 0; }
+void MotorDiscreto::StopT()     { Move.detach();}
+void MotorDiscreto::StepFreq(int n) { StepsBySecond = n; TStep = (1/n);}
+void MotorDiscreto::RunStep(long n){ StepOnHold = n; Move.attach(callback(this,&MotorDiscreto::moveMotor), (TStep/2) ); }// Duración del giro en ms
+void MotorDiscreto::Run(float t)    { StepOnHold = long(t/TStep); Move.attach(callback(this,&MotorDiscreto::moveMotor), (TStep/2) ); }
+void MotorDiscreto::RunRound(int n) { StepOnHold = (NPasos * n); Move.attach(callback(this,&MotorDiscreto::moveMotor), (TStep/2) ); }
+
+void Ustep(int resolucion, DigitalOut* _M0, DigitalOut* _M1, DigitalOut* _M2)
+    {
+        
+        switch(resolucion)
+        {
+            case 1: *_M0 = 0; *_M1 = 0; *_M2 = 0;
+                    break;
+            case 2: *_M0 = 1; *_M1 = 0; *_M2 = 0;
+                    break;
+            case 4: *_M0 = 0; *_M1 = 1; *_M2 = 0;
+                    break; 
+            case 8: *_M0 = 1; *_M1 = 1; *_M2 = 1;
+                    break;
+            case 16: *_M0 = 0; *_M1 = 0; *_M2 = 1;
+                    break;
+            case 32: *_M0 = 1; *_M1 = 0; *_M2 = 1;
+                    break;
+        
+            default: *_M0 = 0; *_M1 = 0; *_M2 = 0;
+        
+        
+        }
+    
+    
+    }
+        
+    
+ /*   
+void scolor_TCS3200::SetMode(uint8_t mode) {
+    switch (mode){
+        case SCALE_100:  _s0= 1; _s1=1; break;
+        case SCALE_20:   _s0=1 ; _s1=0; break;
+        case SCALE_2:    _s0=0 ; _s1=1; break;
+        case POWER_DOWN: _s0=0 ; _s1=0; break;
+    } 
+};
+ 
+ 
+long  scolor_TCS3200::pulsewidth() {
+    while(!_s_in);
+    timer.start();
+    while(_s_in);
+    timer.stop();
+    float pulsewidth_v = timer.read_us();
+    timer.reset();
+    return pulsewidth_v;
+};
+*/          
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.h	Fri Sep 03 05:13:46 2021 +0000
@@ -0,0 +1,102 @@
+#ifndef Motor_H
+#define Motor_H
+#include "mbed.h"
+ /* **************************************************************************
+ 
+@CCastrop
+cristiank.castrop@ecci.edu.co
+
+    L1      Terminal A del motor
+    L2      Terminal B del motor
+    SpeedPin Pin de salida PWM
+    
+ 
+******************************************************************************/
+
+
+class MotorContinuo {
+  public:
+    /// Constructores para Motores Continuos
+    MotorContinuo(PinName _L1, PinName _L2, PinName _speedPin, PinName _encodin, PinName _PosInicial, int _EncodPulses);
+    
+    void Forward();     // Da un sentido de giro al motor
+    void Back();        // Genera un sentido de giro contrario al de Forward
+    void Stop();        // Detiene el motor dejando el movimiento libre
+    void StopT();       // Detiene el motor truncando o enclavando el movimiento(Lo mantiene quieto).
+    void SpeedDuty(float v);   // Varia la velocidad de giro del motor de 0 a 100%
+    void SpinLength_ms(float t);  // Duración del giro en ms
+    void SpinLength(float t);  // Duración del giro
+    
+  private:
+
+    DigitalOut L1;
+    DigitalOut L2;
+    PwmOut     speedPin;
+    DigitalIn  encodin;
+    DigitalIn  PosInicial;
+    int EncodPulses;
+
+
+    
+
+};
+
+
+class MotorDiscreto {
+  public:
+    /// Constructores para Motores Continuos
+    MotorDiscreto(PinName _Dir, PinName _Step, int _NPasos, PinName _encodin, PinName _PosInicial, int _EncodPulses);
+    MotorDiscreto(void);    
+    void Forward();                 // Da un sentido de giro al motor
+    void Back();                    // Genera un sentido de giro contrario al de Forward
+    void Stop();                    // Detiene el motor dejando el movimiento libre. Pin Enable = 0
+    void StopT();                   // Detiene el motor truncando o enclavando el movimiento(Lo mantiene quieto).
+    void StepFreq(int n);           // Configura la Velocidad en Número(n) de pasos por segundos. Es decir la Frecuencia de paso.
+    void RunStep(long n);          // Se mueve el motor la cantidad de pasos ingresada
+    void Run(float t);              // Gira el motor durante el tiempo ingresado, si es 0 indefinidamente hasta llamada a Stop().
+    void RunRound(int n);           // Girar n vueltas
+    void Ustep(int resolucion, PinName M0, PinName M1, PinName M2);
+                                    // Configura los pasos a 1/2, 1/4, 1/8, 1/16, 1/32
+    
+                                    
+    long StepOnHold;                // Pasos faltantes por ejecutar
+    long StepsBySecond;             // Pasos por segundo
+    
+  private:
+    
+    void moveMotor(void);
+    
+    DigitalOut Dir;
+    DigitalOut Step;
+    int        NPasos;
+    DigitalIn  encodin;
+    DigitalIn  PosInicial;
+    int        EncodPulses;
+    
+    DigitalOut* EnablePin;
+    DigitalOut* M0;
+    DigitalOut* M1;
+    DigitalOut* M2;
+    
+    Ticker Move;                /// Timer de interrupcion
+    float TStep;                /// periodo del paso TStep = 1/StepsBySecond;
+    
+    int entradas;               /// registra cada 2 ingresos a la interrupcion
+
+};
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Sep 03 05:13:46 2021 +0000
@@ -0,0 +1,198 @@
+#include "mbed.h"
+#include "Motor.h"
+#define get_values
+//******* puerto serie *********//
+Serial Mbed(PB_6, PB_7);
+
+//******* Entrada de Interrupcion ENpulsosterna (pulso desde el encoder )*********//
+InterruptIn Encoder(PC_9);
+
+//******* Analog IN  *********//
+
+
+//******* Led depuración  *********//
+DigitalOut LED(PD_9);
+
+//******* Button para depuración  *********//
+DigitalIn  Button(PA_0);
+
+//******* Creacion del objeto de la Clase Motor  *********//
+MotorContinuo MotorDC(PD_13, PD_11, PB_15, NC,NC,0);
+
+Timeout Time;
+//******* Declaración de Constantes y variables *********//
+
+#define NPulsesEncoder 12
+#define Nvel         1              /// Cantidad de Velocidades o variaciones del escalon
+float EndTime = 0.83;              /// Tiempo total en el que se realizara todo el muestreo y se ejecutaran
+                                     /// Las distintas variaciones del impulso (velocidades)
+#define NSamples    830           /// Numero de muestras a tomar.
+#define PulseTime    float(EndTime / Nvel)  /// Tiempo que dura cada pulso o variacion de velocidad
+
+long  Npulsos = 0;                   /// Almacena la cantidad de pulsos generada por el sensor de herradura en el encoder
+int   PVelocidadAplicada[NSamples];  /// Almaena Las velocidades que se aplicaron en cada muestra
+int   muestras[NSamples];            /// vector en donde se almacenan las 200 muestras de tipo long
+float RPM[NSamples];                 /// Vector que almacena la convercion de pulsos a RPM del motor
+float RPS[NSamples];                 /// Vector que almacena la convercion de pulsos a RPM del motor
+float FactorConvercion;              /// Almacena el factor de conversion de numero de pulsos a RPM
+float Ts;                            /// Sample Time 
+int   PVel[Nvel] = {100};//, 25, 50, 75, 100, 85, 60, 30, 10, 5}; /// Porcentaje de velocidad: esta es la variable independiente de control. 
+                                     /// Es nuestra señal de entrada. puede ser float
+                                     /// Estos son los valores en porcentaje dentro del rango de 0 a 100, la cual indica el DUTY_CICLE de la
+                                     /// Señal cuadrada que gobierna el movimiento del motor.
+int velActual;                       /// Almacena de manera temporal la velcidad que se esta aplicando.
+int NextSample = 0;                  /// Indice que se Desplaza a la siguiente Muestra
+int v;                               /// Indice que desplaza la Velocidad en PVel[]
+
+//******* Declaración de funciones  *********//
+
+
+void config();
+void Encoder_Int();
+void Sample_Int();
+
+
+//******* Definicion de funciones *********//
+
+                
+void Encoder_Int()        /// Interrupcioin Externa del sensor de Herradura sobre el encoder
+{
+    Npulsos ++;
+}
+
+
+void Sample_Int()
+{
+    // Mientras no excedamos el numero de muestras almacenamos Npulsos tomados en 
+    // la posicion NextSample del vector muestras
+    if (NextSample < NSamples)
+    {
+         muestras[NextSample] = Npulsos;
+         PVelocidadAplicada[NextSample++] = PVel[v];
+         }
+    Npulsos = 0;
+    Time.attach(&Sample_Int, Ts);   
+}
+
+
+void config()
+{
+   // start program messsage 
+    Mbed.printf("Lets Work!!...\n");
+    
+   // Set External Interruption to rise flank
+    Encoder.fall(&Encoder_Int);
+   
+    
+   // Set a Motor movement direction
+    MotorDC.Forward();
+    
+    #ifdef depuration
+   // Sample Time
+    int TotalSamples = NSamples;
+    Ts = float(EndTime / TotalSamples); 
+    Mbed.printf("Sample Time : %.3f!!...\n!", Ts);
+    
+    // Pulse Time
+    Mbed.printf("Pulse Time : %.3f!!...\n!", PulseTime);
+    
+    // FactorConvercion = 60 segundos / Tiempo de muestreo * Numero de pulsos del encoder por vuelta
+    FactorConvercion = float (60/(Ts * NPulsesEncoder));
+    Mbed.printf("FactorConvercion : %.3f!!...\n!", FactorConvercion);
+    #endif
+    
+    #ifdef get_values
+    
+   // Sample Time
+    int TotalSamples = NSamples;
+    Ts = float(EndTime / TotalSamples); 
+    // FactorConvercion = 60 segundos / Tiempo de muestreo * Numero de pulsos del encoder por vuelta
+    FactorConvercion = float (60/(Ts * NPulsesEncoder));
+    
+    Mbed.printf("\t\tCurva Caracteristica del Motor \n\n Modelo del Motor : 40ZYN06 MaxVoltage Supply : 24V  Current : 1.5A   MaxRPM : 3700\n"); 
+    Mbed.printf("\n\tTest Description\n\n Test Voltage Supply: 19.43V   Test Current Supply: 1.5A   \nEncoder Pulses : %d   # de Velocidades Aplicadas: %d ", NPulsesEncoder, Nvel);
+    Mbed.printf("\nTotal Test Time: %.0fs   Samples: %d  Sample Time : %0.3fs   Duracion de Cada Velocidad: %.2fs Factor de Conversion(Pulses To RPM): %0.2f \n\n", EndTime, NSamples, Ts, PulseTime, FactorConvercion);
+    Mbed.printf("\nPorcentaje de WeidthPulse Aplicadas durante el test(ordenado) :\n\t");
+    for(int n = 0; n < Nvel; n++)Mbed.printf("\t%d%%,",PVel[n]);
+    Mbed.printf("\n\t11 Mayo 2019 - Elaborado por Cristian Kevin Castro Parra\n");
+    
+    #endif
+            
+}
+
+
+
+int main() {
+    
+    
+    
+    config();
+   while (1)
+   {
+     
+    while(Button == 1)   /// No ejecuta el código hasta presionar el boton
+    {
+       
+        for(v = 0 ; v < Nvel ; v++ )
+        {
+            // Set Speed of Motor modifying PWM Duty Cicle
+            MotorDC.SpeedDuty(PVel[v]);
+            // Set Time Interrupt each Ts seconds and callback Sample_Int() function.
+            if (v == 0) Time.attach(&Sample_Int, Ts);
+            
+            
+            Mbed.printf("\nMotor Velocidad = %d%% : " , PVel[v]);
+
+            // We wait duration Pulse Time
+            wait(PulseTime); 
+        
+        }
+      
+            Time.detach();
+            MotorDC.SpeedDuty(0);
+            LED = 0;
+            
+            #ifdef depuration
+            
+            for(NextSample = 0; NextSample < NSamples; NextSample++ )
+                {   
+                    RPM[NextSample] = (muestras[NextSample] * FactorConvercion); 
+                    Mbed.printf("Muestra %d : %d Pulses  %0.3f RPM \n", NextSample, muestras[NextSample], RPM[NextSample]); wait_ms(10);
+                    
+                }
+            #endif
+            
+            #ifdef get_values
+            Mbed.printf("\n\nVelocidad\tPulsos\tRPS\tRPM    Obtenidos durante el Test: \n");
+            for(NextSample = 0; NextSample < NSamples; NextSample++ )
+                {   
+                    RPM[NextSample] = (muestras[NextSample] * FactorConvercion); 
+                    RPS[NextSample] = (muestras[NextSample]/ float(NPulsesEncoder * Ts)); 
+                }
+                
+            for(NextSample = 0; NextSample < NSamples; NextSample++ )
+                {   
+                    Mbed.printf("%d, %d, %.3f, %0.3f\n",PVelocidadAplicada[NextSample], muestras[NextSample], RPS[NextSample], RPM[NextSample]); wait_ms(5);
+                    
+                }
+           /* 
+           
+            Mbed.printf("\nRPM Obtenidos durante el Test: \n");
+            for(NextSample = 0; NextSample < NSamples; NextSample++ )
+                {   
+                    RPM[NextSample] = (muestras[NextSample] * FactorConvercion); 
+                    Mbed.printf("%0.0f\n",RPM[NextSample]); wait_ms(1);
+                    
+                }
+             */   
+            #endif
+            
+        
+      }
+        
+       // Mbed.printf("End of Work!!...\n!");
+        
+        
+        
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Sep 03 05:13:46 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file