2024年6月30日 星期日

ESP32CAM + 麥克納姆倫 + Car

Purpose:
這個專題結合了ESP32CAM影像Web傳輸及Web http Control Mecanum wheel Car to move.



Fundamental&BOM:
Mecanum wheel and TT motor
Reference https://en.wikipedia.org/wiki/Mecanum_wheel
方向控制:


L298N

Mega2560


Circuit:









Reference
https://randomnerdtutorials.com/esp32-cam-car-robot-web-server/

All source code put on:

ESP32CAM Code:
#include <WiFi.h>
#include "soc/soc.h"             //用於電源不穩不重開機
#include "soc/rtc_cntl_reg.h"    //用於電源不穩不重開機
#include "esp_camera.h"    

// Select camera model
//#define CAMERA_MODEL_WROVER_KIT
//#define CAMERA_MODEL_M5STACK_PSRAM
#define CAMERA_MODEL_AI_THINKER

const char* ssid = "ras_2.4";   //Enter SSID WIFI Name
const char* password = "181415181415";   //Enter WIFI Password

#if defined(CAMERA_MODEL_WROVER_KIT)
#define PWDN_GPIO_NUM    -1
#define RESET_GPIO_NUM   -1
#define XCLK_GPIO_NUM    21
#define SIOD_GPIO_NUM    26
#define SIOC_GPIO_NUM    27

#define Y9_GPIO_NUM      35
#define Y8_GPIO_NUM      34
#define Y7_GPIO_NUM      39
#define Y6_GPIO_NUM      36
#define Y5_GPIO_NUM      19
#define Y4_GPIO_NUM      18
#define Y3_GPIO_NUM       5
#define Y2_GPIO_NUM       4
#define VSYNC_GPIO_NUM   25
#define HREF_GPIO_NUM    23
#define PCLK_GPIO_NUM    22


#elif defined(CAMERA_MODEL_AI_THINKER)
#define PWDN_GPIO_NUM     32
#define RESET_GPIO_NUM    -1
#define XCLK_GPIO_NUM      0
#define SIOD_GPIO_NUM     26
#define SIOC_GPIO_NUM     27

#define Y9_GPIO_NUM       35
#define Y8_GPIO_NUM       34
#define Y7_GPIO_NUM       39
#define Y6_GPIO_NUM       36
#define Y5_GPIO_NUM       21
#define Y4_GPIO_NUM       19
#define Y3_GPIO_NUM       18
#define Y2_GPIO_NUM        5
#define VSYNC_GPIO_NUM    25
#define HREF_GPIO_NUM     23
#define PCLK_GPIO_NUM     22

#else
#error "Camera model not selected"
#endif

// GPIO Setting
extern int gpLb =  2; // Left 1
extern int gpLf = 14; // Left 2
extern int gpRb = 15; // Right 1
extern int gpRf = 13; // Right 2
extern int gpLed =  4; // Light
extern String WiFiAddr ="";

void startCameraServer();

void setup() {
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);  //關閉電源不穩就重開機的設定

  Serial.begin(115200);
  Serial.setDebugOutput(true);
  Serial.println();

  setupCam();

  pinMode(gpLb, OUTPUT); //Left Backward
  pinMode(gpLf, OUTPUT); //Left Forward
  pinMode(gpRb, OUTPUT); //Right Forward
  pinMode(gpRf, OUTPUT); //Right Backward
  pinMode(gpLed, OUTPUT); //Light

  //initialize
  digitalWrite(gpLb, LOW);
  digitalWrite(gpLf, LOW);
  digitalWrite(gpRb, LOW);
  digitalWrite(gpRf, LOW);
  digitalWrite(gpLed, LOW);

  WifiConnect();

  startCameraServer();

  Serial.print("Camera Ready! Use 'http://");
  Serial.print(WiFi.localIP());
  Serial.println("' to connect");

}

void loop()
{
 
  while(1)
  {
    if (WiFi.status() != WL_CONNECTED)
    {
      WifiConnect();
    }
   
  }

}
//-----------------------------------------
//開始WiFi連線
void WifiConnect()
{
  //開始WiFi連線
  //WiFi.begin("Wokwi-GUEST", "", 6);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED)
  {
    delay(500);
    Serial.print(".");
  }
 
  WiFiAddr = WiFi.localIP().toString();
  Serial.print("IP Address:");
  Serial.println(WiFi.localIP());
  Serial.println("WiFi連線成功");
}
//-----------------------------------------
//鏡頭設定
void setupCam() {  
  // #define CAMERA_MODEL_AI_THINKER
  //視訊組態設定  https://github.com/espressif/esp32-camera/blob/master/driver/include/esp_camera.h
  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sscb_sda = SIOD_GPIO_NUM;
  config.pin_sscb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG;
  //
  // WARNING!!! PSRAM IC required for UXGA resolution and high JPEG quality
  //            Ensure ESP32 Wrover Module or other board with PSRAM is selected
  //            Partial images will be transmitted if image exceeds buffer size
  //  
  // if PSRAM IC present, init with UXGA resolution and higher JPEG quality
  //                      for larger pre-allocated frame buffer.
  if(psramFound()){  //是否有PSRAM(Psuedo SRAM)記憶體IC
    config.frame_size = FRAMESIZE_UXGA;
    config.jpeg_quality = 10;
    config.fb_count = 2;
  } else {
    config.frame_size = FRAMESIZE_SVGA;
    config.jpeg_quality = 12;
    config.fb_count = 1;
  }

  //視訊初始化
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed with error 0x%x", err);
    ESP.restart();
  }

  //可自訂視訊框架預設大小(解析度大小)
  sensor_t * s = esp_camera_sensor_get();
  // initial sensors are flipped vertically and colors are a bit saturated
  if (s->id.PID == OV3660_PID) {
    s->set_vflip(s, 1); // flip it back
    s->set_brightness(s, 1); // up the brightness just a bit
    s->set_saturation(s, -2); // lower the saturation
  }
  // drop down frame size for higher initial frame rate
  s->set_framesize(s, FRAMESIZE_CIF);    //解析度 UXGA(1600x1200), SXGA(1280x1024), XGA(1024x768), SVGA(800x600), VGA(640x480), CIF(400x296), QVGA(320x240), HQVGA(240x176), QQVGA(160x120), QXGA(2048x1564 for OV3660)

  //s->set_vflip(s, 1);  //垂直翻轉
  //s->set_hmirror(s, 1);  //水平鏡像
  //s->set_pRotation(s, 1);  //0=不旋轉 1-轉90度 2- 轉180度 3-轉270
}

Mega2560 Code:
#include <Robojax_L298N_DC_motor.h>
#include <Servo.h>
//-------------L298(1)---------------------------------------------------
// motor 1 settings
#define CHA_1 0
#define ENA_1 2 // this pin must be PWM enabled pin if Arduino board is used
#define IN1_1 30
#define IN2_1 32
// motor 2 settings
#define IN3_1 34
#define IN4_1 36
#define ENB_1 7// this pin must be PWM enabled pin if Arduino board is used
#define CHB_1 1
//-------------L298(2)---------------------------------------------------
// motor 3 settings
#define CHA_2 0
#define ENA_2 2 // this pin must be PWM enabled pin if Arduino board is used
#define IN1_2 38
#define IN2_2 40
// motor 4 settings
#define IN3_2 42
#define IN4_2 44
#define ENB_2 7// this pin must be PWM enabled pin if Arduino board is used
#define CHB_2 1

const int CCW = 2; // do not change
const int CW  = 1; // do not change

#define motor1 1
#define motor2 2

// for two motors without debug information // Watch video instruciton for this line: https://youtu.be/2JTMqURJTwg
Robojax_L298N_DC_motor motors1(IN1_1, IN2_1, ENA_1, CHA_1,  IN3_1, IN4_1, ENB_1, CHB_1);
Robojax_L298N_DC_motor motors2(IN1_2, IN2_2, ENA_2, CHA_2,  IN3_2, IN4_2, ENB_2, CHB_2);

#define Pin22   22
#define Pin23   23
#define Pin24   24
#define Pin25   25
 
Servo servo1;  //底座  
Servo servo2;  //左臂
Servo servo3;  //右臂
Servo servo4;  //夾子

void setup()
{
  Serial.begin(9600);
  pinMode(Pin22,  INPUT);
  pinMode(Pin23,  INPUT);
  pinMode(Pin24,  INPUT);
  pinMode(Pin25,  INPUT);
  Serial.println(F("init"));
  pinMode(IN1_1, OUTPUT);
  pinMode(IN2_1, OUTPUT);
  pinMode(IN3_1, OUTPUT);
  pinMode(IN4_1, OUTPUT);
  pinMode(ENA_1, OUTPUT);
  pinMode(ENB_1, OUTPUT);
  pinMode(IN1_2, OUTPUT);
  pinMode(IN2_2, OUTPUT);
  pinMode(IN3_2, OUTPUT);
  pinMode(IN4_2, OUTPUT);
  pinMode(ENA_2, OUTPUT);
  pinMode(ENB_2, OUTPUT);
  servo1.attach(8);  // 設置舵機控制腳位
  servo2.attach(9);
  servo3.attach(10);
  servo4.attach(11);

  servo1.write(30);
  servo2.write(50);
  servo3.write(20);
  servo4.write(20);
}

void loop()
{
  char key = 0;
  int a=1,b=1,c=1,d=1;
  int fg = 0;
  boolean flag= false;
 
  while(1)
  {  
    if ( Serial.available())
    {
      key = Serial.read();
      flag=true;
    }

    a= digitalRead (Pin22);
    b= digitalRead (Pin23);
    c= digitalRead (Pin24);
    d= digitalRead (Pin25);

    Serial.print(digitalRead(22)) ;  //purple >  14
    Serial.print(digitalRead(23)) ;  //blue 2
    Serial.print(digitalRead(24)) ;  //yellow > 13
    Serial.println(digitalRead(25)); //15
   

    if (flag==false)
    {
       if (a==1 and b==0 and c==1 and d==0) {
          key ='f';
       }
       else if (a==0 and b==1 and c==0 and d==1) {
          key ='b';
       }
       else if (a==1 and b==0 and c==0 and d==1) {
          key ='x';
       }
       else if (a==0 and b==1 and c==1 and d==0) {
         key ='y';
       }
       else if (a==0 and b==0 and c==1 and d==1) {
         key ='a';
       }
       else if (a==1 and b==1 and c==0 and d==0) {
         key ='c';
       }
       else if (a==0 and b==0 and c==0 and d==0) {
         key ='s';
       }
       else if (a==1 and b==1 and c==1 and d==1) {
         key ='s';
       }
       else if (a==0 and b==0 and c==0 and d==1) { //---servo action 1 A1 Stretch
         key ='1';
       }
       else if (a==0 and b==0 and c==1 and d==0) { //---servo action 2 A2 Clip
         key ='2';
       }
       else if (a==0 and b==1 and c==0 and d==0) { //---servo action 3 A3 Retract
         key ='3';
       }
       else if (a==1 and b==0 and c==0 and d==0) { //---servo action 4 A4 Turn
         key ='4';
       }
       else if (a==1 and b==1 and c==1 and d==0) { //---servo action 5 A5 Release<
         key ='5';
       }
       else if (a==1 and b==1 and c==0 and d==1) { //---servo action 6 A6 Origin
         key ='6';
       }
       else if (a==1 and b==0 and c==1 and d==1) { //---servo action 7
         key ='7';
       }
       else if (a==0 and b==1 and c==1 and d==1) { //---servo action 8
         key ='8';
       }
    }

    switch ( key ) {
      case 'f': // 前進
        Serial.println(F("Forward"));
        Forward();
        break;
      case 'b': // 後退
        Serial.println(F("Reverse"));
        Reverse();
        break;
      case 'l': // 左移
        Serial.println(F("Left"));
        Left();
        break;
      case 'r': // 右移
        Serial.println(F("Right"));
        Right();
        break;  
      case 'a': // 左平移
        Serial.println(F("ShiftLeft"));
        S_Left();
        break;
      case 'c': // 右平移
        Serial.println(F("ShiftRight"));
        S_Right();
        break;  
      case 'x': // 逆左旋轉
        Serial.println(F("X"));
        Xtrun();
        break;  
      case 'y': // 順右旋轉
        Serial.println(F("Y"));
        Ytrun();
        break;  
      case '1': // 伸爪
        Serial.println(F("3"));
        servo3.write(40);
        delay(500);
        Serial.println(F("1"));
        servo1.write(50);
        delay(500);
        servo1.write(70);
        delay(500);
        servo1.write(90);
        delay(500);
        servo1.write(130);
        delay(500);
        servo1.write(160);
        delay(500);
        break;  
      case '2': // 夾
        Serial.println(F("2"));
        servo2.write(70);
        delay(500);
        servo2.write(100);
        delay(500);
        servo2.write(140);
        delay(500);
        break;  
      case '3':  //收爪
        Serial.println(F("1"));
        servo1.write(110);
        delay(500);
        servo1.write(90);
        delay(500);
        servo1.write(50);
        delay(500);
        servo1.write(30);
        delay(500);
        break;  
      case '4':  //後轉
        Serial.println(F("4"));
        servo4.write(60);
        delay(500);
        servo4.write(90);
        delay(500);
        servo4.write(120);
        delay(500);
        servo4.write(160);
        delay(500);
        servo4.write(180);
        delay(500);
        break;
      case '5':  //放夾
        Serial.println(F("2"));
        servo2.write(120);
        delay(500);
        servo2.write(90);
        delay(500);
        servo2.write(50);
        delay(500);
        /**
        Serial.println(F("4"));
        servo4.write(160);
        delay(500);;
        servo4.write(120);
        delay(500);
        servo4.write(80);
        delay(500);
        servo4.write(40);
        delay(500);
        servo4.write(20);
        delay(500);**/
        break;  
      case '6': //回原點
        Serial.println(F("4"));
        servo4.write(160);
        delay(500);;
        servo4.write(120);
        delay(500);
        servo4.write(80);
        delay(500);
        servo4.write(40);
        delay(500);
        servo4.write(20);
        delay(500);
        break;
      case 's': // 前進
        //Serial.println(F("Stop"));
        Stop();
        break;
      default:
        Stop();
        flag= false;
        break;

    }
   
  } //---while(1)
}

void Forward() //電機前進
{
  motors1.brake(1);  
  motors1.brake(2);  
  motors2.brake(1);  
  motors2.brake(2);  

  motors1.rotate(motor1, 70, CW);//run motor1 at 60% speed in CW direction (1)
  motors1.rotate(motor2, 70, CCW);//run motor2 at 60% speed in CCW direction (2)
  motors2.rotate(motor1, 70, CCW);//run motor1 at 60% speed in CCW direction (3)
  motors2.rotate(motor2, 70, CW);//run motor2 at 60% speed in CW direction  (4)
}

void Reverse(){
  motors1.brake(1);  
  motors1.brake(2);  
  motors2.brake(1);  
  motors2.brake(2);
  motors1.rotate(motor1, 70, CCW);
  motors1.rotate(motor2, 70, CW);
  motors2.rotate(motor1, 70, CW);
  motors2.rotate(motor2, 70, CCW);
}
void Left()
{
  motors1.brake(1);  
  motors1.brake(2);  
  motors2.brake(1);  
  motors2.brake(2);  
  motors1.rotate(motor1, 70, CW);//run motor1 at 60% speed in CW direction (1)
  //motors1.rotate(motor2, 70, CCW);//run motor2 at 60% speed in CCW direction (2)
  //motors2.rotate(motor1, 70, CCW);//run motor1 at 60% speed in CCW direction (3)
  motors2.rotate(motor2, 70, CW);//run motor2 at 60% speed in CW direction  (4)
}
void S_Left()  //左平移
{
  motors1.brake(1);  
  motors1.brake(2);  
  motors2.brake(1);  
  motors2.brake(2);  
  motors1.rotate(motor1, 70, CW);//run motor1 at 60% speed in CW direction (1)
  motors1.rotate(motor2, 70, CW);//run motor2 at 60% speed in CCW direction (2)
  motors2.rotate(motor1, 70, CW);//run motor1 at 60% speed in CCW direction (3)
  motors2.rotate(motor2, 70, CW);//run motor2 at 60% speed in CW direction  (4)
}
void S_Right()  //右平移
{
  motors1.brake(1);  
  motors1.brake(2);  
  motors2.brake(1);  
  motors2.brake(2);  
  motors1.rotate(motor1, 70, CCW);//run motor1 at 60% speed in CW direction (1)
  motors1.rotate(motor2, 70, CCW);//run motor2 at 60% speed in CCW direction (2)
  motors2.rotate(motor1, 70, CCW);//run motor1 at 60% speed in CCW direction (3)
  motors2.rotate(motor2, 70, CCW);//run motor2 at 60% speed in CW direction  (4)
}
void Right()  //右
{
  motors1.brake(1);  
  motors1.brake(2);  
  motors2.brake(1);  
  motors2.brake(2);  
  //motors1.rotate(motor1, 70, CW);//run motor1 at 60% speed in CW direction (1)
  motors1.rotate(motor2, 70, CCW);//run motor2 at 60% speed in CCW direction (2)
  motors2.rotate(motor1, 70, CCW);//run motor1 at 60% speed in CCW direction (3)
  //motors2.rotate(motor2, 70, CW);//run motor2 at 60% speed in CW direction  (4)
}

void Ytrun() // 逆右旋轉
{
  motors1.brake(1);  
  motors1.brake(2);  
  motors2.brake(1);  
  motors2.brake(2);  

  motors1.rotate(motor1, 70, CCW);//run motor1 at 60% speed in CW direction (1)
  motors1.rotate(motor2, 70, CW);//run motor2 at 60% speed in CCW direction (2)
  motors2.rotate(motor1, 70, CCW);//run motor1 at 60% speed in CCW direction (3)
  motors2.rotate(motor2, 70, CW);//run motor2 at 60% speed in CW direction  (4)
}

void Xtrun() // 逆左旋轉
{
  motors1.brake(1);  
  motors1.brake(2);  
  motors2.brake(1);  
  motors2.brake(2);  

  motors1.rotate(motor1, 70, CW);//run motor1 at 60% speed in CW direction (1)
  motors1.rotate(motor2, 70, CCW);//run motor2 at 60% speed in CCW direction (2)
  motors2.rotate(motor1, 70, CW);//run motor1 at 60% speed in CCW direction (3)
  motors2.rotate(motor2, 70, CCW);//run motor2 at 60% speed in CW direction  (4)
}

void Stop() //電機停止
{
  motors1.brake(1);
  motors1.brake(2);  
  motors2.brake(1);
  motors2.brake(2);  
}


YouTubeDemo:






2024年6月25日 星期二

KIKUSUI 的Crank test system

Purpose:
利用C#設計出一個多功能的介面去載入KIKUSUI測試的pattern檔案格式,並且使用GPIB介面傳輸到KIKUSUI儀器上做Crank Pattern的測試.

Fundamental:
KIKUSUI
PBX系列是一種雙極方式(Bipolar)直流穩壓電源,它不用進行正、負極性的輸出端子切換,即可連續通過0,並可連續切換到任何一極。 此外,由於可分別設定恆壓模式(C.V)和恆流模式(C.C),因而除了可作為恆壓電源使用外,還可作為恆流電源、電壓控制電流源使用。 高速時序模式可以進行暫態停電和過渡現象等複雜波形的模擬。
  • 正負0 - 40V 0 - 2.5A
  • 電壓分辨力 1mV
  • Sequence功能
  • GPIB介面
  • 低脈動雜訊(恒壓模式/正常時)
  • 負載的種類和用途的4種工作模式
  • 功率放大器功能
  • 具備設置記憶體和順序控制功能
GPIB
在GPIB控制器中,把器件與 GPIB 匯流排的一種互動作用定義成一種介面功能。
在一個GPIB標準介面匯流排系統中,要進行有效的通訊聯絡至少有至多一個在工作的「講者」、多個「聽者」、1個「控者」三類儀器裝置。

MDI 介面設計:

原廠設計界面:
單一的pattern載入與執行!


改進後的介面可以複選要執行的pattern將其載入儀器並透過執行時的監控功能查看現在執行到哪一個paterns中的哪一個step!


Demo:



增加功能: 
增加GoodWill的Power Supply的power circle Test