Diff: L9110S.cpp

Revision:

5:5d8bf5144d8f

Parent:

4:d2fb1359716e

--- a/L9110S.cpp	Tue Feb 09 07:46:23 2016 +0000
+++ b/L9110S.cpp	Wed Jul 20 14:12:29 2016 +0000
@@ -32,7 +32,7 @@
   * @param ccw_r is the pin for counter clockwise rotate
   */
   L9110S::L9110S(PinName cw_r, PinName ccw_r):
-    cw_min(14), cw_max(70), ccw_min(14), ccw_max(70), _cw(cw_r), _ccw(ccw_r), periode(1000)
+    cw_min(14), cw_max(70), ccw_min(14), ccw_max(70), _cw(cw_r), _ccw(ccw_r), periode(1000), power_act(0)
    {
     _cw.period_us( periode);
     _ccw.period_us(periode);
@@ -54,7 +54,8 @@
     // Limit PWM -100 to 100
     if (power >  100) power =  100;
     if (power < -100) power = -100; 
-
+    power_act = power;
+    
     // Calc PWM in us 
     if        (power > 0) {power = ((power * ( cw_max -  cw_min)) + ( cw_min * 100)) * periode / 10000;}
     else { if (power < 0) {power = ((power * (ccw_max - ccw_min)) - (ccw_min * 100)) * periode / 10000;}
@@ -78,7 +79,8 @@
    {
     // Limit PWM  0 to 100 and Stop
     if ((power > 100) || (power < 0) || (dir == 0)) power = 0; 
-
+    power_act = power;
+    
     // Calc PWM in us 
     if      (dir > 0) {power = ((power * (cw_max - cw_min )) + (cw_min  * 100)) * periode / 100000;}
     else { if (dir < 0) {power = ((power * (ccw_max- ccw_min)) + (ccw_min * 100)) * periode / 100000;}
@@ -109,15 +111,26 @@
 
   void L9110S::drive_diff(float Soll, float Ist, int kp ,int i_lim, int trash)
    {
-    int p = deg_diff(Soll, Ist) * kp;
- 
-    if (abs(p) < ((kp * trash)/10)) {p = 0;}
-    else
-    {
-     drive_i = drive_i + (p/16);
-     if (drive_i >  i_lim) drive_i =  i_lim;
-     if (drive_i < -i_lim) drive_i = -i_lim;
-    }
+    
+    int     p = 0;                                    //Leistungsvariable
+    
+    float   diff = deg_diff(Soll, Ist      );            //Winkeldiffernz
+            diff = diff - (power_act / 20.0);            //Winkeldiffernz - D Anteil aus Motorleistung 10° bei 100%        
+    
+    if ( fabs(diff) < (trash / 10.0)) {p = 0; drive_i *= 0.9;}        //Leistung 0, wenn innerhalb Threshold
+     else
+      {
+       p    = diff * kp;                              //Leistungsbedarf aus Winkeldifferenz mal Kp
+       drive_i = drive_i + ( p / 20.0);               //Berechnung I Anteil 
+       if (drive_i >  i_lim) drive_i =  i_lim;
+       if (drive_i < -i_lim) drive_i = -i_lim;
+      }
+
+    if (p >  100) (p =  100);
+    if (p < -100) (p = -100);
+    
+    power_act = (6 * power_act + p) / 7;
+     
     drive(drive_i/10 + p);   
    }