Diff: Microduino_Motor.cpp

Revision:

0:362c1482232c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Microduino_Motor.cpp	Thu Jun 02 04:03:31 2016 +0000
@@ -0,0 +1,159 @@
+#include "Microduino_Motor.h"
+
+static motor_t motors[10];                          // static array of key structures
+
+uint8_t MotorCount = 0;                                     // the total number of attached keys
+
+Motor::Motor(PinName _motor_pinA, PinName _motor_pinB)
+{
+    if ( MotorCount < 10) {
+        this->motorIndex = MotorCount++;                    // assign a key index to this instance
+        //if (_motor_pinA < NUM_DIGITAL_PINS && _motor_pinB < NUM_DIGITAL_PINS) {
+        if (true) {
+#if 0
+            pinMode( _motor_pinA, OUTPUT) ;
+#else
+            _period_us = 255; // 500Hz
+            pwmout_init(&_pwmA, _motor_pinA);
+            pwmout_period_us(&_pwmA, _period_us); // 500Hz
+            pwmout_pulsewidth_us(&_pwmA, 1);
+#endif
+            motors[this->motorIndex].Pin.nbr_A = _motor_pinA;
+
+#if 0
+            pinMode( _motor_pinB, OUTPUT) ;
+#else
+            pwmout_init(&_pwmB, _motor_pinB);
+            pwmout_period_us(&_pwmB, _period_us); // 500Hz
+            pwmout_pulsewidth_us(&_pwmB, 1);
+#endif
+            motors[this->motorIndex].Pin.nbr_B = _motor_pinB;
+
+            this->fix=1;
+        }
+    } else {
+        this->motorIndex = 255 ;  // too many keys
+    }
+}
+
+void Motor::Fix(float _fix)
+{
+    this->fix=_fix;
+}
+
+int16_t Motor::GetData(int16_t _throttle, int16_t _steering, uint8_t _dir)
+{
+    this->_motor_vol = _throttle;
+
+    if(_dir == 1)
+    {
+        this->_motor_vol -= _steering / 2;
+    }
+    else
+    {
+        this->_motor_vol += _steering / 2;
+    }
+    if (this->_motor_vol > 255)
+        this->_motor_vol = 255;
+    else if (this->_motor_vol < -255)
+        this->_motor_vol = -255;
+
+    //this->_motor_vol *= fix;
+
+    return this->_motor_vol;
+}
+
+#if 0
+void Motor::Driver(int16_t _motor_driver)
+{
+    int8_t channel_A = motors[this->motorIndex].Pin.nbr_A;
+    int8_t channel_B = motors[this->motorIndex].Pin.nbr_B;
+    if (_motor_driver == 0)   {
+        digitalWrite(channel_A, LOW);
+        digitalWrite(channel_B, LOW);
+    } else if (_motor_driver > 0)   {
+        analogWrite(channel_A, _motor_driver);
+        digitalWrite(channel_B, LOW);
+    } else  {
+        analogWrite(channel_A, 255 + _motor_driver);
+        digitalWrite(channel_B, HIGH);
+    }
+}
+#else
+void Motor::Driver(int16_t _motor_driver)
+{
+    //static bool flag = true;
+    uint32_t pulseWidth = 0;
+    //PinName channel_A = motors[this->motorIndex].Pin.nbr_A;
+    //PinName channel_B = motors[this->motorIndex].Pin.nbr_B;
+    #if 0
+    pwmout_pulsewidth_us(&_pwmA, _period_us/2);
+    pwmout_pulsewidth_us(&_pwmB, 0);
+    return;
+    #endif
+    #if 0
+    pwmout_pulsewidth_us(&_pwmA, 0);
+    pwmout_pulsewidth_us(&_pwmB, _period_us/2);
+    return;
+    #endif
+    if (_motor_driver == 0) {
+        pwmout_pulsewidth_us(&_pwmA, 0);
+        pwmout_pulsewidth_us(&_pwmB, 0);
+    } else if (_motor_driver > 0) {
+        #if 1
+        pulseWidth = _period_us / 255 * _motor_driver;
+        pwmout_pulsewidth_us(&_pwmA, pulseWidth);
+        pwmout_pulsewidth_us(&_pwmB, 2);
+        #else
+        pwmout_pulsewidth_us(&_pwmA, _period_us/2);
+        pwmout_pulsewidth_us(&_pwmB, 0);
+        #endif
+    } else {
+        #if 0
+        _motor_driver = 255 + _motor_driver;
+        pulseWidth = _period_us / 255 * _motor_driver;
+        pwmout_pulsewidth_us(&_pwmA, 0);
+        pwmout_pulsewidth_us(&_pwmB, pulseWidth);
+        #elif 1
+        _motor_driver = abs(_motor_driver);
+        pulseWidth = _period_us / 255 * _motor_driver;
+        pwmout_pulsewidth_us(&_pwmA, 2);
+        pwmout_pulsewidth_us(&_pwmB, pulseWidth);
+        #elif 0
+        _motor_driver = 255 + _motor_driver;
+        pulseWidth = _period_us / 255 * _motor_driver;
+        pwmout_pulsewidth_us(&_pwmA, _period_us);
+        pwmout_pulsewidth_us(&_pwmB, pulseWidth);
+        #else
+        pwmout_pulsewidth_us(&_pwmA, 0);
+        pwmout_pulsewidth_us(&_pwmB, 241);
+        #endif
+    }
+}
+#endif
+
+void Motor::Free()
+{
+    int8_t channel_A = motors[this->motorIndex].Pin.nbr_A;
+    int8_t channel_B = motors[this->motorIndex].Pin.nbr_B;
+#if 0
+    digitalWrite(channel_A, LOW);
+    digitalWrite(channel_B, LOW);
+#else
+    pwmout_pulsewidth_us(&_pwmA, 0);
+    pwmout_pulsewidth_us(&_pwmB, 0);
+#endif
+}
+
+void Motor::Brake()
+{
+    int8_t channel_A = motors[this->motorIndex].Pin.nbr_A;
+    int8_t channel_B = motors[this->motorIndex].Pin.nbr_B;
+#if 0
+    digitalWrite(channel_A, HIGH);
+    digitalWrite(channel_B, HIGH);
+#else
+    pwmout_pulsewidth_us(&_pwmA, _period_us);
+    pwmout_pulsewidth_us(&_pwmB, _period_us);
+#endif
+}
\ No newline at end of file