File content as of revision 0:863eca4e24ff:

#include "Motor.h"
#include "mbed.h"
 

 
L298HBridge::L298HBridge(PinName ENpin, PinName FWDpin, PinName REVpin, PinName chanApin, PinName chanBpin) : _en(ENpin), _fwd(FWDpin), _rev(REVpin), _chanA(chanApin), _chanB(chanBpin) 
{
    _fwd = 0;
    _rev = 0;
    _en = 0.0;
    
    _Time.start(); //start timer
    _currentSpeed = 0.0; //the motor is stopped
    _lastAUp = 0.0;
    
     _rpm_index = rpm.size();
    rpm.push_back(0);
        

    _chanA.rise(callback(this, &L298HBridge::rising)); //attach the function
    
    
    
    _gainProp = 1;
    _gainInt = 0.1;
    
    _maxSpeed = 290.0; //RPM
    _lastOut = 0;
   
    
    }
 
void L298HBridge::Fwd() 
{    
    _fwd = 1;
    _rev = 0;
}

void L298HBridge::Rev() 
{
    _fwd = 0;
    _rev = 1;
}

void L298HBridge::Stop() 
{
    _fwd = 1;
    _rev = 1;
    
    
}


 
void L298HBridge::SetSpeed(float PWMPercentage) 
{
    printf("%d", count_ini);
    if(PWMPercentage>0){
        Fwd();
        _en = PWMPercentage;
    }
    else if(PWMPercentage<0){
        Rev();
        _en = -PWMPercentage;
    }
    else{
        Stop();
    }
}

void L298HBridge::rising(){
       float tempTime= _Time.read();
       if(_chanB){
        _currentSpeed =  60.0f/(11.0f*(tempTime - _lastAUp))/45.0f;
        }
        else{
            this->_currentSpeed = -60.0f/(11.0f*(tempTime - _lastAUp))/45.0f;
            
        }
        rpm[_rpm_index] = _currentSpeed;
        //test = _currentSpeed;
        
        
        /*
        if (fabs(_currentSpeed) < 35.0f){
            _currentSpeed = 0.0;    
        }*/
        //printf("C %d\n\r", _rpm_index);
       _lastAUp = tempTime;   
       
       //printf("update\r\n");
       }
       
float L298HBridge::GetSpeed(int index){
       return rpm[rpm.size() - index];
       
}
/*       
void L298HBridge::SetAsser(float GainProp, float GainInt){
       _gainProp = GainProp;
       _gainInt = GainInt;
       }
       
void L298HBridge::SetSpeedA(float SpeedRPM){
    float error = SpeedRPM - _currentSpeed;
    float commande = error * _gainProp ;
    if (commande >0 ){
        this->Fwd();
        this->SetSpeed(12/fabs(commande));
    } 
    else{
        this->Rev();
        this->SetSpeed(12/fabs(commande));
    }
    _lastOut = commande;     
    }
*/    
float L298HBridge::GetU(){
   return _en;
   } 

string L298HBridge::state(){
    if(_fwd  and not(_rev)){
        return  "fwd";
        }
    else if (_rev and not(_fwd)){
        return "rev";
        }
    else if (_rev == _fwd){
        return "stop";   

    }
    else {
        return "error";
        }
    }