
Modelado de un sistema de control para el movimiento de un motor NEMA.
Diff: Motor.h
- Revision:
- 0:f46e78766b9c
--- /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 + + + + + + + + + + + + + + +