Diff: Motor.cpp

Revision:

0:863eca4e24ff

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.cpp	Wed Nov 06 14:22:03 2019 +0000
@@ -0,0 +1,138 @@
+#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";
+        }
+    }
+
+       
\ No newline at end of file