ESP32로 곧바로 라이다에 연결하기.

ESP32로 라이다 분석용 코드를 포팅했습니다. 그리고 검증용으로 PC로 x,y 점들을 보내게 했더니 나오네요.

카카오를 안써서 이곳에 영상을 못올립니다. 

영상과 선 연결 정보등은 페북에서 :  www.facebook.com/groups/airoboticskr/posts/1146125082811698/

로그인 또는 가입하여 보기

ESP32 쪽은 현재는 거의 초벌수준인 아주 엉성한 코드입니다.   PC쪽은 자신이 원하는 아무 프로그램으로 직접 만드셔야 합니다.

#define RXD2 16 // This line connects to TX of Lidar 
#define TXD2 17 // is not used

/*  On ESP32, there are D/A pin which emits voltage.
 *  You can set 5th pin of lidar to this D/A pin to change speed.
 *  0V = Maximum speed, 5V is Slowest or Stop.
 */

int machine_pack_type = 40; // only 1 machine out of 4 of mine is 60, rests are 40
const int BUFFER_SIZE = 200;
char buf[BUFFER_SIZE];
String s;
String rs;
int i, j, k, counter, rmax, pack_type, data_length, start_angle, stop_angle;
int diff, length3, dataOk = 0;
int distance, data0, data1, data2, angle ;
float steplength, angle_per_sample, distanceDivided, anglef, distanceInMeter;
bool LadarActive = true;
int FullCircleDataSize = 0;
int ThisRoundCount = 0, maxThisRound = 0, x, y;
float anglePlus;

enum States {
   START1 = 0,
   START2 = 1,
   HEADER = 2,
   DATA = 3
};

float angle_list[120];
float distance_list[120];
int   quality_list[120];

States state;

void serial2Flush(){
  while(Serial2.available() > 0) {
    char t = Serial2.read();
  }
}

void setup() {
   steplength = (PI*2);
   anglePlus  = PI/2;
   state = START1;
  
   Serial.begin(230400);  // To PC
   Serial2.begin(153600, SERIAL_8N1, RXD2, TXD2); // To Ridar
   s.reserve(200);    s = "";
   rs.reserve(260); rs = "";  // string to pass data
}

void loop() { 
   if (LadarActive) {
      if (Serial2.available() > 0) {  // Look for AA55
         if (state == START1) {
            dataOk = Serial2.readBytes(buf, 1);
            // Serial.println(dataOk); // 1 came out
            if (dataOk == 1) { 
               if (buf[0] == 0xAA) {
                  state = START2;
               } 
            }
         } else if (state == START2) {  // Confirm 55
            dataOk = Serial2.readBytes(buf, 1); 
            // Serial.println(dataOk); // 1 came out
            if (dataOk == 1) { 
               if (buf[0] == 0x55) {
                  state = HEADER;
               } else {
                  state = START1;
               }
            }
         } else if (state == HEADER) {   // Read main data
            dataOk = Serial2.readBytes(buf, 8);
            // Serial.println(dataOk);  // 8 came out
            if (dataOk == 8) { 
               pack_type = buf[0];
               data_length = int(buf[1]);
               start_angle = int(buf[3] << 8) + int(buf[2]);
               stop_angle  = int(buf[5] << 8) + int(buf[4]);
            
               diff = stop_angle - start_angle;
               if (stop_angle < start_angle) {
                  diff =  0xB400 - start_angle + stop_angle;
               }

               angle_per_sample = 0;
               if ((diff > 1) and ((data_length-1) > 0)) {
                  angle_per_sample = diff / (data_length-1);
               }

               // Debug 
               s = ("#") + String(counter)
                  + (" t:") + pack_type
                  + (" l:") + data_length 
                  + (" sa:")+ start_angle 
                  + (" ta:")+ stop_angle
                  + (" f:") + diff
                  + (" as:")+ round(angle_per_sample);
               //Serial.println(s);
               
               FullCircleDataSize += data_length;

               counter += 1;
               if (pack_type != machine_pack_type) {  // default = 40, for some device = 60
                  // about every 40 cycles, one full circle done.
                  s = ("Counter : ") + String(counter-1) 
                     + (" Cycle Size : ") + String(FullCircleDataSize*3);
                  //Serial.println(s);
                  //Serial.println("*"); // Send End of 1 Full Cycle
                  counter = 0;
                  FullCircleDataSize = 0;
               }
               state = DATA;  
               rs = ""; 
            }       
         } else if (state == DATA) {   // Read main data
            state = START1;
            length3 = data_length * 3;
            dataOk = Serial2.readBytes(buf, (length3));

            s = " Data_length : " + String(data_length) 
               + "  dataOk :" + String(dataOk);
            // Serial.println(s); // usually 40, 120 came out
            
            if (dataOk == length3) { 
               for (int i = 0; i < data_length; i++) {
                  data0 = int(buf[i*3 + 0]);
                  data1 = int(buf[i*3 + 1]);
                  data2 = int(buf[i*3 + 2]); 
                  distance = (data2 << 8) + data1;

                  angle = (start_angle + angle_per_sample * i);
                  // 0xB400 = 46080, steplength = 2*PI, angle sample 20006
                  anglef = (steplength * angle) / 0xB400; // steplength = 2*PI
                  distanceDivided = distance / 100;

                  if (distanceDivided < 60) {
                     distanceInMeter = (distance/5); // div to convert mm to meter
                     y = distanceDivided * cos(anglef);
                     x = distanceDivided * sin(anglef);
                     // 0:556.00-556~0  // 1739:324.00-324~0
                     //rs += String(anglef) +(":") + String(distanceInMeter) + ("-")
                     //   + String(x) + ("~") + String(y) +(", "); 
                     if ((x != 0) and (y != 0)) { 
                        rs += ("[") + String(x) + (":") + String(y) + ("]");  
                     }
                     if (rs.length() > 240) { 
                         Serial.println(rs);  
                         rs = "";
                     }
                     serial2Flush();
                  }
                    
               //   angle_list[i] = anglef;

               //   distance_list[i] = distance / 1000; // div to convert mm to meter
               //   quality_list[i] = data0;
               }
               Serial.println(rs); // send remaining
               if (pack_type != machine_pack_type) { // default = 40, for some device = 60
                  // Draw stuffs now if OLED is attached
                  Serial.println("*"); // Send End of 1 Full Cycle
               }
               /*if (data_length > 0) {  // Draw data or prepare data to send
                  // Debug 
                  s = " Data_length : " + data_length;
                  //Serial.println(s);
                  for (int i = 0; i < data_length; i++) {
                     s += String(angle_list[i]) + (", ");
                  }
                  //Serial.println(s);
               }*/
            }
            
         }
        
      } 
      
   }
}

 

 

Simple pass-thru : (my local folder :  /SHARE/TTGO_Liligo/rider01_path_thru_esp32)

#define RXD2 16  
#define TXD2 17 // is not used

const int BUFFER_SIZE = 500;
char buf[BUFFER_SIZE];

void setup() {
//  Serial.begin(230400);  // To Lazarus App
  Serial.begin(250000);  // To PC
  Serial2.begin(153600, SERIAL_8N1, RXD2, TXD2); // To Ridar
}

void loop() { 
  while (Serial2.available()){
    Serial.print(char(Serial2.read())); 
  }
}