ESP32_OSC_IMU/lib/M5Stack/examples/Modules/BALA2/bala.cpp

#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); }

}