Shinya Matsuyama / Mbed OS MF_FUJIKO_BASE

update from matsu

Dependencies:   libTCS34725

I2CMotorDriver.h@6:fd9d4fed9946, 2019-03-16 (annotated)

Committer:
shinyamatsuyama
Date:
Sat Mar 16 15:42:24 2019 +0000
Revision:
6:fd9d4fed9946
Parent:
0:bed71d1853ef 
091_sendfix; 11_udpread

Who changed what in which revision?

UserRevisionLine numberNew contents of line
poipoi 0:bed71d1853ef 1 #ifndef __I2CMOTORDRIVER_H__
poipoi 0:bed71d1853ef 2 #define __I2CMOTORDRIVER_H__
poipoi 0:bed71d1853ef 3
poipoi 0:bed71d1853ef 4 #include "mbed.h"
poipoi 0:bed71d1853ef 5
poipoi 0:bed71d1853ef 6 #define SCL PB_8
poipoi 0:bed71d1853ef 7 #define SDA PB_9
poipoi 0:bed71d1853ef 8
poipoi 0:bed71d1853ef 9 /******I2C command definitions*************/
poipoi 0:bed71d1853ef 10 #define MotorSpeedSet 0x82
poipoi 0:bed71d1853ef 11 #define PWMFrequenceSet 0x84
poipoi 0:bed71d1853ef 12 #define DirectionSet 0xaa
poipoi 0:bed71d1853ef 13 #define MotorSetA 0xa1
poipoi 0:bed71d1853ef 14 #define MotorSetB 0xa5
poipoi 0:bed71d1853ef 15 #define Nothing 0x01
poipoi 0:bed71d1853ef 16 /**************Motor ID**********************/
poipoi 0:bed71d1853ef 17 #define MOTOR1 1
poipoi 0:bed71d1853ef 18 #define MOTOR2 2
poipoi 0:bed71d1853ef 19 /**************Motor Direction***************/
poipoi 0:bed71d1853ef 20 #define BothClockWise 0x0a
poipoi 0:bed71d1853ef 21 #define BothAntiClockWise 0x05
poipoi 0:bed71d1853ef 22 #define M1CWM2ACW 0x06
poipoi 0:bed71d1853ef 23 #define M1ACWM2CW 0x09
poipoi 0:bed71d1853ef 24 /**************Motor Direction***************/
poipoi 0:bed71d1853ef 25 #define ClockWise 0x0a
poipoi 0:bed71d1853ef 26 #define AntiClockWise 0x05
poipoi 0:bed71d1853ef 27 /**************Prescaler Frequence***********/
poipoi 0:bed71d1853ef 28 #define F_31372Hz 0x01
poipoi 0:bed71d1853ef 29 #define F_3921Hz 0x02
poipoi 0:bed71d1853ef 30 #define F_490Hz 0x03
poipoi 0:bed71d1853ef 31 #define F_122Hz 0x04
poipoi 0:bed71d1853ef 32 #define F_30Hz 0x05
poipoi 0:bed71d1853ef 33
poipoi 0:bed71d1853ef 34 /*************************Class for Grove I2C Motor Driver********************/
poipoi 0:bed71d1853ef 35 class I2CMotorDriver
poipoi 0:bed71d1853ef 36 {
poipoi 0:bed71d1853ef 37
poipoi 0:bed71d1853ef 38 private:
poipoi 0:bed71d1853ef 39 static uint8_t steppTable[2][2][8];
poipoi 0:bed71d1853ef 40
poipoi 0:bed71d1853ef 41 I2C *p_i2c;
poipoi 0:bed71d1853ef 42 Mutex *p_i2c_lock;
poipoi 0:bed71d1853ef 43
poipoi 0:bed71d1853ef 44 unsigned char _i2c_add;
poipoi 0:bed71d1853ef 45 unsigned char _step_cnt;
poipoi 0:bed71d1853ef 46 unsigned int _step_time;
poipoi 0:bed71d1853ef 47
poipoi 0:bed71d1853ef 48 Thread th;
poipoi 0:bed71d1853ef 49
poipoi 0:bed71d1853ef 50 enum COMMAND {
poipoi 0:bed71d1853ef 51 COMMAND_STEP,
poipoi 0:bed71d1853ef 52 COMMAND_FREE,
poipoi 0:bed71d1853ef 53 };
poipoi 0:bed71d1853ef 54
poipoi 0:bed71d1853ef 55 COMMAND cmd;
poipoi 0:bed71d1853ef 56 int step_num;
poipoi 0:bed71d1853ef 57 int step_type;
poipoi 0:bed71d1853ef 58 int step_mode;
poipoi 0:bed71d1853ef 59
poipoi 0:bed71d1853ef 60 void setSpeed(uint8_t speedA, uint8_t speedB) {
poipoi 0:bed71d1853ef 61 uint8_t data[] = {MotorSpeedSet, speedA, speedB};
poipoi 0:bed71d1853ef 62 p_i2c_lock->lock();
poipoi 0:bed71d1853ef 63 p_i2c->write(_i2c_add, (char*)data, sizeof(data));
poipoi 0:bed71d1853ef 64 p_i2c_lock->unlock();
poipoi 0:bed71d1853ef 65 }
poipoi 0:bed71d1853ef 66
poipoi 0:bed71d1853ef 67 void setFrequence(uint8_t frequence) {
poipoi 0:bed71d1853ef 68 if (frequence < F_31372Hz || frequence > F_30Hz) {
poipoi 0:bed71d1853ef 69 return;
poipoi 0:bed71d1853ef 70 }
poipoi 0:bed71d1853ef 71 uint8_t data[] = {PWMFrequenceSet, frequence, Nothing};
poipoi 0:bed71d1853ef 72 p_i2c_lock->lock();
poipoi 0:bed71d1853ef 73 p_i2c->write(_i2c_add, (char*)data, sizeof(data));
poipoi 0:bed71d1853ef 74 p_i2c_lock->unlock();
poipoi 0:bed71d1853ef 75 }
poipoi 0:bed71d1853ef 76
poipoi 0:bed71d1853ef 77 void setDirection(uint8_t direction) {
poipoi 0:bed71d1853ef 78 uint8_t data[] = {DirectionSet, direction, Nothing};
poipoi 0:bed71d1853ef 79 p_i2c_lock->lock();
poipoi 0:bed71d1853ef 80 p_i2c->write(_i2c_add, (char*)data, sizeof(data));
poipoi 0:bed71d1853ef 81 p_i2c_lock->unlock();
poipoi 0:bed71d1853ef 82 }
poipoi 0:bed71d1853ef 83
poipoi 0:bed71d1853ef 84 void step_inner() {
poipoi 0:bed71d1853ef 85 uint8_t dir = step_num >= 0 ? 0 : 1;
poipoi 0:bed71d1853ef 86 step_num = step_num >= 0 ? step_num : -step_num;
poipoi 0:bed71d1853ef 87 setSpeed(255, 255);
poipoi 0:bed71d1853ef 88 uint8_t* table = steppTable[step_type][dir];
poipoi 0:bed71d1853ef 89 for (int i = 0; i < step_num; i++) {
poipoi 0:bed71d1853ef 90 if (step_mode == 0) {
poipoi 0:bed71d1853ef 91 for (int j = 0; j < 8; j++) {
poipoi 0:bed71d1853ef 92 setDirection(table[j]);
poipoi 0:bed71d1853ef 93 wait_ms(_step_time);
poipoi 0:bed71d1853ef 94 }
poipoi 0:bed71d1853ef 95 } else {
poipoi 0:bed71d1853ef 96 setDirection(table[_step_cnt + 0]);
poipoi 0:bed71d1853ef 97 Thread::wait(_step_time);
poipoi 0:bed71d1853ef 98 setDirection(table[_step_cnt + 1]);
poipoi 0:bed71d1853ef 99 Thread::wait(_step_time);
poipoi 0:bed71d1853ef 100 _step_cnt = (_step_cnt + 2) % 8;
poipoi 0:bed71d1853ef 101 }
poipoi 0:bed71d1853ef 102 }
poipoi 0:bed71d1853ef 103 setSpeed(0, 0);
poipoi 0:bed71d1853ef 104 }
poipoi 0:bed71d1853ef 105
poipoi 0:bed71d1853ef 106 void free_inner() {
poipoi 0:bed71d1853ef 107 setSpeed(0, 0);
poipoi 0:bed71d1853ef 108 }
poipoi 0:bed71d1853ef 109
poipoi 0:bed71d1853ef 110 void thread_inner() {
poipoi 0:bed71d1853ef 111 while(true) {
poipoi 0:bed71d1853ef 112 th.signal_wait(0x01);
poipoi 0:bed71d1853ef 113
poipoi 0:bed71d1853ef 114 switch (cmd) {
poipoi 0:bed71d1853ef 115 case COMMAND_STEP:
poipoi 0:bed71d1853ef 116 step_inner();
poipoi 0:bed71d1853ef 117 break;
poipoi 0:bed71d1853ef 118 case COMMAND_FREE:
poipoi 0:bed71d1853ef 119 free_inner();
poipoi 0:bed71d1853ef 120 break;
poipoi 0:bed71d1853ef 121 }
poipoi 0:bed71d1853ef 122 }
poipoi 0:bed71d1853ef 123 }
poipoi 0:bed71d1853ef 124
poipoi 0:bed71d1853ef 125 public:
poipoi 0:bed71d1853ef 126
poipoi 0:bed71d1853ef 127 I2CMotorDriver(I2C& i2c, Mutex& i2cLock) : p_i2c(&i2c), p_i2c_lock(&i2cLock), _step_time(10), _i2c_add(0x0F << 1), _step_cnt(0) {}
poipoi 0:bed71d1853ef 128 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) {}
poipoi 0:bed71d1853ef 129
poipoi 0:bed71d1853ef 130 int begin(unsigned char i2c_add) {
poipoi 0:bed71d1853ef 131 if (i2c_add > 0x0f) {
poipoi 0:bed71d1853ef 132 return(-1);
poipoi 0:bed71d1853ef 133 }
poipoi 0:bed71d1853ef 134 this->_i2c_add = i2c_add << 1;
poipoi 0:bed71d1853ef 135
poipoi 0:bed71d1853ef 136 // Set default frequence to F_3921Hz
poipoi 0:bed71d1853ef 137 setFrequence(F_3921Hz);
poipoi 0:bed71d1853ef 138
poipoi 0:bed71d1853ef 139 th.start(this, &I2CMotorDriver::thread_inner);
poipoi 0:bed71d1853ef 140 return(0);
poipoi 0:bed71d1853ef 141 }
poipoi 0:bed71d1853ef 142
poipoi 0:bed71d1853ef 143 void free() {
poipoi 0:bed71d1853ef 144 cmd = COMMAND_FREE;
poipoi 0:bed71d1853ef 145 th.signal_set(0x01);
poipoi 0:bed71d1853ef 146 }
poipoi 0:bed71d1853ef 147
poipoi 0:bed71d1853ef 148 void step(int stepCount, int type = 0, int mode = 0) {
poipoi 0:bed71d1853ef 149 step_num = stepCount;
poipoi 0:bed71d1853ef 150 step_type = type;
poipoi 0:bed71d1853ef 151 step_mode = mode;
poipoi 0:bed71d1853ef 152 cmd = COMMAND_STEP;
poipoi 0:bed71d1853ef 153 th.signal_set(0x01);
poipoi 0:bed71d1853ef 154 }
poipoi 0:bed71d1853ef 155 };
poipoi 0:bed71d1853ef 156
poipoi 0:bed71d1853ef 157 uint8_t I2CMotorDriver::steppTable[2][2][8] = {
poipoi 0:bed71d1853ef 158 {
poipoi 0:bed71d1853ef 159 { 0b0001, 0b0011, 0b0010, 0b0110, 0b0100, 0b1100, 0b1000, 0b1001 },
poipoi 0:bed71d1853ef 160 { 0b1000, 0b1100, 0b0100, 0b0110, 0b0010, 0b0011, 0b0001, 0b1001 },
poipoi 0:bed71d1853ef 161 },
poipoi 0:bed71d1853ef 162 {
poipoi 0:bed71d1853ef 163 { 0b0001, 0b0101, 0b0100, 0b0110, 0b0010, 0b1010, 0b1000, 0b1001 },
poipoi 0:bed71d1853ef 164 { 0b1000, 0b1010, 0b0010, 0b0110, 0b0100, 0b0101, 0b0001, 0b1001 },
poipoi 0:bed71d1853ef 165 },
poipoi 0:bed71d1853ef 166 };
poipoi 0:bed71d1853ef 167
poipoi 0:bed71d1853ef 168 #endif