File content as of revision 5:5d8bf5144d8f:

/* L9110S H-Brigh Driver Interface
 *
 * By Sebastian Donner
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "L9110S.h"
     
 /** Create a new interface for an L9110S
  *
  * @param cw_r  is the pin for clockwise rotate
  * @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), power_act(0)
   {
    _cw.period_us( periode);
    _ccw.period_us(periode);
    _cw.pulsewidth_us( 0);
    _ccw.pulsewidth_us(0);
   }
    
  /** sets Pulswidth cw(+) ccw(-)
   * 
   * @param power positiv in clockwise Dir
   * @param power negativ in counter clockwise Dir
   *
   *  in % of range from min_(cw/ccw) to max_(cw/ccw)
   *
   */
  void L9110S::drive(int power)
  {

    // 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;}
          else power = 0;}
    
     //cw or ccw Rotate
    if (power >= 0){_ccw.pulsewidth_us(0); _cw.pulsewidth_us(power);}
     else {_cw.pulsewidth_us(0); _ccw.pulsewidth_us(-power);}
  }    
        
  /** sets Pulswidth and Dir 
   * 
   *  @param dir positiv dir is clockwise direction
   *  @param dir negativ dir is counter clockwise direction
   *  @param dir zero is stop 
   *
   *  @param power in % of range from min_(cw/ccw) to max_(cw/ccw) allways positiv
   *
   */
   void L9110S::drive(int dir, int power)      
   {
    // 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;}
           else power = 0;}

    
     //cw or ccw Rotate
    if (dir > 0){_ccw.pulsewidth_us(0); _cw.pulsewidth_us(power);}
     else {_cw.pulsewidth_us(0); _ccw.pulsewidth_us(power);}
   }
         
  /** sets the PWM frequency
   *
   * @param hz is the PWM frequency (default 2000 Hz)
   */
   void L9110S::frequency(int hz)
   {
    periode = 1000000/hz; 
   }
   
   float L9110S::deg_diff(float Soll, float Ist)
   {
    float diff = Soll - Ist;
    if(diff >  180) diff = -360 + diff;
    if(diff < -180) diff =  360 + diff;
    return diff;   
   }

  void L9110S::drive_diff(float Soll, float Ist, int kp ,int i_lim, int trash)
   {
    
    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);   
   }