L9110S H-Bridge Driver
Fork of L9110S by
L9110S.cpp
- Committer:
- HMFK03LST1
- Date:
- 2016-02-09
- Revision:
- 4:d2fb1359716e
- Parent:
- 2:5d4f1e2b27fe
- Child:
- 5:5d8bf5144d8f
File content as of revision 4:d2fb1359716e:
/* 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) { _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; // 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; // 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 = 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; } drive(drive_i/10 + p); }