Guest User

Kugoo S1 Pro Dual Motor Arduino Serial MiddleMan

a guest
Oct 5th, 2020
108
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <SoftwareSerial.h>
  2.  
  3. SoftwareSerial portOne(4, 7);
  4. SoftwareSerial portTwo(4, 5);
  5. SoftwareSerial portThree(8, 9);
  6.  
  7. uint32_t LastControllerTimeStamp = 0;
  8. uint32_t TimeBetweenLastTwoMessages = 100;
  9. uint32_t LastScreenTimeStamp = 0;
  10. bool bIsListening = false;
  11.  
  12. uint8_t bufferRead[32];
  13. uint8_t bufferRead2[32];
  14.  
  15. float CurrentInitValue = 0;
  16.  
  17. void setup() {
  18.   portOne.begin(9600);
  19.   portTwo.begin(9600);
  20.   portThree.begin(9600);
  21.   portThree.listen();
  22.  
  23.   for(int i = 0; i < 5; i++)
  24.   {
  25.     CurrentInitValue += analogRead(1);
  26.   }
  27.   CurrentInitValue /= 5;
  28. }
  29.  
  30. int getCRC(uint8_t *Array, int ByteCount)
  31. {
  32.   uint16_t CRC = 0;
  33.   Array++;
  34.   for(int i = 1; i < ByteCount-2; i++)
  35.   {
  36.     CRC += *Array;
  37.     Array++;
  38.   }
  39.   return (CRC>>8) | (CRC<<8);
  40. }
  41.  
  42. void loop() {
  43.  
  44.   if((millis() - LastControllerTimeStamp) < (TimeBetweenLastTwoMessages - 55))
  45.   {
  46.     if(!bIsListening)
  47.     {
  48.       bIsListening = true;
  49.       portOne.listen();
  50.     }
  51.   }
  52.   else
  53.   {
  54.     if(bIsListening)
  55.     {
  56.       bIsListening = false;
  57.       portThree.listen();
  58.     }
  59.   }
  60.  
  61.  
  62.   if(bIsListening)
  63.   {
  64.     if(portOne.available() > 0)
  65.     {    
  66.       int ByteCount = 0;
  67.       while (portOne.available() > 0) {
  68.         bufferRead[ByteCount] = portOne.read();
  69.         ByteCount++;
  70.         if(portOne.available() == 0)
  71.         {
  72.           delay(2);
  73.         }  
  74.       }
  75.       memcpy(bufferRead2, bufferRead, ByteCount);
  76.  
  77.       if(ByteCount == 8)
  78.       {      
  79.         bufferRead[5] = map(bufferRead[5], 0x00, 0xD3, 0x0A, 0xB0);
  80.         bufferRead2[5] = bufferRead[5];
  81.         if(bufferRead[5] > 0x0A)
  82.         {
  83.             if(bufferRead[5] < 120)
  84.             {
  85.               bufferRead[5] = 0;
  86.               bufferRead[4] = 0;
  87.             }
  88.             else
  89.             {
  90.               bufferRead[5] = 0x0A + ((bufferRead[5]-121) * 3);
  91.               bufferRead[4] = 0;
  92.             }
  93.         }
  94.  
  95.         uint16_t CRC = getCRC(bufferRead, ByteCount);
  96.         uint16_t CRC2 = getCRC(bufferRead2, ByteCount);
  97.         memcpy(&bufferRead[ByteCount-2], &CRC, sizeof(uint16_t));
  98.         memcpy(&bufferRead2[ByteCount-2], &CRC2, sizeof(uint16_t));
  99.  
  100.         LastScreenTimeStamp = millis();
  101.       }
  102.  
  103.       portOne.write(bufferRead2, ByteCount);
  104.       portTwo.write(bufferRead, ByteCount);
  105.  
  106.     }
  107.   }
  108.   else
  109.   {
  110.     // CONTROLLER
  111.  
  112.     if(portThree.available() > 0)
  113.     {
  114.       int ByteCount = 0;
  115.       while (portThree.available() > 0) {
  116.         bufferRead[ByteCount] = portThree.read();
  117.         ByteCount++;
  118.         if(portThree.available() == 0)
  119.         {
  120.           delay(2);
  121.         }
  122.       }
  123.  
  124.       if(ByteCount == 11)
  125.       {
  126.         float ReadCurrent = abs(analogRead(1) - CurrentInitValue) + 7;
  127.         bufferRead[4] = uint8_t( (float) ( ReadCurrent / float(15.06) ) );
  128.         uint16_t CRC = getCRC(bufferRead, ByteCount);
  129.         memcpy(&bufferRead[ByteCount-2], &CRC, sizeof(uint16_t));
  130.       }
  131.      
  132.       portThree.write(bufferRead, ByteCount);
  133.  
  134.       if(ByteCount == 11)
  135.       {
  136.         TimeBetweenLastTwoMessages = millis() - LastControllerTimeStamp;
  137.         if(TimeBetweenLastTwoMessages > 530)
  138.         {
  139.           TimeBetweenLastTwoMessages = 469;
  140.         }
  141.         LastControllerTimeStamp = millis();
  142.       }
  143.     }
  144.   }
  145. }
RAW Paste Data