Hochschule München / L9110S

L9110S H-Bridge Driver

Fork of L9110S by Hochschule München

L9110S.cpp@5:5d8bf5144d8f, 2016-07-20 (annotated)

Committer:
HMFK03LST1
Date:
Wed Jul 20 14:12:29 2016 +0000
Revision:
5:5d8bf5144d8f
Parent:
4:d2fb1359716e 
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HMFK03LST1 1:4c6c89e42fa4 1 /* L9110S H-Brigh Driver Interface
HMFK03LST1 0:1a9b998e0ec1 2 *
HMFK03LST1 0:1a9b998e0ec1 3 * By Sebastian Donner
HMFK03LST1 0:1a9b998e0ec1 4 *
HMFK03LST1 0:1a9b998e0ec1 5 * Permission is hereby granted, free of charge, to any person
HMFK03LST1 0:1a9b998e0ec1 6 * obtaining a copy of this software and associated documentation
HMFK03LST1 0:1a9b998e0ec1 7 * files (the "Software"), to deal in the Software without
HMFK03LST1 0:1a9b998e0ec1 8 * restriction, including without limitation the rights to use,
HMFK03LST1 0:1a9b998e0ec1 9 * copy, modify, merge, publish, distribute, sublicense, and/or sell
HMFK03LST1 0:1a9b998e0ec1 10 * copies of the Software, and to permit persons to whom the
HMFK03LST1 0:1a9b998e0ec1 11 * Software is furnished to do so, subject to the following
HMFK03LST1 0:1a9b998e0ec1 12 * conditions:
HMFK03LST1 0:1a9b998e0ec1 13 *
HMFK03LST1 0:1a9b998e0ec1 14 * The above copyright notice and this permission notice shall be
HMFK03LST1 0:1a9b998e0ec1 15 * included in all copies or substantial portions of the Software.
HMFK03LST1 0:1a9b998e0ec1 16 *
HMFK03LST1 0:1a9b998e0ec1 17 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
HMFK03LST1 0:1a9b998e0ec1 18 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
HMFK03LST1 0:1a9b998e0ec1 19 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
HMFK03LST1 0:1a9b998e0ec1 20 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HMFK03LST1 0:1a9b998e0ec1 21 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
HMFK03LST1 0:1a9b998e0ec1 22 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
HMFK03LST1 0:1a9b998e0ec1 23 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
HMFK03LST1 0:1a9b998e0ec1 24 * OTHER DEALINGS IN THE SOFTWARE.
HMFK03LST1 0:1a9b998e0ec1 25 */
HMFK03LST1 0:1a9b998e0ec1 26
HMFK03LST1 0:1a9b998e0ec1 27 #include "L9110S.h"
HMFK03LST1 0:1a9b998e0ec1 28
HMFK03LST1 0:1a9b998e0ec1 29 /** Create a new interface for an L9110S
HMFK03LST1 0:1a9b998e0ec1 30 *
HMFK03LST1 0:1a9b998e0ec1 31 * @param cw_r is the pin for clockwise rotate
HMFK03LST1 0:1a9b998e0ec1 32 * @param ccw_r is the pin for counter clockwise rotate
HMFK03LST1 0:1a9b998e0ec1 33 */
HMFK03LST1 0:1a9b998e0ec1 34 L9110S::L9110S(PinName cw_r, PinName ccw_r):
HMFK03LST1 5:5d8bf5144d8f 35 cw_min(14), cw_max(70), ccw_min(14), ccw_max(70), _cw(cw_r), _ccw(ccw_r), periode(1000), power_act(0)
HMFK03LST1 0:1a9b998e0ec1 36 {
HMFK03LST1 0:1a9b998e0ec1 37 _cw.period_us( periode);
HMFK03LST1 0:1a9b998e0ec1 38 _ccw.period_us(periode);
HMFK03LST1 0:1a9b998e0ec1 39 _cw.pulsewidth_us( 0);
HMFK03LST1 0:1a9b998e0ec1 40 _ccw.pulsewidth_us(0);
HMFK03LST1 0:1a9b998e0ec1 41 }
HMFK03LST1 0:1a9b998e0ec1 42
HMFK03LST1 2:5d4f1e2b27fe 43 /** sets Pulswidth cw(+) ccw(-)
HMFK03LST1 0:1a9b998e0ec1 44 *
HMFK03LST1 1:4c6c89e42fa4 45 * @param power positiv in clockwise Dir
HMFK03LST1 1:4c6c89e42fa4 46 * @param power negativ in counter clockwise Dir
HMFK03LST1 0:1a9b998e0ec1 47 *
HMFK03LST1 0:1a9b998e0ec1 48 * in % of range from min_(cw/ccw) to max_(cw/ccw)
HMFK03LST1 0:1a9b998e0ec1 49 *
HMFK03LST1 0:1a9b998e0ec1 50 */
HMFK03LST1 0:1a9b998e0ec1 51 void L9110S::drive(int power)
HMFK03LST1 0:1a9b998e0ec1 52 {
HMFK03LST1 4:d2fb1359716e 53
HMFK03LST1 4:d2fb1359716e 54 // Limit PWM -100 to 100
HMFK03LST1 4:d2fb1359716e 55 if (power > 100) power = 100;
HMFK03LST1 4:d2fb1359716e 56 if (power < -100) power = -100;
HMFK03LST1 5:5d8bf5144d8f 57 power_act = power;
HMFK03LST1 5:5d8bf5144d8f 58
HMFK03LST1 0:1a9b998e0ec1 59 // Calc PWM in us
HMFK03LST1 4:d2fb1359716e 60 if (power > 0) {power = ((power * ( cw_max - cw_min)) + ( cw_min * 100)) * periode / 10000;}
HMFK03LST1 4:d2fb1359716e 61 else { if (power < 0) {power = ((power * (ccw_max - ccw_min)) - (ccw_min * 100)) * periode / 10000;}
HMFK03LST1 4:d2fb1359716e 62 else power = 0;}
HMFK03LST1 0:1a9b998e0ec1 63
HMFK03LST1 0:1a9b998e0ec1 64 //cw or ccw Rotate
HMFK03LST1 0:1a9b998e0ec1 65 if (power >= 0){_ccw.pulsewidth_us(0); _cw.pulsewidth_us(power);}
HMFK03LST1 0:1a9b998e0ec1 66 else {_cw.pulsewidth_us(0); _ccw.pulsewidth_us(-power);}
HMFK03LST1 0:1a9b998e0ec1 67 }
HMFK03LST1 0:1a9b998e0ec1 68
HMFK03LST1 2:5d4f1e2b27fe 69 /** sets Pulswidth and Dir
HMFK03LST1 0:1a9b998e0ec1 70 *
HMFK03LST1 1:4c6c89e42fa4 71 * @param dir positiv dir is clockwise direction
HMFK03LST1 1:4c6c89e42fa4 72 * @param dir negativ dir is counter clockwise direction
HMFK03LST1 1:4c6c89e42fa4 73 * @param dir zero is stop
HMFK03LST1 0:1a9b998e0ec1 74 *
HMFK03LST1 1:4c6c89e42fa4 75 * @param power in % of range from min_(cw/ccw) to max_(cw/ccw) allways positiv
HMFK03LST1 0:1a9b998e0ec1 76 *
HMFK03LST1 0:1a9b998e0ec1 77 */
HMFK03LST1 0:1a9b998e0ec1 78 void L9110S::drive(int dir, int power)
HMFK03LST1 0:1a9b998e0ec1 79 {
HMFK03LST1 4:d2fb1359716e 80 // Limit PWM 0 to 100 and Stop
HMFK03LST1 4:d2fb1359716e 81 if ((power > 100) || (power < 0) || (dir == 0)) power = 0;
HMFK03LST1 5:5d8bf5144d8f 82 power_act = power;
HMFK03LST1 5:5d8bf5144d8f 83
HMFK03LST1 0:1a9b998e0ec1 84 // Calc PWM in us
HMFK03LST1 0:1a9b998e0ec1 85 if (dir > 0) {power = ((power * (cw_max - cw_min )) + (cw_min * 100)) * periode / 100000;}
HMFK03LST1 0:1a9b998e0ec1 86 else { if (dir < 0) {power = ((power * (ccw_max- ccw_min)) + (ccw_min * 100)) * periode / 100000;}
HMFK03LST1 0:1a9b998e0ec1 87 else power = 0;}
HMFK03LST1 4:d2fb1359716e 88
HMFK03LST1 0:1a9b998e0ec1 89
HMFK03LST1 0:1a9b998e0ec1 90 //cw or ccw Rotate
HMFK03LST1 0:1a9b998e0ec1 91 if (dir > 0){_ccw.pulsewidth_us(0); _cw.pulsewidth_us(power);}
HMFK03LST1 0:1a9b998e0ec1 92 else {_cw.pulsewidth_us(0); _ccw.pulsewidth_us(power);}
HMFK03LST1 0:1a9b998e0ec1 93 }
HMFK03LST1 0:1a9b998e0ec1 94
HMFK03LST1 0:1a9b998e0ec1 95 /** sets the PWM frequency
HMFK03LST1 0:1a9b998e0ec1 96 *
HMFK03LST1 0:1a9b998e0ec1 97 * @param hz is the PWM frequency (default 2000 Hz)
HMFK03LST1 0:1a9b998e0ec1 98 */
HMFK03LST1 0:1a9b998e0ec1 99 void L9110S::frequency(int hz)
HMFK03LST1 0:1a9b998e0ec1 100 {
HMFK03LST1 0:1a9b998e0ec1 101 periode = 1000000/hz;
HMFK03LST1 0:1a9b998e0ec1 102 }
HMFK03LST1 4:d2fb1359716e 103
HMFK03LST1 4:d2fb1359716e 104 float L9110S::deg_diff(float Soll, float Ist)
HMFK03LST1 4:d2fb1359716e 105 {
HMFK03LST1 4:d2fb1359716e 106 float diff = Soll - Ist;
HMFK03LST1 4:d2fb1359716e 107 if(diff > 180) diff = -360 + diff;
HMFK03LST1 4:d2fb1359716e 108 if(diff < -180) diff = 360 + diff;
HMFK03LST1 4:d2fb1359716e 109 return diff;
HMFK03LST1 4:d2fb1359716e 110 }
HMFK03LST1 4:d2fb1359716e 111
HMFK03LST1 4:d2fb1359716e 112 void L9110S::drive_diff(float Soll, float Ist, int kp ,int i_lim, int trash)
HMFK03LST1 4:d2fb1359716e 113 {
HMFK03LST1 5:5d8bf5144d8f 114
HMFK03LST1 5:5d8bf5144d8f 115 int p = 0; //Leistungsvariable
HMFK03LST1 5:5d8bf5144d8f 116
HMFK03LST1 5:5d8bf5144d8f 117 float diff = deg_diff(Soll, Ist ); //Winkeldiffernz
HMFK03LST1 5:5d8bf5144d8f 118 diff = diff - (power_act / 20.0); //Winkeldiffernz - D Anteil aus Motorleistung 10° bei 100%
HMFK03LST1 5:5d8bf5144d8f 119
HMFK03LST1 5:5d8bf5144d8f 120 if ( fabs(diff) < (trash / 10.0)) {p = 0; drive_i *= 0.9;} //Leistung 0, wenn innerhalb Threshold
HMFK03LST1 5:5d8bf5144d8f 121 else
HMFK03LST1 5:5d8bf5144d8f 122 {
HMFK03LST1 5:5d8bf5144d8f 123 p = diff * kp; //Leistungsbedarf aus Winkeldifferenz mal Kp
HMFK03LST1 5:5d8bf5144d8f 124 drive_i = drive_i + ( p / 20.0); //Berechnung I Anteil
HMFK03LST1 5:5d8bf5144d8f 125 if (drive_i > i_lim) drive_i = i_lim;
HMFK03LST1 5:5d8bf5144d8f 126 if (drive_i < -i_lim) drive_i = -i_lim;
HMFK03LST1 5:5d8bf5144d8f 127 }
HMFK03LST1 5:5d8bf5144d8f 128
HMFK03LST1 5:5d8bf5144d8f 129 if (p > 100) (p = 100);
HMFK03LST1 5:5d8bf5144d8f 130 if (p < -100) (p = -100);
HMFK03LST1 5:5d8bf5144d8f 131
HMFK03LST1 5:5d8bf5144d8f 132 power_act = (6 * power_act + p) / 7;
HMFK03LST1 5:5d8bf5144d8f 133
HMFK03LST1 4:d2fb1359716e 134 drive(drive_i/10 + p);
HMFK03LST1 4:d2fb1359716e 135 }
HMFK03LST1 0:1a9b998e0ec1 136
HMFK03LST1 0:1a9b998e0ec1 137