L9110S H-Bridge Driver
Fork of L9110S by
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?
User | Revision | Line number | New 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 |