Microduino_Motor.cpp

00001 #include "Microduino_Motor.h"
00002 
00003 static motor_t motors[10];                          // static array of key structures
00004 
00005 uint8_t MotorCount = 0;                                     // the total number of attached keys
00006 
00007 Motor::Motor(PinName _motor_pinA, PinName _motor_pinB)
00008 {
00009     if ( MotorCount < 10) {
00010         this->motorIndex = MotorCount++;                    // assign a key index to this instance
00011         //if (_motor_pinA < NUM_DIGITAL_PINS && _motor_pinB < NUM_DIGITAL_PINS) {
00012         if (true) {
00013 #if 0
00014             pinMode( _motor_pinA, OUTPUT) ;
00015 #else
00016             _period_us = 255; // 500Hz
00017             pwmout_init(&_pwmA, _motor_pinA);
00018             pwmout_period_us(&_pwmA, _period_us); // 500Hz
00019             pwmout_pulsewidth_us(&_pwmA, 1);
00020 #endif
00021             motors[this->motorIndex].Pin.nbr_A = _motor_pinA;
00022 
00023 #if 0
00024             pinMode( _motor_pinB, OUTPUT) ;
00025 #else
00026             pwmout_init(&_pwmB, _motor_pinB);
00027             pwmout_period_us(&_pwmB, _period_us); // 500Hz
00028             pwmout_pulsewidth_us(&_pwmB, 1);
00029 #endif
00030             motors[this->motorIndex].Pin.nbr_B = _motor_pinB;
00031 
00032             this->fix=1;
00033         }
00034     } else {
00035         this->motorIndex = 255 ;  // too many keys
00036     }
00037 }
00038 
00039 void Motor::Fix(float _fix)
00040 {
00041     this->fix=_fix;
00042 }
00043 
00044 int16_t Motor::GetData(int16_t _throttle, int16_t _steering, uint8_t _dir)
00045 {
00046     this->_motor_vol = _throttle;
00047 
00048     if(_dir == 1)
00049     {
00050         this->_motor_vol -= _steering / 2;
00051     }
00052     else
00053     {
00054         this->_motor_vol += _steering / 2;
00055     }
00056     if (this->_motor_vol > 255)
00057         this->_motor_vol = 255;
00058     else if (this->_motor_vol < -255)
00059         this->_motor_vol = -255;
00060 
00061     //this->_motor_vol *= fix;
00062 
00063     return this->_motor_vol;
00064 }
00065 
00066 #if 0
00067 void Motor::Driver(int16_t _motor_driver)
00068 {
00069     int8_t channel_A = motors[this->motorIndex].Pin.nbr_A;
00070     int8_t channel_B = motors[this->motorIndex].Pin.nbr_B;
00071     if (_motor_driver == 0)   {
00072         digitalWrite(channel_A, LOW);
00073         digitalWrite(channel_B, LOW);
00074     } else if (_motor_driver > 0)   {
00075         analogWrite(channel_A, _motor_driver);
00076         digitalWrite(channel_B, LOW);
00077     } else  {
00078         analogWrite(channel_A, 255 + _motor_driver);
00079         digitalWrite(channel_B, HIGH);
00080     }
00081 }
00082 #else
00083 void Motor::Driver(int16_t _motor_driver)
00084 {
00085     //static bool flag = true;
00086     uint32_t pulseWidth = 0;
00087     //PinName channel_A = motors[this->motorIndex].Pin.nbr_A;
00088     //PinName channel_B = motors[this->motorIndex].Pin.nbr_B;
00089     #if 0
00090     pwmout_pulsewidth_us(&_pwmA, _period_us/2);
00091     pwmout_pulsewidth_us(&_pwmB, 0);
00092     return;
00093     #endif
00094     #if 0
00095     pwmout_pulsewidth_us(&_pwmA, 0);
00096     pwmout_pulsewidth_us(&_pwmB, _period_us/2);
00097     return;
00098     #endif
00099     if (_motor_driver == 0) {
00100         pwmout_pulsewidth_us(&_pwmA, 0);
00101         pwmout_pulsewidth_us(&_pwmB, 0);
00102     } else if (_motor_driver > 0) {
00103         #if 1
00104         pulseWidth = _period_us / 255 * _motor_driver;
00105         pwmout_pulsewidth_us(&_pwmA, pulseWidth);
00106         pwmout_pulsewidth_us(&_pwmB, 2);
00107         #else
00108         pwmout_pulsewidth_us(&_pwmA, _period_us/2);
00109         pwmout_pulsewidth_us(&_pwmB, 0);
00110         #endif
00111     } else {
00112         #if 0
00113         _motor_driver = 255 + _motor_driver;
00114         pulseWidth = _period_us / 255 * _motor_driver;
00115         pwmout_pulsewidth_us(&_pwmA, 0);
00116         pwmout_pulsewidth_us(&_pwmB, pulseWidth);
00117         #elif 1
00118         _motor_driver = abs(_motor_driver);
00119         pulseWidth = _period_us / 255 * _motor_driver;
00120         pwmout_pulsewidth_us(&_pwmA, 2);
00121         pwmout_pulsewidth_us(&_pwmB, pulseWidth);
00122         #elif 0
00123         _motor_driver = 255 + _motor_driver;
00124         pulseWidth = _period_us / 255 * _motor_driver;
00125         pwmout_pulsewidth_us(&_pwmA, _period_us);
00126         pwmout_pulsewidth_us(&_pwmB, pulseWidth);
00127         #else
00128         pwmout_pulsewidth_us(&_pwmA, 0);
00129         pwmout_pulsewidth_us(&_pwmB, 241);
00130         #endif
00131     }
00132 }
00133 #endif
00134 
00135 void Motor::Free()
00136 {
00137     int8_t channel_A = motors[this->motorIndex].Pin.nbr_A;
00138     int8_t channel_B = motors[this->motorIndex].Pin.nbr_B;
00139 #if 0
00140     digitalWrite(channel_A, LOW);
00141     digitalWrite(channel_B, LOW);
00142 #else
00143     pwmout_pulsewidth_us(&_pwmA, 0);
00144     pwmout_pulsewidth_us(&_pwmB, 0);
00145 #endif
00146 }
00147 
00148 void Motor::Brake()
00149 {
00150     int8_t channel_A = motors[this->motorIndex].Pin.nbr_A;
00151     int8_t channel_B = motors[this->motorIndex].Pin.nbr_B;
00152 #if 0
00153     digitalWrite(channel_A, HIGH);
00154     digitalWrite(channel_B, HIGH);
00155 #else
00156     pwmout_pulsewidth_us(&_pwmA, _period_us);
00157     pwmout_pulsewidth_us(&_pwmB, _period_us);
00158 #endif
00159 }