ESP8266使用记录（三）

通过udp把mpu6050数据发送到PC端

/**********************************************************************
项目名称/Project          : 零基础入门学用物联网
程序名称/Program name     : ESP8266WiFiUdp_12
团队/Team                : 太极创客团队 / Taichi-Maker (www.taichi-maker.com)
作者/Author              : 小凯
日期/Date（YYYYMMDD）     : 20200319
程序目的/Purpose          : 
用于演示ESP8266WiFiUdp库中print函数
-----------------------------------------------------------------------
本示例程序为太极创客团队制作的《零基础入门学用物联网》中示例程序。
该教程为对物联网开发感兴趣的朋友所设计和制作。如需了解更多该教程的信息，请参考以下网页：
http://www.taichi-maker.com/homepage/esp8266-nodemcu-iot/iot-c/esp8266-nodemcu-web-client/http-request/
***********************************************************************/
#include 
#include 
#include 
#include 

#define ssid      "TaichiMaker_WIFI" //这里改成你的设备当前环境下WIFI名字
#define password  "xxxxxxx"          //这里改成你的设备当前环境下WIFI密码
 
#define BTN_1 D4       // 开关按钮
bool btn1_state = false;

WiFiUDP Udp;//实例化WiFiUDP对象
unsigned int localUdpPort = 16651;  // 自定义本地监听端口
unsigned int remoteUdpPort = 16650;  // 自定义远程监听端口
char incomingPacket[255];  // 保存Udp工具发过来的消息
char replyPacket[2048];  //发送的消息,仅支持英文
 
const int MPU6050_addr = 0x68;
int16_t AccX, AccY, AccZ, Temp, GyroX, GyroY, GyroZ;

void setup()
{
  Wire.begin();
  Wire.beginTransmission(MPU6050_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Serial.begin(9600);//打开串口
  Serial.println();
 
  Serial.printf("正在连接 %s ", ssid);
  WiFi.begin(ssid, password);//连接到wifi
  while (WiFi.status() != WL_CONNECTED)//等待连接
  {
    delay(500);
    Serial.print(".");
  }
  Serial.println("连接成功");
 
 //启动Udp监听服务
  if(Udp.begin(localUdpPort))
  {
    Serial.println("监听成功");
      
    //打印本地的ip地址，在UDP工具中会使用到
    //WiFi.localIP().toString().c_str()用于将获取的本地IP地址转化为字符串   
    Serial.printf("现在收听IP：%s, UDP端口：%d\n", WiFi.localIP().toString().c_str(), localUdpPort);
  }
  else
  {
    Serial.println("监听失败");
  }
  pinMode(BTN_1, INPUT_PULLUP);//开关按钮为输入开启上拉电阻
}
 
void loop()
{
  Wire.beginTransmission(MPU6050_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_addr, 14, true);
  AccX = Wire.read() << 8 | Wire.read();
  AccY = Wire.read() << 8 | Wire.read();
  AccZ = Wire.read() << 8 | Wire.read();
  Temp = Wire.read() << 8 | Wire.read();
  GyroX = Wire.read() << 8 | Wire.read();
  GyroY = Wire.read() << 8 | Wire.read();
  GyroZ = Wire.read() << 8 | Wire.read();
 
  DynamicJsonDocument doc(2048);
  // 创建json根节点对象
  JsonObject obj  = doc.to();
  obj ["AccX"] = AccX;
  obj ["AccY"] = AccY;
  obj ["AccZ"] = AccZ;
  obj ["Temp"] = Temp;
  obj ["GyroX"] = GyroX;
  obj ["GyroY"] = GyroY;
  obj ["GyroZ"] = GyroZ;  
  //向udp工具发送消息
  Udp.beginPacket(Udp.remoteIP(), remoteUdpPort);//配置远端ip地址和端口 
  if (digitalRead(BTN_1) == 0)
  {
      obj ["Shoot"] = 1; 
  }
  else
  {
      obj ["Shoot"] = 0; 
  }
  String output;
  serializeJson(doc, replyPacket);    
  Udp.print(replyPacket);//把数据写入发送缓冲区
  Udp.endPacket();//发送数据 
  delay(16);//延时33毫秒
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109

PC端Unity工程代码

using System;
using UnityEngine;

public class MPU6050 : MonoBehaviour
{
    private UdpServer server;

    public Transform trans;
    private bool fire = false;
    public float fireRate = 0.5f;
    private float nextFire = 0.0f;
    public GameObject sphere;

    float AccX;
    float accAngleX;
    float AccY;
    float accAngleY;
    float AccZ;

    float GyroX;
    float GyroY;
    float GyroZ;

    float gyroAngleX;
    float gyroAngleY;

    float roll;
    float pitch;
    float yaw;

    float elapsedTime, currentTime, previousTime;

    // Start is called before the first frame update
    void Start()
    {
        Application.targetFrameRate = 60;
        Loom.Initialize();

        EventCenter.AddListener("Receive", OnMessage);
        EventCenter.AddListener("Error", OnError);

        server = new UdpServer();
        server.Start(16650, "192.168.0.56", 16651);

        server.Send("192.168.0.105");
    }

    // Update is called once per frame
    void Update()
    {
    	//射个球
        if (fire && Time.time > nextFire)
        {
            nextFire = Time.time + fireRate;
            GameObject go = Instantiate(sphere, trans.position, trans.rotation);
            go.SetActive(true);
            go.GetComponent().AddForce(trans.forward * 800);
        }
    }

    private void OnApplicationQuit()
    {
        server.Dispose();
        server = null;
    }

    int count = 0;
    float AccErrorX;
    float AccErrorY;
    float GyroErrorX;
    float GyroErrorY;
    float GyroErrorZ;

    void OnMessage(string msg)
    {
        Loom.QueueOnMainThread(() =>
        {
            //previousTime = currentTime;        // Previous time is stored before the actual time read
            //currentTime = DateTime.Now.Millisecond;            // Current time actual time read
            //elapsedTime = (currentTime - previousTime) / 1000;
            elapsedTime = 0.016f;
            //Debug.Log("elapsedTime:" + elapsedTime);
            //ifReceive.text += msg+ "\r\n";
            MPUMSG mpumsg = new MPUMSG();
            try
            {
                JsonUtility.FromJsonOverwrite(msg, mpumsg);
            }
            catch (Exception ex)
            {
                Debug.LogError(ex);
                return;
            }
            //射个球
            if (mpumsg.Shoot == 1)
            {
                fire = true;
            }
            else
            {
                fire = false;
            }

            AccX = mpumsg.AccX / 16384.0f;
            AccY = mpumsg.AccY / 16384.0f;
            AccZ = mpumsg.AccZ / 16384.0f;

            accAngleX = (MathF.Atan(AccY / MathF.Sqrt(MathF.Pow(AccX, 2) + MathF.Pow(AccZ, 2))) * 180 / MathF.PI) - 0.58f;
            accAngleY = (MathF.Atan(-1 * AccX / MathF.Sqrt(MathF.Pow(AccY, 2) + MathF.Pow(AccZ, 2))) * 180 / MathF.PI) + 1.58f;

            accAngleX += 3.845675f;
            accAngleY += 1.984601f;

            GyroX = mpumsg.GyroX / 131.0f;
            GyroY = mpumsg.GyroY / 131.0f;
            GyroZ = mpumsg.GyroZ / 131.0f;

            GyroX += 1.553333f;
            GyroY += 3.210216f;
            GyroZ += 0.1432245f;

            if (count < 200)
            {
                count++;
                AccErrorX += accAngleX;
                AccErrorY += accAngleY;

                GyroErrorX += GyroX;
                GyroErrorY += GyroY;
                GyroErrorZ += GyroZ;
            }
            if (count == 200)
            {
                AccErrorX = AccErrorX / 200;
                AccErrorY = AccErrorY / 200;

                GyroErrorX = GyroErrorX / 200;
                GyroErrorY = GyroErrorY / 200;
                GyroErrorZ = GyroErrorZ / 200;

                Debug.LogWarning("AccErrorX:" + AccErrorX
                    + " AccErrorY:" + AccErrorY
                    + " GyroErrorX:" + GyroErrorX
                    + " GyroErrorY:" + GyroErrorY
                    + " GyroErrorZ:" + GyroErrorZ);
                //温度
                Debug.LogWarning("Temp:" + (mpumsg.Temp / 340.00 + 36.53));
                count = 0;
            }

            // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
            gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
            gyroAngleY = gyroAngleY + GyroY * elapsedTime;
            yaw = yaw + GyroZ * elapsedTime;
            // Complementary filter - combine acceleromter and gyro angle values
            roll = 0.96f * gyroAngleX + 0.04f * accAngleX;
            pitch = 0.96f * gyroAngleY + 0.04f * accAngleY;
            //Debug.Log("yaw:" + yaw + " roll:" + roll + " pitch:" + pitch);
            trans.eulerAngles = new Vector3(-roll, pitch, yaw);
        });
    }

    void OnError(string error)
    {
        Loom.QueueOnMainThread(() =>
        {
            Debug.LogError(error);
            if (error.Equals("serverSocket == null"))
            {
                fire = false;
            }
        });
    }
}

[Serializable]
public class MPUMSG
{
    public float AccX;
    public float AccY;
    public float AccZ;
    public float Temp;
    public float GyroX;
    public float GyroY;
    public float GyroZ;
    public int Shoot;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187

相关链接
https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/

工程地址
https://github.com/xue-fei/Unity.MPU6050