Diff: I2CMotorDriver.h

Revision:

0:bed71d1853ef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/I2CMotorDriver.h	Wed Jan 30 05:20:36 2019 +0000
@@ -0,0 +1,168 @@
+#ifndef __I2CMOTORDRIVER_H__
+#define __I2CMOTORDRIVER_H__
+
+#include "mbed.h"
+
+#define SCL            PB_8
+#define SDA            PB_9
+
+/******I2C command definitions*************/
+#define MotorSpeedSet             0x82
+#define PWMFrequenceSet           0x84
+#define DirectionSet              0xaa
+#define MotorSetA                 0xa1
+#define MotorSetB                 0xa5
+#define Nothing                   0x01
+/**************Motor ID**********************/
+#define MOTOR1                    1
+#define MOTOR2                    2
+/**************Motor Direction***************/
+#define BothClockWise             0x0a
+#define BothAntiClockWise         0x05
+#define M1CWM2ACW                 0x06
+#define M1ACWM2CW                 0x09
+/**************Motor Direction***************/
+#define ClockWise                 0x0a
+#define AntiClockWise             0x05
+/**************Prescaler Frequence***********/
+#define F_31372Hz                 0x01
+#define F_3921Hz                  0x02
+#define F_490Hz                   0x03
+#define F_122Hz                   0x04
+#define F_30Hz                    0x05
+
+/*************************Class for Grove I2C Motor Driver********************/
+class I2CMotorDriver
+{
+
+private:
+    static uint8_t steppTable[2][2][8];
+
+    I2C *p_i2c;
+    Mutex *p_i2c_lock;
+    
+    unsigned char _i2c_add;
+    unsigned char _step_cnt;
+    unsigned int _step_time;
+    
+    Thread th;
+    
+    enum COMMAND {
+        COMMAND_STEP,
+        COMMAND_FREE,
+    };
+
+    COMMAND cmd;
+    int step_num;
+    int step_type;
+    int step_mode;
+
+    void setSpeed(uint8_t speedA, uint8_t speedB) {
+        uint8_t data[] = {MotorSpeedSet, speedA, speedB};
+        p_i2c_lock->lock();
+        p_i2c->write(_i2c_add, (char*)data, sizeof(data));
+        p_i2c_lock->unlock();
+    }
+    
+    void setFrequence(uint8_t frequence) {
+        if (frequence < F_31372Hz || frequence > F_30Hz) {
+            return;
+        }
+        uint8_t data[] = {PWMFrequenceSet, frequence, Nothing};
+        p_i2c_lock->lock();
+        p_i2c->write(_i2c_add, (char*)data, sizeof(data));
+        p_i2c_lock->unlock();
+    }
+
+    void setDirection(uint8_t direction) {
+        uint8_t data[] = {DirectionSet, direction, Nothing};
+        p_i2c_lock->lock();
+        p_i2c->write(_i2c_add, (char*)data, sizeof(data));
+        p_i2c_lock->unlock();
+    }
+    
+    void step_inner() {
+        uint8_t dir = step_num >= 0 ? 0 : 1;
+        step_num = step_num >= 0 ? step_num : -step_num;
+        setSpeed(255, 255);
+        uint8_t* table = steppTable[step_type][dir];
+        for (int i = 0; i < step_num; i++) {
+            if (step_mode == 0) {
+                for (int j = 0; j < 8; j++) {
+                    setDirection(table[j]);
+                    wait_ms(_step_time);
+                }
+            } else {
+                setDirection(table[_step_cnt + 0]);
+                Thread::wait(_step_time);
+                setDirection(table[_step_cnt + 1]);
+                Thread::wait(_step_time);
+                _step_cnt = (_step_cnt + 2) % 8;
+            }
+        }
+        setSpeed(0, 0);
+    }
+    
+    void free_inner() {
+        setSpeed(0, 0);
+    }
+    
+    void thread_inner() {
+        while(true) {
+            th.signal_wait(0x01);
+            
+            switch (cmd) {
+                case COMMAND_STEP:
+                    step_inner();
+                    break;
+                case COMMAND_FREE:
+                    free_inner();
+                    break;
+            }
+        }
+    }
+
+public:
+
+    I2CMotorDriver(I2C& i2c, Mutex& i2cLock) : p_i2c(&i2c), p_i2c_lock(&i2cLock), _step_time(10), _i2c_add(0x0F << 1), _step_cnt(0) {}
+    I2CMotorDriver(I2C& i2c, Mutex& i2cLock, unsigned int stepTime) : p_i2c(&i2c), p_i2c_lock(&i2cLock), _step_time(stepTime), _i2c_add(0x0F << 1), _step_cnt(0) {}
+    
+    int begin(unsigned char i2c_add) {
+        if (i2c_add > 0x0f) {
+            return(-1);
+        }
+        this->_i2c_add = i2c_add << 1;
+
+        // Set default frequence to F_3921Hz
+        setFrequence(F_3921Hz);
+        
+        th.start(this, &I2CMotorDriver::thread_inner);
+        return(0);
+    }    
+
+    void free() {
+        cmd = COMMAND_FREE;
+        th.signal_set(0x01);
+    }
+    
+    void step(int stepCount, int type = 0, int mode = 0) {
+        step_num = stepCount;
+        step_type = type;
+        step_mode = mode;
+        cmd = COMMAND_STEP;
+        th.signal_set(0x01);
+    }
+};
+
+uint8_t I2CMotorDriver::steppTable[2][2][8] = {
+    {
+        { 0b0001, 0b0011, 0b0010, 0b0110, 0b0100, 0b1100, 0b1000, 0b1001 },
+        { 0b1000, 0b1100, 0b0100, 0b0110, 0b0010, 0b0011, 0b0001, 0b1001 },
+    },
+    {
+        { 0b0001, 0b0101, 0b0100, 0b0110, 0b0010, 0b1010, 0b1000, 0b1001 },
+        { 0b1000, 0b1010, 0b0010, 0b0110, 0b0100, 0b0101, 0b0001, 0b1001 },
+    },
+};
+
+#endif
\ No newline at end of file