mattwoodhead / autosteerpcbv2lb Goto Github PK

#define Neopixel_Pin 16                 // Set this to the pin number you are using for the Neopixel strip controll line
#define Neopixel_Brightness 120        // brightness value between 0 and 255
#define mmPerLightbarPixel  20         // 40 = 4cm

//Neopixels
  #include <Adafruit_NeoPixel.h>
  Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, Neopixel_Pin, NEO_GRBW + NEO_KHZ800);
  const byte centerpixel = (NUMPIXELS-1)/2;
  byte levelcolor[NUMPIXELS][3];

//Parsing PGN
  bool isPGNFound = false, isHeaderFound = false;
  uint8_t pgn = 0, dataLength = 0, idx = 0;
  int16_t tempHeader = 0;

//show life in AgIO
  uint8_t helloAgIO[] = {0x80,0x81, 0x7f, 0xC7, 1, 0, 0x47 };
  uint8_t helloCounter=0;

  int16_t distanceFromLine = 32020;  // cross track error - Lightbar PGN (249) bytes 5 * 6. Start at 32020 so it is ignored untill a value is received from AOG
  int16_t prevDistFromLine = 0;  // Used to only send the lightbar data if the distance has changed

void setup() {
  
//set up communication
  Serial.begin(38400);

//start neopixels
  pixels.begin();
   for (int i =0 ;i < centerpixel;i++){ //Right
     levelcolor[i][0]=0; levelcolor[i][1]=Neopixel_Brightness; levelcolor[i][2]=0; // Green (Right)
   }
   for (int i = centerpixel+1;i < NUMPIXELS;i++){ //Left
     levelcolor[i][0]=Neopixel_Brightness; levelcolor[i][1]=0; levelcolor[i][2]=0; // Red (Left)
   }
   levelcolor[centerpixel][0]=20; levelcolor[centerpixel][1]=20;levelcolor[centerpixel][2]=0;  // Yellow (centerpixel)

}

void loop() {
  
// Serial Receive
    //Do we have a match with 0x8081?    
    if (Serial.available() > 1 && !isHeaderFound && !isPGNFound) 
    {
      uint8_t temp = Serial.read();
      if (tempHeader == 0x80 && temp == 0x81) 
      {
        isHeaderFound = true;
        tempHeader = 0;        
      }
      else  
      {
        tempHeader = temp;     //save for next time
        return;    
      }
    }
  
    //Find Source, PGN, and Length
    if (Serial.available() > 2 && isHeaderFound && !isPGNFound)
    {
      Serial.read(); //The 7F or less
      pgn = Serial.read();
      dataLength = Serial.read();
      isPGNFound = true;
      idx=0;
    } 

    //The data package
    if (Serial.available() > dataLength && isHeaderFound && isPGNFound)
    {
  
  if (pgn==249) //FC Light bar PGN
      {         
        //cross track error byte 5 & 6
        distanceFromLine = ((int16_t)(Serial.read() << 8 | Serial.read()));

        //crc byte 7
        Serial.read();

        //reset for next pgn sentence
        isHeaderFound = isPGNFound = false;
        pgn=dataLength=0;
      }

      //clean up strange pgns
      else
      {
          //reset for next pgn sentence
          isHeaderFound = isPGNFound = false;
          pgn=dataLength=0; 
      }
      } //end if (Serial.available() > dataLength && isHeaderFound && isPGNFound)  

if (distanceFromLine != prevDistFromLine)  // only update the lightbar if it has changed
      {
        lightbar(distanceFromLine);
        prevDistFromLine = distanceFromLine;  // set the previous XTE value to the one we have just used for next time
      }
}

void lightbar(int16_t distanceFromLine ){
    int16_t display_dist = constrain (distanceFromLine / mmPerLightbarPixel , -centerpixel ,centerpixel);
    byte n = display_dist + centerpixel;
      for (int i = 0 ;i < NUMPIXELS; i++){
        if ( (i == centerpixel && i == n)|| //Center
             (display_dist < 0 && i >= n && i <= centerpixel && distanceFromLine != 32020 && distanceFromLine != 32000)|| //Right Bar
             (display_dist > 0 && i <= n && i > centerpixel && distanceFromLine != 32020 && distanceFromLine != 32000) ) //Left Bar
        {
          pixels.setPixelColor(i,levelcolor[i][0],levelcolor[i][1],levelcolor[i][2]);
        }else{
          if (i == centerpixel){
            pixels.setPixelColor(i,10,10,0); // Dimmer yellow (we arent on centre)
          } else {
            pixels.setPixelColor(i,0,0,0); //Clear
          }
        }
      }
    pixels.show();
    delayMicroseconds(50);  // tiny delay after sending the colours to allow the colours to latch in the LEDs
  }