markdowm完善了一下，删除无用的文件
45coll
parent
558777d2c6
commit
4737eb7517
|
|
@ -0,0 +1,3 @@
|
||||||
|
# Default ignored files
|
||||||
|
/shelf/
|
||||||
|
/workspace.xml
|
||||||
|
|
@ -0,0 +1,12 @@
|
||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<module type="PYTHON_MODULE" version="4">
|
||||||
|
<component name="NewModuleRootManager">
|
||||||
|
<content url="file://$MODULE_DIR$" />
|
||||||
|
<orderEntry type="inheritedJdk" />
|
||||||
|
<orderEntry type="sourceFolder" forTests="false" />
|
||||||
|
</component>
|
||||||
|
<component name="PyDocumentationSettings">
|
||||||
|
<option name="format" value="PLAIN" />
|
||||||
|
<option name="myDocStringFormat" value="Plain" />
|
||||||
|
</component>
|
||||||
|
</module>
|
||||||
|
|
@ -0,0 +1,14 @@
|
||||||
|
<component name="InspectionProjectProfileManager">
|
||||||
|
<profile version="1.0">
|
||||||
|
<option name="myName" value="Project Default" />
|
||||||
|
<inspection_tool class="PyPackageRequirementsInspection" enabled="true" level="WARNING" enabled_by_default="true">
|
||||||
|
<option name="ignoredPackages">
|
||||||
|
<value>
|
||||||
|
<list size="1">
|
||||||
|
<item index="0" class="java.lang.String" itemvalue="PyQt5" />
|
||||||
|
</list>
|
||||||
|
</value>
|
||||||
|
</option>
|
||||||
|
</inspection_tool>
|
||||||
|
</profile>
|
||||||
|
</component>
|
||||||
|
|
@ -0,0 +1,6 @@
|
||||||
|
<component name="InspectionProjectProfileManager">
|
||||||
|
<settings>
|
||||||
|
<option name="USE_PROJECT_PROFILE" value="false" />
|
||||||
|
<version value="1.0" />
|
||||||
|
</settings>
|
||||||
|
</component>
|
||||||
|
|
@ -0,0 +1,4 @@
|
||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<project version="4">
|
||||||
|
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
|
||||||
|
</project>
|
||||||
|
|
@ -0,0 +1,8 @@
|
||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<project version="4">
|
||||||
|
<component name="ProjectModuleManager">
|
||||||
|
<modules>
|
||||||
|
<module fileurl="file://$PROJECT_DIR$/.idea/foc.iml" filepath="$PROJECT_DIR$/.idea/foc.iml" />
|
||||||
|
</modules>
|
||||||
|
</component>
|
||||||
|
</project>
|
||||||
|
|
@ -0,0 +1,6 @@
|
||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<project version="4">
|
||||||
|
<component name="VcsDirectoryMappings">
|
||||||
|
<mapping directory="$PROJECT_DIR$" vcs="Git" />
|
||||||
|
</component>
|
||||||
|
</project>
|
||||||
88
README.md
88
README.md
|
|
@ -1,39 +1,75 @@
|
||||||
# FocProject
|
# 自平衡莱洛三角形可充电版
|
||||||
|
|
||||||
#### 介绍
|
#### 介绍
|
||||||
{**以下是 Gitee 平台说明，您可以替换此简介**
|
**在B站[“基于LQR控制器的自平衡莱洛三角形”](https://www.bilibili.com/video/BV19v411n7mN)基础上添加了充电模块**
|
||||||
Gitee 是 OSCHINA 推出的基于 Git 的代码托管平台（同时支持 SVN）。专为开发者提供稳定、高效、安全的云端软件开发协作平台
|
主控芯片使用ESP32，并配置了调参上位机，可以很方便的通过wifi无线调参。无刷控制使用灯哥开源FOC。制作出一个方便复刻的自平衡莱洛三角形，在桌面上作为一个摆件还是非常不错的[展示视频]()
|
||||||
无论是个人、团队、或是企业，都能够用 Gitee 实现代码托管、项目管理、协作开发。企业项目请看 [https://gitee.com/enterprises](https://gitee.com/enterprises)}
|
|
||||||
|
|
||||||
#### 软件架构
|
#### 1 软件架构
|
||||||
软件架构说明
|
在原作者的自平衡控制**电压**算法上进行修改，将电压控制改为**速度**控制。使对模型的控制在物理上更加容易理解。并且代码的调参都可以通过连接ESP32的wifi调整。具体特性如下：
|
||||||
|
|
||||||
|
- **基于 Arduino**：运行在 ESP32 Arduino 上
|
||||||
|
- **控制模式丰富**：电压控制和速度控制
|
||||||
|
|
||||||
#### 安装教程
|
|
||||||
|
#### 2 硬件特性
|
||||||
|
|
||||||
1. xxxx
|
| 说明 | 参数 |
|
||||||
2. xxxx
|
| ---------------- |---------------------- |
|
||||||
3. xxxx
|
| 莱洛三角形尺寸 | 100*100 mm |
|
||||||
|
|动量轮尺寸|80*80 mm|
|
||||||
|
| 输入电压 | 3.7v锂电池*3|
|
||||||
|
|充电电压| 5V 从Type-C口输入|
|
||||||
|
|充电芯片CS5095|5V输入,最大1.2A充电电流|
|
||||||
|
|串口芯片CH340C|需要打开开关才能下载|
|
||||||
|
| 主控 | ESP-WROOM-32 |
|
||||||
|
|电机驱动芯片L6234PD|引脚:32, 33, 25, 22; 22为enable|
|
||||||
|
| AS5600 编码器 |SDA-23 SCL-5 |
|
||||||
|
| MPU6050六轴传感器 | SDA-19 SCL-18 |
|
||||||
|
|
||||||
#### 使用说明
|
#### 3 使用说明
|
||||||
|
|
||||||
1. xxxx
|
1. 前往灯哥开源[FOCgit](https://gitee.com/ream_d/Deng-s-foc-controller)下载Arduino开发环境（~~也可自行下载Arduino并安装SimpleFOC~~）并打开本项目内的Arduino内的main，烧录程序到ESP32。
|
||||||
2. xxxx
|
2. 打开本项目内的python_gui内的FOC_gui.exe并连接上WIFI：ESP32。点击设置开始调参。
|
||||||
3. xxxx
|
|
||||||
|
|
||||||
#### 参与贡献
|
比如让平衡角度为90度，则输入：TA90，并且会存入eeprom的位置0中 注：wifi发送**命令不能过快**，因为每次都会保存进eeprom，K参数没有保存到EEPROM所以可以使用滑条调整。
|
||||||
|
|
||||||
1. Fork 本仓库
|
| 参数命令 | 说明 |
|
||||||
2. 新建 Feat_xxx 分支
|
| ---------------- |---------------------- |
|
||||||
3. 提交代码
|
| TA | target_angle平衡角度 例如TA89.3 设置平衡角度89.3|
|
||||||
4. 新建 Pull Request
|
| SV | swing_up_voltage摇摆电压 左右摇摆的电压，越大越快到平衡态，但是过大会翻过头|
|
||||||
|
|SA|swing_up_angle摇摆角度 离平衡角度还有几度时候，切换到自平衡控制|
|
||||||
|
|VP1|速度环的PID的P，1是稳定在平衡角度之前的P值|
|
||||||
|
|VI1|速度环的PID的I，1是稳定在平衡角度之前的I值|
|
||||||
|
|VP2|速度环的PID的P，2是稳定后的P值|
|
||||||
|
|VI2|速度环的PID的I，2是稳定后的I值|
|
||||||
|
|K**1**1|**1和2**是电压控制时参数。LQR的参数1*角度差值|
|
||||||
|
|K**1**2|**1和2**是电压控制时参数。LQR的参数2*左右倾倒加速度|
|
||||||
|
|K**1**3|**1和2**是电压控制时参数，**3和4**是速度控制时参数。LQR的参数3*当前速度|
|
||||||
|
|
||||||
|
#### 4 硬件设计
|
||||||
|
使用立创EDA绘制电路原理图，LaserMaker绘制莱洛三角形和动量轮（有激光切割机可以事先切割结构作为参考）。将绘制完的图形导入到立创EDA中可作为PCB的外框。丝印图案分别是**Gawr Gura**、**ouro kronii** ~~helicopter~~
|
||||||
|
|
||||||
#### 特技
|
LaserMaker绘制的plt在**莱洛三角结构**文件夹内
|
||||||
|
|
||||||
1. 使用 Readme\_XXX.md 来支持不同的语言，例如 Readme\_en.md, Readme\_zh.md
|
感谢嘉立创的PCB制板，使DIY电路制作变得非常便利
|
||||||
2. Gitee 官方博客 [blog.gitee.com](https://blog.gitee.com)
|
|
||||||
3. 你可以 [https://gitee.com/explore](https://gitee.com/explore) 这个地址来了解 Gitee 上的优秀开源项目
|
[莱洛三角形PCB](https://lceda.cn/45coll/zi-ping-heng-di-lai-luo-san-jiao_10-10-ban-ben)
|
||||||
4. [GVP](https://gitee.com/gvp) 全称是 Gitee 最有价值开源项目，是综合评定出的优秀开源项目
|
[动量轮]()
|
||||||
5. Gitee 官方提供的使用手册 [https://gitee.com/help](https://gitee.com/help)
|
|
||||||
6. Gitee 封面人物是一档用来展示 Gitee 会员风采的栏目 [https://gitee.com/gitee-stars/](https://gitee.com/gitee-stars/)
|
具体需要购买的物品在**物料清单.xlsx**中
|
||||||
|
|
||||||
|
#### 5 Ctrl+C +V（参考）
|
||||||
|
Arduino上的控制算法抄的是原作者的LQR，无刷电机控制是抄灯哥开源的。电机控制引脚定义与传感器定义和灯哥开源FOC控制板2.0版完全一样。
|
||||||
|
|
||||||
|
Python的GUI是抄SimpleFOC的SimpleFOCStudio。
|
||||||
|
|
||||||
|
充电电路完全抄的是立创广场开源的CS5095充电方案。
|
||||||
|
1. 原作者：基于LQR控制器的自平衡莱洛三角形[BV19v411n7mN](https://www.bilibili.com/video/BV19v411n7mN)
|
||||||
|
2. 灯哥开源FOC [https://gitee.com/ream_d/Deng-s-foc-controller](https://gitee.com/ream_d/Deng-s-foc-controller)
|
||||||
|
3. 充电芯片电路[https://oshwhub.com/Aknice/cs5095e-san-jie-li-dian-chi-sheng-ya-chong-dian-dian-lu](https://oshwhub.com/Aknice/cs5095e-san-jie-li-dian-chi-sheng-ya-chong-dian-dian-lu)
|
||||||
|
|
||||||
|
#### 6 有用的地方
|
||||||
|
|
||||||
|
1. Arduino的程序中的command.h、command.cpp可以支持任意的字符串输入。在其他项目中一样可以用，无论是wifi接收到的字符串数据或者是串口的字符串数据。
|
||||||
|
|
||||||
|
2. GUI上位机可以在其他wifi项目中可以继续使用，用来调参还是很方便。
|
||||||
|
|
@ -1,28 +0,0 @@
|
||||||
#include "Command.h"
|
|
||||||
|
|
||||||
void Command::run(char* str){
|
|
||||||
for(int i=0; i < call_count; i++){
|
|
||||||
if(isSentinel(call_ids[i],str)){ // case : call_ids = "T2" str = "T215.155"
|
|
||||||
call_list[i](str+strlen(call_ids[i])); // get 15.155 input function
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
void Command::add(char* id, CommandCallback onCommand){
|
|
||||||
call_list[call_count] = onCommand;
|
|
||||||
call_ids[call_count] = id;
|
|
||||||
call_count++;
|
|
||||||
}
|
|
||||||
void Command::scalar(float* value, char* user_cmd){
|
|
||||||
*value = atof(user_cmd);
|
|
||||||
}
|
|
||||||
bool Command::isSentinel(char* ch,char* str)
|
|
||||||
{
|
|
||||||
char s[strlen(ch)+1];
|
|
||||||
strncpy(s,str,strlen(ch));
|
|
||||||
s[strlen(ch)] = '\0'; //strncpy need add end '\0'
|
|
||||||
if(strcmp(ch, s) == 0)
|
|
||||||
return true;
|
|
||||||
else
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
@ -1,17 +0,0 @@
|
||||||
#include <Arduino.h>
|
|
||||||
// callback function pointer definiton
|
|
||||||
typedef void (* CommandCallback)(char*); //!< command callback function pointer
|
|
||||||
class Command
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
void add(char* id , CommandCallback onCommand);
|
|
||||||
void run(char* str);
|
|
||||||
void scalar(float* value, char* user_cmd);
|
|
||||||
bool isSentinel(char* ch,char* str);
|
|
||||||
private:
|
|
||||||
// Subscribed command callback variables
|
|
||||||
CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number
|
|
||||||
char* call_ids[20]; //!< added callback commands
|
|
||||||
int call_count;//!< number callbacks that are subscribed
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
@ -1,19 +0,0 @@
|
||||||
#include "Command.h"
|
|
||||||
|
|
||||||
|
|
||||||
//命令设置
|
|
||||||
float target_velocity = 0;
|
|
||||||
Command comm;//声明一个自己的该类的对象
|
|
||||||
void doTarget(char* cmd) { comm.scalar(&target_velocity, cmd); }
|
|
||||||
|
|
||||||
void setup() {
|
|
||||||
// put your setup code here, to run once:
|
|
||||||
Serial.begin(115200);
|
|
||||||
comm.add("T",doTarget);
|
|
||||||
}
|
|
||||||
|
|
||||||
void loop() {
|
|
||||||
// put your main code here, to run repeatedly:
|
|
||||||
comm.run("T233.2548");
|
|
||||||
Serial.println(target_velocity);
|
|
||||||
}
|
|
||||||
|
|
@ -1,63 +0,0 @@
|
||||||
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
|
|
||||||
|
|
||||||
This software may be distributed and modified under the terms of the GNU
|
|
||||||
General Public License version 2 (GPL2) as published by the Free Software
|
|
||||||
Foundation and appearing in the file GPL2.TXT included in the packaging of
|
|
||||||
this file. Please note that GPL2 Section 2[b] requires that all works based
|
|
||||||
on this software must also be made publicly available under the terms of
|
|
||||||
the GPL2 ("Copyleft").
|
|
||||||
|
|
||||||
Contact information
|
|
||||||
-------------------
|
|
||||||
|
|
||||||
Kristian Lauszus, TKJ Electronics
|
|
||||||
Web : http://www.tkjelectronics.com
|
|
||||||
e-mail : kristianl@tkjelectronics.com
|
|
||||||
*/
|
|
||||||
|
|
||||||
const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
|
|
||||||
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication
|
|
||||||
|
|
||||||
uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
|
|
||||||
return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
|
|
||||||
Wire.beginTransmission(IMUAddress);
|
|
||||||
Wire.write(registerAddress);
|
|
||||||
Wire.write(data, length);
|
|
||||||
uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
|
|
||||||
if (rcode) {
|
|
||||||
Serial.print(F("i2cWrite failed: "));
|
|
||||||
Serial.println(rcode);
|
|
||||||
}
|
|
||||||
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
|
|
||||||
uint32_t timeOutTimer;
|
|
||||||
Wire.beginTransmission(IMUAddress);
|
|
||||||
Wire.write(registerAddress);
|
|
||||||
uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
|
|
||||||
if (rcode) {
|
|
||||||
Serial.print(F("i2cRead failed: "));
|
|
||||||
Serial.println(rcode);
|
|
||||||
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
|
|
||||||
}
|
|
||||||
Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
|
|
||||||
for (uint8_t i = 0; i < nbytes; i++) {
|
|
||||||
if (Wire.available())
|
|
||||||
data[i] = Wire.read();
|
|
||||||
else {
|
|
||||||
timeOutTimer = micros();
|
|
||||||
while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
|
|
||||||
if (Wire.available())
|
|
||||||
data[i] = Wire.read();
|
|
||||||
else {
|
|
||||||
Serial.println(F("i2cRead timeout"));
|
|
||||||
return 5; // This error value is not already taken by endTransmission
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return 0; // Success
|
|
||||||
}
|
|
||||||
|
|
@ -1,111 +0,0 @@
|
||||||
#include <Wire.h>
|
|
||||||
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
|
|
||||||
#define gyroZ_OFF -0.72
|
|
||||||
Kalman kalmanZ;
|
|
||||||
/* ----IMU Data---- */
|
|
||||||
double accX, accY, accZ;
|
|
||||||
double gyroX, gyroY, gyroZ;
|
|
||||||
int16_t tempRaw;
|
|
||||||
|
|
||||||
double gyroZangle; // Angle calculate using the gyro only
|
|
||||||
double compAngleZ; // Calculated angle using a complementary filter
|
|
||||||
double kalAngleZ; // Calculated angle using a Kalman filter
|
|
||||||
|
|
||||||
uint32_t timer;
|
|
||||||
uint8_t i2cData[14]; // Buffer for I2C data
|
|
||||||
|
|
||||||
/* ----FOC Data---- */
|
|
||||||
|
|
||||||
// driver instance
|
|
||||||
double acc2rotation(double x, double y);
|
|
||||||
void setup() {
|
|
||||||
Serial.begin(115200);
|
|
||||||
/* ----IMU init---- */
|
|
||||||
Wire.begin(19, 18,400000);// Set I2C frequency to 400kHz
|
|
||||||
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
|
|
||||||
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
|
|
||||||
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
|
|
||||||
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
|
|
||||||
while (i2cWrite(0x19, i2cData, 4, false))
|
|
||||||
; // Write to all four registers at once
|
|
||||||
while (i2cWrite(0x6B, 0x01, true))
|
|
||||||
; // PLL with X axis gyroscope reference and disable sleep mode
|
|
||||||
|
|
||||||
while (i2cRead(0x75, i2cData, 1))
|
|
||||||
;
|
|
||||||
if (i2cData[0] != 0x68)
|
|
||||||
{ // Read "WHO_AM_I" register
|
|
||||||
Serial.print(F("Error reading sensor"));
|
|
||||||
while (1)
|
|
||||||
;
|
|
||||||
}
|
|
||||||
|
|
||||||
delay(100); // Wait for sensor to stabilize
|
|
||||||
|
|
||||||
/* Set kalman and gyro starting angle */
|
|
||||||
while (i2cRead(0x3B, i2cData, 6))
|
|
||||||
;
|
|
||||||
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
|
|
||||||
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
|
|
||||||
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
|
|
||||||
double pitch = acc2rotation(accX, accY);
|
|
||||||
|
|
||||||
kalmanZ.setAngle(pitch);
|
|
||||||
gyroZangle = pitch;
|
|
||||||
|
|
||||||
timer = micros();
|
|
||||||
/* ----FOC init---- */
|
|
||||||
}
|
|
||||||
|
|
||||||
void loop() {
|
|
||||||
/*----IMU loop----*/
|
|
||||||
while (i2cRead(0x3B, i2cData, 14));
|
|
||||||
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
|
|
||||||
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
|
|
||||||
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
|
|
||||||
tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
|
|
||||||
gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
|
|
||||||
gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
|
|
||||||
gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);
|
|
||||||
|
|
||||||
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
|
|
||||||
timer = micros();
|
|
||||||
|
|
||||||
double pitch = acc2rotation(accX, accY);
|
|
||||||
double gyroZrate = gyroZ / 131.0; // Convert to deg/s
|
|
||||||
|
|
||||||
kalAngleZ = kalmanZ.getAngle(pitch, gyroZrate + gyroZ_OFF, dt);
|
|
||||||
gyroZangle += (gyroZrate + gyroZ_OFF) * dt;
|
|
||||||
compAngleZ = 0.93 * (compAngleZ + (gyroZrate + gyroZ_OFF) * dt) + 0.07 * pitch;
|
|
||||||
|
|
||||||
// Reset the gyro angle when it has drifted too much
|
|
||||||
if (gyroZangle < -180 || gyroZangle > 180)
|
|
||||||
gyroZangle = kalAngleZ;
|
|
||||||
|
|
||||||
#if 1 // Set to 1 to activate
|
|
||||||
Serial.print(pitch); Serial.print("\t");
|
|
||||||
Serial.print(gyroZangle); Serial.print("\t");
|
|
||||||
Serial.print(compAngleZ); Serial.print("\t");
|
|
||||||
Serial.print(kalAngleZ); Serial.print("\t");
|
|
||||||
#endif
|
|
||||||
Serial.print("\r\n");
|
|
||||||
delay(2);
|
|
||||||
}
|
|
||||||
/* mpu6050加速度转换为角度
|
|
||||||
acc2rotation(ax, ay)
|
|
||||||
acc2rotation(az, ay) */
|
|
||||||
double acc2rotation(double x, double y)
|
|
||||||
{
|
|
||||||
if (y < 0)
|
|
||||||
{
|
|
||||||
return atan(x / y) / 1.570796 * 90 + 180;
|
|
||||||
}
|
|
||||||
else if (x < 0)
|
|
||||||
{
|
|
||||||
return (atan(x / y) / 1.570796 * 90 + 360);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
return (atan(x / y) / 1.570796 * 90);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
@ -13,10 +13,11 @@ AS5600霍尔传感器 SDA-23 SCL-5 MPU6050六轴传感器 SDA-19 SCL-18
|
||||||
#include "Command.h"
|
#include "Command.h"
|
||||||
#include <WiFi.h>
|
#include <WiFi.h>
|
||||||
#include <AsyncUDP.h> //引用以使用异步UDP
|
#include <AsyncUDP.h> //引用以使用异步UDP
|
||||||
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
|
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter
|
||||||
#include "EEPROM.h"
|
#include "EEPROM.h"
|
||||||
Kalman kalmanZ;
|
Kalman kalmanZ;
|
||||||
#define gyroZ_OFF -0.19
|
#define gyroZ_OFF -0.19
|
||||||
|
#define FLAG_V 0
|
||||||
/* ----IMU Data---- */
|
/* ----IMU Data---- */
|
||||||
|
|
||||||
double accX, accY, accZ;
|
double accX, accY, accZ;
|
||||||
|
|
@ -47,7 +48,7 @@ void wifi_print(char * s,double num);
|
||||||
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
|
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
|
||||||
TwoWire I2Ctwo = TwoWire(1);
|
TwoWire I2Ctwo = TwoWire(1);
|
||||||
LowPassFilter lpf_throttle{0.00};
|
LowPassFilter lpf_throttle{0.00};
|
||||||
#define FLAG_V 0
|
|
||||||
//倒立摆参数
|
//倒立摆参数
|
||||||
float LQR_K1_1 = 4; //摇摆到平衡
|
float LQR_K1_1 = 4; //摇摆到平衡
|
||||||
float LQR_K1_2 = 1.5; //
|
float LQR_K1_2 = 1.5; //
|
||||||
|
|
@ -73,6 +74,10 @@ float target_angle = 89.3;
|
||||||
float target_voltage = 0;
|
float target_voltage = 0;
|
||||||
float swing_up_voltage = 1.8;
|
float swing_up_voltage = 1.8;
|
||||||
float swing_up_angle = 18;
|
float swing_up_angle = 18;
|
||||||
|
float v_i_1 = 20;
|
||||||
|
float v_p_1 = 0.5;
|
||||||
|
float v_i_2 = 10;
|
||||||
|
float v_p_2 = 0.2;
|
||||||
//命令设置
|
//命令设置
|
||||||
Command comm;
|
Command comm;
|
||||||
bool Motor_enable_flag = 0;
|
bool Motor_enable_flag = 0;
|
||||||
|
|
@ -97,8 +102,10 @@ void do_K21(char* cmd) { comm.scalar(&LQR_K2_1, cmd); }
|
||||||
void do_K22(char* cmd) { comm.scalar(&LQR_K2_2, cmd); }
|
void do_K22(char* cmd) { comm.scalar(&LQR_K2_2, cmd); }
|
||||||
void do_K23(char* cmd) { comm.scalar(&LQR_K2_3, cmd); }
|
void do_K23(char* cmd) { comm.scalar(&LQR_K2_3, cmd); }
|
||||||
#else
|
#else
|
||||||
void do_vp(char* cmd) { comm.scalar(&motor.PID_velocity.P, cmd); EEPROM.writeFloat(12, motor.PID_velocity.P);}
|
void do_vp1(char* cmd) { comm.scalar(&v_p_1, cmd); EEPROM.writeFloat(12, v_p_1);}
|
||||||
void do_vi(char* cmd) { comm.scalar(&motor.PID_velocity.I, cmd);EEPROM.writeFloat(16, motor.PID_velocity.I); }
|
void do_vi1(char* cmd) { comm.scalar(&v_i_1, cmd);EEPROM.writeFloat(16, v_p_1); }
|
||||||
|
void do_vp2(char* cmd) { comm.scalar(&v_p_2, cmd); EEPROM.writeFloat(20, v_p_2);}
|
||||||
|
void do_vi2(char* cmd) { comm.scalar(&v_i_2, cmd);EEPROM.writeFloat(24, v_i_2); }
|
||||||
void do_tv(char* cmd) { comm.scalar(&target_velocity, cmd); }
|
void do_tv(char* cmd) { comm.scalar(&target_velocity, cmd); }
|
||||||
void do_K31(char* cmd) { comm.scalar(&LQR_K3_1, cmd); }
|
void do_K31(char* cmd) { comm.scalar(&LQR_K3_1, cmd); }
|
||||||
void do_K32(char* cmd) { comm.scalar(&LQR_K3_2, cmd); }
|
void do_K32(char* cmd) { comm.scalar(&LQR_K3_2, cmd); }
|
||||||
|
|
@ -145,8 +152,10 @@ swing_up_angle = EEPROM.readFloat(8);
|
||||||
comm.add("K22",do_K22);
|
comm.add("K22",do_K22);
|
||||||
comm.add("K23",do_K23);
|
comm.add("K23",do_K23);
|
||||||
#else
|
#else
|
||||||
comm.add("VP",do_vp);
|
comm.add("VP1",do_vp1);
|
||||||
comm.add("VI",do_vi);
|
comm.add("VI1",do_vi1);
|
||||||
|
comm.add("VP2",do_vp2);
|
||||||
|
comm.add("VI2",do_vi2);
|
||||||
comm.add("TV",do_tv);
|
comm.add("TV",do_tv);
|
||||||
comm.add("K31",do_K31);
|
comm.add("K31",do_K31);
|
||||||
comm.add("K32",do_K32);
|
comm.add("K32",do_K32);
|
||||||
|
|
@ -154,8 +163,12 @@ swing_up_angle = EEPROM.readFloat(8);
|
||||||
comm.add("K41",do_K41);
|
comm.add("K41",do_K41);
|
||||||
comm.add("K42",do_K42);
|
comm.add("K42",do_K42);
|
||||||
comm.add("K43",do_K43);
|
comm.add("K43",do_K43);
|
||||||
motor.PID_velocity.P = EEPROM.readFloat(12);
|
v_p_1 = EEPROM.readFloat(12);
|
||||||
motor.PID_velocity.I = EEPROM.readFloat(16);
|
v_i_1 = EEPROM.readFloat(16);
|
||||||
|
v_p_2 = EEPROM.readFloat(20);
|
||||||
|
v_i_2 = EEPROM.readFloat(24);
|
||||||
|
motor.PID_velocity.P = v_p_1;
|
||||||
|
motor.PID_velocity.I = v_i_1;
|
||||||
#endif
|
#endif
|
||||||
// kalman mpu6050 init
|
// kalman mpu6050 init
|
||||||
Wire.begin(19, 18,400000);// Set I2C frequency to 400kHz
|
Wire.begin(19, 18,400000);// Set I2C frequency to 400kHz
|
||||||
|
|
@ -420,10 +433,14 @@ float controllerLQR(float p_angle, float p_vel, float m_vel)
|
||||||
#else
|
#else
|
||||||
if (!stable)
|
if (!stable)
|
||||||
{
|
{
|
||||||
|
motor.PID_velocity.P = v_p_1;
|
||||||
|
motor.PID_velocity.I = v_i_1;
|
||||||
u = LQR_K3_1 * p_angle + LQR_K3_2 * p_vel + LQR_K3_3 * m_vel;
|
u = LQR_K3_1 * p_angle + LQR_K3_2 * p_vel + LQR_K3_3 * m_vel;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
motor.PID_velocity.P = v_p_2;
|
||||||
|
motor.PID_velocity.I = v_i_2;
|
||||||
//u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel;
|
//u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel;
|
||||||
u = LQR_K4_1 * p_angle + LQR_K4_2 * p_vel + LQR_K4_3 * m_vel;
|
u = LQR_K4_1 * p_angle + LQR_K4_2 * p_vel + LQR_K4_3 * m_vel;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,63 +0,0 @@
|
||||||
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
|
|
||||||
|
|
||||||
This software may be distributed and modified under the terms of the GNU
|
|
||||||
General Public License version 2 (GPL2) as published by the Free Software
|
|
||||||
Foundation and appearing in the file GPL2.TXT included in the packaging of
|
|
||||||
this file. Please note that GPL2 Section 2[b] requires that all works based
|
|
||||||
on this software must also be made publicly available under the terms of
|
|
||||||
the GPL2 ("Copyleft").
|
|
||||||
|
|
||||||
Contact information
|
|
||||||
-------------------
|
|
||||||
|
|
||||||
Kristian Lauszus, TKJ Electronics
|
|
||||||
Web : http://www.tkjelectronics.com
|
|
||||||
e-mail : kristianl@tkjelectronics.com
|
|
||||||
*/
|
|
||||||
|
|
||||||
const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
|
|
||||||
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication
|
|
||||||
|
|
||||||
uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
|
|
||||||
return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
|
|
||||||
Wire.beginTransmission(IMUAddress);
|
|
||||||
Wire.write(registerAddress);
|
|
||||||
Wire.write(data, length);
|
|
||||||
uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
|
|
||||||
if (rcode) {
|
|
||||||
Serial.print(F("i2cWrite failed: "));
|
|
||||||
Serial.println(rcode);
|
|
||||||
}
|
|
||||||
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
|
|
||||||
uint32_t timeOutTimer;
|
|
||||||
Wire.beginTransmission(IMUAddress);
|
|
||||||
Wire.write(registerAddress);
|
|
||||||
uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
|
|
||||||
if (rcode) {
|
|
||||||
Serial.print(F("i2cRead failed: "));
|
|
||||||
Serial.println(rcode);
|
|
||||||
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
|
|
||||||
}
|
|
||||||
Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
|
|
||||||
for (uint8_t i = 0; i < nbytes; i++) {
|
|
||||||
if (Wire.available())
|
|
||||||
data[i] = Wire.read();
|
|
||||||
else {
|
|
||||||
timeOutTimer = micros();
|
|
||||||
while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
|
|
||||||
if (Wire.available())
|
|
||||||
data[i] = Wire.read();
|
|
||||||
else {
|
|
||||||
Serial.println(F("i2cRead timeout"));
|
|
||||||
return 5; // This error value is not already taken by endTransmission
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return 0; // Success
|
|
||||||
}
|
|
||||||
|
|
@ -1,98 +0,0 @@
|
||||||
#include <Wire.h>
|
|
||||||
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
|
|
||||||
#define gyroZ_OFF -0.72
|
|
||||||
Kalman kalmanZ;
|
|
||||||
/* ----IMU Data---- */
|
|
||||||
double accX, accY, accZ;
|
|
||||||
double gyroX, gyroY, gyroZ;
|
|
||||||
int16_t tempRaw;
|
|
||||||
|
|
||||||
double gyroZangle; // Angle calculate using the gyro only
|
|
||||||
double compAngleZ; // Calculated angle using a complementary filter
|
|
||||||
double kalAngleZ; // Calculated angle using a Kalman filter
|
|
||||||
|
|
||||||
uint32_t timer;
|
|
||||||
uint8_t i2cData[14]; // Buffer for I2C data
|
|
||||||
|
|
||||||
/* ----FOC Data---- */
|
|
||||||
|
|
||||||
// driver instance
|
|
||||||
double acc2rotation(double x, double y);
|
|
||||||
void kalman_init(){
|
|
||||||
Wire.begin(19, 18,400000);// Set I2C frequency to 400kHz
|
|
||||||
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
|
|
||||||
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
|
|
||||||
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
|
|
||||||
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
|
|
||||||
while (i2cWrite(0x19, i2cData, 4, false))
|
|
||||||
; // Write to all four registers at once
|
|
||||||
while (i2cWrite(0x6B, 0x01, true))
|
|
||||||
; // PLL with X axis gyroscope reference and disable sleep mode
|
|
||||||
|
|
||||||
while (i2cRead(0x75, i2cData, 1))
|
|
||||||
;
|
|
||||||
if (i2cData[0] != 0x68)
|
|
||||||
{ // Read "WHO_AM_I" register
|
|
||||||
Serial.print(F("Error reading sensor"));
|
|
||||||
while (1)
|
|
||||||
;
|
|
||||||
}
|
|
||||||
|
|
||||||
delay(100); // Wait for sensor to stabilize
|
|
||||||
|
|
||||||
/* Set kalman and gyro starting angle */
|
|
||||||
while (i2cRead(0x3B, i2cData, 6))
|
|
||||||
;
|
|
||||||
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
|
|
||||||
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
|
|
||||||
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
|
|
||||||
double pitch = acc2rotation(accX, accY);
|
|
||||||
|
|
||||||
kalmanZ.setAngle(pitch);
|
|
||||||
gyroZangle = pitch;
|
|
||||||
|
|
||||||
timer = micros();
|
|
||||||
}
|
|
||||||
double kalman_read(){
|
|
||||||
while (i2cRead(0x3B, i2cData, 14));
|
|
||||||
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
|
|
||||||
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
|
|
||||||
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
|
|
||||||
tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
|
|
||||||
gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
|
|
||||||
gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
|
|
||||||
gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);
|
|
||||||
|
|
||||||
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
|
|
||||||
timer = micros();
|
|
||||||
|
|
||||||
double pitch = acc2rotation(accX, accY);
|
|
||||||
double gyroZrate = gyroZ / 131.0; // Convert to deg/s
|
|
||||||
|
|
||||||
kalAngleZ = kalmanZ.getAngle(pitch, gyroZrate + gyroZ_OFF, dt);
|
|
||||||
gyroZangle += (gyroZrate + gyroZ_OFF) * dt;
|
|
||||||
compAngleZ = 0.93 * (compAngleZ + (gyroZrate + gyroZ_OFF) * dt) + 0.07 * pitch;
|
|
||||||
|
|
||||||
// Reset the gyro angle when it has drifted too much
|
|
||||||
if (gyroZangle < -180 || gyroZangle > 180)
|
|
||||||
gyroZangle = kalAngleZ;
|
|
||||||
return kalAngleZ;
|
|
||||||
}
|
|
||||||
/* mpu6050加速度转换为角度
|
|
||||||
acc2rotation(ax, ay)
|
|
||||||
acc2rotation(az, ay) */
|
|
||||||
double acc2rotation(double x, double y)
|
|
||||||
{
|
|
||||||
if (y < 0)
|
|
||||||
{
|
|
||||||
return atan(x / y) / 1.570796 * 90 + 180;
|
|
||||||
}
|
|
||||||
else if (x < 0)
|
|
||||||
{
|
|
||||||
return (atan(x / y) / 1.570796 * 90 + 360);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
return (atan(x / y) / 1.570796 * 90);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
@ -1,68 +0,0 @@
|
||||||
#include <WiFi.h>
|
|
||||||
#include <AsyncUDP.h> //引用以使用异步UDP
|
|
||||||
#include<Wire.h>
|
|
||||||
|
|
||||||
const char *ssid = "esp32";
|
|
||||||
const char *password = "12345678";
|
|
||||||
|
|
||||||
double kalZ;
|
|
||||||
AsyncUDP udp; //创建UDP对象
|
|
||||||
unsigned int localUdpPort = 2333; //本地端口号
|
|
||||||
|
|
||||||
|
|
||||||
unsigned int broadcastPort = localUdpPort;
|
|
||||||
//const char *broadcastData = "broadcast data";
|
|
||||||
const uint8_t broadcastData[] = {"broadcast data"};
|
|
||||||
|
|
||||||
void onPacketCallBack(AsyncUDPPacket packet)
|
|
||||||
{
|
|
||||||
Serial.print("UDP数据包来源类型: ");
|
|
||||||
Serial.println(packet.isBroadcast() ? "广播数据" : (packet.isMulticast() ? "组播" : "单播"));
|
|
||||||
Serial.print("远端地址及端口号: ");
|
|
||||||
Serial.print(packet.remoteIP());
|
|
||||||
Serial.print(":");
|
|
||||||
Serial.println(packet.remotePort());
|
|
||||||
Serial.print("目标地址及端口号: ");
|
|
||||||
Serial.print(packet.localIP());
|
|
||||||
Serial.print(":");
|
|
||||||
Serial.println(packet.localPort());
|
|
||||||
Serial.print("数据长度: ");
|
|
||||||
Serial.println(packet.length());
|
|
||||||
Serial.print("数据内容: ");
|
|
||||||
Serial.write(packet.data(), packet.length());
|
|
||||||
Serial.println();
|
|
||||||
|
|
||||||
// packet.print("reply data");
|
|
||||||
// broadcastPort = packet.remotePort();
|
|
||||||
}
|
|
||||||
|
|
||||||
void setup()
|
|
||||||
{
|
|
||||||
Serial.begin(115200);
|
|
||||||
// kalman mpu6050 init
|
|
||||||
kalman_init();
|
|
||||||
|
|
||||||
//wifi初始化
|
|
||||||
WiFi.mode(WIFI_AP);
|
|
||||||
while(!WiFi.softAP(ssid, password)){}; //启动AP
|
|
||||||
Serial.println("AP启动成功");
|
|
||||||
while (!udp.listen(localUdpPort)) //等待udp监听设置成功
|
|
||||||
{
|
|
||||||
}
|
|
||||||
udp.onPacket(onPacketCallBack); //注册收到数据包事件
|
|
||||||
}
|
|
||||||
char s[255];
|
|
||||||
void loop()
|
|
||||||
{
|
|
||||||
kalZ = kalman_read();
|
|
||||||
Serial.print(kalZ);Serial.print("\t");
|
|
||||||
sprintf(s, "%.2f", kalZ); //将100转为16进制表示的字符串
|
|
||||||
// Serial.println(strlen(s));
|
|
||||||
|
|
||||||
delay(10);
|
|
||||||
|
|
||||||
// udp.broadcastTo(broadcastData, broadcastPort); //可以使用该方法广播信息
|
|
||||||
|
|
||||||
IPAddress broadcastAddr((~(uint32_t)WiFi.subnetMask())|((uint32_t)WiFi.localIP())); //计算广播地址
|
|
||||||
udp.writeTo((const unsigned char*)s, strlen(s), broadcastAddr, localUdpPort); //广播数据
|
|
||||||
}
|
|
||||||
|
|
@ -1,92 +0,0 @@
|
||||||
#include <WiFi.h>
|
|
||||||
#include <AsyncUDP.h> //引用以使用异步UDP
|
|
||||||
#include<Wire.h>
|
|
||||||
|
|
||||||
const char *ssid = "esp32";
|
|
||||||
const char *password = "12345678";
|
|
||||||
|
|
||||||
const int MPU_addr=0x68; // I2C address of the MPU-6050
|
|
||||||
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
|
|
||||||
|
|
||||||
AsyncUDP udp; //创建UDP对象
|
|
||||||
unsigned int localUdpPort = 2333; //本地端口号
|
|
||||||
|
|
||||||
|
|
||||||
unsigned int broadcastPort = localUdpPort;
|
|
||||||
//const char *broadcastData = "broadcast data";
|
|
||||||
const uint8_t broadcastData[] = {"broadcast data"};
|
|
||||||
|
|
||||||
void onPacketCallBack(AsyncUDPPacket packet)
|
|
||||||
{
|
|
||||||
Serial.print("UDP数据包来源类型: ");
|
|
||||||
Serial.println(packet.isBroadcast() ? "广播数据" : (packet.isMulticast() ? "组播" : "单播"));
|
|
||||||
Serial.print("远端地址及端口号: ");
|
|
||||||
Serial.print(packet.remoteIP());
|
|
||||||
Serial.print(":");
|
|
||||||
Serial.println(packet.remotePort());
|
|
||||||
Serial.print("目标地址及端口号: ");
|
|
||||||
Serial.print(packet.localIP());
|
|
||||||
Serial.print(":");
|
|
||||||
Serial.println(packet.localPort());
|
|
||||||
Serial.print("数据长度: ");
|
|
||||||
Serial.println(packet.length());
|
|
||||||
Serial.print("数据内容: ");
|
|
||||||
Serial.write(packet.data(), packet.length());
|
|
||||||
Serial.println();
|
|
||||||
|
|
||||||
// packet.print("reply data");
|
|
||||||
// broadcastPort = packet.remotePort();
|
|
||||||
}
|
|
||||||
|
|
||||||
void setup()
|
|
||||||
{
|
|
||||||
Serial.begin(115200);
|
|
||||||
// mpu6050初始化
|
|
||||||
Wire.begin(19, 18, 400000);
|
|
||||||
Wire.beginTransmission(MPU_addr);
|
|
||||||
Wire.write(0x6B); // PWR_MGMT_1 register
|
|
||||||
Wire.write(0); // set to zero (wakes up the MPU-6050)
|
|
||||||
Wire.endTransmission(true);
|
|
||||||
|
|
||||||
//wifi初始化
|
|
||||||
WiFi.mode(WIFI_AP);
|
|
||||||
while(!WiFi.softAP(ssid, password)){}; //启动AP
|
|
||||||
Serial.println("AP启动成功");
|
|
||||||
while (!udp.listen(localUdpPort)) //等待udp监听设置成功
|
|
||||||
{
|
|
||||||
}
|
|
||||||
udp.onPacket(onPacketCallBack); //注册收到数据包事件
|
|
||||||
}
|
|
||||||
char s[255];
|
|
||||||
void loop()
|
|
||||||
{
|
|
||||||
Wire.beginTransmission(MPU_addr);
|
|
||||||
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
|
|
||||||
Wire.endTransmission(false);
|
|
||||||
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
|
|
||||||
// AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
|
|
||||||
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
|
|
||||||
// AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
|
|
||||||
// Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
|
|
||||||
// GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
|
|
||||||
// GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
|
|
||||||
// GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
|
|
||||||
|
|
||||||
// Serial.print("AcX = "); Serial.print(AcX);
|
|
||||||
Serial.print(" | AcY = "); Serial.println(AcY);
|
|
||||||
// itoa(AcY,s,10);
|
|
||||||
|
|
||||||
sprintf(s, "%d", AcY); //将100转为16进制表示的字符串
|
|
||||||
Serial.println(strlen(s));
|
|
||||||
// Serial.print(" | AcZ = "); Serial.print(AcZ);
|
|
||||||
// Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet
|
|
||||||
// Serial.print(" | GyX = "); Serial.print(GyX);
|
|
||||||
// Serial.print(" | GyY = "); Serial.print(GyY);
|
|
||||||
// Serial.print(" | GyZ = "); Serial.println(GyZ);
|
|
||||||
delay(10);
|
|
||||||
|
|
||||||
// udp.broadcastTo(broadcastData, broadcastPort); //可以使用该方法广播信息
|
|
||||||
|
|
||||||
IPAddress broadcastAddr((~(uint32_t)WiFi.subnetMask())|((uint32_t)WiFi.localIP())); //计算广播地址
|
|
||||||
udp.writeTo((const unsigned char*)s, strlen(s), broadcastAddr, localUdpPort); //广播数据
|
|
||||||
}
|
|
||||||
|
|
@ -1,58 +0,0 @@
|
||||||
#include <WiFi.h>
|
|
||||||
#include <AsyncUDP.h> //引用以使用异步UDP
|
|
||||||
|
|
||||||
const char *ssid = "esp32";
|
|
||||||
const char *password = "12345678";
|
|
||||||
|
|
||||||
AsyncUDP udp; //创建UDP对象
|
|
||||||
unsigned int localUdpPort = 2333; //本地端口号
|
|
||||||
|
|
||||||
unsigned int broadcastPort = localUdpPort;
|
|
||||||
const char *broadcastData = "broadcast data";
|
|
||||||
// const uint8_t broadcastData[] = {"broadcast data"};
|
|
||||||
|
|
||||||
void onPacketCallBack(AsyncUDPPacket packet)
|
|
||||||
{
|
|
||||||
// Serial.print("UDP数据包来源类型: ");
|
|
||||||
// Serial.println(packet.isBroadcast() ? "广播数据" : (packet.isMulticast() ? "组播" : "单播"));
|
|
||||||
// Serial.print("远端地址及端口号: ");
|
|
||||||
// Serial.print(packet.remoteIP());
|
|
||||||
// Serial.print(":");
|
|
||||||
// Serial.println(packet.remotePort());
|
|
||||||
// Serial.print("目标地址及端口号: ");
|
|
||||||
// Serial.print(packet.localIP());
|
|
||||||
// Serial.print(":");
|
|
||||||
// Serial.println(packet.localPort());
|
|
||||||
// Serial.print("数据长度: ");
|
|
||||||
// Serial.println(packet.length());
|
|
||||||
Serial.print("数据内容: ");
|
|
||||||
// Serial.write(packet.data(), packet.length());
|
|
||||||
Serial.println(atoi( (char*)(packet.data())));
|
|
||||||
|
|
||||||
// packet.print("reply data");
|
|
||||||
// broadcastPort = packet.remotePort();
|
|
||||||
}
|
|
||||||
|
|
||||||
void setup()
|
|
||||||
{
|
|
||||||
Serial.begin(115200);
|
|
||||||
Serial.println();
|
|
||||||
|
|
||||||
WiFi.mode(WIFI_AP);
|
|
||||||
while(!WiFi.softAP(ssid, password)){}; //启动AP
|
|
||||||
Serial.println("AP启动成功");
|
|
||||||
while (!udp.listen(localUdpPort)) //等待udp监听设置成功
|
|
||||||
{
|
|
||||||
}
|
|
||||||
udp.onPacket(onPacketCallBack); //注册收到数据包事件
|
|
||||||
}
|
|
||||||
|
|
||||||
void loop()
|
|
||||||
{
|
|
||||||
// delay(5000);
|
|
||||||
//
|
|
||||||
// udp.broadcastTo(broadcastData, broadcastPort); //可以使用该方法广播信息
|
|
||||||
|
|
||||||
// IPAddress broadcastAddr((~(uint32_t)WiFi.subnetMask())|((uint32_t)WiFi.localIP())); //计算广播地址
|
|
||||||
// udp.writeTo(broadcastData, sizeof(broadcastData), broadcastAddr, localUdpPort); //广播数据
|
|
||||||
}
|
|
||||||
|
|
@ -1,30 +0,0 @@
|
||||||
#include "Command.h"
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
void Command::run(char* str){
|
|
||||||
for(int i=0; i < call_count; i++){
|
|
||||||
if(isSentinel(call_ids[i],str)){ // case : call_ids = "T2" str = "T215.15"
|
|
||||||
call_list[i](str+strlen(call_ids[i])); // get 15.15 input function
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
void Command::add(char* id, CommandCallback onCommand){
|
|
||||||
call_list[call_count] = onCommand;
|
|
||||||
call_ids[call_count] = id;
|
|
||||||
call_count++;
|
|
||||||
}
|
|
||||||
void Command::scalar(float* value, char* user_cmd){
|
|
||||||
*value = atof(user_cmd);
|
|
||||||
}
|
|
||||||
bool Command::isSentinel(char* ch,char* str)
|
|
||||||
{
|
|
||||||
char s[strlen(ch)+1];
|
|
||||||
strncpy(s,str,strlen(ch));
|
|
||||||
s[strlen(ch)] = '\0'; //strncpy need add end '\0'
|
|
||||||
if(strcmp(ch, s) == 0)
|
|
||||||
return true;
|
|
||||||
else
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
@ -1,16 +0,0 @@
|
||||||
// callback function pointer definiton
|
|
||||||
typedef void (* CommandCallback)(char*); //!< command callback function pointer
|
|
||||||
class Command
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
void add(char* id , CommandCallback onCommand);
|
|
||||||
void run(char* str);
|
|
||||||
void scalar(float* value, char* user_cmd);
|
|
||||||
bool isSentinel(char* ch,char* str);
|
|
||||||
private:
|
|
||||||
// Subscribed command callback variables
|
|
||||||
CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number
|
|
||||||
char* call_ids[20]; //!< added callback commands
|
|
||||||
int call_count;//!< number callbacks that are subscribed
|
|
||||||
|
|
||||||
};
|
|
||||||
BIN
ctest/Command.o
BIN
ctest/Command.o
Binary file not shown.
|
|
@ -1,31 +0,0 @@
|
||||||
# Project: ÏîÄ¿2
|
|
||||||
# Makefile created by Dev-C++ 5.11
|
|
||||||
|
|
||||||
CPP = g++.exe
|
|
||||||
CC = gcc.exe
|
|
||||||
WINDRES = windres.exe
|
|
||||||
OBJ = main.o Command.o
|
|
||||||
LINKOBJ = main.o Command.o
|
|
||||||
LIBS = -L"C:/Program Files (x86)/Dev-Cpp/MinGW64/lib" -L"C:/Program Files (x86)/Dev-Cpp/MinGW64/x86_64-w64-mingw32/lib" -static-libgcc
|
|
||||||
INCS = -I"C:/Program Files (x86)/Dev-Cpp/MinGW64/include" -I"C:/Program Files (x86)/Dev-Cpp/MinGW64/x86_64-w64-mingw32/include" -I"C:/Program Files (x86)/Dev-Cpp/MinGW64/lib/gcc/x86_64-w64-mingw32/4.9.2/include"
|
|
||||||
CXXINCS = -I"C:/Program Files (x86)/Dev-Cpp/MinGW64/include" -I"C:/Program Files (x86)/Dev-Cpp/MinGW64/x86_64-w64-mingw32/include" -I"C:/Program Files (x86)/Dev-Cpp/MinGW64/lib/gcc/x86_64-w64-mingw32/4.9.2/include" -I"C:/Program Files (x86)/Dev-Cpp/MinGW64/lib/gcc/x86_64-w64-mingw32/4.9.2/include/c++"
|
|
||||||
BIN = ÏîÄ¿2.exe
|
|
||||||
CXXFLAGS = $(CXXINCS)
|
|
||||||
CFLAGS = $(INCS)
|
|
||||||
RM = rm.exe -f
|
|
||||||
|
|
||||||
.PHONY: all all-before all-after clean clean-custom
|
|
||||||
|
|
||||||
all: all-before $(BIN) all-after
|
|
||||||
|
|
||||||
clean: clean-custom
|
|
||||||
${RM} $(OBJ) $(BIN)
|
|
||||||
|
|
||||||
$(BIN): $(OBJ)
|
|
||||||
$(CPP) $(LINKOBJ) -o $(BIN) $(LIBS)
|
|
||||||
|
|
||||||
main.o: main.cpp
|
|
||||||
$(CPP) -c main.cpp -o main.o $(CXXFLAGS)
|
|
||||||
|
|
||||||
Command.o: Command.cpp
|
|
||||||
$(CPP) -c Command.cpp -o Command.o $(CXXFLAGS)
|
|
||||||
|
|
@ -1,26 +0,0 @@
|
||||||
#include <stdio.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include "Command.h"
|
|
||||||
Command command;
|
|
||||||
char buf[255];
|
|
||||||
void wifi_print(char * s,double num)
|
|
||||||
{
|
|
||||||
char str[255];
|
|
||||||
char n[255];
|
|
||||||
sprintf(n, "%.2f",num);
|
|
||||||
strcpy(str,s);
|
|
||||||
strcat(str, n);
|
|
||||||
strcat(buf+strlen(buf), str);
|
|
||||||
strcat(buf, ",");
|
|
||||||
}
|
|
||||||
float v = 0.1;
|
|
||||||
void re_command(float *num)
|
|
||||||
{
|
|
||||||
*num = *num + 1;
|
|
||||||
}
|
|
||||||
//void doTarget(char* cmd) { command.scalar(&v, cmd); }
|
|
||||||
main()
|
|
||||||
{
|
|
||||||
command.run();
|
|
||||||
}
|
|
||||||
|
|
@ -1,32 +0,0 @@
|
||||||
#include <stdio.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include "Command.h"
|
|
||||||
Command command;
|
|
||||||
char buf[255];
|
|
||||||
void wifi_print(char * s,double num)
|
|
||||||
{
|
|
||||||
char str[255];
|
|
||||||
char n[255];
|
|
||||||
sprintf(n, "%.2f",num);
|
|
||||||
strcpy(str,s);
|
|
||||||
strcat(str, n);
|
|
||||||
strcat(buf+strlen(buf), str);
|
|
||||||
strcat(buf, ",");
|
|
||||||
}
|
|
||||||
float v = 0;
|
|
||||||
float a = 0;
|
|
||||||
void re_command(float *num)
|
|
||||||
{
|
|
||||||
*num = *num + 1;
|
|
||||||
}
|
|
||||||
void doTarget_v(char* cmd) { command.scalar(&v, cmd); }
|
|
||||||
void doTarget_a(char* cmd) { command.scalar(&a, cmd); }
|
|
||||||
main()
|
|
||||||
{
|
|
||||||
command.add("T", doTarget_v);
|
|
||||||
command.add("A", doTarget_a);
|
|
||||||
command.run("A2233");
|
|
||||||
printf("%.2f",v);
|
|
||||||
printf("%.2f",a);
|
|
||||||
}
|
|
||||||
BIN
ctest/main.o
BIN
ctest/main.o
Binary file not shown.
|
|
@ -1,8 +0,0 @@
|
||||||
[Editors]
|
|
||||||
Order=0
|
|
||||||
Focused=0
|
|
||||||
[Editor_0]
|
|
||||||
CursorCol=17
|
|
||||||
CursorRow=14
|
|
||||||
TopLine=8
|
|
||||||
LeftChar=1
|
|
||||||
|
|
@ -1,82 +0,0 @@
|
||||||
[Project]
|
|
||||||
FileName=ÏîÄ¿2.dev
|
|
||||||
Name=ÏîÄ¿2
|
|
||||||
Type=1
|
|
||||||
Ver=2
|
|
||||||
ObjFiles=
|
|
||||||
Includes=
|
|
||||||
Libs=
|
|
||||||
PrivateResource=
|
|
||||||
ResourceIncludes=
|
|
||||||
MakeIncludes=
|
|
||||||
Compiler=
|
|
||||||
CppCompiler=
|
|
||||||
Linker=
|
|
||||||
IsCpp=1
|
|
||||||
Icon=
|
|
||||||
ExeOutput=
|
|
||||||
ObjectOutput=
|
|
||||||
LogOutput=
|
|
||||||
LogOutputEnabled=0
|
|
||||||
OverrideOutput=0
|
|
||||||
OverrideOutputName=
|
|
||||||
HostApplication=
|
|
||||||
UseCustomMakefile=0
|
|
||||||
CustomMakefile=
|
|
||||||
CommandLine=
|
|
||||||
Folders=
|
|
||||||
IncludeVersionInfo=0
|
|
||||||
SupportXPThemes=0
|
|
||||||
CompilerSet=0
|
|
||||||
CompilerSettings=0000000000000000000000000
|
|
||||||
UnitCount=3
|
|
||||||
|
|
||||||
[VersionInfo]
|
|
||||||
Major=1
|
|
||||||
Minor=0
|
|
||||||
Release=0
|
|
||||||
Build=0
|
|
||||||
LanguageID=1033
|
|
||||||
CharsetID=1252
|
|
||||||
CompanyName=
|
|
||||||
FileVersion=
|
|
||||||
FileDescription=Developed using the Dev-C++ IDE
|
|
||||||
InternalName=
|
|
||||||
LegalCopyright=
|
|
||||||
LegalTrademarks=
|
|
||||||
OriginalFilename=
|
|
||||||
ProductName=
|
|
||||||
ProductVersion=
|
|
||||||
AutoIncBuildNr=0
|
|
||||||
SyncProduct=1
|
|
||||||
|
|
||||||
[Unit1]
|
|
||||||
FileName=main.cpp
|
|
||||||
CompileCpp=1
|
|
||||||
Folder=
|
|
||||||
Compile=1
|
|
||||||
Link=1
|
|
||||||
Priority=1000
|
|
||||||
OverrideBuildCmd=0
|
|
||||||
BuildCmd=
|
|
||||||
|
|
||||||
[Unit2]
|
|
||||||
FileName=Command.cpp
|
|
||||||
CompileCpp=1
|
|
||||||
Folder=
|
|
||||||
Compile=1
|
|
||||||
Link=1
|
|
||||||
Priority=1000
|
|
||||||
OverrideBuildCmd=0
|
|
||||||
BuildCmd=
|
|
||||||
|
|
||||||
[Unit3]
|
|
||||||
FileName=Command.h
|
|
||||||
CompileCpp=1
|
|
||||||
Folder=
|
|
||||||
Compile=1
|
|
||||||
Link=1
|
|
||||||
Priority=1000
|
|
||||||
OverrideBuildCmd=0
|
|
||||||
BuildCmd=
|
|
||||||
|
|
||||||
BIN
ctest/项目2.exe
BIN
ctest/项目2.exe
Binary file not shown.
|
|
@ -1,8 +0,0 @@
|
||||||
[Editors]
|
|
||||||
Order=0
|
|
||||||
Focused=1
|
|
||||||
[Editor_0]
|
|
||||||
CursorCol=2
|
|
||||||
CursorRow=20
|
|
||||||
TopLine=12
|
|
||||||
LeftChar=1
|
|
||||||
|
|
@ -1,47 +0,0 @@
|
||||||
# -*- coding: utf-8 -*-
|
|
||||||
"""
|
|
||||||
Demonstrates basic use of LegendItem
|
|
||||||
|
|
||||||
"""
|
|
||||||
# import initExample ## Add path to library (just for examples; you do not need this)
|
|
||||||
|
|
||||||
import pyqtgraph as pg
|
|
||||||
from pyqtgraph.Qt import QtCore, QtGui
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
win = pg.plot()
|
|
||||||
win.setWindowTitle('pyqtgraph example: BarGraphItem')
|
|
||||||
|
|
||||||
# # option1: only for .plot(), following c1,c2 for example-----------------------
|
|
||||||
# win.addLegend(frame=False, rowCount=1, colCount=2)
|
|
||||||
|
|
||||||
# bar graph
|
|
||||||
x = np.arange(10)
|
|
||||||
y = np.sin(x+2) * 3
|
|
||||||
bg1 = pg.BarGraphItem(x=x, height=y, width=0.3, brush='b', pen='w', name='bar')
|
|
||||||
win.addItem(bg1)
|
|
||||||
|
|
||||||
# curve
|
|
||||||
c1 = win.plot([np.random.randint(0,8) for i in range(10)], pen='r', symbol='t', symbolPen='r', symbolBrush='g', name='curve1')
|
|
||||||
c2 = win.plot([2,1,4,3,1,3,2,4,3,2], pen='g', fillLevel=0, fillBrush=(255,255,255,30), name='curve2')
|
|
||||||
|
|
||||||
# scatter plot
|
|
||||||
s1 = pg.ScatterPlotItem(size=10, pen=pg.mkPen(None), brush=pg.mkBrush(255, 255, 255, 120), name='scatter')
|
|
||||||
spots = [{'pos': [i, np.random.randint(-3, 3)], 'data': 1} for i in range(10)]
|
|
||||||
s1.addPoints(spots)
|
|
||||||
win.addItem(s1)
|
|
||||||
|
|
||||||
# # option2: generic method------------------------------------------------
|
|
||||||
legend = pg.LegendItem((80,60), offset=(70,20))
|
|
||||||
legend.setParentItem(win.graphicsItem())
|
|
||||||
legend.addItem(bg1, 'bar')
|
|
||||||
legend.addItem(c1, 'curve1')
|
|
||||||
legend.addItem(c2, 'curve2')
|
|
||||||
legend.addItem(s1, 'scatter')
|
|
||||||
|
|
||||||
|
|
||||||
## Start Qt event loop unless running in interactive mode or using pyside.
|
|
||||||
if __name__ == '__main__':
|
|
||||||
import sys
|
|
||||||
if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
|
|
||||||
QtGui.QApplication.instance().exec_()
|
|
||||||
|
|
@ -1,36 +0,0 @@
|
||||||
from PyQt5.QtWidgets import *
|
|
||||||
from PyQt5.QtCore import *
|
|
||||||
from PyQt5.QtGui import *
|
|
||||||
import sys
|
|
||||||
|
|
||||||
'''pyqt5动态添加删除控件'''
|
|
||||||
|
|
||||||
|
|
||||||
class DynAddObject(QDialog):
|
|
||||||
def __init__(self, parent=None):
|
|
||||||
super(DynAddObject, self).__init__(parent)
|
|
||||||
self.widgetList = []
|
|
||||||
addButton = QPushButton(u"添加控件")
|
|
||||||
delBUtton = QPushButton(u"删除控件")
|
|
||||||
self.layout = QGridLayout()
|
|
||||||
self.layout.addWidget(addButton, 1, 0)
|
|
||||||
self.layout.addWidget(delBUtton, 2, 0)
|
|
||||||
self.setLayout(self.layout)
|
|
||||||
addButton.clicked.connect(self.add)
|
|
||||||
delBUtton.clicked.connect(self.delete)
|
|
||||||
|
|
||||||
def add(self):
|
|
||||||
btncont = self.layout.count()
|
|
||||||
widget = QPushButton(str(btncont - 1), self)
|
|
||||||
self.layout.addWidget(widget)
|
|
||||||
|
|
||||||
def delete(self):
|
|
||||||
for i in range(self.layout.count())[2:]:
|
|
||||||
self.layout.itemAt(i).widget().deleteLater()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
app = QApplication(sys.argv)
|
|
||||||
form = DynAddObject()
|
|
||||||
form.show()
|
|
||||||
app.exec_()
|
|
||||||
|
|
@ -1,34 +0,0 @@
|
||||||
import socket #引入套接字
|
|
||||||
import threading #引入并行
|
|
||||||
udp_data = None
|
|
||||||
def udp_send(udp_socket):
|
|
||||||
|
|
||||||
while True:
|
|
||||||
num1 = '192.168.4.1'
|
|
||||||
num2 = 2333
|
|
||||||
send_data = input('请输入要发送的数据：')
|
|
||||||
send_data = send_data.encode('utf-8')
|
|
||||||
udp_socket.sendto(send_data,(num1,num2)) #sendto（发送数据，发送地址）
|
|
||||||
|
|
||||||
def udp_recv(udp_socket):
|
|
||||||
global udp_data
|
|
||||||
while True:
|
|
||||||
recv_data = udp_socket.recv(1024)
|
|
||||||
recv_data = recv_data.decode('utf-8')
|
|
||||||
udp_data = recv_data
|
|
||||||
print('收到信息为:%s'%recv_data)
|
|
||||||
|
|
||||||
def main():
|
|
||||||
udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) #创建套接字
|
|
||||||
ip = '192.168.4.2' #服务器ip和端口
|
|
||||||
port = 2333
|
|
||||||
udp_socket.bind(("192.168.4.2",2333)) #服务器绑定ip和端口
|
|
||||||
#发送数据
|
|
||||||
t=threading.Thread(target=udp_send,args=(udp_socket,)) # Thread函数用于并行
|
|
||||||
#接收数据
|
|
||||||
t1=threading.Thread(target=udp_recv,args=(udp_socket,))
|
|
||||||
t.start() #并行开始
|
|
||||||
t1.start()
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
4324
莱洛三角切割/base.dxf
4324
莱洛三角切割/base.dxf
File diff suppressed because it is too large
Load Diff
BIN
莱洛三角切割/base.lcp
BIN
莱洛三角切割/base.lcp
Binary file not shown.
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Loading…
Reference in New Issue