Hello, could you help me solve the problem?
I use an Arduino Uno as a controller, four bts7960 drivers (IBT-2) and four 24v 250w motors with worm gear (120 rpm output).
I have the principle of tank control.
I use PWM control, the right drivers are connected to pins 6, 5. The left drivers are connected to pins 10,9.
Everything works fine as I intended, but there is one thing.
After working from 5 to 15 minutes, one driver stops working in one direction, this is always driver number 4. I thought the problem is that the control signals to the driver quickly switches its potentials and the h-bridge does not have time to switch, so I set the switching time to almost the maximum(....setDeadtime(16000);), but the result is the same.
I have already burned 3 drivers, having tested the burnt driver, it does not burn out completely, only one direction of rotation burns out (when the maximum PWM signal is applied at the output of 0 volts), in the second direction of rotation, when the maximum PWM signal is applied, approximately 70% volts from the battery supply voltage.
Do you have any idea what could be the problem?
I use Li-ion 24 volts to power the power section and Twinhex 2596 LM2596 to convert to 5 volts and power the logic.
PWM_input_chanel_1 2 and PWM_input_chanel_2 3 - these are inputs from radio control receiver, but I think it does not matter, since before that I just controlled the joystick reading the analog signal.
arduino code
#include <GyverMotor2.h>
#include <avr/wdt.h>
#define PWM_input_chanel_1 2
#define PWM_input_chanel_2 3
volatile unsigned long start_time_pwm_1 = 0;
volatile unsigned long current_time_pwm_1 = 0;
volatile unsigned long pulses_pwm_1 = 0;
int pulses_width_1 = 0;
volatile unsigned long start_time_pwm_2 = 0;
volatile unsigned long current_time_pwm_2 = 0;
volatile unsigned long pulses_pwm_2 = 0;
int pulses_width_2 = 0;
volatile int int_1 = 0;
volatile int int_2 = 0;
GMotor2<DRIVER2WIRE_PWM> MOTOR_R(6, 5);
GMotor2<DRIVER2WIRE_PWM> MOTOR_L(10, 9);
void setup() {
wdt_enable(WDTO_8S);
MOTOR_L.setMinDuty(0);
MOTOR_L.smoothMode(true);
MOTOR_L.setSmoothSpeed(15);
//MOTOR_L.reverse(1);
MOTOR_L.setDeadtime(16000);
MOTOR_R.setMinDuty(0);
MOTOR_R.smoothMode(true);
MOTOR_R.setSmoothSpeed(15);
//MOTOR_R.reverse(1);
MOTOR_R.setDeadtime(16000);
pinMode(PWM_input_chanel_1, INPUT_PULLUP);
pinMode(PWM_input_chanel_2, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(PWM_input_chanel_1),PulseTimer1,CHANGE);
attachInterrupt(digitalPinToInterrupt(PWM_input_chanel_2),PulseTimer2,CHANGE);
MOTOR_R.setSpeed(0);
MOTOR_L.setSpeed(0);
}
void loop() {
wdt_reset();
if(micros() - start_time_pwm_1 > 300000){
int_1 = 0;
}
if(micros() - start_time_pwm_2 > 300000){
int_2 = 0;
}
if(pulses_pwm_1 <= 2100 && pulses_pwm_2 >= 900){
pulses_width_1 = pulses_pwm_1;
}
if(pulses_pwm_2 <= 2100 && pulses_pwm_2 >= 900){
pulses_width_2 = pulses_pwm_2;
}
int spd_1 = percentSpeedFromRaw(pulses_width_1);
int spd_2 = percentSpeedFromRaw(pulses_width_2);
MOTOR_L.tick();
MOTOR_R.tick();
if(int_1 == 1 && int_2 == 1){
MOTOR_R.setSpeed(spd_1);
MOTOR_L.setSpeed(spd_2);
}else{
MOTOR_R.setSpeed(0);
MOTOR_L.setSpeed(0);
}
}
int percentSpeedFromRaw(int value) {
value = (value < 1000) ? 1000 : value;
value = (value > 2000) ? 2000 : value;
int valueDelta = 50;
int middleRawValue = 1500;
int minSpeedPercent = 5;
if ( value > (middleRawValue + valueDelta) ) {
value = (value - (middleRawValue + valueDelta)) * ( (255.0 - minSpeedPercent) / (2000.0 - (middleRawValue + valueDelta))) + minSpeedPercent;
value = (value > 250) ? 255 : value;
return (value < minSpeedPercent) ? minSpeedPercent : value;
}
if ( value < (middleRawValue - valueDelta) ) {
value = middleRawValue - value + middleRawValue;
value = (value - (middleRawValue + valueDelta)) * ( (255.0 - minSpeedPercent) / (2000.0 - (middleRawValue + valueDelta))) + minSpeedPercent;
value = (value > 250) ? 255 : value;
return (value < minSpeedPercent) ? -minSpeedPercent : -value;
}
return 0;
}
void PulseTimer1(){
int_1 = 1;
current_time_pwm_1 = micros();
if(current_time_pwm_1 > start_time_pwm_1){
pulses_pwm_1 = current_time_pwm_1 - start_time_pwm_1;
start_time_pwm_1 = current_time_pwm_1;
}else if(current_time_pwm_1 < start_time_pwm_1){
start_time_pwm_1 = current_time_pwm_1;
}
}
void PulseTimer2(){
int_2 = 1;
current_time_pwm_2 = micros();
if(current_time_pwm_2 > start_time_pwm_2){
pulses_pwm_2 = current_time_pwm_2 - start_time_pwm_2;
start_time_pwm_2 = current_time_pwm_2;
}else if(current_time_pwm_2 < start_time_pwm_2){
start_time_pwm_2 = current_time_pwm_2;
}
}
engine control library
#ifndef _GyverMotor2_h
#define _GyverMotor2_h
#include <Arduino.h>
enum GM_driver {
DRIVER2WIRE, //(dir + PWM)
DRIVER2WIRE_NO_INVERT, //(dir + PWM)
DRIVER2WIRE_PWM, //(PWM + PWM)
DRIVER3WIRE, //(dir + dir + PWM)
RELAY2WIRE, //(dir + dir)
};
#define _GM_SMOOTH_PRD 50
template <GM_driver GM_TYPE, uint8_t GM_RES = 8>
class GMotor2 {
public:
GMotor2(uint8_t pa, uint8_t pb, uint8_t pc = 255) : pinA(pa), pinB(pb), pinC(pc) {
pinMode(pinA, OUTPUT);
pinMode(pinB, OUTPUT);
if (pinC != 255) pinMode(pinC, OUTPUT);
setAll(0);
}
void setMinDuty(uint16_t mduty) {
minD = mduty;
}
void setMinDutyPerc(uint16_t mduty) {
mduty = constrain(mduty, 0, 100);
minD = (int32_t)getMax() * mduty / 100;
}
void reverse(bool r) {
rev = r ? -1 : 1;
}
void stop() {
setSpeed(0);
}
void brake() {
setAll(1);
speed = duty = 0;
}
void setSpeed(int16_t s) {
speed = s;
if (!smooth) duty = speed;
run(duty);
}
void setSpeedPerc(int16_t s) {
s = constrain(s, -100, 100);
setSpeed((int32_t)getMax() * s / 100);
}
void setDeadtime(uint16_t us) {
dead = us;
}
void tick() {
if ((speed || duty) && smooth && millis() - tmr >= _GM_SMOOTH_PRD) {
tmr = millis();
if (abs(duty - speed) > ds) duty += (duty < speed) ? ds : -ds;
else duty = speed;
run(duty);
}
}
void setSmoothSpeed(uint8_t s) {
ds = s;
}
void setSmoothSpeedPerc(uint8_t s) {
s = constrain(s, 0, 100);
ds = (int32_t)getMax() * s / 100;
}
void smoothMode(bool mode) {
smooth = mode;
}
int8_t getState() {
return dir;
}
int16_t getSpeed() {
return duty;
}
private:
int16_t getMax() {
return (1 << GM_RES) - 1;
}
void run(int16_t sp) {
if (!sp) return setAll(0);
int8_t ndir = (sp > 0) ? rev : -rev;
if (dead && ndir != dir) {
setAll(0);
delayMicroseconds(dead);
}
dir = ndir;
int16_t maxD = getMax();
sp = constrain(sp, -maxD, maxD);
if (minD) sp = (int32_t)sp * (maxD - minD) >> GM_RES;
sp = abs(sp) + minD;
if (GM_RES > 8 && sp == 255) sp++;
switch (GM_TYPE) {
case DRIVER2WIRE:
digitalWrite(pinA, dir < 0);
analogWrite(pinB, (dir > 0) ? sp : (maxD - sp));
break;
case DRIVER2WIRE_NO_INVERT:
digitalWrite(pinA, dir < 0);
analogWrite(pinB, sp);
break;
case DRIVER2WIRE_PWM:
if (dir > 0) {
digitalWrite(pinA, 0);
analogWrite(pinB, sp);
} else {
analogWrite(pinA, sp);
digitalWrite(pinB, 0);
}
break;
case DRIVER3WIRE:
digitalWrite(pinA, dir < 0);
digitalWrite(pinB, dir > 0);
analogWrite(pinC, sp);
break;
case RELAY2WIRE:
digitalWrite(pinA, dir < 0);
digitalWrite(pinB, dir > 0);
break;
}
}
void setAll(uint8_t val) {
digitalWrite(pinA, val);
digitalWrite(pinB, val);
if (pinC != 255) digitalWrite(pinC, val);
dir = 0;
}
const uint8_t pinA, pinB, pinC;
bool smooth = 0;
int8_t dir = 0, rev = 1;
uint8_t dead = 0;
int16_t minD = 0, speed = 0, duty = 0, ds = 20;
uint32_t tmr = 0;
};
#endif