109 lines
3.0 KiB
C
109 lines
3.0 KiB
C
#include "mgos.h"
|
|
#include "mgos_system.h"
|
|
#include "mgos_i2c.h"
|
|
#include "WEMOS_Motor.h"
|
|
|
|
#define SUPPRESS_SENDING_UNCHANGED_VALUSE 0
|
|
|
|
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];
|
|
|
|
LOG(LL_INFO, ("Writing frequency %d to controller of motor %d (addr=%d)", freq, motorNumber, motor_i2c_address[motorNumber]));
|
|
if (motorNumber < MAX_NUM_MOTORS || !initialized) {
|
|
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);
|
|
LOG(LL_INFO, ("mgos_i2c_write(i2c, addr=%d, data={%d,%d,%d,%d}, 4, stop=true)", motor_i2c_address[motorNumber], data[0], data[1], data[2], data[3]));
|
|
// mgos_msleep(100);
|
|
} else {
|
|
LOG(LL_ERROR, ("Invalid motor number %d, maximum %d motors known", motorNumber, MAX_NUM_MOTORS));
|
|
}
|
|
}
|
|
|
|
/* 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();
|
|
LOG(LL_INFO, ("wemos_motor_initMotor(motorNumber=%d, address=%d, freq=%d", motorNumber, address, freq));
|
|
if (motorNumber < MAX_NUM_MOTORS) {
|
|
motor_freq[motorNumber] = freq;
|
|
motor_i2c_address[motorNumber] = address;
|
|
wemos_motor_set_freq(motorNumber, freq);
|
|
} else {
|
|
LOG(LL_ERROR, ("Invalid motor number %d, maximum %d motors known", motorNumber, MAX_NUM_MOTORS));
|
|
}
|
|
}
|
|
|
|
|
|
/* setmotor() -- set motor
|
|
motorNumber: 0..n
|
|
dir:
|
|
_SHORT_BRAKE 0
|
|
_CW 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];
|
|
|
|
#if SUPPRESS_SENDING_UNCHANGED_VALUSE
|
|
static int lastDir = -1;
|
|
static int lastPwm = -1.0;
|
|
|
|
if (lastDir == dir && lastPwm == pwm_val) return;
|
|
lastDir = dir;
|
|
lastPwm = pwm_val;
|
|
#endif
|
|
|
|
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);
|
|
// LOG(LL_INFO, ("mgos_i2c_write(Mot=%d, adr=%d, data={%d,%d,%d,%d}, 4, stop=true", motorNumber, motor_i2c_address[motorNumber], data[0], data[1], data[2], data[3]));
|
|
} else {
|
|
LOG(LL_ERROR, ("Invalid motor number %d, maximum %d motors known", motorNumber, MAX_NUM_MOTORS));
|
|
}
|
|
}
|
|
|
|
void wemos_motor_stop(uint8_t motorNumber) {
|
|
wemos_motor_setmotor(motorNumber, _STOP, 0.0);
|
|
}
|
|
|
|
void wemos_motor_standby(uint8_t motorNumber) {
|
|
wemos_motor_setmotor(motorNumber, _STANDBY, 0.0);
|
|
} |