GMD_impl.hpp

00001 #include <algorithm>
00002 #include "GMD.hpp"
00003 
00004 template <size_t MIN, size_t MAX>
00005 const float GMD<MIN, MAX>::_frequency_to_tick_coeff = 3300.0f;
00006 
00007 template <size_t MIN, size_t MAX>
00008 const uint32_t GMD<MIN, MAX>::_default_frequency_kHz = 20;
00009 
00010 template <size_t MIN, size_t MAX>
00011 const float GMD<MIN, MAX>::_threshold = 0.02f;
00012 
00013 template <size_t MIN, size_t MAX>
00014 GMD<MIN, MAX>::GMD(PinName p0, PinName p1, PinName shut_down) : _shut_down(shut_down, 1) {
00015     _pwm[0] = new FastPWM(p0);
00016     _pwm[1] = new FastPWM(p1);
00017     
00018     _pwm[0]->write(0.0f);
00019     _pwm[1]->write(0.0f);
00020     
00021     set_frequency_kHz(_default_frequency_kHz);
00022 }
00023 
00024 template <size_t MIN, size_t MAX>
00025 void GMD<MIN, MAX>::set_frequency_kHz(float f_kHz) {
00026     _pwm[0]->period_ticks(_frequency_to_tick_coeff / f_kHz);
00027     _pwm[1]->period_ticks(_frequency_to_tick_coeff / f_kHz);
00028 }
00029 
00030 float sign(float n) {
00031     if (n < 0.0f) {
00032         return -1.0f;
00033     }
00034     return 1.0f;
00035 }
00036 
00037 template <size_t MIN, size_t MAX>
00038 float GMD<MIN, MAX>::set(float p) {
00039     if (abs(p) < _threshold) {
00040         _pwm[0]->write(0.0f);
00041         _pwm[1]->write(0.0f);
00042         _shut_down = 1;
00043         return p;
00044     }
00045     
00046     p = std::max(-1.0f, std::min(p, 1.0f));
00047 
00048     p = (abs(p) * (MAX - MIN) + MIN) * sign(p);
00049     p *= 0.01f;
00050     
00051     if (p < 0.0f) {
00052         _pwm[0]->write(-p);
00053         _pwm[1]->write(0.0f);
00054     } else {
00055         _pwm[0]->write(0.0f);
00056         _pwm[1]->write(p);
00057     }
00058     _shut_down = 1;
00059     return p;
00060 }
00061 
00062 template <size_t MIN, size_t MAX>
00063 void GMD<MIN, MAX>::release() {
00064     _shut_down = 0;
00065 }
00066 
00067 template <size_t MIN, size_t MAX>
00068 float GMD<MIN, MAX>::operator=(float p) {
00069     return set(p);
00070 }
00071