云天 发表于 2023-9-17 21:20:47

ESP32 麦克纳姆轮 视频车

本帖最后由 云天 于 2023-9-17 21:28 编辑


【项目背景】
想做一个组装简易的麦克纳姆轮视频车,之前制作的小车,电机需要驱动模块,也需要单独的电源,主控也需要电源。所以硬件设备较多,不好布局,很多时候显得很凌乱。在DF商城上看到了“Romeo ESP32-S3 开发板” Romeo ESP32-S3是为机器人项目设计的一款开发板,可驱动4路电机、图像传输、远程控制。
【项目设计】

使用“便携式多路可调电源”的USB-A电源口为主控电源供电,9V端口为电机驱动供电。使用“ESPAsyncWebServer库”及“AsyncTCP库”,利用“App Inventor2”的浏览器模块访问192.168.4.1,即可实现手机App驱动小车和查看摄像头数据。

【硬件组装】

1.所用到硬件

2.安装电机
3.安装挡板

4.安装麦克纳姆轮

5.安装主控,接电机线

6.安装电源

7.接电源线

8.固定摄像头

【制作APP】
1.界面设计
一个网页浏览框和一个按钮

2.程序设计
手机连接“Romeo ESP32-S3 开发板”的WIFI热点,网页浏览框加载“192.168.4.1”。按钮用于当连接中断时,重新连接。

【修改ESP32示例代码】

控制页面增加四个方向按钮

    <table id="mainTable" style="width:355px;margin:auto;table-layout:fixed" CELLSPACING=10>
      <tr>
      <img id="cameraImage" src="" style="width:355px;height:222px"></td>
      </tr>
      <tr>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","5")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >↖</span></td>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","1")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >⇧</span></td>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","6")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >↗</span></td>
      </tr>
      <tr>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","3")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >⇦</span></td>
      <td class="button"></td>   
      <td class="button" ontouchstart='sendButtonInput("MoveCar","4")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >⇨</span></td>
      </tr>
      <tr>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","7")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >↙</span></td>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","2")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >⇩</span></td>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","8")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >↘</span></td>
      </tr>
      <tr/><tr/>
      <tr>
      <td style="text-align:left"><b>Speed:</b></td>
      <td colspan=2>
         <div class="slidecontainer">
            <input type="range" min="0" max="100" value="50" class="slider" id="Speed" oninput='sendButtonInput("Speed",value)'>
          </div>
      </td>
      </tr>      

    </table>


根据麦克纳姆运动特点修改代码。轮麦克纳姆轮与普通轮子的区别在于麦克纳姆轮旋转时,由于存在斜向的从动轮,会同时产生一个斜向的力,当我们控制轮子旋转的速度与方向时,将斜向的力增强或抵消,从而实现小车的全向移动。可以完成横移、斜方向移动等普通小车无法完成的高难度动作,轮子的转动方向与小车的运动方向关系如下图:
https://mc.dfrobot.com.cn/forum.php?mod=attachment&aid=Nzg4ODd8NTUzZDA4Mjd8MTY5NDk1NTUzMXw4Mjc3ODR8Mjk2Nzgz&noupdate=yes

void moveCar(uint8_t inputValue,uint8_t speed)
{
switch(inputValue)
{
    case UP:
      retreat(1,speed);
      retreat(2,speed);
      advance(3,speed);
      advance(4,speed);
      break;

    case DOWN:
      advance(1,speed);
      advance(2,speed);
      retreat(3,speed);
      retreat(4,speed);
      break;

    case LEFT:
      retreat(2,speed);
      retreat(4,speed);
      advance(1,speed);
      advance(3,speed);
      break;

    case RIGHT:
      retreat(1,speed);
      retreat(3,speed);
      advance(2,speed);
      advance(4,speed);
      break;
    case Upper_Left:
      breake(1);
      retreat(2,speed);
      advance(3,speed);
      breake(4);
      break;
    case Upper_Right:
      retreat(1,speed);
      breake(2);
      breake(3);
      advance(4,speed);
      break;
    case Bottom_Left:
      advance(1,speed);
      breake(2);
      breake(3);
      retreat(4,speed);
      break;
    case Bottom_Right:
      breake(1);
      advance(2,speed);
      retreat(3,speed);
      breake(4);
      break;
    case STOP:
      breake(1);
      breake(2);
      breake(3);
      breake(4);
      break;
}
}

【ESP32 完整代码】

#include "esp_camera.h"
#include <Arduino.h>
#include <WiFi.h>
#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <iostream>
#include <sstream>
#include "DFRobot_AXP313A.h"
#include "driver/mcpwm.h"
#include "soc/mcpwm_struct.h"
#include "soc/mcpwm_reg.h"

DFRobot_AXP313A axp;

#define M1_EN 12
#define M1_PN 13
#define M2_EN 14
#define M2_PN 21
#define M3_EN 9
#define M3_PN 10
#define M4_EN 47
#define M4_PN 11


#define UP 1
#define DOWN 2
#define LEFT 3
#define RIGHT 4
#define Upper_Left 5
#define Upper_Right 6
#define Bottom_Left 7
#define Bottom_Right 8
#define STOP 0

#define FORWARD 1
#define BACKWARD -1

//Camera related constants
#define PWDN_GPIO_NUM   -1
#define RESET_GPIO_NUM    -1
#define XCLK_GPIO_NUM   45
#define SIOD_GPIO_NUM   1
#define SIOC_GPIO_NUM   2

#define Y9_GPIO_NUM       48
#define Y8_GPIO_NUM       46
#define Y7_GPIO_NUM       8
#define Y6_GPIO_NUM       7
#define Y5_GPIO_NUM       4
#define Y4_GPIO_NUM       41
#define Y3_GPIO_NUM       40
#define Y2_GPIO_NUM       39
#define VSYNC_GPIO_NUM    6
#define HREF_GPIO_NUM   42
#define PCLK_GPIO_NUM   5

const char* ssid   = "******";
const char* password = "*******";

AsyncWebServer server(80);
AsyncWebSocket wsCamera("/Camera");
AsyncWebSocket wsCarInput("/CarInput");
uint32_t cameraClientId = 0;

uint8_t Speed = 50;

const char* htmlHomePage PROGMEM = R"HTMLHOMEPAGE(
<!DOCTYPE html>
<html>
<head>
<meta name="viewport" content="width=device-width, initial-scale=1, maximum-scale=1, user-scalable=no">
    <style>
    .arrows {
      font-size:30px;
      color:red;
    }
    td.button {
      background-color:black;
      border-radius:25%;
      box-shadow: 5px 5px #888888;
    }
    td.button:active {
      transform: translate(5px,5px);
      box-shadow: none;
    }

    .noselect {
      -webkit-touch-callout: none; /* iOS Safari */
      -webkit-user-select: none; /* Safari */
         -khtml-user-select: none; /* Konqueror HTML */
         -moz-user-select: none; /* Firefox */
            -ms-user-select: none; /* Internet Explorer/Edge */
                user-select: none; /* Non-prefixed version, currently
                                    supported by Chrome and Opera */
    }

    .slidecontainer {
      width: 100%;
    }

    .slider {
      -webkit-appearance: none;
      width: 100%;
      height: 15px;
      border-radius: 5px;
      background: #d3d3d3;
      outline: none;
      opacity: 0.7;
      -webkit-transition: .2s;
      transition: opacity .2s;
    }

    .slider:hover {
      opacity: 1;
    }

    .slider::-webkit-slider-thumb {
      -webkit-appearance: none;
      appearance: none;
      width: 25px;
      height: 25px;
      border-radius: 50%;
      background: red;
      cursor: pointer;
    }

    .slider::-moz-range-thumb {
      width: 25px;
      height: 25px;
      border-radius: 50%;
      background: red;
      cursor: pointer;
    }

    </style>

</head>
<body class="noselect" align="center" style="background-color:white">
    <table id="mainTable" style="width:355px;margin:auto;table-layout:fixed" CELLSPACING=10>
      <tr>
      <img id="cameraImage" src="" style="width:355px;height:222px"></td>
      </tr>
      <tr>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","5")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >↖</span></td>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","1")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >⇧</span></td>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","6")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >↗</span></td>
      </tr>
      <tr>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","3")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >⇦</span></td>
      <td class="button"></td>   
      <td class="button" ontouchstart='sendButtonInput("MoveCar","4")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >⇨</span></td>
      </tr>
      <tr>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","7")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >↙</span></td>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","2")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >⇩</span></td>
      <td class="button" ontouchstart='sendButtonInput("MoveCar","8")' ontouchend='sendButtonInput("MoveCar","0")'><span class="arrows" >↘</span></td>
      </tr>
      <tr/><tr/>
      <tr>
      <td style="text-align:left"><b>Speed:</b></td>
      <td colspan=2>
         <div class="slidecontainer">
            <input type="range" min="0" max="100" value="50" class="slider" id="Speed" oninput='sendButtonInput("Speed",value)'>
          </div>
      </td>
      </tr>      

    </table>

    <script>
      var webSocketCameraUrl = "ws:\/\/" + window.location.hostname + "/Camera";
      var webSocketCarInputUrl = "ws:\/\/" + window.location.hostname + "/CarInput";      
      var websocketCamera;
      var websocketCarInput;

      function initCameraWebSocket()
      {
      websocketCamera = new WebSocket(webSocketCameraUrl);
      websocketCamera.binaryType = 'blob';
      websocketCamera.onopen    = function(event){};
      websocketCamera.onclose   = function(event){setTimeout(initCameraWebSocket, 2000);};
      websocketCamera.onmessage = function(event)
      {
          var imageId = document.getElementById("cameraImage");
          imageId.src = URL.createObjectURL(event.data);
      };
      }

      function initCarInputWebSocket()
      {
      websocketCarInput = new WebSocket(webSocketCarInputUrl);
      websocketCarInput.onopen    = function(event)
      {
          sendButtonInput("Speed", document.getElementById("Speed").value);

      };
      websocketCarInput.onclose   = function(event){setTimeout(initCarInputWebSocket, 2000);};
      websocketCarInput.onmessage = function(event){};      
      }

      function initWebSocket()
      {
      initCameraWebSocket ();
      initCarInputWebSocket();
      }

      function sendButtonInput(key, value)
      {
      var data = key + "," + value;
      websocketCarInput.send(data);
      }

      window.onload = initWebSocket;
      document.getElementById("mainTable").addEventListener("touchend", function(event){
      event.preventDefault()
      });      
    </script>
</body>   
</html>
)HTMLHOMEPAGE";


void advance(uint8_t motorNumber,uint8_t speed)
{
switch(motorNumber)
{
    case 1:
      mcpwm_set_signal_high(MCPWM_UNIT_0,MCPWM_TIMER_0,MCPWM_GEN_B);//给PH一个持续的高电平
      mcpwm_set_duty_type(MCPWM_UNIT_0,MCPWM_TIMER_0,MCPWM_GEN_A,MCPWM_DUTY_MODE_0);
      mcpwm_set_duty(MCPWM_UNIT_0,MCPWM_TIMER_0,MCPWM_GEN_A,speed);//EN引脚输出占空比为“speed”的PWM波
      break;

    case 2:
      mcpwm_set_signal_high(MCPWM_UNIT_0,MCPWM_TIMER_1,MCPWM_GEN_B);//给PH一个持续的高电平
      mcpwm_set_duty_type(MCPWM_UNIT_0,MCPWM_TIMER_1,MCPWM_GEN_A,MCPWM_DUTY_MODE_0);
      mcpwm_set_duty(MCPWM_UNIT_0,MCPWM_TIMER_1,MCPWM_GEN_A,speed);//EN引脚输出占空比为“speed”的PWM波
      break;

    case 3:
      mcpwm_set_signal_high(MCPWM_UNIT_1,MCPWM_TIMER_0,MCPWM_GEN_B);//给PH一个持续的高电平
      mcpwm_set_duty_type(MCPWM_UNIT_1,MCPWM_TIMER_0,MCPWM_GEN_A,MCPWM_DUTY_MODE_0);
      mcpwm_set_duty(MCPWM_UNIT_1,MCPWM_TIMER_0,MCPWM_GEN_A,speed);//EN引脚输出占空比为“speed”的PWM波
      break;

    case 4:
      mcpwm_set_signal_high(MCPWM_UNIT_1,MCPWM_TIMER_1,MCPWM_GEN_B);//给PH一个持续的高电平
      mcpwm_set_duty_type(MCPWM_UNIT_1,MCPWM_TIMER_1,MCPWM_GEN_A,MCPWM_DUTY_MODE_0);
      mcpwm_set_duty(MCPWM_UNIT_1,MCPWM_TIMER_1,MCPWM_GEN_A,speed);//EN引脚输出占空比为“speed”的PWM波
      break;
}
}

void retreat(uint8_t motorNumber,uint8_t speed)
{
switch(motorNumber)
{
    case 1:
      mcpwm_set_signal_low(MCPWM_UNIT_0,MCPWM_TIMER_0,MCPWM_GEN_B);//给PH一个持续的低电平
      mcpwm_set_duty_type(MCPWM_UNIT_0,MCPWM_TIMER_0,MCPWM_GEN_A,MCPWM_DUTY_MODE_0);
      mcpwm_set_duty(MCPWM_UNIT_0,MCPWM_TIMER_0,MCPWM_GEN_A,speed);//EN引脚输出占空比为“speed”的PWM波
      break;

    case 2:
      mcpwm_set_signal_low(MCPWM_UNIT_0,MCPWM_TIMER_1,MCPWM_GEN_B);//给PH一个持续的低电平
      mcpwm_set_duty_type(MCPWM_UNIT_0,MCPWM_TIMER_1,MCPWM_GEN_A,MCPWM_DUTY_MODE_0);
      mcpwm_set_duty(MCPWM_UNIT_0,MCPWM_TIMER_1,MCPWM_GEN_A,speed);//EN引脚输出占空比为“speed”的PWM波
      break;

    case 3:
      mcpwm_set_signal_low(MCPWM_UNIT_1,MCPWM_TIMER_0,MCPWM_GEN_B);//给PH一个持续的低电平
      mcpwm_set_duty_type(MCPWM_UNIT_1,MCPWM_TIMER_0,MCPWM_GEN_A,MCPWM_DUTY_MODE_0);
      mcpwm_set_duty(MCPWM_UNIT_1,MCPWM_TIMER_0,MCPWM_GEN_A,speed);//EN引脚输出占空比为“speed”的PWM波
      break;

    case 4:
      mcpwm_set_signal_low(MCPWM_UNIT_1,MCPWM_TIMER_1,MCPWM_GEN_B);//给PH一个持续的低电平
      mcpwm_set_duty_type(MCPWM_UNIT_1,MCPWM_TIMER_1,MCPWM_GEN_A,MCPWM_DUTY_MODE_0);
      mcpwm_set_duty(MCPWM_UNIT_1,MCPWM_TIMER_1,MCPWM_GEN_A,speed);//EN引脚输出占空比为“speed”的PWM波
      break;
}
}

void breake(uint8_t motorNumber)
{
switch(motorNumber)
{
    case 1:
      mcpwm_set_signal_low(MCPWM_UNIT_0,MCPWM_TIMER_0,MCPWM_GEN_A);
      break;

    case 2:
      mcpwm_set_signal_low(MCPWM_UNIT_0,MCPWM_TIMER_1,MCPWM_GEN_A);
      break;

    case 3:
      mcpwm_set_signal_low(MCPWM_UNIT_1,MCPWM_TIMER_0,MCPWM_GEN_A);
      break;

    case 4:
      mcpwm_set_signal_low(MCPWM_UNIT_1,MCPWM_TIMER_1,MCPWM_GEN_A);
      break;
}
}

void moveCar(uint8_t inputValue,uint8_t speed)
{
switch(inputValue)
{
    case UP:
      retreat(1,speed);
      retreat(2,speed);
      advance(3,speed);
      advance(4,speed);
      break;

    case DOWN:
      advance(1,speed);
      advance(2,speed);
      retreat(3,speed);
      retreat(4,speed);
      break;

    case LEFT:
      retreat(2,speed);
      retreat(4,speed);
      advance(1,speed);
      advance(3,speed);
      break;

    case RIGHT:
      retreat(1,speed);
      retreat(3,speed);
      advance(2,speed);
      advance(4,speed);
      break;
    case Upper_Left:
      breake(1);
      retreat(2,speed);
      advance(3,speed);
      breake(4);
      break;
    case Upper_Right:
      retreat(1,speed);
      breake(2);
      breake(3);
      advance(4,speed);
      break;
    case Bottom_Left:
      advance(1,speed);
      breake(2);
      breake(3);
      retreat(4,speed);
      break;
    case Bottom_Right:
      breake(1);
      advance(2,speed);
      retreat(3,speed);
      breake(4);
      break;
    case STOP:
      breake(1);
      breake(2);
      breake(3);
      breake(4);
      break;
}
}

void handleRoot(AsyncWebServerRequest *request)
{
request->send_P(200, "text/html", htmlHomePage);
}

void handleNotFound(AsyncWebServerRequest *request)
{
    request->send(404, "text/plain", "File Not Found");
}

void onCarInputWebSocketEvent(AsyncWebSocket *server,
                      AsyncWebSocketClient *client,
                      AwsEventType type,
                      void *arg,
                      uint8_t *data,
                      size_t len)
{                     
switch (type)
{
    case WS_EVT_CONNECT:
      Serial.printf("WebSocket client #%u connected from %s\n", client->id(), client->remoteIP().toString().c_str());
      break;
    case WS_EVT_DISCONNECT:
      Serial.printf("WebSocket client #%u disconnected\n", client->id());
      moveCar(STOP,0);
      //ledcWrite(PWMLightChannel, 0);
      //panServo.write(90);
      //tiltServo.write(90);      
      break;
    case WS_EVT_DATA:
      AwsFrameInfo *info;
      info = (AwsFrameInfo*)arg;
      if (info->final && info->index == 0 && info->len == len && info->opcode == WS_TEXT)
      {
      std::string myData = "";
      myData.assign((char *)data, len);
      std::istringstream ss(myData);
      std::string key, value;
      std::getline(ss, key, ',');
      std::getline(ss, value, ',');
      Serial.printf("Key [%s] Value[%s]\n", key.c_str(), value.c_str());
      int valueInt = atoi(value.c_str());   
      if (key == "MoveCar")
      {
          moveCar(valueInt,Speed);      
      }
      else if (key == "Speed")
      {
          Speed = valueInt;
      }
      /*else if (key == "Light")
      {
          //ledcWrite(PWMLightChannel, valueInt);         
      }
      else if (key == "Pan")
      {
          //panServo.write(valueInt);
      }
      else if (key == "Tilt")
      {
          //tiltServo.write(valueInt);   
      }*/         
      }
      break;
    case WS_EVT_PONG:
    case WS_EVT_ERROR:
      break;
    default:
      break;
}
}

void onCameraWebSocketEvent(AsyncWebSocket *server,
                      AsyncWebSocketClient *client,
                      AwsEventType type,
                      void *arg,
                      uint8_t *data,
                      size_t len)
{                     
switch (type)
{
    case WS_EVT_CONNECT:
      Serial.printf("WebSocket client #%u connected from %s\n", client->id(), client->remoteIP().toString().c_str());
      cameraClientId = client->id();
      break;
    case WS_EVT_DISCONNECT:
      Serial.printf("WebSocket client #%u disconnected\n", client->id());
      cameraClientId = 0;
      break;
    case WS_EVT_DATA:
      break;
    case WS_EVT_PONG:
    case WS_EVT_ERROR:
      break;
    default:
      break;
}
}

void setupCamera()
{
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.frame_size = FRAMESIZE_UXGA;
config.pixel_format = PIXFORMAT_JPEG; // for streaming
//config.pixel_format = PIXFORMAT_RGB565; // for face detection/recognition
config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
config.fb_location = CAMERA_FB_IN_PSRAM;
config.jpeg_quality = 12;
config.fb_count = 1;

    // if PSRAM IC present, init with UXGA resolution and higher JPEG quality
//                      for larger pre-allocated frame buffer.
if(config.pixel_format == PIXFORMAT_JPEG){
    if(psramFound()){
      config.jpeg_quality = 10;
      config.fb_count = 2;
      config.grab_mode = CAMERA_GRAB_LATEST;
    } else {
      // Limit the frame size when PSRAM is not available
      config.frame_size = FRAMESIZE_SVGA;
      config.fb_location = CAMERA_FB_IN_DRAM;
    }
} else {
    // Best option for face detection/recognition
    config.frame_size = FRAMESIZE_240X240;
#if CONFIG_IDF_TARGET_ESP32S3
    config.fb_count = 2;
#endif
}


// camera init
esp_err_t err = esp_camera_init(&config);
if (err != ESP_OK)
{
    Serial.printf("Camera init failed with error 0x%x", err);
    return;
}

if (psramFound())
{
    heap_caps_malloc_extmem_enable(20000);
    Serial.printf("PSRAM initialized. malloc to take memory from psram above this size");   
}
}

void sendCameraPicture()
{
if (cameraClientId == 0)
{
    return;
}
unsigned longstartTime1 = millis();
//capture a frame
camera_fb_t * fb = esp_camera_fb_get();
if (!fb)
{
      Serial.println("Frame buffer could not be acquired");
      return;
}

unsigned longstartTime2 = millis();
wsCamera.binary(cameraClientId, fb->buf, fb->len);
esp_camera_fb_return(fb);

//Wait for message to be delivered
while (true)
{
    AsyncWebSocketClient * clientPointer = wsCamera.client(cameraClientId);
    if (!clientPointer || !(clientPointer->queueIsFull()))
    {
      break;
    }
    delay(1);
}

unsigned longstartTime3 = millis();
Serial.printf("Time taken Total: %d|%d|%d\n",startTime3 - startTime1, startTime2 - startTime1, startTime3-startTime2 );
}

//初始化需要产生PWM信号的引脚
void mcpwm_init(void)
{
//配置mcpwm信息
mcpwm_config_t pwm_config;
pwm_config.frequency = 1000;
pwm_config.cmpr_a = 0;
pwm_config.cmpr_b = 0;
pwm_config.counter_mode = MCPWM_UP_COUNTER;
pwm_config.duty_mode = MCPWM_DUTY_MODE_0;

//A速度B方向
mcpwm_gpio_init(MCPWM_UNIT_0,MCPWM0A,M1_EN);//电机1 GPIO
mcpwm_gpio_init(MCPWM_UNIT_0,MCPWM0B,M1_PN);
mcpwm_init(MCPWM_UNIT_0,MCPWM_TIMER_0,&pwm_config);//初始化mcpwm的某一个单元并绑定时钟

mcpwm_gpio_init(MCPWM_UNIT_0,MCPWM1A,M2_EN);//电机2 GPIO
mcpwm_gpio_init(MCPWM_UNIT_0,MCPWM1B,M2_PN);
mcpwm_init(MCPWM_UNIT_0,MCPWM_TIMER_1,&pwm_config);//初始化mcpwm的某一个单元并绑定时钟

mcpwm_gpio_init(MCPWM_UNIT_1,MCPWM0A,M3_EN);//电机3 GPIO
mcpwm_gpio_init(MCPWM_UNIT_1,MCPWM0B,M3_PN);
mcpwm_init(MCPWM_UNIT_1,MCPWM_TIMER_0,&pwm_config);//初始化mcpwm的某一个单元并绑定时钟

mcpwm_gpio_init(MCPWM_UNIT_1,MCPWM1A,M4_EN);//电机4 GPIO
mcpwm_gpio_init(MCPWM_UNIT_1,MCPWM1B,M4_PN);
mcpwm_init(MCPWM_UNIT_1,MCPWM_TIMER_1,&pwm_config);//初始化mcpwm的某一个单元并绑定时钟
}


void setup(void)
{
Serial.begin(115200);
while(axp.begin() != 0){
    Serial.println("init error");
    delay(1000);
}
axp.enableCameraPower(axp.eOV2640);//设置摄像头供电
mcpwm_init();

WiFi.softAP(ssid, password);
IPAddress IP = WiFi.softAPIP();
Serial.print("AP IP address: ");
Serial.println(IP);

server.on("/", HTTP_GET, handleRoot);
server.onNotFound(handleNotFound);

wsCamera.onEvent(onCameraWebSocketEvent);
server.addHandler(&wsCamera);

wsCarInput.onEvent(onCarInputWebSocketEvent);
server.addHandler(&wsCarInput);

server.begin();
Serial.println("HTTP server started");

setupCamera();
}


void loop()
{
wsCamera.cleanupClients();
wsCarInput.cleanupClients();
sendCameraPicture();
//Serial.printf("SPIRam Total heap %d, SPIRam Free Heap %d\n", ESP.getPsramSize(), ESP.getFreePsram());
}


【演示视频】
https://www.bilibili.com/video/BV1AH4y1S7Up/?share_source=copy_web&vd_source=98855d5b99ff76982639c5ca6ff6f528

_深蓝_ 发表于 2023-9-24 15:14:52

云片出品,必属精品!!!

Amos Young 发表于 2023-10-4 14:54:38

这个创意有趣,云天老师好帅!

曾剑波 发表于 2023-11-8 00:48:30

这个不错哦!值得研究一下

沉寂TM 发表于 2024-7-10 16:42:54

第7张图到第8张图,右边2个轮子方向有变化,大家要注意下{:5_165:}。要按照X方向安装
页: [1]
查看完整版本: ESP32 麦克纳姆轮 视频车