Microduino_Motor.cpp

00001 #include "Microduino_Motor.h"
00002 
00003 Motor::Motor(PinName _motor_pinA, PinName _motor_pinB)
00004 {
00005             _period_us = 255; // 500Hz
00006             pwmout_init(&_pwmA, _motor_pinA);
00007             pwmout_period_us(&_pwmA, _period_us); // 500Hz
00008             pwmout_pulsewidth_us(&_pwmA, 0);
00009 
00010 
00011             gpio_init_out(&gpioB, _motor_pinB);
00012 
00013             this->fix=1;
00014 }
00015 
00016 void Motor::Fix(float _fix)
00017 {
00018     this->fix=_fix;
00019 }
00020 
00021 int16_t Motor::GetData(int16_t _throttle, int16_t _steering, uint8_t _dir)
00022 {
00023     this->_motor_vol = _throttle;
00024 
00025     if(_dir == 1)
00026     {
00027         this->_motor_vol -= _steering / 2;
00028     }
00029     else
00030     {
00031         this->_motor_vol += _steering / 2;
00032     }
00033     if (this->_motor_vol > 255)
00034         this->_motor_vol = 255;
00035     else if (this->_motor_vol < -255)
00036         this->_motor_vol = -255;
00037 
00038     this->_motor_vol *= fix;
00039 
00040     return this->_motor_vol;
00041 }
00042 
00043 void Motor::Driver(int16_t _motor_driver)
00044 {
00045     uint32_t pulseWidth = 0;
00046 
00047     if (_motor_driver == 0) {
00048         pwmout_pulsewidth_us(&_pwmA, 0);
00049         gpio_write(&gpioB, 0);
00050     } else if (_motor_driver > 0) {
00051         pulseWidth = _period_us / 255 * _motor_driver;
00052         pwmout_pulsewidth_us(&_pwmA, pulseWidth);
00053         gpio_write(&gpioB, 0);
00054     } else {
00055         //_motor_driver = abs(_motor_driver);
00056         pulseWidth = _period_us / 255 * (255 + _motor_driver);
00057         pwmout_pulsewidth_us(&_pwmA, pulseWidth);
00058         gpio_write(&gpioB, 1);
00059     }
00060 }
00061 
00062 void Motor::Free()
00063 {
00064     pwmout_pulsewidth_us(&_pwmA, 0);
00065     gpio_write(&gpioB, 0);
00066 }
00067 
00068 void Motor::Brake()
00069 {
00070     pwmout_pulsewidth_us(&_pwmA, _period_us);
00071     gpio_write(&gpioB, 1);
00072 }