Robert_l

ibus

Jul 29th, 2021 (edited)
895
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "BTS7960.h" //https://github.com/luisllamasbinaburo/Arduino-BTS7960
  2. #include <string.h>  //Na podstawie https://github.com/MikeysLab/ConsumingIBus/blob/master/DriveController.ino
  3.  
  4. #define IBUS_BUFFSIZE 32    // Max iBus packet size
  5. #define IBUS_MAXCHANNELS 10 // My TX only has 10 channels
  6.  
  7. static uint8_t ibusIndex = 0;
  8. static uint8_t ibus[IBUS_BUFFSIZE] = {0};
  9. static uint16_t rcValue[IBUS_MAXCHANNELS];
  10. static boolean rxFrameDone;
  11.  
  12. //BTS7960 motor1(8, 9, 10); // 8-enable L i R
  13. //BTS7960 motor2(4, 5, 6);  // 4-enable L i R
  14.  
  15. void setup()
  16. {
  17.     //ustawiamy piny jako wyjścia
  18.     pinMode(4, OUTPUT);
  19.     pinMode(5, OUTPUT);
  20.     pinMode(6, OUTPUT);
  21.     pinMode(8, OUTPUT);
  22.     pinMode(9, OUTPUT);
  23.     pinMode(10, OUTPUT);
  24.     //pwm na 0
  25.     analogWrite(9, 0);  // M1 L
  26.     analogWrite(10, 0); // M1 R
  27.     analogWrite(5, 0);  // M2 L
  28.     analogWrite(6, 0);  // M2 R
  29.     //enable na 1
  30.     digitalWrite(8, HIGH);
  31.     digitalWrite(4, HIGH);
  32.  
  33.     Serial.begin(115200);
  34.     Serial.println("setup done!");
  35.     /*
  36.     for (int speed = 0; speed < 255; speed += 10)
  37.     {
  38.         motor1.TurnLeft(speed);
  39.         delay(100);
  40.     }
  41.  
  42.     motor1.Stop();
  43.  
  44.     for (int speed = 255; speed > 0; speed -= 10)
  45.     {
  46.         motor1.TurnLeft(speed);
  47.         delay(100);
  48.     }
  49.     motor1.Stop();
  50.  
  51.     motor1.Disable();
  52.  
  53.     {
  54.         for (int speed = 0; speed < 255; speed += 10) //drugi motor
  55.             motor2.TurnLeft(speed);
  56.         delay(100);
  57.     }
  58.  
  59.     motor2.Stop();
  60.  
  61.     for (int speed = 255; speed > 0; speed -= 10)
  62.     {
  63.         motor2.TurnLeft(speed);
  64.         delay(100);
  65.     }
  66.     motor2.Stop();
  67.  
  68.     motor2.Disable();
  69.     */
  70.  
  71.     // motor1.Enable();
  72.     // motor2.Enable();
  73. }
  74.  
  75. void loop()
  76. {
  77.     readRx();
  78. }
  79.  
  80. void steruj()
  81. {
  82.  
  83.     int kanal_1 = rcValue[1];                        // odczyt z ibusa
  84.     int pwm_1 = map(kanal_1, 1000, 2000, -255, 255); // mapujemy zakresy
  85.     if (pwm_1 > 0)
  86.     {
  87.         analogWrite(9, 0);      // M1 L
  88.         analogWrite(10, pwm_1); // M1 R
  89.  
  90.         //motor1.TurnRight(pwm_1); // jeśli pwm > 0, kręcimy w prawo
  91.     }
  92.     // else if (pwm_1 == 0)
  93.     // {
  94.     //     //motor1.Stop();
  95.     // }
  96.     else
  97.     {
  98.         analogWrite(10, 0);    // M1 R
  99.         analogWrite(9, pwm_1); // M1 L
  100.         //motor1.TurnLeft(pwm_1 * -1); // jeśli pwm < 0, kręcimy w lewo
  101.     }
  102.  
  103.     int kanal_2 = rcValue[2];                        // odczyt z ibusa
  104.     int pwm_2 = map(kanal_2, 1000, 2000, -255, 255); // mapujemy zakresy
  105.     if (pwm_2 > 0)
  106.     {
  107.  
  108.         analogWrite(5, 0);     // M2 L
  109.         analogWrite(6, pwm_2); // M2 R
  110.         //#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
  111.     }
  112.     // else if (pwm_2 == 0)
  113.     // {
  114.     //     motor2.Stop();
  115.     // }
  116.     else
  117.     {
  118.         analogWrite(6, 0);     // M2 R
  119.         analogWrite(5, pwm_2); // M2 L
  120.         //#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
  121.     }
  122.     // jeśli silnik_1 kręci się w złą stronę, zamień Right <-> Left
  123.     // w liniach 33 i 37 (43 i 47 dla silnik_2)
  124.     Serial.print("\t M1:");
  125.     Serial.print(pwm_1);
  126.     Serial.print("\t M2:");
  127.     Serial.print(pwm_2);
  128. }
  129.  
  130. void readRx()
  131. {
  132.     rxFrameDone = false;
  133.  
  134.     if (Serial.available())
  135.     {
  136.         uint8_t val = Serial.read();
  137.         // Look for 0x2040 as start of packet
  138.         if (ibusIndex == 0 && val != 0x20)
  139.         {
  140.             ibusIndex = 0;
  141.             return;
  142.         }
  143.         if (ibusIndex == 1 && val != 0x40)
  144.         {
  145.             ibusIndex = 0;
  146.             return;
  147.         }
  148.  
  149.         if (ibusIndex == IBUS_BUFFSIZE)
  150.         {
  151.             ibusIndex = 0;
  152.             int high = 3;
  153.             int low = 2;
  154.             for (int i = 0; i < IBUS_MAXCHANNELS; i++)
  155.             {
  156.                 // left shift away the first 8 bits of the first byte and add the whole
  157.                 // value of the previous one
  158.                 rcValue[i] = (ibus[high] << 8) + ibus[low];
  159.                 Serial.print(rcValue[i]);
  160.                 Serial.print("     ");
  161.                 high += 2;
  162.                 low += 2;
  163.             }
  164.  
  165.             rxFrameDone = true;
  166.             steruj();
  167.             Serial.println();
  168.             return;
  169.         }
  170.         else
  171.         {
  172.             ibus[ibusIndex] = val;
  173.             ibusIndex++;
  174.         }
  175.     }
  176. }
RAW Paste Data