이 기체는 6 TOPS의 속도의 NPU를 장착하고 있어서 인공지능 기능이 가능합니다.

가령 stereo camera로부터 추출해 내는 3D Depth와 YOLOv8이 가능합니다.

만들다 보니 지금 드론 자체의 무게만 해도 330g 짜리 배터리까지 합치면 거의 1kg이 넘어간다.

어쩌면 아마도 지금 달아놓은 모터로는 뜨지도 못할 가능성까지 있다.

거기에 지금 필요하다고 생각되는 Payload를 2.5kg으로 추가로 잡는다면 드론 모터부터 바꿔야 할 지 모른다.

그래서 모터 찾는 방법들을 찾아보았다.

넉넉잡고 드론까지 합쳐서 4kg의 Load를 날릴 수 있는 모터를 찾아야겠다.

https://robu.in/selecting-quadcopter-motor-a-detailed-guide-2021/

https://www.alibaba.com/product-detail/MOTORS-big-thrust-brushless-drone-OEM_1600799748053.html

https://www.aliexpress.us/item/3256805045951351.html?spm=a2g0o.order_list.order_list_main.5.14af1802RRDMP2&gatewayAdapt=glo2usa 

Orange Pi 5에서 모터와 기타 단순 관리용 서브모듈로 사용하는 Arduino Nano와의 시리얼 통신용 코드.

이 방식은 아래와 같이 형태의 4글자 명령어와 최대 4개의 파라메터를 함께 보낼 수 있는 방식이다. 기체의 두뇌인 Orange Pi 5에서 온갖 명령어를 보내서 단순 작업들을 아두이노 나노에게 실행시키는 방법이다.
<ESPC:3,4,-29334,4>
<ESCT:0>

Parses.h

#define BUFFER_SIZE 50

int P2B_NEWLINE  = 11; // < -- Beginning of Command to Remote Control that controls R/C Car
int P2B_COM_TYPE = 12; // 0..9 -- First 2 letters following "<"
                       // Total Command Types : 10 x 10 = 100
int P2B_DATA     = 12; // Data section
int P2B_ENDING   = 13; // > -- End of Message Signal

int P2B_STAT_NONE = 1; // Do nothing until '<' has received
int P2B_STAT_CMD  = 2; // Accepting Command
int P2B_STAT_DATA = 3; // Accepting Command data
int P2B_STAT_END  = 4; // '>' has receieve, it is the end of   command;

// PI to Nano command string
char p2b_command[5];
char p2b_data[BUFFER_SIZE];
int  p2b_paramcount = 0;
int  p2b_status = 0;

int i = 0; // dumb int
int j = 0; // dumb int
int k = 0; // dumb int
int n = 0; // dumb int
int lp =0; // dumb last position of input command string

int r = 0; // Simple Counter to replace delay() in the main loop

String s;   // Temporary string for general purpose
String rs; 
String cmd; // used on ExecuteLogicalCommand() 

boolean debugSerial = true;
boolean ContionousMove = true;  

char    inputString[200];        // a string to hold incoming data
boolean stringComplete = false;  // whether the string is complete
char    inChar;
String  tempStr;
char    buffer[12]; // buffer used for itoa  (int to string conversion)
int     inlen = 0;  // Length of inputString
char    RemoteString[50];    
// value from PC Command (integer params 1 .. 4 max) 
int     paramcount = 0; // How many params in input String
int     param1 = -1;  // value from PC Command (Param 1)
int     param2 = -1;  // value from PC Command (Param 2)
int     param3 = -1;  // value from PC Command (Param 3)
int     param4 = -1;  // value from PC Command (Param 4)

int tempVal = 0;  
int nPos = 0;
int sign = 1;  // 1 = plus, -1 = minus

float fparam1 = 0.0; 
float fparam2 = 0.0; 
float fparam3 = 0.0; 
float fparam4 = 0.0; 

boolean SecondParam = false;   // Now gettign second parameter (after ,)


// NEW PINS 18 = cam
const int CAMPin = 5;// the number of the LED pin

int freq = 5000;
int resolution = 8;

int dist = 0;

int StartingSpeed = 0; // 0 - 255 (8 bit)

int targetHeight = 0;
int msAi = 0;  
int msBi = 0;
int msCi = 0;
int msDi = 0;
int maxSpeed = 50;

int height = 0;
int targetLR = 0;  // Target Left/Right
int targetFB = 0;  // Target Forward / Backward
int targetTN = 0;  // Target Turn (+ CW, - CCW)

boolean mStop      = false; // MS, Stop Motors
boolean mGoInAngle = false; // MG, Set Motor Speeds and Directions

boolean contGetAll = false; // 
boolean AutoBalancing = false;
boolean NoOffset = false;

////////////////////////////////////////////////////////////////

void ClearCommand() { 
   p2b_command[0] = 0;  // null
   for (int i=0; i <= 49; i++){
      p2b_data[i] = 0;  // null
   }
   
   // set as none, 0 could be value
   param1 = -1;  // 
   param2 = -1;  // 
   param3 = -1;  // 
   param4 = -1;  // 

   SecondParam = false;
}

void ClearRemoteString() {
   for (i=0;i<=49;i++) {
      RemoteString[i] = 0;
   };
}

void ClearParamsAndInputstring(){
   // clear the string:
   for (i=0;i<=49;i++) {
      inputString[i] = 0;
   };
   i = 0;
   j = 0;

   paramcount = 0; // How many params in input String
   param1 = -1; 
   param2 = -1; 
   param3 = -1; 
   param4 = -1; 
   
   fparam1 = 0.0; 
   fparam2 = 0.0; 
   fparam3 = 0.0; 
   fparam4 = 0.0;
      
   stringComplete = false;
}

MAR01AR.ino

/*  This is written by Henry Kim, as a M.A.R. (Machine of Attack & Return)
   Currently testing level @ Oct. 1, 2023 
   
   3 ESC for Propellers
      - Center Tail Propeller
      - Left Wing Propeller
      - Right Wing Propeller
   3 Servo 메인 모터 180도 회전
      - for 180 degree rotation of 3 Propeller Motors
   10 LED
   1 A/D Battery Level Monitor
   1 A/D Light Sensor
   3 A/D Sound Sensor  
   1 A/D Temperature Sensor   
*/

#include <Servo.h>
#include "Parses.h"

// ---------------------------------------------------------------------------
// Customize here pulse lengths as needed
#define MIN_PULSE_LENGTH 1000 // Minimum pulse length in µs  (1 ms : min speed)
#define MAX_PULSE_LENGTH 2000 // Maximum pulse length in µs  (2 ms : max speed)

// ---------------------------------------------------------------------------
Servo escLeft1, escRight2, escCenter3;
Servo motLeft4, motRight5, motCenter6;

// ---------------------------------------------------------------------------

void setup() {   
   ClearCommand();
   
   Serial.begin(115200);
   Serial.println("#MAR-NANO>");

   // 3 main ESC for Props
   escLeft1.attach(4, MIN_PULSE_LENGTH, MAX_PULSE_LENGTH);
   escRight2.attach(5, MIN_PULSE_LENGTH, MAX_PULSE_LENGTH);
   escCenter3.attach(6, MIN_PULSE_LENGTH, MAX_PULSE_LENGTH);
   
   // Servos - Change angles of Prop motors
   motLeft4.attach(7);
   motRight5.attach(8);
   motCenter6.attach(9);

   // Prevent motor run at the start
   stopAllProps();  // set in MIN_PULSE_LENGTH
   
   // Servos heading top initially
   basePosition4Servos();  // Heading Top
   
}

void loop() {
   r++;   // 
   if (r > 300000) { r = 0; }   // Reset r if it is too large 
   if ((r % 20000) == 0) {      // Do some sensor reading
      //
   }
   
   while (Serial.available()) {
      inChar = (char)Serial.read(); 
      // get the new byte:
      //Serial.print(inChar);  // for debut
      // add it to the inputString:
      inputString[i] = inChar;
      i++;
      
      // if the incoming character is a newline, set a flag
      if (inChar == '<') {
         p2b_status = P2B_STAT_CMD;
         ClearCommand();
         j = i-1;         

      } else if (inChar == '>') {  // so the main loop can do something about it:

         p2b_command[0] = inputString[j+1]; // Get Command Character
         p2b_command[1] = inputString[j+2]; // Get Command Character
         p2b_command[2] = inputString[j+3]; // Get Command Character
         p2b_command[3] = inputString[j+4]; // Get Command Character
                                            // O : Orientation
                                            // D : Distance
         p2b_paramcount = int(inputString[j+6])-48; // take out 0
            
         j = j + 8;  // now j is pointing param starting position 
         if (p2b_paramcount > 0) {
            tempVal = 0; 
            nPos = 0;
            sign = 1;  // 1 = plus, -1 = minus
            k = 1;
            for (n=j;n<=lp+1;n++){
               // end of param 
               if ((inputString[n] == ',') or (inputString[n] == '>')){  
                  if (k == 1){
                     param1 = tempVal * sign;
                  }
                  if (k == 2){
                     param2 = tempVal * sign;
                  }
                  if (k == 3){
                     param3 = tempVal * sign;
                  }
                  if (k == 4){
                     param4 = tempVal * sign;
                  }
                  tempVal = 0; 
                  k++;  // Param No.
                  nPos = 0;
                  sign = 1;  // 1 = plus, -1 = minus
               } else {
                  if (inputString[n] == '-'){
                     sign = -1;
                  } else {
                     nPos++; 
                     if (nPos == 1) {
                        tempVal = (byte(inputString[n]) - 48); 
                     } else {
                        tempVal = tempVal * 10 + (byte(inputString[n]) - 48); 
                     }
                  }
               }
            }
         } // end of if command is for this Machine
         
         Serial.println("ESCT TEST 00"); 
         ExecuteLogicalCommand();   
         
         i = 100;  // refresh counter
         stringComplete = true;
         p2b_status = P2B_STAT_NONE;
            
      } else {
         lp = i-1;  // set last position on each entry;
      }
   }  /* while (Serial.available()) { */  
   if (stringComplete) {  
      uPrint();  // Return for debug
      ClearParamsAndInputstring(); 
   }
                    
}

void ExecuteLogicalCommand() {
   cmd = (char)p2b_command[0]+(char)p2b_command[1]+(char)p2b_command[2]+(char)p2b_command[3];
          
   if (cmd == "ESCT"){  // Test ESC for Prop Motors
      testESC(); 
   };
   if (cmd == "SRVT"){  // Test All Servos
      testServos();
   }

}

void basePosition4Servos(){
   // Servos heading top initially
   motLeft4.write(180);    // Heading Top
   motRight5.write(180);   // Heading Top
   motCenter6.write(180);  // Heading Top
}

void testServos()  // 3 Servos : escLeft1, escRight2, escCenter3
{
   // Heading top to bottom
   for (int i = 0; i <= 180; i += 10) {
      motLeft4.write(180-i);
      motRight5.write(180-i);
      motCenter6.write(180-i);      
      delay(100);
   // Heading bottom to top
   for (int i = 0; i <= 180; i += 10) {
      motLeft4.write(i);
      motRight5.write(i);
      motCenter6.write(i);      
      delay(100);
   }
   basePosition4Servos();
}

void stopAllProps() {
   escLeft1.writeMicroseconds(MIN_PULSE_LENGTH);
   escRight2.writeMicroseconds(MIN_PULSE_LENGTH);
   escCenter3.writeMicroseconds(MIN_PULSE_LENGTH);
}

void testESC()  // 3 ESC Test : escLeft1, escRight2, escCenter3
{
   for (int i = MIN_PULSE_LENGTH; i <= MAX_PULSE_LENGTH-700; i += 40) {
      // Serial.print("Pulse length = ");
      // Serial.println(i);
      escLeft1.writeMicroseconds(i);
      escRight2.writeMicroseconds(i);
      escCenter3.writeMicroseconds(i);      
      delay(200);
   }   
   stopAllProps();
}

초벌.  만들어 가면서 계속 수정 중임.

=========================

===================================================

현재 기초 기능 코딩용 기체. 

(날개도 180도 움직이지 않는 고정형, 날개를 움직이는 서보는 그냥 따로 옆에 붙여서 컨트롤 실험만 한 후에 나중에 최종 프레임을 디자인 할 계획임.)

좋은 출처 : https://www.electronicwings.com/raspberry-pi/mpu6050-accelerometergyroscope-interfacing-with-raspberry-pi

MPU6050 Code for Raspberry Pi using Python 

'''
        Read Gyro and Accelerometer by Interfacing Raspberry Pi with MPU6050 using Python
	http://www.electronicwings.com
'''
import smbus				#import SMBus module of I2C
from time import sleep        #import

#some MPU6050 Registers and their Address
PWR_MGMT_1   = 0x6B
SMPLRT_DIV   = 0x19
CONFIG       = 0x1A
GYRO_CONFIG  = 0x1B
INT_ENABLE   = 0x38
ACCEL_XOUT_H = 0x3B
ACCEL_YOUT_H = 0x3D
ACCEL_ZOUT_H = 0x3F
GYRO_XOUT_H  = 0x43
GYRO_YOUT_H  = 0x45
GYRO_ZOUT_H  = 0x47


def MPU_Init():
	#write to sample rate register
	bus.write_byte_data(Device_Address, SMPLRT_DIV, 7)
	
	#Write to power management register
	bus.write_byte_data(Device_Address, PWR_MGMT_1, 1)
	
	#Write to Configuration register
	bus.write_byte_data(Device_Address, CONFIG, 0)
	
	#Write to Gyro configuration register
	bus.write_byte_data(Device_Address, GYRO_CONFIG, 24)
	
	#Write to interrupt enable register
	bus.write_byte_data(Device_Address, INT_ENABLE, 1)

def read_raw_data(addr):
	#Accelero and Gyro value are 16-bit
        high = bus.read_byte_data(Device_Address, addr)
        low = bus.read_byte_data(Device_Address, addr+1)
    
        #concatenate higher and lower value
        value = ((high << 8) | low)
        
        #to get signed value from mpu6050
        if(value > 32768):
                value = value - 65536
        return value


bus = smbus.SMBus(5) 	# or bus = smbus.SMBus(0) for older version boards
Device_Address = 0x68   # MPU6050 device address

MPU_Init()

print (" Reading Data of Gyroscope and Accelerometer")

while True:
	
	#Read Accelerometer raw value
	acc_x = read_raw_data(ACCEL_XOUT_H)
	acc_y = read_raw_data(ACCEL_YOUT_H)
	acc_z = read_raw_data(ACCEL_ZOUT_H)
	
	#Read Gyroscope raw value
	gyro_x = read_raw_data(GYRO_XOUT_H)
	gyro_y = read_raw_data(GYRO_YOUT_H)
	gyro_z = read_raw_data(GYRO_ZOUT_H)
	
	#Full scale range +/- 250 degree/C as per sensitivity scale factor
	Ax = acc_x/16384.0
	Ay = acc_y/16384.0
	Az = acc_z/16384.0
	
	Gx = gyro_x/131.0
	Gy = gyro_y/131.0
	Gz = gyro_z/131.0
	

	print ("Gx=%.2f" %Gx, u'\u00b0'+ "/s", "\tGy=%.2f" %Gy, u'\u00b0'+ "/s", "\tGz=%.2f" %Gz, u'\u00b0'+ "/s", "\tAx=%.2f g" %Ax, "\tAy=%.2f g" %Ay, "\tAz=%.2f g" %Az) 	
	sleep(1)

Code 실행 결과 

 Reading Data of Gyroscope and Accelerometer
Gx=0.00 °/s     Gy=0.00 °/s     Gz=0.00 °/s     Ax=0.00 g       Ay=0.00 g       Az=0.00 g
Gx=-0.25 °/s    Gy=-0.05 °/s    Gz=-0.04 °/s    Ax=-0.18 g      Ay=0.56 g       Az=0.72 g
Gx=-0.28 °/s    Gy=-0.10 °/s    Gz=-0.02 °/s    Ax=-0.17 g      Ay=0.56 g       Az=0.74 g
Gx=21.01 °/s    Gy=-46.73 °/s   Gz=-66.13 °/s   Ax=-0.60 g      Ay=0.53 g       Az=1.98 g
Gx=-1.12 °/s    Gy=0.08 °/s     Gz=-2.44 °/s    Ax=0.91 g       Ay=0.25 g       Az=-0.42 g
Gx=-20.89 °/s   Gy=22.92 °/s    Gz=-12.68 °/s   Ax=0.57 g       Ay=0.91 g       Az=-0.08 g
Gx=-8.25 °/s    Gy=6.10 °/s     Gz=-2.79 °/s    Ax=-0.14 g      Ay=-0.28 g      Az=0.79 g
Gx=0.19 °/s     Gy=-1.33 °/s    Gz=0.41 °/s     Ax=-0.27 g      Ay=-0.19 g      Az=0.89 g
Gx=-1.12 °/s    Gy=0.74 °/s     Gz=-1.82 °/s    Ax=0.01 g       Ay=-0.03 g      Az=0.93 g
Gx=-16.98 °/s   Gy=-8.69 °/s    Gz=-0.35 °/s    Ax=0.53 g       Ay=0.11 g       Az=0.41 g
Gx=-0.86 °/s    Gy=-3.69 °/s    Gz=-3.56 °/s    Ax=0.41 g       Ay=-0.85 g      Az=1.86 g
Gx=8.50 °/s     Gy=-13.91 °/s   Gz=-2.17 °/s    Ax=1.00 g       Ay=0.22 g       Az=-0.11 g
Gx=1.95 °/s     Gy=-15.69 °/s   Gz=-6.44 °/s    Ax=0.88 g       Ay=0.49 g       Az=0.22 g
Gx=-0.90 °/s    Gy=-19.22 °/s   Gz=4.11 °/s     Ax=0.79 g       Ay=0.20 g       Az=0.20 g
Gx=2.63 °/s     Gy=-0.53 °/s    Gz=3.50 °/s     Ax=0.57 g       Ay=0.22 g       Az=0.47 g
Gx=7.98 °/s     Gy=-5.44 °/s    Gz=-4.53 °/s    Ax=0.16 g       Ay=-0.44 g      Az=1.19 g
Gx=-1.49 °/s    Gy=2.40 °/s     Gz=-1.51 °/s    Ax=0.50 g       Ay=-0.40 g      Az=0.69 g
Gx=9.06 °/s     Gy=-0.34 °/s    Gz=11.00 °/s    Ax=-0.20 g      Ay=-0.04 g      Az=0.61 g
Gx=8.08 °/s     Gy=-0.40 °/s    Gz=-2.18 °/s    Ax=-0.12 g      Ay=0.08 g       Az=0.93 g
Gx=-0.26 °/s    Gy=-0.06 °/s    Gz=-0.07 °/s    Ax=-0.04 g      Ay=0.12 g       Az=0.94 g
Gx=-0.23 °/s    Gy=-0.02 °/s    Gz=-0.08 °/s    Ax=-0.04 g      Ay=0.12 g       Az=0.94 g
Gx=-0.25 °/s    Gy=-0.08 °/s    Gz=-0.09 °/s    Ax=-0.06 g      Ay=0.15 g       Az=0.93 g
Gx=-0.23 °/s    Gy=-0.05 °/s    Gz=-0.03 °/s    Ax=-0.06 g      Ay=0.17 g       Az=0.93 g
Gx=-0.29 °/s    Gy=-0.06 °/s    Gz=-0.05 °/s    Ax=-0.07 g      Ay=0.15 g       Az=0.92 g
Gx=-0.40 °/s    Gy=0.06 °/s     Gz=-0.11 °/s    Ax=-0.07 g      Ay=0.16 g       Az=0.94 g
Gx=-0.28 °/s    Gy=-0.04 °/s    Gz=-0.08 °/s    Ax=-0.08 g      Ay=0.17 g       Az=0.93 g
Gx=-0.25 °/s    Gy=-0.07 °/s    Gz=-0.04 °/s    Ax=-0.06 g      Ay=0.14 g       Az=0.94 g
Gx=-0.27 °/s    Gy=-0.05 °/s    Gz=-0.02 °/s    Ax=-0.06 g      Ay=0.16 g       Az=0.93 g
Gx=6.27 °/s     Gy=8.89 °/s     Gz=2.79 °/s     Ax=0.06 g       Ay=-0.07 g      Az=0.90 g
Gx=4.08 °/s     Gy=36.98 °/s    Gz=-14.33 °/s   Ax=0.28 g       Ay=0.08 g       Az=0.66 g
Gx=-2.27 °/s    Gy=-2.36 °/s    Gz=-1.52 °/s    Ax=-0.13 g      Ay=0.01 g       Az=0.95 g
Gx=0.41 °/s     Gy=-0.18 °/s    Gz=-0.15 °/s    Ax=-0.09 g      Ay=0.30 g       Az=0.89 g
^CTraceback (most recent call last):
  File "/root/o/2.py", line 82, in <module>
    sleep(1)

원본 영상 : youtu.be/AwRrOxVjAWE 

윗 영상의 코드를 직접 따라 해 봤습니다.  아무래도 원본에 에러가 있는 듯 한데 직접 고쳤습니다.

윗 영상은 저처럼 파이션이 낯선 사람들이 공부용으로는 좋은데 로봇 이미지와 코드는 무료가 아닙니다.

이건 제가 직접 만든 로봇 이미지와 입력한 코드입니다. (Zip 화일로 올려놓았습니다)

설명을 들으면서 치다보니 pygame에 익숙해 지는 느낌이라 오히려 좋았습니다.

# File name : difdrive.py
# Link : https://www.youtube.com/watch?v=zHboXMY45YU
# Download Car image & rename to DDR.png : https://imgur.com/SpeVm6L 
# x1 = v*cos(theta)
# y1 = v*sin(theta)
# Theta1 = diff(v) / W

import pygame
import math

class Envir:
   def __init__(self, dimensions):
      # colours
      self.black = (0, 0, 0)
      self.white = (255, 255, 255)
      self.green = (0, 255, 0)
      self.blue  = (0, 0, 255)
      self.red   = (255, 0, 0)
      self.yel   = (70, 70, 70)
      # map dimensions
      self.height = dimensions[0]
      self.width  = dimensions[1]
      # window settings
      pygame.display.set_caption("Differential drive robot")
      self.map = pygame.display.set_mode((self.width, self.height))
      # text variables
      self.font = pygame.font.Font('freesansbold.ttf', 50)
      self.text = self.font.render('default', True, self.white, self.black)
      self.textRect = self.text.get_rect()
      self.textRect.center = (dimensions[1] - 600,
                              dimensions[0] - 100)
      #trail
      self.trail_set=[]                        
                              
   def write_info(self, Vl, Vr, theta):
      txt = f"Vl = {Vl} Vr = {Vr}  theta = {int(math.degrees(theta))}"
      self.text = self.font.render(txt, True, self.white, self.black)
      self.map.blit(self.text, self.textRect)
      
   def trail(self, pos):
      for i in range(0, len(self.trail_set)-1):
         pygame.draw.line(self.map, self.yel, 
            (self.trail_set[i][0], self.trail_set[i][1]),
            (self.trail_set[i+1][0], self.trail_set[i+1][1]))
      if self.trail_set.__sizeof__() > 10000:
         self.trail_set.pop(0)
      self.trail_set.append(pos)   
      
   def robot_frame(self, pos, rotation):
      n = 80
      
      centerx, centery = pos
      x_axis = (centerx + n * math.cos(-rotation), centery + n * math.sin(-rotation))
      y_axis = (centerx + n * math.cos(-rotation + math.pi/2), 
                centery + n * math.sin(-rotation + math.pi/2))
      pygame.draw.line(self.map, self.red, (centerx, centery), x_axis, 3)
      pygame.draw.line(self.map, self.green, (centerx, centery), y_axis, 3)      
                 
class Robot:
   def __init__(self, startpos, robotImg, width):
      self.m2p = 3779.52  # meters to pixels ratio
      # robot dimensions
      self.w = width
      self.x = startpos[0]
      self.y = startpos[1]
      self.theta = 0
      self.vl = 0.01 * self.m2p  # meters / second
      self.vr = 0.01 * self.m2p  # meters / second
      self.maxspeed = 0.02 * self.m2p
      self.minspeed = -0.02 * self.m2p
      # graphics
      self.img = pygame.image.load(robotImg)
      self.rotated = self.img
      self.rect=self.rotated.get_rect(center=(self.x, self.y))
      
   def draw(self, map):
      map.blit(self.rotated, self.rect)   
      
   def move(self, event=None):
      if event is not None:
         if event.type == pygame.KEYDOWN:
            if event.key == pygame.K_KP4:  # Left Arrow in Numpad     
               self.vl += 0.001 * self.m2p
            elif event.key == pygame.K_KP1:  # 1  
               self.vl -= 0.001 * self.m2p    
            elif event.key == pygame.K_KP6:  # Right Arrow in Numpad  
               self.vr += 0.001 * self.m2p
            elif event.key == pygame.K_KP3:  # 3  
               self.vr -= 0.001 * self.m2p
               
      # v=(Vr+Vl)/2                  
      self.x += ((self.vl + self.vr) / 2) * math.cos(self.theta) * dt   
      self.y -= ((self.vl + self.vr) / 2) * math.sin(self.theta) * dt
      self.theta += (self.vr - self.vl) / self.w * dt
      # reset theta
      if self.theta > 2 * math.pi or self.theta < -2 * math.pi :
         self.theta = 0
      # set max speed
      self.vr = min(self.vr, self.maxspeed)
      self.vl = min(self.vl, self.maxspeed)
      # set min speed      
      self.vr = max(self.vr, self.minspeed)
      self.vl = max(self.vl, self.minspeed)
      
      self.rotated = pygame.transform.rotozoom(self.img,
         math.degrees(self.theta), 1)         
      self.rect = self.rotated.get_rect(center=(self.x, self.y))
      
         
      
# initialization
pygame.init()

# start postion
start = (200, 200)

# dimension
dims=(600, 1200)

# running or not
running = True

# the envir
environment = Envir(dims)

# the Robot (begining location, image of robot, width)
robot = Robot(start, "./DDR.png", 0.01 * 3779.52)

# dt (Change in time)
dt = 0
lasttime = pygame.time.get_ticks()

# simulation loop
while running:
   for event in pygame.event.get():
      if event.type == pygame.QUIT:
         running = False
      robot.move(event)
       
   dt = (pygame.time.get_ticks()-lasttime) / 1000     
   lasttime = pygame.time.get_ticks()
   pygame.display.update()
   environment.map.fill(environment.black)
   robot.move()
   environment.write_info(int(robot.vl),
                          int(robot.vr),
                          robot.theta)
   robot.draw(environment.map)          
   environment.trail((robot.x, robot.y))             
   environment.robot_frame((robot.x, robot.y), robot.theta)
                              
   #if pygame.mouse.get_focused():
      #sensorOn = True

(나중에 추가) 아래 코드는 원작자도 에라가 있는 걸 못 고친 듯 합니다.   알고보니 원본 데모 영상에도 똑같은 에러가 있더군요.

별로 가치가 없는 내용이었습니다.

===============================================

알고보틱의 https://www.youtube.com/watch?v=ZxaXfahaP2s 비디오 코드인데 작가의 코드와 똑같지 않은듯 합니다.

약간 결과가 다르게 나오더군요.   

총 5편의 비디오 강의의 결과물인데 강의 영상과는 결과가 다르게 부실하게 나왔습니다.

알고보틱은 예전에는 코드를 판매했는데 살만한 가치까지는 없어보여서 그냥 웹에 뒤지니 대충 나왔습니다.

요즘에는 판매하는 account도 없애버린 것 같더군요.   

env.py

import math
import pygame

class buildEnvironment:
    def __init__(self, MapDimensions):
        pygame.init()
        self.pointCloud = []
        self.externalMap = pygame.image.load('floorplan.png')
        self.maph, self.mapw = MapDimensions
        self.MapWindowName = 'RRT path planning'
        pygame.display.set_caption(self.MapWindowName)
        self.map = pygame.display.set_mode((self.mapw,self.maph))
        self.map.blit(self.externalMap,(0,0))
        #Colours
        self.black = (0, 0, 0)
        self.grey = (70, 70, 70)
        self.Blue = (0, 0, 255)
        self.Green = (0, 255, 0)
        self.Red = (255, 0, 0)
        self.white = (255, 255, 255)


    def AD2pos(self, distance, angle, robotPosition):
        x = distance * math.cos(angle) + robotPosition[0]
        y = -distance * math.sin(angle) + robotPosition[1]
        return (int(x), int(y))

    def dataStorage(self, data):
        print(len(self.pointCloud))
        for element in data:
            point = self.AD2pos(element[0], element[1], element[2])
            if point not in self.pointCloud:
                self.pointCloud.append(point)

    def show_sensorData(self):
        self.infomap = self.map.copy()
        for point in self.pointCloud:
            self.infomap.set_at((int(point[0]), int(point[1])), (255, 0, 0))

sensors.py

import math
import pygame
import numpy as np

def uncertaninty_add(distance, angle, sigma):
    mean = np.array([distance, angle])
    covariance = np.diag(sigma**2)
    distance, angle = np.random.multivariate_normal(mean, covariance)
    distance = max(distance, 0)
    angle = max(angle, 0)
    return [distance, angle]


class LaserSensor:
    def __init__(self, Range, map, uncertainty):
        self.Range = Range
        self.speed = 3 #rounds per second
        self.sigma = np.array([uncertainty[0], uncertainty[1]])
        self.position = (0,0)
        self.map = map
        self.W, self.H = pygame.display.get_surface().get_size()
        self.sensedObstacles = []

    def distance(self, obsatclePosition):
        px = (obsatclePosition[0] - self.position[0])**2
        py = (obsatclePosition[1] - self.position[1])**2
        return math.sqrt(px + py)

    def sense_obstacles(self):
        data = []
        x1, y1 = self.position[0], self.position[1]
        for angle in np.linspace(0, 2*math.pi, 180, False):
            x2, y2 = (x1 + self.Range * math.cos(angle), y1 - self.Range * math.sin(angle))
            for i in range(0,100):
                u = i / 100
                x = int(x2*u + x1*(1-u))
                y = int(y2*u + y1*(1-u))
                if 0 < x < self.W and 0 < y < self.H:
                    color = self.map.get_at((x,y))
                    if (color[0], color[1], color[2]) == (0,0,0):
                        distance = self.distance((x,y))
                        output = uncertaninty_add(distance, angle, self.sigma)
                        output.append(self.position)
                        # store the mesurement
                        data.append(output)
                        break
        if len(data) > 0:
            return data
        else:
            return False

Features.py

# Features.py

import numpy as np
import math
from fractions import Fraction
from scipy.odr import *

# landmarks
Landmarks = []

class featuresDetection:
   def __init__(self):
      # variables
      self.EPSILON = 10
      self.DELTA = 10
      self.SNUM = 6
      self.PMIN = 20
      self.GMAX = 20
      self.SEED_SEGMENTS = []
      self.LINE_SEGMENTS = []
      self.LASERPOINTS = []
      self.LINE_PARAMS = None
      self.NP = len(self.LASERPOINTS) - 1
      self.LMIN = 5 # minimum length of a line segment
      self.LR = 0 # real length of a line segment
      self.PR = 0 # the number of laser points contained
      self.FEATURES = []

   # euclidian distance from point1 to point2
   def dist_point2point(self, point1, point2):
   #def dist_point2point(point1, point2):
      Px = (point1[0] - point2[0]) ** 2
      Py = (point1[1] - point2[1]) ** 2
      return math.sqrt(Px + Py)
      
   # distance point to line written in general form
   def dist_point2line(self, params, point):
      A, B, C = params
      distance = abs(A * point[0] + B * point[1] + C) / math.sqrt(A ** 2 + B ** 2)
      return distance
      
   # extract two points from a line equation under the slope intercepts form
   def line_2points(self, m, b):
      x = 5
      y = m * x + b
      x2 = 2000
      y2 = m * x2 + b
      return [(x, y), (x2, y2)]
      
   # general form to slope-intercept
   def lineForm_G2SI(self, A, B, C):
      m = -A / B
      B = -C / B
      return m, B
      
   # slope-intercept to general form
   def lineForm_Si2G(self, m, B):
      A, B, C = -m, 1, -B
      if A < 0:
         A, B, C = -A, -B, -C
      den_a = Fraction(A).limit_denominator(1000).as_integer_ratio()[1]   
      den_c = Fraction(C).limit_denominator(1000).as_integer_ratio()[1]
   
      gcd = np.gcd(den_a, den_c)
      lcm = den_a * den_c / gcd
      
      A = A * lcm
      B = B * lcm
      C = C * lcm
      return A, B, C
      
   def line_intersect_general(self, params1, params2):
      a1, b1, c1 = params1
      a2, b2, c2 = params2
      if (b1 * a2 - a1 * b2) != 0:
         x = (c1 * b2 - b1 * c2) / (b1 * a2 - a1 * b2)
         y = (a1 * c2 - a2 * c1) / (b1 * a2 - a1 * b2)
      return x, y
      
   def points_2line(self, point1, point2):
      m, b = 0, 0
      if point2[0] == point1[0]:
         pass
      else:
         m = (point2[1] - point1[1]) / (point2[0] - point1[0])
         b = point2[1] - m * point2[0]
      return m, b

   def projection_point2line(self, point, m, b):
      x, y = point
      m2 = -1 / m
      c2 = y - m2 * x
      
      intersection_x = - (b - c2) / (m - m2)
      intersection_y = m2 * intersection_x + c2
      return intersection_x, intersection_y      
      
   def AD2pos(self, distance, angle, robot_position):
      x = distance * math.cos(angle) + robot_position[0]
      y = -distance * math.sin(angle) + robot_position[1]
      return (int(x), int(y))
      
   def laser_points_set(self, data):
      self.LASERPOINTS = []   
      if not data:
         pass
      else:
         for point in data:
            coordinates = self.AD2pos(point[0], point[1], point[2])
            self.LASERPOINTS.append([coordinates, point[1]])
      self.NP = len(self.LASERPOINTS) - 1
   
   # Define a function (quadratic in our case) to fit the data with.
   def linear_func(self, p, x):
      m, b = p
      return m * x + b
      
   def odr_fit(self, laser_points):
      x = np.array([i[0][0] for i in laser_points])
      y = np.array([i[0][1] for i in laser_points])   
      
      # Create a model for fitting.
      linear_model = Model(self.linear_func)
      
      # Create a RealData object using our initiated data from above.
      data = RealData(x, y)
      
      # Set up ODR with the model and data.
      odr_model = ODR(data, linear_model, beta0=[0., 0.])
      
      # Run the regression.
      out = odr_model.run()
      m, b = out.beta
      return m, b
      
   def predictPoint(self, line_params, sensed_point, robotpos):
      m, b = self.points_2line(robotpos, sensed_point)
      params1 = self.lineForm_Si2G(m, b)
      predx, predy = self.line_intersect_general(params1, line_params)
      return predx, predy
      
   def seed_segment_detection(self, robot_position, break_point_ind):
      flag = True
      self.NP = max(0, self.NP)
      self.SEED_SEGMENTS = []
      for i in range(break_point_ind, (self.NP - self.PMIN)):
         predicted_points_to_draw = []
         j = i + self.SNUM
         m, c = self.odr_fit(self.LASERPOINTS[i:j])
         
         params = self.lineForm_Si2G(m, c)
         
         for k in range(i, j):
            predicted_point = self.predictPoint(params, self.LASERPOINTS[k][0], robot_position)
            predicted_points_to_draw.append(predicted_point)
            d1 = self.dist_point2point(predicted_point, self.LASERPOINTS[k][0])
            
            if d1 > self.DELTA:
               flag = False
               break
            d2 = self.dist_point2line(params, predicted_point)
            
            if d2 > self.EPSILON:
               flag = False
               break
         if flag:
            self.LINE_PARAMS = params
            return [self.LASERPOINTS[i:j], predicted_points_to_draw, (i, j)]
            
      return False
   
   def seed_segment_growing(self, indices, break_point):
      line_eq = self.LINE_PARAMS
      i, j = indices
      # Beginning and Final points in the line segment
      PB, PF = max(break_point, i - 1), min(j + 1, len(self.LASERPOINTS) - 1)
      
      while self.dist_point2line(line_eq, self.LASERPOINTS[PF][0]) < self.EPSILON: 
         if PF > self.NP - 1:
            break
         else:
            m, b = self.odr_fit(self.LASERPOINTS[PB:PF])
            line_eq = self.lineForm_Si2G(m, b)
            
            POINT = self.LASERPOINTS[PF][0]
          
         PF = PF + 1
         NEXTPOINT = self.LASERPOINTS[PF][0]
         if self.dist_point2point(POINT, NEXTPOINT) > self.GMAX:
            break
            
      PF = PF - 1
      
      while self.dist_point2line(line_eq, self.LASERPOINTS[PB][0]) < self.EPSILON:
         if PB < break_point:
            break
         else:
            m, b = self.odr_fit(self.LASERPOINTS[PB:PF])
            line_eq = self.lineForm_Si2G(m, b)
            POINT = self.LASERPOINTS[PB][0]
            
         PB = PB - 1
         NEXTPOINT = self.LASERPOINTS[PB][0]
         if self.dist_point2point(POINT, NEXTPOINT) > self.GMAX:
            break
      PB = PB + 1
      
      LR = self.dist_point2point(self.LASERPOINTS[PB][0], self.LASERPOINTS[PF][0])
      PR = len(self.LASERPOINTS[PB:PF])
      
      if (LR >= self.LMIN) and (PR >= self.PMIN):
         self.LINE_PARAMS = line_eq
         m, b = self.lineForm_G2SI(line_eq[0], line_eq[1], line_eq[2])
         self.two_points = self.line_2points(m, b)
         self.LINE_SEGMENTS.append((self.LASERPOINTS[PB + 1][0], self.LASERPOINTS[PF - 1][0]))
         return [self.LASERPOINTS[PB:PF], self.two_points,
         (self.LASERPOINTS[PB + 1][0], self.LASERPOINTS[PF - 1][0]), PF, line_eq, (m, b)]
      else:
         return False
         
   def lineFeats2point(self):
      new_rep = [] # the new representation of the features
      
      for feature in self.FEATURES:
         projection = self.projection_point2line((0,0), feature[0][0], feature[0][1])
         new_rep.append([feature[0], feature[1], projection])
         
      return new_rep
         
def landmark_association(landmarks):
   thresh = 10   # 10 was default
   for l in landmarks:
   
      flag = False
      for i, Landmark in enumerate(Landmarks):
         dist = featuresDetection.dist_point2point(l[2], Landmark[2])
         if dist < thresh:
            if not is_overlap(l[1], Landmark[1]):
               continue
            else:
               Landmarks.pop(i)   
               Landmarks.insert(i, l)
               flag = True      
               
               break
         if not flag:
            Landmarks.append(l)
            
def is_overlap(seg1, seg2):
   length1 = featuresDetection.dist_point2point(seg1[0], seg1[1])
   length2 = featuresDetection.dist_point2point(seg2[0], seg2[1])               
   center1 = ((seg1[0][0] + seg1[1][0]) / 2, (seg1[0][1] + seg1[1][1]) / 2)
   center2 = ((seg2[0][0] + seg2[1][0]) / 2, (seg2[0][1] + seg2[1][1]) / 2)
   dist = featuresDetection.dist_point2point(center1, center2)
   if dist > (length1 + length2) / 2:
      return False
   else:
      return True

main.py

# Main for Feature Extraction from 2D LIDAR
#import SLAM_YTB import env, sensors, Features
import env, sensors, Features
import random
import pygame

def random_color():
   levels = range(32,256,32)
   return tuple(random.choice(levels) for _ in range(3))
FeatureMAP=Features.featuresDetection()   
environment = env.buildEnvironment((600,1200))
originalMap = environment.map.copy()
laser = sensors.LaserSensor(200, originalMap, uncertainty=(0.5, 0.01))
environment.map.fill((255,255,255))
environment.infomap = environment.map.copy()
originalMap = environment.map.copy()
running = True
FEATURE_DETECTION=True
BREAK_POINT_IND = 0

while running :
   #environment.infomap = originalMap.copy()
   FEATURE_DETECTION = True
   BREAK_POINT_IND = 0
   ENDPOINTS=[0,0]
   sensorOn = False
   PREDICTED_POINTS_TODRAW=[]
      
   for envent in pygame.event.get():
      if envent.type == pygame.QUIT:
         running = False
   if pygame.mouse.get_focused():
      sensorOn = True
   elif not pygame.mouse.get_focused():
      sensorOn = False        
   if sensorOn: 
      position = pygame.mouse.get_pos()
      laser.position = position
      sensor_data = laser.sense_obstacles()
      FeatureMAP.laser_points_set(sensor_data)
      while BREAK_POINT_IND < (FeatureMAP.NP - FeatureMAP.PMIN):
         seedSeg = FeatureMAP.seed_segment_detection(laser.position,BREAK_POINT_IND)
         if seedSeg == False:
            break
         else:
            seedSegment = seedSeg[0]
            PREDICTED_POINTS_TODRAW = seedSeg[1]
            INDICES = seedSeg[2] 
            results = FeatureMAP.seed_segment_growing(INDICES, BREAK_POINT_IND)
            if results == False:
               BREAK_POINT_IND = INDICES[1]
               continue
            else:
               line_eq = results[1]
               m, c = results[5]
               line_seg = results[0]
               OUTERMOST = results[2]
               BREAK_POINT_IND = results[3]
               
               ENDPOINTS[0] = FeatureMAP.projection_point2line(OUTERMOST[0], m, c)
               ENDPOINTS[1] = FeatureMAP.projection_point2line(OUTERMOST[1], m, c)
               FeatureMAP.FEATURES.append([[m, c], ENDPOINTS])
               
               COLOR = random_color()
               for point in line_seg:
                  #environment.infomap.set_at((int(point[0][0]), int(point[0][1])), (0, 255, 0))
                  pygame.draw.circle(environment.infomap, COLOR, (int(point[0][0]), int(point[0][1])), 2, 0)
               #pygame.draw.line(environment.infomap, (255, 0, 0), ENDPOINTS[0], ENDPOINTS[1], 2)

               pygame.draw.line(environment.infomap, (0, 255, 100), ENDPOINTS[0], ENDPOINTS[1], 1)
               environment.dataStorage(sensor_data)
               
               FeatureMAP.FEATURES=FeatureMAP.lineFeats2point()
               Features.landmark_association(FeatureMAP.FEATURES)
      for landmark in Features.Landmarks:
         pygame.draw.line(environment.infomap, (100, 100, 155), landmark[1][0], landmark[1][1], 4)
               
   environment.map.blit(environment.infomap, (0, 0))
   pygame.display.update()

원본을 다운받은 곳은 초기 영상분이었습니다.   이 곳에 Map 화일들이 있습니다.  https://github.com/kabbel/SLAM

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())); 
  }
}