File content as of revision 9:0d8dbe2671cd:

#include <algorithm>
#include "GMD.hpp"

template <size_t MIN, size_t MAX>
const float GMD<MIN, MAX>::_frequency_to_tick_coeff = 3300.0f;

template <size_t MIN, size_t MAX>
const uint32_t GMD<MIN, MAX>::_default_frequency_kHz = 20;

template <size_t MIN, size_t MAX>
GMD<MIN, MAX>::GMD(PinName p0, PinName p1, PinName shut_down) : _shut_down(shut_down, 1) {
    _pwm[0] = new FastPWM(p0);
    _pwm[1] = new FastPWM(p1);
    
    _pwm[0]->write(0.0f);
    _pwm[1]->write(0.0f);
    
    set_frequency_kHz(_default_frequency_kHz);
}

template <size_t MIN, size_t MAX>
void GMD<MIN, MAX>::set_frequency_kHz(float f_kHz) {
    _pwm[0]->period_ticks(_frequency_to_tick_coeff / f_kHz);
    _pwm[1]->period_ticks(_frequency_to_tick_coeff / f_kHz);
}

float sign(float n) {
    if (n < 0.0f) {
        return -1.0f;
    }
    return 1.0f;
}

template <size_t MIN, size_t MAX>
float GMD<MIN, MAX>::set(float p) {
    p = std::max(-1.0f, std::min(p, 1.0f));

    p = (abs(p) * (MAX - MIN) + MIN) * sign(p);
    p *= 0.01f;
    
    if (p < 0.0f) {
        _pwm[0]->write(-p);
        _pwm[1]->write(0.0f);
    } else {
        _pwm[0]->write(0.0f);
        _pwm[1]->write(p);
    }
    _shut_down = 1;
    return p;
}

template <size_t MIN, size_t MAX>
void GMD<MIN, MAX>::release() {
    _shut_down = 0;
}

template <size_t MIN, size_t MAX>
float GMD<MIN, MAX>::operator=(float p) {
    return set(p);
}