ibus

#include "BTS7960.h" //https://github.com/luisllamasbinaburo/Arduino-BTS7960
#include <string.h>  //Na podstawie https://github.com/MikeysLab/ConsumingIBus/blob/master/DriveController.ino

#define IBUS_BUFFSIZE 32    // Max iBus packet size
#define IBUS_MAXCHANNELS 10 // My TX only has 10 channels

static uint8_t ibusIndex = 0;
static uint8_t ibus[IBUS_BUFFSIZE] = {0};
static uint16_t rcValue[IBUS_MAXCHANNELS];
static boolean rxFrameDone;

//BTS7960 motor1(8, 9, 10); // 8-enable L i R
//BTS7960 motor2(4, 5, 6);  // 4-enable L i R

void setup()
{
    //ustawiamy piny jako wyjścia
    pinMode(4, OUTPUT);
    pinMode(5, OUTPUT);
    pinMode(6, OUTPUT);
    pinMode(8, OUTPUT);
    pinMode(9, OUTPUT);
    pinMode(10, OUTPUT);
    //pwm na 0
    analogWrite(9, 0);  // M1 L
    analogWrite(10, 0); // M1 R
    analogWrite(5, 0);  // M2 L
    analogWrite(6, 0);  // M2 R
    //enable na 1
    digitalWrite(8, HIGH);
    digitalWrite(4, HIGH);

    Serial.begin(115200);
    Serial.println("setup done!");
    /*
    for (int speed = 0; speed < 255; speed += 10)
    {
        motor1.TurnLeft(speed);
        delay(100);
    }

    motor1.Stop();

    for (int speed = 255; speed > 0; speed -= 10)
    {
        motor1.TurnLeft(speed);
        delay(100);
    }
    motor1.Stop();

    motor1.Disable();

    {
        for (int speed = 0; speed < 255; speed += 10) //drugi motor
            motor2.TurnLeft(speed);
        delay(100);
    }

    motor2.Stop();

    for (int speed = 255; speed > 0; speed -= 10)
    {
        motor2.TurnLeft(speed);
        delay(100);
    }
    motor2.Stop();

    motor2.Disable();
    */

    // motor1.Enable();
    // motor2.Enable();
}

void loop()
{
    readRx();
}

void steruj()
{

    int kanal_1 = rcValue[1];                        // odczyt z ibusa
    int pwm_1 = map(kanal_1, 1000, 2000, -255, 255); // mapujemy zakresy
    if (pwm_1 > 0)
    {
        analogWrite(9, 0);      // M1 L
        analogWrite(10, pwm_1); // M1 R

        //motor1.TurnRight(pwm_1); // jeśli pwm > 0, kręcimy w prawo
    }
    // else if (pwm_1 == 0)
    // {
    //     //motor1.Stop();
    // }
    else
    {
        analogWrite(10, 0);    // M1 R
        analogWrite(9, pwm_1); // M1 L
        //motor1.TurnLeft(pwm_1 * -1); // jeśli pwm < 0, kręcimy w lewo
    }

    int kanal_2 = rcValue[2];                        // odczyt z ibusa
    int pwm_2 = map(kanal_2, 1000, 2000, -255, 255); // mapujemy zakresy
    if (pwm_2 > 0)
    {

        analogWrite(5, 0);     // M2 L
        analogWrite(6, pwm_2); // M2 R
        //#3  motor2.TurnRight(pwm_2); // jeśli pwm > 0, kręcimy w prawo //po odkomentowaniu tej linii kodu pojawiają się zakłócenia w odczycie Ibus w monitorze portu
    }
    // else if (pwm_2 == 0)
    // {
    //     motor2.Stop();
    // }
    else
    {
        analogWrite(6, 0);     // M2 R
        analogWrite(5, pwm_2); // M2 L
        //#3  motor2.TurnLeft(pwm_2 * -1); // jeśli pwm < 0, kręcimy w lewo //po odkomentowaniu tej linii kodu pojawiają się zakłócenia w odczycie Ibus w monitorze portu
    }
    // jeśli silnik_1 kręci się w złą stronę, zamień Right <-> Left
    // w liniach 33 i 37 (43 i 47 dla silnik_2)
    Serial.print("\t M1:");
    Serial.print(pwm_1);
    Serial.print("\t M2:");
    Serial.print(pwm_2);
}

void readRx()
{
    rxFrameDone = false;

    if (Serial.available())
    {
        uint8_t val = Serial.read();
        // Look for 0x2040 as start of packet
        if (ibusIndex == 0 && val != 0x20)
        {
            ibusIndex = 0;
            return;
        }
        if (ibusIndex == 1 && val != 0x40)
        {
            ibusIndex = 0;
            return;
        }

        if (ibusIndex == IBUS_BUFFSIZE)
        {
            ibusIndex = 0;
            int high = 3;
            int low = 2;
            for (int i = 0; i < IBUS_MAXCHANNELS; i++)
            {
                // left shift away the first 8 bits of the first byte and add the whole
                // value of the previous one
                rcValue[i] = (ibus[high] << 8) + ibus[low];
                Serial.print(rcValue[i]);
                Serial.print("     ");
                high += 2;
                low += 2;
            }

            rxFrameDone = true;
            steruj();
            Serial.println();
            return;
        }
        else
        {
            ibus[ibusIndex] = val;
            ibusIndex++;
        }
    }
}