ESP32로 라이다 분석용 코드를 포팅했습니다. 그리고 검증용으로 PC로 x,y 점들을 보내게 했더니 나오네요.
카카오를 안써서 이곳에 영상을 못올립니다.
영상과 선 연결 정보등은 페북에서 : www.facebook.com/groups/airoboticskr/posts/1146125082811698/
로그인 또는 가입하여 보기
Facebook에서 게시물, 사진 등을 확인하세요.
www.facebook.com
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()));
}
}
'폐품 Lidar 살리기' 카테고리의 다른 글
| 헐값 폐품 Lidar 살리기. ($13 + 배송) (0) | 2021.07.18 |
|---|
