89 lines
1.8 KiB
C
89 lines
1.8 KiB
C
#include "mgos_system.h"
|
|
#include "mgos_i2c.h"
|
|
#include "WEMOS_Motor.h"
|
|
|
|
|
|
static struct mgos_i2c *i2c;
|
|
static uint32_t motor_freq[MAX_NUM_MOTORS];
|
|
static uint16_t motor_i2c_address[MAX_NUM_MOTORS];
|
|
static bool initialized = false;
|
|
|
|
void wemos_motor_init() {
|
|
if (!initialized) {
|
|
i2c = mgos_i2c_get_global();
|
|
initialized = true;
|
|
}
|
|
}
|
|
|
|
/* setfreq() -- set PWM's frequency
|
|
motorNumber: 0..n
|
|
freq: PWM's frequency
|
|
*/
|
|
void wemos_motor_set_freq(uint8_t motorNumber, uint32_t freq) {
|
|
uint8_t data[4];
|
|
|
|
wemos_motor_init();
|
|
if (motorNumber < MAX_NUM_MOTORS) {
|
|
data[0] = (uint8_t) (freq >> 16) & 0x0f;
|
|
data[1] = (uint8_t) (freq >> 16);
|
|
data[2] = (uint8_t) (freq >> 8);
|
|
data[3] = (uint8_t) freq;
|
|
mgos_i2c_write(i2c, motor_i2c_address[motorNumber], data, 4, true);
|
|
// mgos_msleep(100);
|
|
} else {
|
|
// LOG ERROR
|
|
}
|
|
}
|
|
|
|
/* Motor()
|
|
motorNumber: 0..n
|
|
address: Shield I2C address
|
|
freq: PWM's frequency
|
|
*/
|
|
void wemos_motor_initMotor(uint8_t motorNumber, uint8_t address, uint32_t freq) {
|
|
wemos_motor_init();
|
|
|
|
if (motorNumber < MAX_NUM_MOTORS) {
|
|
motor_freq[motorNumber] = freq;
|
|
motor_i2c_address[motorNumber] = address;
|
|
wemos_motor_set_freq(motorNumber, freq);
|
|
} else {
|
|
// LOG ERROR
|
|
}
|
|
}
|
|
|
|
|
|
/* setmotor() -- set motor
|
|
motorNumber: 0..n
|
|
dir:
|
|
_SHORT_BRAKE 0
|
|
_CCW 1
|
|
_CCW 2
|
|
_STOP 3
|
|
_STANDBY 4
|
|
|
|
pwm_val:
|
|
0.00 - 100.00 (%)
|
|
*/
|
|
void wemos_motor_setmotor(uint8_t motorNumber, uint8_t dir, float pwm_val)
|
|
{
|
|
uint16_t _pwm_val;
|
|
uint8_t data[4];
|
|
|
|
wemos_motor_init();
|
|
if (motorNumber < MAX_NUM_MOTORS) {
|
|
data[0] = (uint8_t) (motorNumber & 0x01) | (uint8_t) 0x10;
|
|
data[1] = dir;
|
|
_pwm_val = (uint16_t) (pwm_val * 100);
|
|
|
|
if(_pwm_val>10000)
|
|
_pwm_val=10000;
|
|
data[2] = (uint8_t) (_pwm_val >> 8);
|
|
data[3] = (uint8_t) _pwm_val;
|
|
mgos_i2c_write(i2c, motor_i2c_address[motorNumber], data, 4, true);
|
|
// mgos_msleep(100);
|
|
} else {
|
|
// LOG ERROR
|
|
}
|
|
}
|