mirror of
https://github.com/m5stack/M5Stack.git
synced 2026-05-20 10:06:46 -07:00
Gitshaoxiang
97 lines
2.6 KiB
C++
97 lines
2.6 KiB
C++
#include "bala.h"
|
|
#include "M5Stack.h"
|
|
|
|
Bala::Bala() {
|
|
wheel_left_encoder = 0;
|
|
wheel_right_encoder = 0;
|
|
i2c_mutex = NULL;
|
|
}
|
|
|
|
void Bala::ClearEncoder() {
|
|
SetEncoder(0, 0);
|
|
wheel_left_encoder = 0;
|
|
wheel_right_encoder = 0;
|
|
}
|
|
|
|
void Bala::GetEncoder(int32_t* wheel_left, int32_t* wheel_right) {
|
|
UpdateEncoder();
|
|
*wheel_left = wheel_left_encoder;
|
|
*wheel_right = wheel_right_encoder;
|
|
}
|
|
|
|
void Bala::SetEncoder(int32_t wheel_left, int32_t wheel_right) {
|
|
uint8_t data_out[8];
|
|
data_out[0] = (uint8_t)(wheel_left >> 24);
|
|
data_out[1] = (uint8_t)(wheel_left >> 16);
|
|
data_out[2] = (uint8_t)(wheel_left >> 8);
|
|
data_out[3] = (uint8_t)(wheel_left >> 0);
|
|
|
|
data_out[4] = (uint8_t)(wheel_right >> 24);
|
|
data_out[5] = (uint8_t)(wheel_right >> 16);
|
|
data_out[6] = (uint8_t)(wheel_right >> 8);
|
|
data_out[7] = (uint8_t)(wheel_right >> 0);
|
|
if(i2c_mutex != NULL) { xSemaphoreTake(i2c_mutex, portMAX_DELAY); }
|
|
M5.I2C.writeBytes(0x3A, 0x10, data_out, 8);
|
|
if(i2c_mutex != NULL) { xSemaphoreGive(i2c_mutex); }
|
|
}
|
|
|
|
void Bala::UpdateEncoder() {
|
|
uint8_t data_in[8];
|
|
if(i2c_mutex != NULL) { xSemaphoreTake(i2c_mutex, portMAX_DELAY); }
|
|
M5.I2C.readBytes(0x3A, 0x10, 8, data_in);
|
|
if(i2c_mutex != NULL) { xSemaphoreGive(i2c_mutex); }
|
|
|
|
wheel_left_encoder = (data_in[0] << 24) | (data_in[1] << 16) | (data_in[2] << 8) | data_in[3];
|
|
wheel_right_encoder = (data_in[4] << 24) | (data_in[5] << 16) | (data_in[6] << 8) | data_in[7];
|
|
}
|
|
|
|
void Bala::SetSpeed(int16_t wheel_left, int16_t wheel_right) {
|
|
uint8_t data_out[4];
|
|
|
|
data_out[0] = (int8_t)(wheel_left >> 8);
|
|
data_out[1] = (int8_t)(wheel_left >> 0);
|
|
data_out[2] = (int8_t)(wheel_right >> 8);
|
|
data_out[3] = (int8_t)(wheel_right >> 0);
|
|
|
|
if(i2c_mutex != NULL) { xSemaphoreTake(i2c_mutex, portMAX_DELAY); }
|
|
M5.I2C.writeBytes(0x3A, 0x00, data_out, 4);
|
|
if(i2c_mutex != NULL) { xSemaphoreGive(i2c_mutex); }
|
|
}
|
|
|
|
void Bala::SetMutex(SemaphoreHandle_t* mutex) {
|
|
i2c_mutex = *mutex;
|
|
}
|
|
|
|
void Bala::SetServoAngle(uint8_t pos, uint8_t angle) {
|
|
if (pos < 1) {
|
|
pos = 1;
|
|
} else if (pos > 8) {
|
|
pos = 8;
|
|
}
|
|
|
|
pos = pos - 1;
|
|
|
|
if(i2c_mutex != NULL) { xSemaphoreTake(i2c_mutex, portMAX_DELAY); }
|
|
M5.I2C.writeBytes(0x3A, 0x20 | pos, &angle, 1);
|
|
if(i2c_mutex != NULL) { xSemaphoreGive(i2c_mutex); }
|
|
}
|
|
|
|
void Bala::SetServoPulse(uint8_t pos, uint16_t width) {
|
|
if (pos < 1) {
|
|
pos = 1;
|
|
} else if (pos > 8) {
|
|
pos = 8;
|
|
}
|
|
|
|
pos = (pos - 1) << 1;
|
|
uint8_t buff_out[2];
|
|
buff_out[0] = width >> 8;
|
|
buff_out[1] = width & 0xff;
|
|
|
|
if(i2c_mutex != NULL) { xSemaphoreTake(i2c_mutex, portMAX_DELAY); }
|
|
M5.I2C.writeBytes(0x3A, 0x30 | pos, buff_out, 2);
|
|
if(i2c_mutex != NULL) { xSemaphoreGive(i2c_mutex); }
|
|
|
|
}
|
|
|