Прошу помощи со скетчем.

Дело в следующем. У сына есть танчик на дешевом китайском управлении, которое накрылось. Механическая часть в полном порядке. Хочу его реанимировать. Компоненты в наличии. Драйвер для двигателей TB6612FNG. Не могу найти скетч с которым работал бы этот драйвер. Может у кого есть и не жалко поделиться? Ну или помогите подправить этот. Он почему-то перестает работать через секунд 10.

Код (C++):

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
const uint64_t pipe = 0xF0F1F2F3F4LL;
int time;
int rull1;
int rull2;
int gaz1;
int gaz2;
int bash;
int fire;
int firepin;
int smoke;
int flag;
RF24 radio(9, 2);
byte msg[8];
void setup(){
  pinMode(7, OUTPUT); //подъем башни
  pinMode(3, OUTPUT);//~pwm //скорость поворота башни
  pinMode(8, OUTPUT); //опуск башни
  pinMode(5, OUTPUT);//~pwm //скорость гусеницы лев
  pinMode(6, OUTPUT);//~pwm //скорость гусеницы прав
  pinMode(10, OUTPUT); //привод башни влево
  pinMode(4, OUTPUT); //привод башни вправо
  pinMode(A0, OUTPUT); //привод гусениц лев вперед
  pinMode(A1, OUTPUT); //привод гусениц лев назад
  pinMode(A2, OUTPUT); //привод гусениц прав вперед
  pinMode(A3, OUTPUT); //привод гусениц прав назад
  pinMode(A4, OUTPUT); //дым
  pinMode(A5, OUTPUT); //выстрел
  pinMode(A6, OUTPUT); //концевик
  pinMode(A7, OUTPUT);  //пулемет

radio.begin();
radio.setChannel(9);
radio.setPayloadSize(32);
radio.setDataRate(RF24_1MBPS);
radio.setPALevel(RF24_PA_MAX);
radio.setAutoAck(1);
radio.openReadingPipe(1,pipe);
radio.startListening();
Serial.begin(9600);
delay(20);}
void loop(){
if (radio.available()){time = millis();
radio.read(&msg, sizeof(msg));}
if (millis() - time > 1000){
  msg[0] = 130;
  msg[1] = 130;
  msg[2] = 130;
  msg[3] = 130;
  msg[4] = 0;
  msg[5] = 0;
 
}

if (msg[3] > 132) //ТОРМОЗ правой гусеницы
{
rull2 = map(msg[3], 132, 255, 255, 0);
}
else
{
rull2 = 255;
}

if (msg[3] < 127) // тормоз левой гусеницы
{
rull1 = map(msg[3], 127, 0, 255, 0);
}
else
{
  rull1 = 255;
}
if (msg[2] > 135) //движение назад
{
gaz1 = map(msg[2], 132, 255, 0, rull1);
gaz2 = map(msg[2], 132, 255, 0, rull2);
  digitalWrite(A0, LOW);
  digitalWrite(A1, HIGH);
  digitalWrite(A2, LOW);
  digitalWrite(A3, HIGH);
  analogWrite(5, gaz1);
  analogWrite(6, gaz2);
}
if (msg[2] < 125)// движение вперед
{
gaz1 = map(msg[2], 128, 0, 0, rull1);
gaz2 = map(msg[2], 128, 0, 0, rull2);
  digitalWrite(A0, HIGH);
  digitalWrite(A1, LOW);
  digitalWrite(A2, HIGH);
  digitalWrite(A3, LOW);
  analogWrite(5, gaz1);
  analogWrite(6, gaz2);
}
if(125 <= msg[2] && msg[2] < 135)
{
  digitalWrite(A0, LOW);
  digitalWrite(A1, LOW);
  digitalWrite(A2, LOW);
  digitalWrite(A3, LOW);
  analogWrite(5, 0);
  analogWrite(6, 0);
}

if (msg[1] > 130) //поворот бвшни
{
  bash = map(msg[1], 130, 255, 0, 255);
  digitalWrite(4, LOW);
  digitalWrite(2, HIGH);
  analogWrite(3, bash);
}
if (msg[1] < 124)
{
  bash = map(msg[1], 124, 0, 0, 255);
  digitalWrite(4, HIGH);
  digitalWrite(2, LOW);
  analogWrite(3, bash);
}
if (124 < msg[1] && msg[1] < 130)
{
   digitalWrite(2, LOW);
  digitalWrite(4, LOW);
}
if ( 135 < msg[0] && msg[0] < 255) {digitalWrite(8, HIGH);}
  else
  {
    digitalWrite(8, LOW);
  }

if (0 <= msg[0] && msg[0] < 125) digitalWrite(7, HIGH);
else
  {
    digitalWrite(7, LOW);
  }
fire = msg[4]; // ВЫСТРЕЛ ПУШКИ

if (fire == 1)
{
  digitalWrite(A4, HIGH);
}
else
{
digitalWrite(A4, LOW);
}

smoke = msg[5]; // ДЫМ

if (smoke == 1&&flag == 0)
{
digitalWrite(A5, !digitalRead(A5));
flag=1;
}
if (smoke == 0&&flag == 1)
{
  flag=0;
}

Serial.println(msg[3]);
}

 

Работать надо на прерываниях с радиотрансивера. И обязательно вычитывать всю очередь сообщений.
Здесь косяк с millis.

 

А как исправить этот косяк? Дело в том что скетч не мой, а я далек от программирования. Там где я этот скетч откопал решения данной проблемы не было. А от программирования я далек.