ESPHome  2024.3.2
grove_tb6612fng.cpp
Go to the documentation of this file.
1 #include "grove_tb6612fng.h"
2 #include "esphome/core/log.h"
3 #include "esphome/core/hal.h"
4 
5 namespace esphome {
6 namespace grove_tb6612fng {
7 
8 static const char *const TAG = "GroveMotorDriveTB6612FNG";
9 
10 static const uint8_t GROVE_MOTOR_DRIVER_I2C_CMD_BRAKE = 0x00;
11 static const uint8_t GROVE_MOTOR_DRIVER_I2C_CMD_STOP = 0x01;
12 static const uint8_t GROVE_MOTOR_DRIVER_I2C_CMD_CW = 0x02;
13 static const uint8_t GROVE_MOTOR_DRIVER_I2C_CMD_CCW = 0x03;
14 static const uint8_t GROVE_MOTOR_DRIVER_I2C_CMD_STANDBY = 0x04;
15 static const uint8_t GROVE_MOTOR_DRIVER_I2C_CMD_NOT_STANDBY = 0x05;
16 static const uint8_t GROVE_MOTOR_DRIVER_I2C_CMD_STEPPER_RUN = 0x06;
17 static const uint8_t GROVE_MOTOR_DRIVER_I2C_CMD_STEPPER_STOP = 0x07;
18 static const uint8_t GROVE_MOTOR_DRIVER_I2C_CMD_STEPPER_KEEP_RUN = 0x08;
19 static const uint8_t GROVE_MOTOR_DRIVER_I2C_CMD_SET_ADDR = 0x11;
20 
22  ESP_LOGCONFIG(TAG, "GroveMotorDriveTB6612FNG:");
23  LOG_I2C_DEVICE(this);
24 }
25 
27  ESP_LOGCONFIG(TAG, "Setting up Grove Motor Drive TB6612FNG ...");
28  if (!this->standby()) {
29  this->mark_failed();
30  return;
31  }
32 }
33 
35  uint8_t status = 0;
36  if (this->write_register(GROVE_MOTOR_DRIVER_I2C_CMD_STANDBY, &status, 1) != i2c::ERROR_OK) {
37  ESP_LOGW(TAG, "Set standby failed!");
38  this->status_set_warning();
39  return false;
40  }
41  return true;
42 }
43 
45  uint8_t status = 0;
46  if (this->write_register(GROVE_MOTOR_DRIVER_I2C_CMD_NOT_STANDBY, &status, 1) != i2c::ERROR_OK) {
47  ESP_LOGW(TAG, "Set not standby failed!");
48  this->status_set_warning();
49  return false;
50  }
51  return true;
52 }
53 
55  if (addr == 0x00 || addr >= 0x80) {
56  return;
57  }
58  if (this->write_register(GROVE_MOTOR_DRIVER_I2C_CMD_SET_ADDR, &addr, 1) != i2c::ERROR_OK) {
59  ESP_LOGW(TAG, "Set new i2c address failed!");
60  this->status_set_warning();
61  return;
62  }
63  this->set_i2c_address(addr);
64 }
65 
66 void GroveMotorDriveTB6612FNG::dc_motor_run(uint8_t channel, int16_t speed) {
67  speed = clamp<int16_t>(speed, -255, 255);
68 
69  buffer_[0] = channel;
70  if (speed >= 0) {
71  buffer_[1] = speed;
72  } else {
73  buffer_[1] = (uint8_t) (-speed);
74  }
75 
76  if (speed >= 0) {
77  if (this->write_register(GROVE_MOTOR_DRIVER_I2C_CMD_CW, buffer_, 2) != i2c::ERROR_OK) {
78  ESP_LOGW(TAG, "Run motor failed!");
79  this->status_set_warning();
80  return;
81  }
82  } else {
83  if (this->write_register(GROVE_MOTOR_DRIVER_I2C_CMD_CCW, buffer_, 2) != i2c::ERROR_OK) {
84  ESP_LOGW(TAG, "Run motor failed!");
85  this->status_set_warning();
86  return;
87  }
88  }
89 }
90 
92  if (this->write_register(GROVE_MOTOR_DRIVER_I2C_CMD_BRAKE, &channel, 1) != i2c::ERROR_OK) {
93  ESP_LOGW(TAG, "Break motor failed!");
94  this->status_set_warning();
95  return;
96  }
97 }
98 
100  if (this->write_register(GROVE_MOTOR_DRIVER_I2C_CMD_STOP, &channel, 1) != i2c::ERROR_OK) {
101  ESP_LOGW(TAG, "Stop dc motor failed!");
102  this->status_set_warning();
103  return;
104  }
105 }
106 
107 void GroveMotorDriveTB6612FNG::stepper_run(StepperModeTypeT mode, int16_t steps, uint16_t rpm) {
108  uint8_t cw = 0;
109  // 0.1ms_per_step
110  uint16_t ms_per_step = 0;
111 
112  if (steps > 0) {
113  cw = 1;
114  }
115  // stop
116  else if (steps == 0) {
117  this->stepper_stop();
118  return;
119  } else if (steps == INT16_MIN) {
120  steps = INT16_MAX;
121  } else {
122  steps = -steps;
123  }
124 
125  rpm = clamp<uint16_t>(rpm, 1, 300);
126 
127  ms_per_step = (uint16_t) (3000.0 / (float) rpm);
128  buffer_[0] = mode;
129  buffer_[1] = cw; //(cw=1) => cw; (cw=0) => ccw
130  buffer_[2] = steps;
131  buffer_[3] = (steps >> 8);
132  buffer_[4] = ms_per_step;
133  buffer_[5] = (ms_per_step >> 8);
134 
135  if (this->write_register(GROVE_MOTOR_DRIVER_I2C_CMD_STEPPER_RUN, buffer_, 1) != i2c::ERROR_OK) {
136  ESP_LOGW(TAG, "Run stepper failed!");
137  this->status_set_warning();
138  return;
139  }
140 }
141 
143  if (this->write_register(GROVE_MOTOR_DRIVER_I2C_CMD_STEPPER_STOP, nullptr, 1) != i2c::ERROR_OK) {
144  ESP_LOGW(TAG, "Send stop stepper failed!");
145  this->status_set_warning();
146  return;
147  }
148 }
149 
151  // 4=>infinite ccw 5=>infinite cw
152  uint8_t cw = (is_cw) ? 5 : 4;
153  // 0.1ms_per_step
154  uint16_t ms_per_step = 0;
155 
156  rpm = clamp<uint16_t>(rpm, 1, 300);
157  ms_per_step = (uint16_t) (3000.0 / (float) rpm);
158 
159  buffer_[0] = mode;
160  buffer_[1] = cw; //(cw=1) => cw; (cw=0) => ccw
161  buffer_[2] = ms_per_step;
162  buffer_[3] = (ms_per_step >> 8);
163 
164  if (this->write_register(GROVE_MOTOR_DRIVER_I2C_CMD_STEPPER_KEEP_RUN, buffer_, 4) != i2c::ERROR_OK) {
165  ESP_LOGW(TAG, "Write stepper keep run failed");
166  this->status_set_warning();
167  return;
168  }
169 }
170 } // namespace grove_tb6612fng
171 } // namespace esphome
void status_set_warning(const char *message="unspecified")
Definition: component.cpp:146
int speed
Definition: fan.h:35
No error found during execution of method.
Definition: i2c_bus.h:13
BedjetMode mode
BedJet operating mode.
Definition: bedjet_codec.h:151
void dc_motor_run(uint8_t channel, int16_t speed)
uint8_t status
Definition: bl0942.h:23
void stepper_run(StepperModeTypeT mode, int16_t steps, uint16_t rpm)
virtual void mark_failed()
Mark this component as failed.
Definition: component.cpp:113
This is a workaround until we can figure out a way to get the tflite-micro idf component code availab...
Definition: a01nyub.cpp:7
ErrorCode write_register(uint8_t a_register, const uint8_t *data, size_t len, bool stop=true)
writes an array of bytes to a specific register in the I²C device
Definition: i2c.cpp:25
void stepper_keep_run(StepperModeTypeT mode, uint16_t rpm, bool is_cw)
void set_i2c_address(uint8_t address)
We store the address of the device on the bus.
Definition: i2c.h:140