Revision 0:f1ea96183c35, committed 2021-09-03
- Comitter:
- CCastrop1012
- Date:
- Fri Sep 03 05:07:18 2021 +0000
- Commit message:
- Se crea la clase Traccion que implementa varios objetos del tipo motor para controlar la direccion de un vehiulo a base de 2 motores, tanto continuos como discretos.
Changed in this revision
diff -r 000000000000 -r f1ea96183c35 Motor.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.cpp Fri Sep 03 05:07:18 2021 +0000
@@ -0,0 +1,180 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "Ticker.h"
+
+#define Pi 3.14159265
+#define Horario 1
+#define AntiHorario 0
+
+///************ MOTOR CONTINUO ********************************//
+
+/// CONSTRUCTOR
+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(int v) { speedPin.write(float(v/100.0));}
+void MotorContinuo::SpinLength_ms(float t) { t++;}// Duración del giro en ms
+void MotorContinuo::SpinLength(float t) { t++;};
+
+
+
+///************ MOTOR DISCRETO ********************************//
+
+/// CONSTRUCTOR
+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)
+{
+};
+
+
+ // ************* METODOS ***************** ///
+
+void MotorDiscreto::moveMotor(void)
+ {
+ if (StepOnHold != 0 && _moveMotorStopped == false)
+ {
+ 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 || _moveMotorStopped == true) // si no hay mas pasos pendientes
+ {
+ Move.detach(); // desabilita la interrupcion
+ entradas = 0;
+ }
+ }
+
+
+void MotorDiscreto::Forward() { Dir=1;}
+void MotorDiscreto::Back() { Dir=0;}
+void MotorDiscreto::Stop() { EnablePin = 0; _moveMotorStopped = true;}
+void MotorDiscreto::StopT() { Move.detach(); _moveMotorStopped = true;}
+void MotorDiscreto::StepFreq(long n) { StepsBySecond = n; TStep = (1/float(n));}
+void MotorDiscreto::RunStep(long n) { StepOnHold = n; Move.attach(callback(this,&MotorDiscreto::moveMotor), (TStep/2) ); _moveMotorStopped = false; }// Duración del giro en ms
+void MotorDiscreto::Run(float t) { float Fp = (1/TStep); StepOnHold = long(t*Fp); Move.attach(callback(this,&MotorDiscreto::moveMotor), (TStep/2) ); _moveMotorStopped = false; }
+void MotorDiscreto::RunRound(int n) { StepOnHold = (NPasos * n); Move.attach(callback(this,&MotorDiscreto::moveMotor), (TStep/2) ); _moveMotorStopped = false; }
+long MotorDiscreto::getStepOnHold(void){return StepOnHold; }
+
+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;
+
+
+ }
+
+
+ }
+
+
+///************ TRACCIONDISCRETA ********************************//
+
+
+/// ***************** CONSTRUCTOR ********************** ///
+TraccionD::TraccionD(PinName _StepMI, PinName _DirMI, PinName _StepMD, PinName _DirMD, int _NPasos, float _r, float _L ):
+ StepMI(_StepMI), DirMI(_DirMI), StepMD(_StepMD), DirMD(_DirMD), NPasos(_NPasos), r(_r), L(_L)
+ {
+ // 1. se halla la Relacion(Rl) de distacia entre los dos Radios(r y L)
+ Rl = (L / r);
+
+ };
+
+ // ************* METODOS ***************** ///
+
+void TraccionD::moveMotor(void)
+ {
+ if (StepOnHold != 0)
+ {
+
+
+ StepMI = !StepMI; // Se hace elcambio de estado en el pin Step para ambos motores
+ StepMD = StepMI;
+
+ 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,&TraccionD::moveMotor), (TStep/2) ); // Reiniciamos el tiempo de imterrupcion
+
+ }
+ else if (StepOnHold == 0) // si no hay mas pasos para dar se deshabilita las interrupciones.
+ {
+ Move.detach();
+ }
+ }
+
+
+void TraccionD::Forward() { DirMI=1; DirMD=1;} // Configua de giro de ambos motores el el mismo sentido para avanzar hacia adelante
+void TraccionD::Back() { DirMI=0; DirMD=0;} // Configua el sentido giro en ambos motores contrario al de Forward(), para avanzar hacia atras
+void TraccionD::Left() { DirMI=0; DirMD=1;} // Configura el sentido de giro de ambos motores de manera contraria para girar hacia la izquierda
+void TraccionD::Right() { DirMI=1; DirMD=0;} // Configura el sentido de giro de ambos motores de manera contraria e inverso al de Left() para girar hacia la derecha
+void TraccionD::Stop() { EnablePinM1 = 0; EnablePinM2 = 0;}
+void TraccionD::StopT() { Move.detach();}
+void TraccionD::StepFreq(long n) { StepsBySecond = n; TStep = (1/float(n));}
+void TraccionD::RunStep(long n){ StepOnHold = n; Move.attach(callback(this,&TraccionD::moveMotor), (TStep/2) ); }// Duración del giro en ms
+void TraccionD::Run(float t) { float Fp = (1/TStep); StepOnHold = long(t*Fp); Move.attach(callback(this,&TraccionD::moveMotor), (TStep/2) ); }
+void TraccionD::RunRound(int n) { StepOnHold = (NPasos * n); Move.attach(callback(this,&TraccionD::moveMotor), (TStep/2) ); }
+long TraccionD::getStepOnHold(void){return StepOnHold; }
+
+void TraccionD::Giro(long grados, bool sentido)
+{
+
+ StepOnHold = grados * int(Rl * NPasos);
+ StepOnHold /= 360;
+ if(sentido == true) Right(); else if (sentido == false) Left();
+ Move.attach(callback(this,&TraccionD::moveMotor), (TStep/2) );
+
+}
+
+
+
+
+
+
+
+
+
+
+
diff -r 000000000000 -r f1ea96183c35 Motor.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.h Fri Sep 03 05:07:18 2021 +0000
@@ -0,0 +1,160 @@
+#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
+
+ LastVersion: 21 - Abril - 2019 7:45pm
+******************************************************************************/
+
+
+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(int 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(long 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 getStepOnHold(void);
+
+ 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
+ bool _moveMotorStopped; // en 1 cuando se da la orden al motor de detenerse
+};
+
+
+
+
+class TraccionD
+{
+
+ public:
+
+ TraccionD(PinName _StepMI, PinName _DirMI, PinName _StepMD, PinName _DirMD, int _NPasos, float _r, float _L); // para giro declarar primero la frecuencia de paso
+
+ void Forward(); // Da un sentido de giro al motor
+ void Back(); // Genera un sentido de giro contrario al de Forward
+ void Left(); // Configura el sentido de giro de ambos motores de manera contraria para girar hacia la izquierda
+ void Right(); // Configura el sentido de giro de ambos motores de manera contraria e inverso al de Left() para girar hacia la derecha
+ 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(long n);//maximo~350 // 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 Giro(long grados, bool sentido); // IMPORTANTE: Para llamar esta funcino PRIMERO se debe definir la frecuencia de paso
+ // Gira el movil la cantidad de grados indicados en sentido indicado.
+ // en función del radio de las llantas y el radio del eje que une las llantas.
+ // ejemplo: (90, Horario) o (120, AntiHorario)
+
+ long getStepOnHold(void);
+
+
+ private:
+
+ void moveMotor(void); // Funcion que es ejecutada cada interupcion del Ticker Move
+
+ // Configuracion de los pines
+ DigitalOut StepMI,DirMI;
+ DigitalOut StepMD, DirMD;
+ int NPasos;
+ float r; // Radio de la llanta
+ float L; // Longitud desde del eje medido entre las 2 llantas dividido en 2.
+ float Rl; // Relacion(Rl) de distacia entre los dos Radios(r y L)
+ long StepOnHold; // Pasos faltantes por ejecutar
+ long StepsBySecond; // Pasos por segundo
+
+ DigitalOut* EnablePinM1;
+ DigitalOut* EnablePinM2;
+
+ Ticker Move; /// Timer de interrupcion
+ float TStep; /// periodo del paso TStep = 1/StepsBySecond;
+
+ int entradas; /// registra cada 2 ingresos a la interrupcion
+
+
+
+
+
+};
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+