小猪学arduino—Wifi图传+遥控坦克

最近调研4G图传控制模组,发现一个不错的wifi图传模块,用起来很简单,但是只能基于模组自身的wifi信号传输而不能走4G。虽然没能满足4G的需求,但是依然可以在很多场景发挥作用,比如近场遥控汽艇、潜艇、无人机、坦克的视频监控,Wifi 2.4G的信号也还是可以传很远距离的,既然还有这么多应用场景,这里就记录下模组的设计和使用方法吧,方便做小玩意时用。

一、概述

1.模组说明

1.1.图传

  • 模组内置单片机+wifi模块,供电后自动启动wifi热点,使用WLUFO APP连接热点后,自动实现图传显示能力
  • 图传和控制都在SWLTOY-WIFI视频模组+APP里,arduino仅用于通过模组接收来自APP的控制信号;图传能力与arduino无关,可单独v3.3供电使用

1.2.控制

  • 模组信号线连接到arduino RX端口后,arduino可通过19200串口与模组通信,获取来自WLUFO APP的操作控制信号,之后在arduino端实现后续舵机或电机控制即可
  • 串口发送数据格式(串口波特率 19200,1 个启始位,1 个停止位,其它无),发送间隔 40MS,一次发送 8 个 BYTE,数据格式(定高状态):66 80 80 80 80 40 40 99:
    • BYTE[0]:数据头,固定为 0X66。速度为 30%时,(副翼,升降舵)0x59-0x80-0xA8 线性变化。速度为 60%时,(副翼,升降舵)0x45-0x80-0xBC 线性变化。速度为 100%时,(副翼,升降舵)0x00-0x80-0xFF 线性变化。
    • BYTE[1]:AIL——副翼(4-5): 中间值 0x80,左边最大为 0x00,右边最大为 0xff。
    • BYTE[2]:ELE——升降舵(6-7):中间值 0x80,后最大为 0x00,前最大为 0xff。
    • BYTE[3]:THR——油门(1):0x00 为最小,0xff 为最大。油门在没有开启定高模式的时候,就是 0,开启定调模式后是居中(0x80)。(微调说明:前后/侧飞:±24 步 步进 1 128+偏移量,每次重启后回归默认值 128)。
    • BYTE[4]:RUDD——方向舵(2-3):中间值 0x80,左转最大为 0x00,右转最大为 0xff。
    • BYTE[5]:标志位。bit0=一键起飞 先置 1,1 秒后置 0 默认为 0,0x41。bit1=一键下降 先置 1,1 秒后置 0 默认为 0,0x42。
    • BYTE[6]:校验位。(BYTE[1]^BYTE[2]^BYTE[3]^BYTE[4]^BYTE[5])&0xff。
    • BYTE[7]:数据尾,固定为 0x99。

2.优点

  • 摄像头图传模组独立封装,与arduino分离,只关心控制信号部分实现即可

3.不足

  • 模组配套的APP没有源码,如果要二次开发,需要反编译APK研究后再自己实现

 

二、连线

硬件连线:

Arduino——-模组
3.3V ——– VCC
GND ——– GND
RX ——– TX

实际连线图:

注:坦克底盘采用的l298n+2个小马达,具体接线方法不在赘述,大家翻一翻我之前发的l298n电驱版的教程即可。

 

三、调试

1.图传测试

1.1.安装APP

1.3.测试图传

模块自带 WiFi 信号,将模块通电并连接 WiFi 信号即可实时传输视频。模块上电后蓝灯常亮, 手机连接WIFI名称 WIFIUFO-XXXX,无 WiFi 信号时重新将模块上电即可。

WIFI连接成功后打开WL-UFO APP,点击右下角PLAY按钮,打开界面后即可查看实时传输回来的图像。模块通电情况下为蓝灯常亮模式(可从侧面查看),当打开软件时灯闪烁,此时即可控制,如需连接信号读取处理可打开OFF按键,出现控制状态界面即可控制输出信号,单片机获取信号后处理即可控制需要的模块,如电机,舵机,灯。

 

2.控制信号接收测试

调试程序:

/*
  Wifi 视频传输 控制舵机转向小车

  硬件连线:
    Arduino Pin     Wifi视频传输模块
    3.3v --------   VCC
    GND  --------   GND
    RX   --------   TX

  原理说明:图传和控制都在SWLTOY-WIFI视频模组+APP里,arduino仅用于通过模组接收来自APP的控制信号
    -Wifi摄像头模组内置有单片机,供电线供电后即启动wifi,并提供与WLUFO APP图传和控制信号交互能力。和部分能力与arduino无关,离开arduino单独v3.3供电即可使用
    -信号线连接到arduino RX端口后,arduino可通过19200串口与Wifi摄像头模组通信,获取WLUFO APP里操作的控制信号,并进行后续舵机或电机操作即可
*/

#include <Servo.h>
#define DATA_LENGHT 5

static uint8_t AIL = 0;
static uint8_t ELE = 1;
static uint8_t THR = 2;
static uint8_t RUDD = 3;
static uint8_t FLAG = 4;
static uint8_t Data[5] = {0, 0, 0, 0, 0};
static uint32_t checksum;


void setup() {   //程序初始化,只运行一次
  Serial.begin(19200);
  Serial.println("Begin...");
  
  delay(100);
}

void loop() {      //主程序运行
  serialCom();
}

void serialCom() {  //数据通讯
  uint8_t c, checks;
  static uint8_t offset;
  static enum _serial_state {
    IDLE,
    HEADER_66,
    HEADER_99,
  }
  c_state;
  while (Serial.available()) {
    c = Serial.read();                                        // 读串口缓冲区
    if (c_state == IDLE) {                                    // 串口状态空闲,等待HEADER_66状态的到来
      c_state = (c == 0x66) ? HEADER_66 : IDLE;               // 判断是否进入HEADER_66
      offset = 0;
      checksum = 0;
      for (int i = 0 ; i < 5; i++)
        Data[i] = 0;
    } else if (c_state == HEADER_66 && offset < DATA_LENGHT) {   // 进入 HEADER_66 状态,读取数据
      Data[offset] = c;
      checksum ^= c;                                             // 校验和
      offset++;
    } else if (c_state == HEADER_66 && offset >= DATA_LENGHT) {  // 数据结束,读取校验和
      checks = c;
      c_state = HEADER_99;
    } else if (c_state == HEADER_99) {
      if ((checksum & 0xFF) == checks && c == 0x99)           // 判断校验和
        evaluateCommand();                                    // 数据处理程序
      c_state = IDLE;                                         // 返回IDLE状态
    }
  }
}

/*****************************************************************
    数据处理程序,将获取到的数据做处理,执行相对应的指令
 ******************************************************************/
void evaluateCommand() {
  Serial.print("evaluateCommand :");
  Serial.print(Data[ELE]);
  Serial.print(" ");
  Serial.print(Data[AIL]);
  Serial.print(" ");
  Serial.print(Data[THR]);
  Serial.print(" ");
  Serial.print(Data[RUDD]);
  Serial.print(" ");
  Serial.print(Data[FLAG]);
  Serial.println("");
  
  // Control Servo or DC Motor
  
}

注意:

  • 烧制程序时必须拔掉RX线,否则会因为0、1串口被占用导致avrdude: stk500_getsync() attempt 1 of 10: not in sync: resp=0x00错误,拔掉RX线重新烧制,烧制完毕再插上就行了。
  • 烧制完毕后如果图传不能正常显示,可以给板子重新上下电,并检查连的WIFI是否正确。

 

3.小车控制测试

代码:

/*
  wifi图传模块+arduino+l298n制作的wifi视频小车

  硬件连线:
    Arduino Pin     Wifi视频传输模块
    3.3v --------   VCC
    GND  --------   GND
    RX   --------   TX

  原理说明:图传和控制都在SWLTOY-WIFI视频模组+APP里,arduino仅用于通过模组接收来自APP的控制信号
    -Wifi摄像头模组内置有单片机,供电线供电后即启动wifi,并提供与WLUFO APP图传和控制信号交互能力。和部分能力与arduino无关,离开arduino单独v3.3供电即可使用
    -信号线连接到arduino RX端口后,arduino可通过19200串口与Wifi摄像头模组通信,获取WLUFO APP里操作的控制信号,并进行后续舵机或电机操作即可
*/


// 图传模块串口数据
#define DATA_LENGHT 5
static uint8_t Data[5] = {0, 0, 0, 0, 0};
static uint8_t DIR = 0;   //左转128-88,右转128-168
static uint8_t RUN = 1;   //前进128-168,后退128-88
static uint8_t THR = 2;   //油门0-255
static uint8_t RUDD = 3;  //方向0-128-255
static uint8_t FLAG = 4;
static uint32_t checksum;

// 马达引脚
int DC_LEFT1 = 5; // 定义uno的pin 5 向 DC_LEFT1 输出
int DC_LEFT2 = 6; // 定义uno的pin 6 向 DC_LEFT2 输出
int DC_RIGHT1 = 9; // 定义uno的pin 9 向 DC_RIGHT1 输出
int DC_RIGHT2 = 10; // 定义uno的pin 10 向 DC_RIGHT2 输出
int DC_SPEED_LEFT = 180;  //速度
int DC_SPEED_RIGHT = 170;  //速度

//程序初始化
void setup() {
  // 启动图传串口通信并发送指令
  Serial.begin(19200);
  Serial.println("Begin...");
  
  // 初始化马达引脚为
  pinMode(DC_LEFT1, OUTPUT);
  pinMode(DC_LEFT2, OUTPUT);
  pinMode(DC_RIGHT1, OUTPUT);
  pinMode(DC_RIGHT2, OUTPUT);
  
  delay(100);
}


//主程序运行
void loop() {      
  // 读取串口指令数据
  readSerialCom();
}


//读取串口数据
void readSerialCom() {  
  uint8_t c, checks;
  static uint8_t offset;
  static enum _serial_state {
    IDLE,
    HEADER_66,
    HEADER_99,
  }
  c_state;
  while (Serial.available()) {
    c = Serial.read();                                        // 读串口缓冲区
    if (c_state == IDLE) {                                    // 串口状态空闲,等待HEADER_66状态的到来
      c_state = (c == 0x66) ? HEADER_66 : IDLE;               // 判断是否进入HEADER_66
      offset = 0;
      checksum = 0;
      for (int i = 0 ; i < 5; i++)
        Data[i] = 0;
    } else if (c_state == HEADER_66 && offset < DATA_LENGHT) {   // 进入 HEADER_66 状态,读取数据
      Data[offset] = c;
      checksum ^= c;                                             // 校验和
      offset++;
    } else if (c_state == HEADER_66 && offset >= DATA_LENGHT) {  // 数据结束,读取校验和
      checks = c;
      c_state = HEADER_99;
    } else if (c_state == HEADER_99) {
      if ((checksum & 0xFF) == checks && c == 0x99){           // 判断校验和
        // 执行串口发来的指令
        execSerialCommand();       
      }                             
      c_state = IDLE;                                         // 返回IDLE状态
    }
  }
}


// 执行串口发来的指令
void execSerialCommand() {
  /*Serial.print("execSerialCommand :");
  Serial.print(Data[DIR]);
  Serial.print(" ");
  Serial.print(Data[RUN]);
  Serial.print(" ");
  Serial.print(Data[THR]);
  Serial.print(" ");
  Serial.print(Data[RUDD]);
  Serial.print(" ");
  Serial.print(Data[FLAG]);
  Serial.println("");*/

  //指令处理
  //  RUN 前进138-168,后退118-88
  //  DIR 左转118-88,右转138-168
  if (Data[RUN] > 0 && Data[DIR] > 0) {
    if (Data[RUN] >= 138 && Data[RUN] <= 168 && Data[DIR] >= 138 && Data[DIR] <= 168) { //前+右
      //Serial.println("UP RIGHT");
      //forward + right 向前右转
      analogWrite(DC_LEFT1, DC_SPEED_LEFT);   //pwm调速
      digitalWrite(DC_LEFT2, LOW);
      analogWrite(DC_RIGHT1, DC_SPEED_RIGHT * (168 - Data[DIR])/(168-128));   //pwm调速
      digitalWrite(DC_RIGHT2, LOW);
    }
    else if (Data[RUN] >= 138 && Data[RUN] <= 168 && Data[DIR] >= 88 && Data[DIR] <= 118) { //前+左
      //Serial.println("UP LEFT");
      //forward + left 向前左转
      analogWrite(DC_LEFT1, DC_SPEED_LEFT * (Data[DIR] - 88)/(128-88));   //pwm调速
      digitalWrite(DC_LEFT2, LOW);
      analogWrite(DC_RIGHT1, DC_SPEED_RIGHT);   //pwm调速
      digitalWrite(DC_RIGHT2, LOW);
    }
    else if (Data[RUN] >= 88 && Data[RUN] <= 118 && Data[DIR] >= 88 && Data[DIR] <= 118) { //后+左
      //Serial.println("DOWN LEFT");
      //back + left 向左后转
      digitalWrite(DC_LEFT1, LOW);
      analogWrite(DC_LEFT2, DC_SPEED_LEFT);
      digitalWrite(DC_RIGHT1, LOW);
      analogWrite(DC_RIGHT2, DC_SPEED_RIGHT * (Data[DIR] - 88)/(128-88));
    }
    else if (Data[RUN] >= 88 && Data[RUN] <= 118 && Data[DIR] >= 138 && Data[DIR] <= 168) { //后+右
      //Serial.println("DOWN RIGHT");
      //back + right 向右后转
      digitalWrite(DC_LEFT1, LOW);
      analogWrite(DC_LEFT2, DC_SPEED_LEFT * (168 - Data[DIR])/(168-128));
      digitalWrite(DC_RIGHT1, LOW);
      analogWrite(DC_RIGHT2, DC_SPEED_RIGHT);
    }
    else if (Data[RUN] >= 138 && Data[RUN] <= 168) { //前
      //Serial.println("UP");
      //forward 向前转
      analogWrite(DC_LEFT1, DC_SPEED_LEFT);   //pwm调速
      digitalWrite(DC_LEFT2, LOW);
      analogWrite(DC_RIGHT1, DC_SPEED_RIGHT);   //pwm调速
      digitalWrite(DC_RIGHT2, LOW);
    }
    else if (Data[RUN] >= 88 && Data[RUN] <= 118) { //后
      //Serial.println("DOWN");
      //back 向后转
      digitalWrite(DC_LEFT1, LOW);
      analogWrite(DC_LEFT2, DC_SPEED_LEFT);
      digitalWrite(DC_RIGHT1, LOW);
      analogWrite(DC_RIGHT2, DC_SPEED_RIGHT);
    }
    else if (Data[DIR] >= 88 && Data[DIR] <= 118) { //左
      //Serial.println("LEFT");
      //left
      digitalWrite(DC_LEFT1, LOW);
      analogWrite(DC_LEFT2, DC_SPEED_LEFT);
      analogWrite(DC_RIGHT1, DC_SPEED_RIGHT);
      digitalWrite(DC_RIGHT2, LOW);
    }
    else if (Data[DIR] >= 138 && Data[DIR] <= 168) { //右
      //Serial.println("RIGHT");
      //right
      analogWrite(DC_LEFT1, DC_SPEED_LEFT);   //pwm调速
      digitalWrite(DC_LEFT2, LOW);
      digitalWrite(DC_RIGHT1, LOW);
      analogWrite(DC_RIGHT2, DC_SPEED_RIGHT);
    }
    else if (Data[RUN] == 128 && Data[DIR] == 128) {  //停
      //Serial.println("STOP");
      //stop 停止
      digitalWrite(DC_LEFT1,LOW);
      digitalWrite(DC_LEFT2,LOW);
      digitalWrite(DC_RIGHT1,LOW);
      digitalWrite(DC_RIGHT2,LOW);
    }
  }

}

效果视频:

 

 

 

yan 22.7.7

欢迎关注下方“非著名资深码农“公众号进行交流~

发表评论

邮箱地址不会被公开。