현자 (HJ)

현재 상태 : PC와의 UPD 통신, PWM, Motor Controller 용 GPIO, Encoder Reading 완료.

추가할 내용 : Timer Interrupt, I2C for GY-BNO055, PID 

#include <WiFi.h>
#include <WiFiUdp.h>
#include <Wire.h>
#include "Parse.h"
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>


// ESP32's I2C Pin No.
#define _ESP32_HAL_I2C_H_

#ifdef _ESP32_HAL_I2C_H_
#define SDA_PIN 32
#define SCL_PIN 12
#endif

#define BUFFER_SIZE 100

#define THIS_MACHINE "002"  // Set this bot's no. : 2
const int ThisMachine = 2;

//IP address to send UDP data to:
const char * udpAddress = "192.168.1.100"; // 저의 PC의 고정주소
const int udpPort = 6767;    // 제가 로봇용으로 주로 사용하는 포트번호

const char serverip[]="192.168.1.100";
char packetBuffer[255];

//The udp library class
WiFiUDP udp;

//Are we currently connected?
boolean connected = false;

float ax, ay, az, aSqrt, gx, gy, gz, mDirection, mx, my, mz;

const char * host = "ESP32-02";
const char * networkName = "****";  // 집이나 사무실 와이파이 아이디
const char * networkPswd = "****";  // 와이파이 연결 비밀번호
char botresp[50] = "001-CCCC:4,000,000,000,000";  // default init size should be size of string + 1

String ToServerStr;

char ReplyBuffer[] = "acknowledged"; // a string to send back
unsigned long preMillis = 0;
unsigned long curMillis = 0;

///////////  BNO-055
TwoWire I2CBNO = TwoWire(0);
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x29, &I2CBNO);
double xPos = 0, yPos = 0, headingVel = 0;
uint16_t BNO055_SAMPLERATE_DELAY_MS = 10; //how often to read data from the board

void ExecuteLogicalCommand() {
   // WHON : Wheel StandBy : Params ( on / off )
   if (((char)p2b_command[0] == 'W') and ((char)p2b_command[1] == 'H')) { 
      if (debugSerial) {
         s = "<*WHON=";  s += p2b_paramcount; s += ">";     
         Serial.println(s);
      }
      digitalWrite(StandBy, param1);
      reset_encoder_count();
   }; 

   // FWDM : Forward Move 양 바퀴 전진, 첫 파라메터는 좌측 바퀴 속도, 둘째는 우측 바퀴 속도.
   if (((char)p2b_command[0] == 'F') and ((char)p2b_command[1] == 'W')) { 
      if (debugSerial) {
         s = "<*FWDM=";  s += p2b_paramcount; s += ">";     
         Serial.println(s);
      }
      reset_encoder_count();
      // Left Wheel (A)
      digitalWrite(Ain1, 1);
      digitalWrite(Ain2, 0);
      ledcWrite(WheelChannelLeft, param1);
      // Right Wheel (B)
      digitalWrite(Bin1, 0);
      digitalWrite(Bin2, 1);
      ledcWrite(WheelChannelRight, param2);   
   }; 

      // BWDM : Backward Move 양 바퀴 후진, 첫 파라메터는 좌측 바퀴 속도, 둘째는 우측 바퀴 속도.
   if (((char)p2b_command[0] == 'B') and ((char)p2b_command[1] == 'W')) { 
      if (debugSerial) {
         s = "<*BWDM=";  s += p2b_paramcount; s += ">";     
         Serial.println(s);
      }
      reset_encoder_count();
      // Left Wheel (A)
      digitalWrite(Ain1, 0);
      digitalWrite(Ain2, 1);
      ledcWrite(WheelChannelLeft, param1);
      // Right Wheel (B) // Currently one signal not working on B (33, 34)
      digitalWrite(Bin1, 1);
      digitalWrite(Bin2, 0);
      ledcWrite(WheelChannelRight, param2);
   };    

}  

void setupControlPins(){ 
   freq = 5000;
   resolution = 8;
   StartingSpeed = 0; // 0 - 255 (8 bit)   

   ledcSetup(WheelChannelLeft,  freq, resolution);
   ledcSetup(WheelChannelRight, freq, resolution);

   ledcAttachPin(WheelLeft,  WheelChannelLeft);
   ledcAttachPin(WheelRight, WheelChannelRight);

   ledcWrite(WheelRight, StartingSpeed);
   pinMode(Ain2, OUTPUT);
   pinMode(Ain1, OUTPUT);
   pinMode(StandBy, OUTPUT);
   pinMode(Bin1, OUTPUT);
   pinMode(Bin2, OUTPUT);
   ledcWrite(WheelLeft,  StartingSpeed);

   // Two encoder pins for wheels
   pinMode(Encoder2_A_Left, INPUT);
   pinMode(Encoder2_B_Right, INPUT);
}

void reset_encoder_count() {  // Reset before start moving
   count_AL = 0;
   count_BR = 0;
   last_count_AL = 0;
   last_count_BR = 0;
}

void ISR_Left_Encoder() {
   count_AL++;
}

void ISR_Right_Encoder() {
   count_BR++;
}

void setup() {
  
   setupControlPins(); // IO 핀들 세팅할 것.

   // 모든 시리얼은 드론에는 사용 못하고 다른 ESP32 Development Board에서 개발/디버깅 중에만 사용 가능합니다.
   Serial.begin(115200);  // 시리얼을 안쓰더라도 반드시 이렇게 놔 둘 것.

   //Connect to the WiFi network  네트웍에 연결
   connectToWiFi(networkName, networkPswd);

   // Wait for connection
   while (WiFi.status() != WL_CONNECTED) {
     delay(500);
     Serial.print(".");
   }   

   // 모든 시리얼은 무선 작동시에는 사용 못하고 디버깅 중에만 사용 가능합니다.

/*   Wire.begin(SDA_PIN, SCL_PIN); // ESP32는 반드시 이렇게.
   //Serial.println("I2C enabled! ");

   I2CBNO.begin(SDA_PIN, SCL_PIN);
   if (!bno.begin()){
      Serial.print("No BNO055 detected");
      while (1);
   } */

   delay(1000);

   r = 0;

   // Encoder Setting
   pinMode(Encoder2_A_Left, INPUT_PULLUP);
   attachInterrupt(Encoder2_A_Left, ISR_Left_Encoder, FALLING);
   pinMode(Encoder2_B_Right, INPUT_PULLUP);
   attachInterrupt(Encoder2_B_Right, ISR_Right_Encoder, FALLING);
}

void uPrint(){   // print whatever in "udpStr" with station # and ending ">"
   s = "#";  s += THIS_MACHINE;  s += ":"; s += udpStr; s += ">";
   udp.beginPacket(udpAddress,udpPort);
   udp.print(s);
   udp.endPacket();
}

void loop() {
   r++;

   if ((r % 3000) == 0){ 
      if(connected){
         //Send a packet
         udpStr = "#L/R: "; udpStr += count_AL; udpStr += ", ";  udpStr += count_BR; ;
         uPrint(); // it prints whatever in udpString      
      }
   }

   // Expected message format : <002-MECH:4,10,10,70,70>
   // "<"  beginning mark, 002 mechine no., MECH command, 4 params, + 4 values ,">" ending mark 
   //  3 digit machine no., 4 char command, # of params, params seperated by ","
   int packetSize = udp.parsePacket();
   if (packetSize) {
        
      int len = udp.read(packetBuffer, 255); // here, len = packetSize
      if (len > 0) {
         packetBuffer[len] = 0;
         if (len < 51) {
            for (i=1;i<51;i++)
               botresp[i+3] = packetBuffer[i];            
         }
      };  
      i = 0;

      while ((packetSize) >= i){
         inChar = packetBuffer[i];  // get the new byte:
         inputString[i] = inChar;
         i++;
      
         // if the incoming character is a newline, set a flag
         if ((inChar == '#') or (inChar == '<')) {
            p2b_status = P2B_STAT_CMD;
            ClearP2BCommand();
            j = i-1;         
            // ===== Debug Purpose =====
            Serial.println("시작점  #, <");
            
         } else if (inChar == '>') {  // so the main loop can do something about it:

            // First, get the machine # first, who is this command to
            p2b_machno  = 0;
            if (inputString[j+1] != '0') {
               p2b_machno = (int(inputString[j+1])-48) * 100;
            }
            if (inputString[j+2] != '0') {
               p2b_machno = p2b_machno + (int(inputString[j+2])-48) * 10;
            }
            if (inputString[j+3] != '0') {
               p2b_machno = p2b_machno + (int(inputString[j+3])-48);
            }
 
            // If this command is for ME (This machine)
            if (ThisMachine == p2b_machno){
           
               p2b_mach[0] = inputString[j+1];
               p2b_mach[1] = inputString[j+2];
               p2b_mach[2] = inputString[j+3];
               
               p2b_command[0] = inputString[j+5];
               p2b_command[1] = inputString[j+6];
               p2b_command[2] = inputString[j+7];
               p2b_command[3] = inputString[j+8];

               p2b_paramcount = int(inputString[j+10])-48; // take out 0

               Serial.print("Machine : ");
               Serial.println(p2b_machno); 
               Serial.print("Command :");
               Serial.println(p2b_command); 
               Serial.print("Parameter Count : ");
               Serial.println(p2b_paramcount);              

               j = j + 12;  // now j is pointing param starting position 
               if (p2b_paramcount > 0) {
                  tempVal = 0; 
                  nPos = 0;
                  k = 1;
                  for (n=j;n<=packetSize;n++){
                     // end of param 
                     if ((inputString[n] == ',') or (inputString[n] == '>')){  
                        if (k == 1){
                           param1 = tempVal;
                           Serial.print("Param 1 : ");
                           Serial.println(param1);
                        }
                        if (k == 2){
                           param2 = tempVal;
                           Serial.print("Param 2 : ");
                           Serial.println(param2);
                        }
                        if (k == 3){
                           param3 = tempVal;
                           Serial.print("Param 3 : ");
                           Serial.println(param3);
                        }
                        if (k == 4){
                           param4 = tempVal;
                           Serial.print("Param 4 : ");
                           Serial.println(param4);
                        }
                        tempVal = 0; 
                        k++;  // Param No.
                        nPos = 0;
                     } else {
                        if (nPos == 1) {
                           tempVal = (byte(inputString[n]) - 48); 
                        } else {
                           tempVal = tempVal * 10 + (byte(inputString[n]) - 48); 
                        }
                     }
                  }
               }
            //   Serial.println("");  
            
               ExecuteLogicalCommand();
            } // end of if command is for this Machine
         
            i = 100;  // refresh counter
            stringComplete = true;
            p2b_status = P2B_STAT_NONE;
            
         } else {
            lp = i-1;  // set last position on each entry;
         }
      }
   }   //  while ((packetSize) >= i){
 
   if (stringComplete) {
      ClearParamsAndInputstring(); 
   };      

   if (r == 5000) { 
//      if (contGetAll) {  GetAll();     }
//      if (contAccel)  {  Get3DAccel(); } 
//      if (contGyro)   {  Get3DGyro();  } 
//      if (contMag)    {  Get3DMag();   }  
//      if (contHeight) {  GetHeight();  }        
   }
   
   // Reset r
   if (r > 1000000) { r = 0; }

}

void connectToWiFi(const char * ssid, const char * pwd){
  Serial.println("Connecting to WiFi network: " + String(ssid));

  // delete old config
  WiFi.disconnect(true);
  //register event handler
  WiFi.onEvent(WiFiEvent);
  
  //Initiate connection
  WiFi.begin(ssid, pwd);

  connected = true;

  Serial.println("Waiting for WIFI connection...");
}

//wifi event handler
void WiFiEvent(WiFiEvent_t event){
    switch(event) {
      case SYSTEM_EVENT_STA_GOT_IP:
          //When connected set 
          Serial.print("WiFi connected! IP address: ");
          Serial.println(WiFi.localIP());  
          //initializes the UDP state
          //This initializes the transfer buffer
          udp.begin(WiFi.localIP(),udpPort);
          connected = true;
          break;
      case SYSTEM_EVENT_STA_DISCONNECTED:
          Serial.println("WiFi lost connection");
          connected = false;
          break;
      default: break;
    }
}

Parse.h 화일

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;

char p2b_mach[4];
char p2b_command[5];
char p2b_data[50];
int  p2b_machno = 0;
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 udpStr; 

boolean debugSerial = false;

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 = 1;

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

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

// Wheel PWM Related Definitions
// A = Left, B = Right

int WheelChannelLeft  = 0; // motor A PWM Channel
int WheelChannelRight = 1; // motor B PWM Channel

// NEW PINS 19, 23, 26, 5, 18
const int WheelLeft  = 19; // GREY - motor A on my design sheet
const int Ain2 = 25;       // WHITE
const int Ain1 = 23;       // ORANGE
const int StandBy = 5;     // YELLOW
const int Bin1 = 14;       // PINK (34 didn't work, changed to 14
const int Bin2 = 33;       // BROWN
const int WheelRight = 18; // BLUE - motor B on my design sheet

// Speed Read from Encoder
const int Encoder2_A_Left  = 36;  // Yellow Color 
const int Encoder2_B_Right = 26;  // Yellow Color 
int count_AL = 0;
int count_BR = 0;
int last_count_AL = 0;
int last_count_BR = 0;

int freq = 5000;
int resolution = 8;

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

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

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

boolean contMag = false;
boolean contAccel = false;
boolean contGyro = false;
boolean contHeight = false;
boolean contGetAll = false;
boolean AutoBalancing = true;
boolean NoOffset = false;

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

void ClearP2BCommand() {
   p2b_mach[0] = 0;  // null
   p2b_mach[1] = 0;  // null
   p2b_mach[2] = 0;  // null
   p2b_mach[3] = 0;  // null   
     
   p2b_command[0] = 0;  // null
   p2b_command[1] = 0;  // null
   p2b_command[2] = 0;  // null   
   p2b_command[3] = 0;  // null
   p2b_command[4] = 0;  // null 
   for (int i=0; i <= 49; i++){
      p2b_data[i] = 0;  // null
   }
   p2b_machno = 0;

   // 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;
}

이때 주의할 사항은 PWM과 사용하는 타이머가 겹칠수 있으므로 반드시 실험해서 잘 작동하나 확인해야 함.

좋은 설명 : https://youtu.be/LONGI_JcwEQ

샘플 코드. 출처 : techtutorialsx.com/2017/10/07/esp32-arduino-timer-interrupts/

volatile int interruptCounter;
int totalInterruptCounter;
 
hw_timer_t * timer = NULL;
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
 
void IRAM_ATTR onTimer() {
  portENTER_CRITICAL_ISR(&timerMux);
  interruptCounter++;
  portEXIT_CRITICAL_ISR(&timerMux);
 
}
 
void setup() {
 
  Serial.begin(115200);
 
  timer = timerBegin(0, 80, true);
  timerAttachInterrupt(timer, &onTimer, true);
  timerAlarmWrite(timer, 1000000, true);
  timerAlarmEnable(timer);
 
}
 
void loop() {
 
  if (interruptCounter > 0) {
 
    portENTER_CRITICAL(&timerMux);
    interruptCounter--;
    portEXIT_CRITICAL(&timerMux);
 
    totalInterruptCounter++;
 
    Serial.print("An interrupt as occurred. Total number: ");
    Serial.println(totalInterruptCounter);
 
  }
}

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

ESP32 하드웨어 타이머 관련 기본 지식 :

  2 그룹의 2개씩의 64비트 하드웨어 타이머 (총 4개, 0 ~ 3)

   16 bit prescaler (2 ~ 2^16 사이의 값)

   ESP32는 80 Mhz짜리 수정(Crystal)을 사용하므로 (80,000,000 / prescaler) tics / sec 의 속도임.

   만약 80으로 나눠주면 80,000,000 / 80 = 1,000,000 tics / sec 이 됨.

   아두이노 코드에서 타이머를 사용하려면 다음과 같이 포인터를 만들어 주면 됨.

      hw_timer_t * timer = NULL;   // 포인터 설정.

   그 다음에는 타이머를 지정할 때 3개의 값을 주는데,

         1: 타이머 ID (0~3),

         2: 나눌 Prescaler 값,

         3: true (Rising Edge), false (Falling Edge) 

      timer = timerBegin(0, 80, true);  // 이건 타이머 0를 사용하고, 틱 80,000,000을 80으로 나누고, Rising Edge 사용.

  타이머를 Real Time용으로 만들기 위해서 아두이노 IDE에서는 다음과 같은 portMUX_TYPE 오브젝트를 사용함.

    portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;

   타이머 함수를 만들어 줄 것. 

      (*) 타이머 함수를 만들때 주의사항.

        1. 코드가 아주 빨리 실행되는 일만 시킬 것.  (시리얼 통신같은 무거운 일 시키면 안됨)

        2. 각 코드는 고유의 타이머를 연결시킬 것.

        3. 타이머 함수에서 함수 외부 코드와 공유할 수 있는 변수는 반드시 (volatile)로 지정할 것.

           예 :  volatile  int  t0count;

        4. PWM 코드와 사용하는 타이머가 겹치는 지 반드시 확인할 것.

     코드 예 :

      void  IRAM_ATTR  onTimer() {

           portENTER_CRITICAL_ISR(&timerMux);

           t0count++;  // ... critical code executed in the RAM of the ESP32 

           portEXIT_CRITICAL_ISR(&timerMux);

      }

   타이머를 활성화(Enable) 시키기 전에 반드시 타이머 함수를 지정하고 연결시킬 것.

      timerAttachInterrupt(timer, &onTimer, true);

   그 다음에 알람용 (실제 시간마다 실행될 시간)을 지정함.

      timerAlarmWrite(timer, 1000000, true);    // 이전에 80으로 나눠서 1초에 백만 tic이 돌게 세팅된 상태에 

                                                                       // 백만 tic마다 실행하게 함.  그러므로 이 코드는 1초에 1번 작동함. 

   마지막에 타이머를 활성화(Enable) 시킴.

      timerAlarmEnable(timer);

티스토리가 카카오를 통해서만 동영상을 올리게 제한해 놓아서 관련 동영상은 모두 facebook에 올립니다. 

동영상 링크 :  www.facebook.com/permalink.php?story_fbid=4335689146551601&id=100003316754962

개요 

총 부품비 10만원 이하의 이 조그만 로봇은 기초 이론을 구현을 연습하기 위한 실습용으로 만들어 보는 로봇입니다.

이 로봇으로 이번에 실험할 내용들을 나열하자면

1. GPIO on/off (로봇의 모터 컨트롤러의 방향 세팅용)

2. PWM Timer (로봇의 바퀴 속도 조절용)

3. 센서와 I2C 통신 (BNO055 Absolute Orientation 9축 센서)

4. 인터럽트 (두개의 바퀴의 엔코더로부터 실제 바퀴가 도는 속도를 읽어 냄)

5. Wifi UDP 통신 (PC의 통합 모니터링 프로그램과의 교신)

      5.1 통신시 메세지 분석을 위한 Parsing 실력 양상. 

         (* 파싱은 프로그래머의 기초 체력,

            파싱 실력이 약하면 자료구조 알고리즘 공부가 불가능 함.   

            자료구조 알고리즘 공부가 안 된 프로그래머는 거의 단순 노가다임.  *)

      5.2 네트워크 관련 전반적 지식.  

6. PID - 1 (1개의 바퀴의 엔코더로부터 PID를 적용해 보기 - 속도 조절)

7. PID - 2 (2개의 바퀴로 PID 적용을 확산시키기 - 직선 주행)

8. PID - 3 (9축 센서에도 PID를 확장시키기 -  직선 주행)

9. PID - 4 (이젠 바닥을 불규칙하게 만들어서 실험하기)

10. 윗 PID 에 완전히 익숙해 지면 이젠 여러 속도와 다각도 커브 주행해 보기.

윗 작업이 다 끝나면 똑같은 작업을 Linux 환경인 파이에서 해 보는 것입니다.

파이에서 작동하는 방식은 Jetson과 거의 동일하기 때문에 저렴한 파이에서 실험합니다.

이때 주의할 점은 지나치게 raspberry pi 전용으로 만들어진 라이브러리는 피해야 합니다.   

그래야 Jetson이나 다른 리눅스용 SBC에 포팅이 쉽습니다.

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

아두이노용 라이브러리

PID  링크 : r-downing.github.io/AutoPID/

BNO055 링크 : github.com/adafruit/Adafruit_BNO055

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

사용하는 하드웨어 :

TTGO ESP32 Board,

N20 Low noise geared moter w/ encoder (60 rpm, 6V) x 2

TB6612FNG : Dual H-Bridge motor controller

14500 Li-Ion Batteries (Ultrafire brand) x 2 : 1,800 mAh each

GY-BNO055 : 9-axis sensor (absolute orientation), GY버전이 쌈.

TFT 1.3" LCD : for quick feedback. (취소. PC 모니터링 프로그램이 좋아서 불필요)

Step-down buck converter x 2. (8V => 5V, 6V)

MISC. : Some holdered, plastic plate, rubber wheels, battery holder

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

ESP32의 기본 I2C 핀은 다음과 같습니다.  GPIO 22 (SCL), GPIO 21 (SDA)

이건 I2C 부품 주소 스캐닝 코드입니다.

#include <Wire.h>
 
void setup() {
  Wire.begin();
  Serial.begin(115200);
  Serial.println("\nI2C Scanner");
}
 
void loop() {
  byte error, address;
  int nDevices;
  Serial.println("Scanning...");
  nDevices = 0;
  for(address = 1; address < 127; address++ ) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
    if (error == 0) {
      Serial.print("I2C device found at address 0x");
      if (address<16) {
        Serial.print("0");
      }
      Serial.println(address,HEX);
      nDevices++;
    }
    else if (error==4) {
      Serial.print("Unknow error at address 0x");
      if (address<16) {
        Serial.print("0");
      }
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0) {
    Serial.println("No I2C devices found\n");
  }
  else {
    Serial.println("done\n");
  }
  delay(5000);          
}

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

하지만 ESP32은 아무 핀이나 I2C로 사용할 수 있습니다.  이번 프로젝트중 예기치 않게 발생한 문제점은 메인 보드가 불량이었던지 기본 I2C 핀들이 제대로 작동하지 않았던 점이었습니다.   그래서 부품을 갈아야 하나 하다가 그냥 다른 핀들을 사용해서 해결했습니다.

참조 : randomnerdtutorials.com/esp32-i2c-communication-arduino-ide/

ESP32에서 다른 주소를 사용해서 주소 스캔한 예 : (SDA : gpio32, SCL : gpio12)

#include <Wire.h>

#define I2C_SDA 32
#define I2C_SCL 12
 
void setup() {
  Wire.begin(I2C_SDA, I2C_SCL);
  Serial.begin(115200);
  Serial.println("\nI2C Scanner");
}
 
void loop() {
  byte error, address;
  int nDevices;
  Serial.println("Scanning...");
  nDevices = 0;
  for(address = 1; address < 127; address++ ) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
    if (error == 0) {
      Serial.print("I2C device found at address 0x");
      if (address<16) {
        Serial.print("0");
      }
      Serial.println(address,HEX);
      nDevices++;
    }
    else if (error==4) {
      Serial.print("Unknow error at address 0x");
      if (address<16) {
        Serial.print("0");
      }
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0) {
    Serial.println("No I2C devices found\n");
  }
  else {
    Serial.println("done\n");
  }
  delay(5000);          
}

ESP32로 장난감을 만든 다음 붙여있는 센서들의 input을 기초로 모델을 만들어 저장하려고 했는데, 가지고 있는 ESP32 보드들이 4MB PSRAM이 없는 WROOM 칩들을 사용하는 옛날 보드들이었습니다.

WROVER 버전들만 (4M / 8M / 16M)짜리 PSRAM이 있더군요.

가지고 있는 WROOM에도 SPIFFS 기능을 사용해서 화일을 쓰고 읽고 삭제할 수 있지만 크기가 많이 줄어듭니다.

실험용으로는 별 문제는 없겠지만 그래도 무척 아쉽군요.

온라인에 ESP32로 AI를 사용하는 대부분의 경우는 Training을 PC에서 텐서플로같은 툴들을 사용해서 한 후 결과값만 코드로 옮겨서 사용하더군요. 단순한 센서의 값을 이용하는 경우의 대부분이 단순한 linear regression 문제라서 ESP32 자체적으로 금방 훈련도 될 것 같은데 원시적인 코딩이 불편한가 봅니다.

저도 원시적인 코딩이 귀찮아서 라이브러리들을 설치하게 되더군요.

새로 설치한 ESP32에서 쓸 수 있는 인공지능용 라이브러리들.  (key term : Neural, Tensor)

/*
 *   https://www.iotsharing.com/2017/09/simple-machine-learning-neural-network-demo-using-arduino-esp32.html
 * 
 */
#include <math.h>

#define B1  12
#define B2  13
#define LED 14

int X[1][2]    =   {{1,0}};
/*these matrices was calculated by python */
float W1[2][3] =   {{0.74000854,  4.47769531, -0.98692059}, 
                    {0.83034991,  4.48772758, -0.55733578}};
float W2[3][1] =   {{-6.17234487}, 
                    {4.8835918}, 
                    {1.28875386}};
float Wo1[1][3];
float sum = 0;
float Y = 0;

/*sigmoid function*/
float sigmoid (float x)
{
    return 1/(1 + exp(-x));
}


void setup()
{
  Serial.begin(115200);
  pinMode(B1, INPUT_PULLUP); 
  pinMode(B2, INPUT_PULLUP); 
  pinMode(LED, OUTPUT); 
  digitalWrite(LED, LOW);
}

void loop()
{
  X[0][0] = digitalRead(B1);
  X[0][1] = digitalRead(B2);
  printf("B1 = %d, B2 = %d\n", X[0][0], X[0][1]);
  
  /* calculate forward part based on weights */
  //hidden layer
  for(int i=0; i<1; i++)
  {
      for(int j=0;j <3; j++)
      {
          for(int k=0; k<2; k++)
          {
              sum += X[i][k]*W1[k][j];
          }
          Wo1[i][j] = sigmoid(sum);
          sum = 0;  
      }
  }
  //output layer
  for(int i=0; i<1; i++)
  {
      for(int j=0;j <1; j++)
      {
          for(int k=0; k<3; k++)
          {
              Y += Wo1[i][k]*W2[k][j];
          } 
      }
  }
  printf("Y = %f\n", (Y));
  Y = round(Y);
  digitalWrite(LED, int(Y));
  Y = 0;
  delay(1000);
}

특별한 인공지능용 라이브러리 전혀 안쓰고 시도해 봤습니다.

옛날 1960년대 전산자원이 지극히 열악한 환경에서 시도한 방식이라 XOR을 시도하면 옛날처럼 틀린 값이 나옵니다.

결과 :

코드 :

/* "OR" problem with 1 neuron in ESP32
  ----------------------------------
   input1   |  input2   |  output
  ----------+-----------+-----------
   1(True)  |  1(True)  |  1(True)
   1(True)  |  0(False) |  1(True)
   0(False) |  1(True)  |  1(True)
   0(False) |  0(False) |  0(False)
  ----------------------------------*/
#include <math.h>

const int PatternCount = 4;  // 4 cases, 총 가능한 경우의 수
const int InputCol = 2;      // 2 key values (input1, input2)
const float learning_rate = 0.1;  // 학습률
const float x[PatternCount][InputCol] = {  // Inputs of OR - All cases
   {1, 1},  // 1(True)  |  1(True)
   {1, 0},  // 1(True)  |  0(False)
   {0, 1},  // 0(False) |  1(True)
   {0, 0}   // 0(False) |  0(False)
};

// known results (원하는 결과값)
const float y[PatternCount] = { 
   1, 
   1, 
   1, 
   0 
};

float w[InputCol]; // 가중치 (입력항 갯수만큼), 난수(Random)로 초기화할 것.
float b;           // 편향치 (항상 1개만 사용) - bias, 난수(Random)로 초기화할 것.
float b_x = 1.0;
int j;
float output;
float error;
float error_sum;
float tempval;
String s;

// 활성화 함수 (Activation Function)
float sigmoid (float n) {
    return 1/(1 + exp(-n));
}

void setup() {
   Serial.begin(19200);
   Serial.println(" ");
   Serial.println("OR problem solving using ESP32 with only 1 neuron.");
   
   // Initialize, 난수(Random)로 초기화할 것.  이번에는 대충 초기화했슴.
   w[0] = 0.7245;  // Any number (0 < n < 1) to initialize
   w[1] = 0.4356;  // Any number (0 < n < 1) to initialize
   b = 0.76254;    // Any number (0 < n < 1) to initialize

  // 첫번째 루프 : 2000번의 학습을 진행시킬 예정.
   for (int i = 1; i <= 2000; i++) {  
      error_sum = 0;  // 한번의 학습 결과를 저장 (기본 값은 0)

      // 두번째 루프  : 4번의 인스턴스 (ROW)를 사용함.
      j = 0;
      do {
         output = sigmoid((x[j][0] * w[0]) + (x[j][1] * w[1]) + b_x * b);   
         error = y[j] - output;
         w[0] = w[0] + (x[j][0]) * learning_rate * error;
         w[1] = w[1] + (x[j][1]) * learning_rate * error;
         b = b + b_x * learning_rate * error;
         error_sum += error;    
         j++;
      } while (j < 4);   
      
      if ((i % 200)==0) {  // 결과 표시 매 200번마다
         s = "";  s = s + i;  s = s +" - Output :";   s = s + output;
         s = s + "   Error_Sum :";  s = s + error_sum;
         Serial.println(s);
      }     
   }
   // 최종 확인
   Serial.println("-------- Final -----------");
   for (int i = 0; i < 4; i++) {
      s = i;  s = s + " - Output :";
      tempval = sigmoid((x[i][0] * w[0]) + (x[i][1] * w[1]) + b);
      s = s + tempval;  s = s + "   Round : ";   s = s + round(tempval);
      s = s + "  Exp.Y : ";  s = s + y[i];
      Serial.println(s);
   }
}

void loop() {
  
}

드디어 데이타를 읽고 화면에 내보내기 시작했습니다.     

아직 거리 배율값이 정확히 맞지 않습니다.    형태와 방향은 정확한 듯 합니다.

이젠 그 문제만 해결하면 됩니다.

영상 :

# -*- coding: utf-8 -*-
"""
From main library's example, ScatterPlot.py, 
I have taken out one of chart to make it for lidar data display.
The lidar related codes were taken from working code of Vidicon@DISCORD.
Last updated : Aug. 26, 2021 
"""
from pyqtgraph.Qt import QtGui, QtCore
import pyqtgraph as pg
import numpy as np
from collections import namedtuple
from itertools import chain
import sys

import serial   # For this one, you must install pyserial, not serial
from enum import Enum
import time
import math

#SERIAL_PORT = "/dev/ttyS5"   # for Orange Pi Zero 2's serial port
SERIAL_PORT = "/dev/ttyUSB0"  # for Other PC's USB to Serial module

class State(Enum):
   START1 = 0
   START2 = 1
   HEADER = 2
   DATA = 3

def readbytes(file, count):
   data = ser.read(count)
   #data = f.read(count)
   if len(data) != count:
      print("End of file")
      return False
   return data
   
step = (math.pi*2)   
anglePlus = math.pi / 2
fullround = 1300     # max dots in 1 round, it is larger than the real max dots in slowest mode
pos = np.zeros(shape=(2, fullround))
spots = [{'pos': pos[:,i], 'data': 1} for i in range(fullround)] + [{'pos': [0,0], 'data': 1}]
   
file_name = "RAW_DATA.LOG"
try:
   #f = open(file_name, "rb")
   ser = serial.Serial(SERIAL_PORT, 153600, timeout=0.1) 
   time.sleep(1)
except:
   print("could not connect to device")
   exit()

app = QtGui.QApplication([])
mw = QtGui.QMainWindow()
mw.resize(530,500)
view = pg.GraphicsLayoutWidget()  ## GraphicsView with GraphicsLayout inserted by default
mw.setCentralWidget(view)
mw.show()
mw.setWindowTitle('Lidar Test, unit in mm')

## create areas to add plots
w1 = view.addPlot()
w1.setAspectLocked()
  
###### Refresh Screen   
def RefreshScreen():
   global spots   # using globla spots array will ensure that it stores & clears data in same spot
   # Add polar grid lines
   w1.clear()   # clear screen and start drawing basic lines
   w1.addLine(x=0, pen=0.3)  # draw vertical center line
   w1.addLine(y=0, pen=0.3)  # draw horizontal center line
   for radius in range(200, 2000, 200):   # Draw 9 circles 200 ~ 2000 step 200
      # Adding circle (x, y, width, height)
      circleWidth = radius * 2
      circleHeight = radius * 2
      circle = pg.QtGui.QGraphicsEllipseItem(-radius, -radius, circleWidth, circleHeight)
      circle.setPen(pg.mkPen(0.3))
      w1.addItem(circle)  # addItem means draw or plot.  Here, draw circle
   # clear all data in the global spots array, make sure there will be no residue dots from previous round
   emptyone = np.zeros(shape=(2, fullround))   
   spots = [{'pos': emptyone[:,i], 'data': 1} for i in range(fullround)] + [{'pos': [0,0], 'data': 1}]   
      
###### Get Full Circle of Data 
def GetDataFromOneFullCycle():
   counter = 0
   ThisRoundCount = 0 # counts within one round
   maxThisRound = 0   # Number of good numbers for this cycle
   global pos         # using globla pos array will ensure we as storing data in same spot
   global spots       # using globla spots array will ensure we as storing data in same spot
   run = True
   try:
      state = State.START1
      while run:
         if state == State.START1:
            data = ser.read(1)
            #data = readbytes(f, 1)
            if data[0] == 0xAA:
               state = State.START2
            continue
         elif state == State.START2:
            data = ser.read(1)
            #data = readbytes(f, 1)
            if data[0] == 0x55:
               state = State.HEADER
            else:
               state = State.START1           
            continue
         elif state == State.HEADER:
            data = ser.read(8)
            #data = readbytes(f, 8)
            pack_type = data[0]
            data_lenght = int(data[1])
            start_angle = int(data[3] << 8) + int(data[2])
            stop_angle = int(data[5] << 8) + int(data[4])
            #unknown = int(data[7] << 8) + int(data[6])

            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_lenght-1) > 0:
               angle_per_sample = diff / (data_lenght-1)      
				
            #print("[{}]\ttype:{},\tlenght {},\tstart: {},\tstop: {}, \tdiff: {} \tdiff: {}"
            #   .format(counter, pack_type, data_lenght, start_angle, stop_angle, diff, angle_per_sample), end="\n")
				
            counter += 1
            #if pack_type != 40:
            #   counter = 0

            state = State.DATA
            continue
            
         elif state == State.DATA:
            state = State.START1
            #read data
            data = ser.read(data_lenght * 3)
            #data = readbytes(f, data_lenght * 3) 
            if data == False:
               break

            for i in range(0, data_lenght):

               data0 = int(data[i*3 + 0]) 
               data1 = int(data[i*3 + 1])
               data2 = int(data[i*3 + 2]) 
               distance = (data2  << 8) + data1

               angle = (start_angle + angle_per_sample * i)
               anglef = step * (angle / 0xB400) 
               #print("[{}]\tangle:{},\tanglef {},\tdist: {}".format(i, data0, (anglef + anglePlus), (distance/1000)), end="\n")
               distanceDivided = distance / 1000   # div to convert mm to meter
               #if (data0 != 1) & (distanceDivided < 3) :
               if (distanceDivided < 60) :
                  distanceDivided = (distance/5)   # Adjust distance ratio.  It is too large
          
                  x = distanceDivided * np.cos(anglef)
                  y = distanceDivided * np.sin(anglef)   
                  pos[0][ThisRoundCount] = y
                  pos[1][ThisRoundCount] = x
                  #print("[{}]\tDistance:{},\tanglef {},\tx:y: {}{}".format(ThisRoundCount, distanceDivided, anglef, x, y), end="\n")
                  ThisRoundCount += 1

            if pack_type != 40:  # After 1 full round
               spots = [{'pos': pos[:,i], 'data': 1} for i in range(ThisRoundCount)] + [{'pos': [0,0], 'data': 1}]
               ThisRoundCount = 0   
               ser.reset_input_buffer()    # This will clear serial line buffer, make update almost realtime.
               run = False   # Completed the mission of filling data in spots, now exit to draw step.
         else:
            print("error")

   except KeyboardInterrupt:
      run = False
      exit()               
   
   
######  I have to focus on putting data here.   
def _update():
   RefreshScreen()  # Draw basic chart with no data dots
   
   GetDataFromOneFullCycle()  # Get Full cycle of data from either File or Serial, prepare "spots"
   
   s1 = pg.ScatterPlotItem(size=5, pen=pg.mkPen(None), brush=pg.mkBrush(127, 255, 127, 120))
   s1.addPoints(spots)
   # addItem means draw or plot.  Here, plot all points
   w1.addItem(s1, ignoreBounds = True)  # ignoreBounds will prevent annoying rescaling

timer = QtCore.QTimer(interval=1)
timer.timeout.connect(_update)
#timer.start(0.1)  # duration number in millisecond
timer.start()  # A.S.A.P.

if __name__ == '__main__':
    import sys
    if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
        QtGui.QApplication.instance().exec_()

360도마다 스크린을 새로 시작하게 만들기.

# -*- coding: utf-8 -*-
"""
From main library's example, ScatterPlot.py, 
I will take out one of chart to make it for Lidar data display.
"""
from pyqtgraph.Qt import QtGui, QtCore
import pyqtgraph as pg
import numpy as np
from collections import namedtuple
from itertools import chain
import sys

app = QtGui.QApplication([])
mw = QtGui.QMainWindow()
mw.resize(530,500)
view = pg.GraphicsLayoutWidget()  ## GraphicsView with GraphicsLayout inserted by default
mw.setCentralWidget(view)
mw.show()
mw.setWindowTitle('Lidar Test, unit in mm')

## create areas to add plots
w1 = view.addPlot()
  
###### Refresh Screen   
def RefreshScreen():
   # Add polar grid lines
   w1.clear()   # clear screen and start drawing basic lines
   w1.addLine(x=0, pen=0.3)  # draw vertical center line
   w1.addLine(y=0, pen=0.3)  # draw horizontal center line
   for radius in range(200, 2000, 200):   # Draw 9 circles 200 ~ 2000 step 200
      # Adding circle (x, y, width, height)
      circleWidth = radius * 2
      circleHeight = radius * 2
      circle = pg.QtGui.QGraphicsEllipseItem(-radius, -radius, circleWidth, circleHeight)
      circle.setPen(pg.mkPen(0.3))
      w1.addItem(circle)  # addItem means draw or plot.  Here, draw circle
   
######  I have to focus on putting data here.   
def _update():
   RefreshScreen()  # Draw basic graph with no data dots
   
   n = 360
   
   s1 = pg.ScatterPlotItem(size=5, pen=pg.mkPen(None), brush=pg.mkBrush(255, 255, 255, 120))
   # Make 2 Dimension (X,Y) points array of 360 dots on scale of 500 for 500 mm.
   pos = np.random.normal(size=(2,n), scale=500)
   spots = [{'pos': pos[:,i], 'data': 1} for i in range(n)] + [{'pos': [0,0], 'data': 1}]
   s1.addPoints(spots)
   # addItem means draw or plot.  Here, plot all points
   w1.addItem(s1, ignoreBounds = True)  # ignoreBounds will prevent annoying rescaling
   

timer = QtCore.QTimer(interval=1)
timer.timeout.connect(_update)
timer.start(500)  # duration number in millisecond

# RefreshScreen()  # Calling once, later it will be called from _update every 360 degree
_update()   # Calling only once, timer will keep calling them 


if __name__ == '__main__':
    import sys
    if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
        QtGui.QApplication.instance().exec_()

이젠 마구 Timer가 마구 계속 찍어내기 시작.

Timer가 지속적으로 부르는 _Update 안에 Lidar 데이타를 업데이트해 줘야만 한다.

# -*- coding: utf-8 -*-
"""
From main library's example, ScatterPlot.py, 
I will take out one of chart to make it for Lidar data display.
"""
from pyqtgraph.Qt import QtGui, QtCore
import pyqtgraph as pg
import numpy as np
from collections import namedtuple
from itertools import chain
import sys

app = QtGui.QApplication([])
mw = QtGui.QMainWindow()
mw.resize(530,500)
view = pg.GraphicsLayoutWidget()  ## GraphicsView with GraphicsLayout inserted by default
mw.setCentralWidget(view)
mw.show()
mw.setWindowTitle('Lidar Test ')

## create four areas to add plots
w1 = view.addPlot()

# Add polar grid lines
w1.addLine(x=0, pen=0.3)
w1.addLine(y=0, pen=0.3)
for r in range(2, 2000, 200):   # Draw 10 circles
   # Adding circle (x, y, width, height)
   cwidth = r * 2
   cheight = r * 2
   circle = pg.QtGui.QGraphicsEllipseItem(-r, -r, cwidth, cheight)
   circle.setPen(pg.mkPen(0.3))
   w1.addItem(circle)  
   
   
######  I have to focus on putting data here.   
def _update():
   n = 360
   
   s1 = pg.ScatterPlotItem(size=5, pen=pg.mkPen(None), brush=pg.mkBrush(255, 255, 255, 120))
   # Make 2 Dimension (X,Y) points array of 360 dots on scale of 500 for 500 mm.
   pos = np.random.normal(size=(2,n), scale=500)
   spots = [{'pos': pos[:,i], 'data': 1} for i in range(n)] + [{'pos': [0,0], 'data': 1}]
   s1.addPoints(spots)
   w1.addItem(s1)

timer = QtCore.QTimer(interval=1)
timer.timeout.connect(_update)
timer.start(300)  # Update every 300 millisecond

_update()   # Calling only once, timer will keep calling them 


if __name__ == '__main__':
    import sys
    if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
        QtGui.QApplication.instance().exec_()