Motor.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "Ticker.h"
00004 
00005 #define Pi 3.14159265
00006 #define Horario         1
00007 #define AntiHorario     0
00008 
00009 ///************  MOTOR CONTINUO ********************************//
00010 
00011 /// CONSTRUCTOR
00012 MotorContinuo::MotorContinuo(PinName _L1, PinName _L2, PinName _speedPin, PinName _encodin, PinName _PosInicial, int _EncodPulses) :
00013 L1(_L1), L2(_L2), speedPin(_speedPin), encodin(_encodin), PosInicial(_PosInicial), EncodPulses(_EncodPulses)
00014 {
00015     speedPin.period_ms(2);
00016     speedPin.write(0);
00017     
00018 };
00019 
00020 
00021 
00022 void MotorContinuo::Forward()   { L1=1;  L2=0;}
00023 void MotorContinuo::Back()      { L1=0;  L2=1;}
00024 void MotorContinuo::Stop()      { L1=0;  L2=0;}
00025 void MotorContinuo::StopT()     { L1=1;  L2=1;}
00026 void MotorContinuo::SpeedDuty(int v) { speedPin.write(float(v/100.0));}
00027 void MotorContinuo::SpinLength_ms(float t)    { t++;}// Duración del giro en ms
00028 void MotorContinuo::SpinLength(float t)       { t++;};
00029     
00030     
00031 
00032 ///************  MOTOR DISCRETO ********************************//    
00033 
00034 /// CONSTRUCTOR
00035 MotorDiscreto::MotorDiscreto(PinName _Dir, PinName _Step, int _NPasos, PinName _encodin, PinName _PosInicial, int _EncodPulses) :
00036 Dir(_Dir), Step(_Step), NPasos(_NPasos), encodin(_encodin), PosInicial(_PosInicial), EncodPulses(_EncodPulses)
00037 {
00038 };    
00039  
00040  
00041  // *************   METODOS ***************** ///
00042     
00043 void MotorDiscreto::moveMotor(void)  
00044     {
00045         if (StepOnHold != 0 && _moveMotorStopped == false) 
00046             {
00047                 Step = !Step; // Se hace elcambio de estado en el pin Step
00048                 entradas++; // se registra cada paso efectivo el cual es valido cada 2 entradas 
00049                             // 1ra entrada: tiempo en Bajo
00050                             // 2da entrada: tiempo en Alto
00051                             // Es decir cuando Step cumple un periodo.
00052                 if (entradas >= 2) // cuando se registran 2 entradas se disminuye un paso
00053                 {
00054                     StepOnHold--; // Se elimina 1 paso de los pendientes
00055                     entradas = 0; 
00056                 }
00057                 
00058                 Move.attach(callback(this,&MotorDiscreto::moveMotor), (TStep/2) ); // Reiniciamos el tiempo de imterrupcion
00059                 
00060             }
00061         else if (StepOnHold == 0 || _moveMotorStopped == true) // si no hay mas pasos pendientes
00062             {
00063                 Move.detach(); // desabilita la interrupcion
00064                 entradas = 0;
00065             }
00066     }
00067 
00068 
00069 void MotorDiscreto::Forward()   { Dir=1;}
00070 void MotorDiscreto::Back()      { Dir=0;}
00071 void MotorDiscreto::Stop()      { EnablePin = 0; _moveMotorStopped = true;}
00072 void MotorDiscreto::StopT()     { Move.detach(); _moveMotorStopped = true;}
00073 void MotorDiscreto::StepFreq(long n) { StepsBySecond = n; TStep = (1/float(n));}
00074 void MotorDiscreto::RunStep(long n) { StepOnHold = n; Move.attach(callback(this,&MotorDiscreto::moveMotor), (TStep/2) ); _moveMotorStopped = false; }// Duración del giro en ms
00075 void MotorDiscreto::Run(float t)    { float Fp = (1/TStep); StepOnHold = long(t*Fp); Move.attach(callback(this,&MotorDiscreto::moveMotor), (TStep/2) ); _moveMotorStopped = false; }
00076 void MotorDiscreto::RunRound(int n) { StepOnHold = (NPasos * n); Move.attach(callback(this,&MotorDiscreto::moveMotor), (TStep/2) ); _moveMotorStopped = false; }
00077 long MotorDiscreto::getStepOnHold(void){return StepOnHold; }
00078 
00079 void Ustep(int resolucion, DigitalOut* _M0, DigitalOut* _M1, DigitalOut* _M2)
00080     {
00081         
00082         switch(resolucion)
00083         {
00084             case 1: *_M0 = 0; *_M1 = 0; *_M2 = 0;
00085                     break;
00086             case 2: *_M0 = 1; *_M1 = 0; *_M2 = 0;
00087                     break;
00088             case 4: *_M0 = 0; *_M1 = 1; *_M2 = 0;
00089                     break; 
00090             case 8: *_M0 = 1; *_M1 = 1; *_M2 = 1;
00091                     break;
00092             case 16: *_M0 = 0; *_M1 = 0; *_M2 = 1;
00093                     break;
00094             case 32: *_M0 = 1; *_M1 = 0; *_M2 = 1;
00095                     break;
00096         
00097             default: *_M0 = 0; *_M1 = 0; *_M2 = 0;
00098         
00099         
00100         }
00101     
00102     
00103     }
00104 
00105       
00106 ///************  TRACCIONDISCRETA ********************************//    
00107 
00108 
00109 /// ***************** CONSTRUCTOR ********************** ///
00110 TraccionD::TraccionD(PinName _StepMI, PinName _DirMI, PinName _StepMD, PinName _DirMD, int _NPasos, float _r, float _L ):
00111                     StepMI(_StepMI), DirMI(_DirMI), StepMD(_StepMD), DirMD(_DirMD), NPasos(_NPasos), r(_r), L(_L)
00112     {
00113         // 1. se halla la Relacion(Rl) de distacia entre los dos Radios(r y L)
00114             Rl  = (L / r); 
00115             
00116         };
00117 
00118  // *************   METODOS ***************** ///
00119 
00120 void TraccionD::moveMotor(void)  
00121     {
00122         if (StepOnHold != 0) 
00123             {
00124             
00125                 
00126                 StepMI = !StepMI; // Se hace elcambio de estado en el pin Step para ambos motores
00127                 StepMD =  StepMI; 
00128                 
00129                 entradas++; // se registra cada paso efectivo el cual es valido cada 2 entradas 
00130                             // 1ra entrada: tiempo en Bajo
00131                             // 2da entrada: tiempo en Alto
00132                             // Es decir cuando Step cumple un periodo.
00133                 if (entradas >= 2) // cuando se registran 2 entradas se disminuye un paso
00134                 {
00135                     StepOnHold--; // Se elimina 1 paso de los pendientes
00136                     entradas = 0; 
00137                 }
00138                 
00139                 Move.attach(callback(this,&TraccionD::moveMotor), (TStep/2) ); // Reiniciamos el tiempo de imterrupcion
00140                 
00141             }
00142         else if (StepOnHold == 0) // si no hay mas pasos para dar se deshabilita las interrupciones.
00143             {
00144                 Move.detach(); 
00145             }
00146     }
00147 
00148 
00149 void TraccionD::Forward()   { DirMI=1; DirMD=1;} // Configua de giro de ambos motores el el mismo sentido para avanzar hacia adelante
00150 void TraccionD::Back()      { DirMI=0; DirMD=0;} // Configua el sentido giro en ambos motores contrario al de Forward(), para avanzar hacia atras
00151 void TraccionD::Left()      { DirMI=0; DirMD=1;} // Configura el sentido de giro de ambos motores de manera contraria para girar hacia la izquierda
00152 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
00153 void TraccionD::Stop()      { EnablePinM1 = 0; EnablePinM2 = 0;}
00154 void TraccionD::StopT()     { Move.detach();}
00155 void TraccionD::StepFreq(long n) { StepsBySecond = n; TStep = (1/float(n));}
00156 void TraccionD::RunStep(long n){ StepOnHold = n; Move.attach(callback(this,&TraccionD::moveMotor), (TStep/2) ); }// Duración del giro en ms
00157 void TraccionD::Run(float t)    { float Fp = (1/TStep); StepOnHold = long(t*Fp); Move.attach(callback(this,&TraccionD::moveMotor), (TStep/2) ); }
00158 void TraccionD::RunRound(int n) { StepOnHold = (NPasos * n); Move.attach(callback(this,&TraccionD::moveMotor), (TStep/2) ); }
00159 long TraccionD::getStepOnHold(void){return StepOnHold; }
00160 
00161 void TraccionD::Giro(long grados, bool sentido)     
00162 { 
00163     
00164     StepOnHold = grados * int(Rl * NPasos);
00165     StepOnHold /= 360; 
00166     if(sentido == true) Right(); else if (sentido == false) Left();
00167     Move.attach(callback(this,&TraccionD::moveMotor), (TStep/2) );
00168 
00169 }
00170 
00171 
00172 
00173 
00174 
00175 
00176 
00177 
00178 
00179 
00180