Open source demo.
This commit is contained in:
MuShibo committed
commit
e64e304b9c
44 files changed
+285979
No files matched your search
@@ -0,0 +1,150 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// Copyright (c) 2024 Mu Shibo
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
// THE SOFTWARE.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
//飞特STS3032舵机同步控制指令
|
||||
|
||||
#include "Servo_STS3032.h"
|
||||
#include <stddef.h>
|
||||
|
||||
//飞特串行舵机通信层协议程序
|
||||
SCS::SCS()
|
||||
{
|
||||
Level = 1;//除广播指令所有指令返回应答
|
||||
Error = 0;
|
||||
}
|
||||
SCS::SCS(u8 End)
|
||||
{
|
||||
Level = 1;
|
||||
this->End = End;
|
||||
Error = 0;
|
||||
}
|
||||
SCS::SCS(u8 End, u8 Level)
|
||||
{
|
||||
this->Level = Level;
|
||||
this->End = End;
|
||||
Error = 0;
|
||||
}
|
||||
//1个16位数拆分为2个8位数
|
||||
//DataL为低位，DataH为高位
|
||||
void SCS::Host2SCS(u8 *DataL, u8* DataH, u16 Data)
|
||||
{
|
||||
if(End){
|
||||
*DataL = (Data>>8);
|
||||
*DataH = (Data&0xff);
|
||||
}else{
|
||||
*DataH = (Data>>8);
|
||||
*DataL = (Data&0xff);
|
||||
}
|
||||
}
|
||||
//同步写指令
|
||||
//舵机ID[]数组，IDN数组长度，MemAddr内存表地址，写入数据，写入长度
|
||||
void SCS::syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen)
|
||||
{
|
||||
rFlushSCS();
|
||||
u8 mesLen = ((nLen+1)*IDN+4);
|
||||
u8 Sum = 0;
|
||||
u8 bBuf[7];
|
||||
bBuf[0] = 0xff;
|
||||
bBuf[1] = 0xff;
|
||||
bBuf[2] = 0xfe;
|
||||
bBuf[3] = mesLen;
|
||||
bBuf[4] = INST_SYNC_WRITE;
|
||||
bBuf[5] = MemAddr;
|
||||
bBuf[6] = nLen;
|
||||
writeSCS(bBuf, 7);
|
||||
|
||||
Sum = 0xfe + mesLen + INST_SYNC_WRITE + MemAddr + nLen;
|
||||
u8 i, j;
|
||||
for(i=0; i<IDN; i++){
|
||||
writeSCS(ID[i]);
|
||||
writeSCS(nDat+i*nLen, nLen);
|
||||
Sum += ID[i];
|
||||
for(j=0; j<nLen; j++){
|
||||
Sum += nDat[i*nLen+j];
|
||||
}
|
||||
}
|
||||
writeSCS(~Sum);
|
||||
wFlushSCS();
|
||||
}
|
||||
|
||||
//飞特串行舵机硬件接口层程序
|
||||
SCSerial::SCSerial()
|
||||
{
|
||||
IOTimeOut = 10;
|
||||
pSerial = NULL;
|
||||
}
|
||||
SCSerial::SCSerial(u8 End):SCS(End)
|
||||
{
|
||||
IOTimeOut = 10;
|
||||
pSerial = NULL;
|
||||
}
|
||||
SCSerial::SCSerial(u8 End, u8 Level):SCS(End, Level)
|
||||
{
|
||||
IOTimeOut = 10;
|
||||
pSerial = NULL;
|
||||
}
|
||||
int SCSerial::writeSCS(unsigned char *nDat, int nLen)
|
||||
{
|
||||
if(nDat==NULL){
|
||||
return 0;
|
||||
}
|
||||
return pSerial->write(nDat, nLen);
|
||||
}
|
||||
int SCSerial::writeSCS(unsigned char bDat)
|
||||
{
|
||||
return pSerial->write(&bDat, 1);
|
||||
}
|
||||
void SCSerial::rFlushSCS()
|
||||
{
|
||||
while(pSerial->read()!=-1);
|
||||
}
|
||||
void SCSerial::wFlushSCS()
|
||||
{
|
||||
}
|
||||
|
||||
//飞特SMS_STS系列串行舵机应用层程序
|
||||
void SMS_STS::SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[])
|
||||
{
|
||||
u8 offbuf[7*IDN];
|
||||
for(u8 i = 0; i<IDN; i++){
|
||||
if(Position[i]<0){
|
||||
Position[i] = -Position[i];
|
||||
Position[i] |= (1<<15);
|
||||
}
|
||||
u16 V;
|
||||
if(Speed){
|
||||
V = Speed[i];
|
||||
}else{
|
||||
V = 0;
|
||||
}
|
||||
if(ACC){
|
||||
offbuf[i*7] = ACC[i];
|
||||
}else{
|
||||
offbuf[i*7] = 0;
|
||||
}
|
||||
Host2SCS(offbuf+i*7+1, offbuf+i*7+2, Position[i]);
|
||||
Host2SCS(offbuf+i*7+3, offbuf+i*7+4, 0);
|
||||
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, V);
|
||||
}
|
||||
syncWrite(ID, IDN, SMS_STS_ACC, offbuf, 7);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,104 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// Copyright (c) 2024 Mu Shibo
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
// THE SOFTWARE.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
|
||||
//飞特STS3032舵机同步控制指令
|
||||
//调用方式为sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
#ifndef _SERVO_STS3032_H
|
||||
#define _SERVO_STS3032_H
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include "WProgram.h"
|
||||
#endif
|
||||
|
||||
//数据结构定义
|
||||
typedef char s8;
|
||||
typedef unsigned char u8;
|
||||
typedef unsigned short u16;
|
||||
typedef short s16;
|
||||
typedef unsigned long u32;
|
||||
typedef long s32;
|
||||
|
||||
//内存表定义
|
||||
#define SMS_STS_ACC 41
|
||||
#define INST_SYNC_WRITE 0x83
|
||||
|
||||
//飞特串行舵机通信层协议程序
|
||||
class SCS{
|
||||
public:
|
||||
SCS();
|
||||
SCS(u8 End);
|
||||
SCS(u8 End, u8 Level);
|
||||
void syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen);//同步写指令
|
||||
public:
|
||||
u8 Level;//舵机返回等级
|
||||
u8 End;//处理器大小端结构
|
||||
u8 Error;//舵机状态
|
||||
u8 syncReadRxPacketIndex;
|
||||
u8 syncReadRxPacketLen;
|
||||
u8 *syncReadRxPacket;
|
||||
u8 *syncReadRxBuff;
|
||||
u16 syncReadRxBuffLen;
|
||||
u16 syncReadRxBuffMax;
|
||||
u32 syncTimeOut;
|
||||
protected:
|
||||
virtual int writeSCS(unsigned char *nDat, int nLen) = 0;
|
||||
virtual int writeSCS(unsigned char bDat) = 0;
|
||||
virtual void rFlushSCS() = 0;
|
||||
virtual void wFlushSCS() = 0;
|
||||
protected:
|
||||
void Host2SCS(u8 *DataL, u8* DataH, u16 Data);//1个16位数拆分为2个8位数
|
||||
int Ack(u8 ID);//返回应答
|
||||
int checkHead();//帧头检测
|
||||
};
|
||||
|
||||
//飞特串行舵机硬件接口层程序
|
||||
class SCSerial : public SCS
|
||||
{
|
||||
public:
|
||||
SCSerial();
|
||||
SCSerial(u8 End);
|
||||
SCSerial(u8 End, u8 Level);
|
||||
protected:
|
||||
int writeSCS(unsigned char *nDat, int nLen);//输出nLen字节
|
||||
int writeSCS(unsigned char bDat);//输出1字节
|
||||
void rFlushSCS();//
|
||||
void wFlushSCS();//
|
||||
public:
|
||||
unsigned long IOTimeOut;//输入输出超时
|
||||
HardwareSerial *pSerial;//串口指针
|
||||
int Err;
|
||||
public:
|
||||
virtual int getErr(){ return Err; }
|
||||
};
|
||||
|
||||
//飞特SMS/STS系列串行舵机应用层程序
|
||||
class SMS_STS : public SCSerial
|
||||
{
|
||||
public:
|
||||
virtual void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]);//同步写多个舵机位置指令
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,719 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// Copyright (c) 2024 Mu Shibo
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
// THE SOFTWARE.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#include <WiFi.h>
|
||||
|
||||
/***********html&javascript网页构建**********/
|
||||
const char basic_web[] PROGMEM = R"=====(
|
||||
|
||||
<html>
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||
<title>legged wheel robot web ctrl</title>
|
||||
<style>
|
||||
h2{
|
||||
width: auto;
|
||||
height: 60px;
|
||||
line-height: 60px;
|
||||
text-align: center;
|
||||
font-family: 等线;
|
||||
color: white;
|
||||
background-color:cornflowerblue;
|
||||
border-radius: 12px;
|
||||
}
|
||||
input{
|
||||
width: 160px;
|
||||
height: 30px;
|
||||
margin: 0px;
|
||||
}
|
||||
.sliderLabel{
|
||||
float: left;
|
||||
text-align: center;
|
||||
line-height: 30px;
|
||||
height: 30px;
|
||||
/* border: 2px solid red; */
|
||||
width: 200px;
|
||||
}
|
||||
|
||||
.sliders{
|
||||
width: 400px;
|
||||
height: 150px;
|
||||
/* border: 2px red solid; */
|
||||
margin: 10px auto;
|
||||
position: relative;
|
||||
}
|
||||
|
||||
.selects{
|
||||
width: 300px;
|
||||
height: 50px;
|
||||
padding: 10px;
|
||||
margin: 10px auto;
|
||||
/* border: 2px red solid; */
|
||||
position: relative;
|
||||
border-radius: 10px;
|
||||
|
||||
}
|
||||
.view {
|
||||
width: 250px;
|
||||
height: 50px;
|
||||
padding: 0px;
|
||||
margin: 20px auto;
|
||||
/* border: 2px red solid; */
|
||||
}
|
||||
.view2 {
|
||||
width: 140px;
|
||||
height: 30px;
|
||||
padding: 0px;
|
||||
margin: 15px auto;
|
||||
vertical-align:middle;
|
||||
/* border: 2px red solid; */
|
||||
}
|
||||
.btn1 {
|
||||
width: 80px;
|
||||
height: 40px;
|
||||
padding: 0px;
|
||||
margin: 0px;
|
||||
border-radius: 10px;
|
||||
background-color: white;
|
||||
display: inline-block;
|
||||
}
|
||||
form {
|
||||
width: 80px;
|
||||
height: 40px;
|
||||
padding: 0px;
|
||||
margin: 10px auto;
|
||||
display: inline-block;
|
||||
}
|
||||
.buttons{
|
||||
width: 300px;
|
||||
height: 180px;
|
||||
padding: 10px;
|
||||
margin: 10px auto;
|
||||
position: relative;
|
||||
/* border: 2px red solid; */
|
||||
}
|
||||
.dir{
|
||||
font-size: 15px;
|
||||
/* font-family: 'Courier New', Courier, monospace; */
|
||||
/* font-weight: bold; */
|
||||
width: 100px;
|
||||
height: 60px;
|
||||
text-align: center;
|
||||
border-radius: 12px;
|
||||
background-color: white;
|
||||
color: cornflowerblue;
|
||||
border: 3px solid cornflowerblue;
|
||||
padding: 0px;
|
||||
transition: all 0.3s;
|
||||
}
|
||||
option {
|
||||
text-align: center;
|
||||
font-size: 15px;
|
||||
}
|
||||
|
||||
#reset {
|
||||
width: 100px;
|
||||
height: 40px;
|
||||
text-align: center;
|
||||
border-radius: 12px;
|
||||
border: 2px solid;
|
||||
font-size: 15px;
|
||||
display: inline-block;
|
||||
position: absolute;
|
||||
top: 10px;
|
||||
left: 0px;
|
||||
/* 上 右 下 左 */
|
||||
}
|
||||
#gait_select {
|
||||
width: 100px;
|
||||
height: 40px;
|
||||
border: 2px solid red;
|
||||
font-size: 15px;
|
||||
color: red;
|
||||
border-radius: 12px;
|
||||
display: inline-block;
|
||||
position: absolute;
|
||||
top: 10px;
|
||||
left: 110px;
|
||||
}
|
||||
#gait_submit {
|
||||
width: 100px;
|
||||
height: 40px;
|
||||
text-align: center;
|
||||
border: 2px solid red;
|
||||
color: red;
|
||||
font-size: 15px;
|
||||
display: inline-block;
|
||||
background-color: white;
|
||||
border-radius: 12px;
|
||||
position: absolute;
|
||||
top: 10px;
|
||||
left: 220px;
|
||||
}
|
||||
#forward{
|
||||
display: inline-block;
|
||||
position: absolute;
|
||||
top: 0px;
|
||||
left: 110px;
|
||||
}
|
||||
#back{
|
||||
display: inline-block;
|
||||
position: absolute;
|
||||
left: 110px;
|
||||
top: 140px;
|
||||
}
|
||||
#jump{
|
||||
display: inline-block;
|
||||
position: absolute;
|
||||
top: 70px;
|
||||
left: 110px;
|
||||
}
|
||||
#right{
|
||||
display: inline-block;
|
||||
position: absolute;
|
||||
top: 70px;
|
||||
left: 220px;
|
||||
}
|
||||
#left{
|
||||
display: inline-block;
|
||||
position: absolute;
|
||||
top: 70px;
|
||||
left: 0px;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* Switch开关样式 */
|
||||
/* 必须是input为 checkbox class 添加 switch 才能实现以下效果 */
|
||||
input[type='checkbox'].switch{
|
||||
outline: none;
|
||||
appearance: none;
|
||||
-webkit-appearance: none;
|
||||
-moz-appearance: none;
|
||||
position: relative;
|
||||
width: 40px;
|
||||
height: 20px;
|
||||
background: #ccc;
|
||||
border-radius: 10px;
|
||||
transition: border-color .3s, background-color .3s;
|
||||
margin: 0px 20px 0px 0px;
|
||||
}
|
||||
|
||||
input[type='checkbox'].switch::after {
|
||||
content: '';
|
||||
display: inline-block;
|
||||
width: 1rem;
|
||||
height:1rem;
|
||||
border-radius: 50%;
|
||||
background: #fff;
|
||||
box-shadow: 0, 0, 2px, #999;
|
||||
transition:.4s;
|
||||
top: 2px;
|
||||
position: absolute;
|
||||
left: 2px;
|
||||
}
|
||||
|
||||
input[type='checkbox'].switch:checked {
|
||||
background: rgb(78, 78, 240);
|
||||
}
|
||||
/* 当input[type=checkbox]被选中时：伪元素显示下面样式 位置发生变化 */
|
||||
input[type='checkbox'].switch:checked::after {
|
||||
content: '';
|
||||
position: absolute;
|
||||
left: 55%;
|
||||
top: 2px;
|
||||
}
|
||||
*{
|
||||
-webkit-touch-callout:none;
|
||||
-webkit-user-select:none;
|
||||
-khtml-user-select:none;
|
||||
-moz-user-select:none;
|
||||
-ms-user-select:none;
|
||||
user-select:none;
|
||||
}
|
||||
|
||||
/*摇杆内容*/
|
||||
.row
|
||||
{
|
||||
display: inline-flex;
|
||||
clear: both;
|
||||
}
|
||||
.columnLateral
|
||||
{
|
||||
float: left;
|
||||
width: 15%;
|
||||
min-width: 300px;
|
||||
}
|
||||
|
||||
#joystick
|
||||
{
|
||||
border: 0px solid #FF0000;
|
||||
}
|
||||
|
||||
.dimSlide { transform: scaleX(2) rotate(0deg); width: 200px;position: relative}
|
||||
|
||||
</style>
|
||||
</head>
|
||||
|
||||
<!-- onload 事件在页面载入完成后立即触发 -->
|
||||
<body onload="javascript:socket_init()">
|
||||
<h2>WL-PRO WiFi遥控模式</h2>
|
||||
<div class="view2">
|
||||
<input type="checkbox" id="stable" class="switch" onclick="is_stable()" style="vertical-align:middle">Robot Go!</input>
|
||||
</div>
|
||||
<center><body>
|
||||
<!-- Example of two JoyStick integrated in the page structure -->
|
||||
<div class="row">
|
||||
<div class="columnLateral">
|
||||
<div id="joy1Div" style="width:200px;height:200px;margin:10px"></div>
|
||||
|
||||
<!-- angular_vel=<input id="joy1X" type="text" style="border-style:none;width:30px;" /> -->
|
||||
<!-- linear_vel=<input id="joy1Y" type="text" style="border-style:none;width:30px;" /> -->
|
||||
</div>
|
||||
</center>
|
||||
<div class="sliders">
|
||||
<div>
|
||||
<input type="range" min="32" max="85" value="38" id="hSlider" oninput="setHeight()" />
|
||||
<label class="sliderLabel" for="hSlider" id="hLabel">BaseHeight: 38mm</label>
|
||||
</div>
|
||||
<div>
|
||||
<input type="range" min="-30" max="30" value="0" id="rollSlider" oninput="setroll()" />
|
||||
<label class="sliderLabel" for="rollSlider" id="rollLabel">Roll: 0°</label>
|
||||
</div>
|
||||
|
||||
<div>
|
||||
<input type="range" min="-200" max="200" value="0" id="linearSlider" oninput="setLinear()" />
|
||||
<label class="sliderLabel" for="linearSlider" id="linearLabel">LinearVel: 0mm/s</label>
|
||||
</div>
|
||||
<div>
|
||||
<input type="range" min="-100" max="100" value="0" id="angularSlider" oninput="setAngular()" />
|
||||
<label class="sliderLabel" for="angularSlider" id="angularLabel">AngularVel: 0°/s</label>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="buttons">
|
||||
<button class="dir" id="forward">Forward</button>
|
||||
<button class="dir" id="back">Back</button>
|
||||
<button class="dir" id="left">Left</button>
|
||||
<button class="dir" id="right">Right</button>
|
||||
<button class="dir" id="jump">Jump</button>
|
||||
</div>
|
||||
|
||||
|
||||
<!-- <p id="tips" align="center" style="font-size: 20;"></p> -->
|
||||
<script>
|
||||
|
||||
var socket; // socket通信
|
||||
var g_roll=0; g_h=38;
|
||||
var g_linear = 0; g_angular = 0; g_stable = 0;
|
||||
var joyX = 0;
|
||||
var joyY = 0;
|
||||
// socket_init在页面载入完成后触发
|
||||
function socket_init() {
|
||||
// 初始化websocket客户端
|
||||
//socket = new WebSocket('ws://' + '192.168.4.1' + ':81/'); // AP模式
|
||||
socket = new WebSocket('ws://' + window.location.hostname + ':81/'); // sta模式
|
||||
}
|
||||
|
||||
function setroll() {
|
||||
val = document.getElementById("rollSlider").value;
|
||||
val = parseInt(val);
|
||||
document.getElementById("rollLabel").innerHTML = "Roll: " + val + "°";
|
||||
g_roll = val;
|
||||
send_data();
|
||||
}
|
||||
function setHeight() {
|
||||
val = document.getElementById("hSlider").value;
|
||||
val = parseInt(val);
|
||||
document.getElementById("hLabel").innerHTML = "BaseHeight: " + val + "mm";
|
||||
g_h = val;
|
||||
send_data();
|
||||
}
|
||||
|
||||
function setLinear() {
|
||||
val = document.getElementById("linearSlider").value;
|
||||
val = parseInt(val);
|
||||
document.getElementById("linearLabel").innerHTML = "LinearVel: " + val + "mm/s";
|
||||
g_linear = val;
|
||||
send_data();
|
||||
}
|
||||
function setAngular() {
|
||||
val = document.getElementById("angularSlider").value;
|
||||
val = parseInt(val);
|
||||
document.getElementById("angularLabel").innerHTML = "AngularVel: " + val + "°/s";
|
||||
g_angular = val;
|
||||
send_data();
|
||||
}
|
||||
|
||||
|
||||
function send_data() {
|
||||
var data = {'roll':g_roll,'height':g_h,
|
||||
'linear':g_linear,'angular':g_angular,'stable':g_stable,
|
||||
'mode':'basic','dir':"stop",
|
||||
'joy_y':joyY,'joy_x':joyX,};
|
||||
socket.send(JSON.stringify(data));
|
||||
// console.log(data);
|
||||
}
|
||||
function is_stable() {
|
||||
var obj = document.getElementById("stable");
|
||||
if(obj.checked) {
|
||||
// alert("is_stable checked");
|
||||
g_stable = 1;
|
||||
}else {
|
||||
// alert("is_stable unchecked");
|
||||
g_stable = 0;
|
||||
}
|
||||
send_data();
|
||||
}
|
||||
|
||||
var buttons = document.getElementsByClassName("dir");
|
||||
for(i=0;i<buttons.length;i++) {
|
||||
buttons[i].addEventListener("mousedown",move,true);
|
||||
buttons[i].addEventListener("mouseup",stop,true);
|
||||
buttons[i].addEventListener("touchstart",move,true);
|
||||
buttons[i].addEventListener("touchend",stop,true);
|
||||
}
|
||||
function move() {
|
||||
this.style = "background-color: cornflowerblue; color: white;";
|
||||
var data = {'dir':this.id,'mode':'basic',
|
||||
'roll':g_roll,'height':g_h,
|
||||
'linear':g_linear,'angular':g_angular,'stable':g_stable,
|
||||
'joy_x':joyX,'joy_y':joyY,};
|
||||
socket.send(JSON.stringify(data));
|
||||
// console.log(data);
|
||||
}
|
||||
function stop() {
|
||||
this.style = "background-color: white; color: cornflowerblue;";
|
||||
var data = {'dir':"stop",'mode':'basic',
|
||||
'roll':g_roll,'height':g_h,
|
||||
'linear':g_linear,'angular':g_angular,'stable':g_stable,
|
||||
'joy_x':joyX,'joy_y':joyY,};
|
||||
socket.send(JSON.stringify(data));
|
||||
// console.log(data); // 打印测试
|
||||
}
|
||||
|
||||
|
||||
/*摇杆内容*/
|
||||
var JoyStick = (function(container, parameters)
|
||||
{
|
||||
parameters = parameters || {};
|
||||
var title = (typeof parameters.title === "undefined" ? "joystick" : parameters.title),
|
||||
width = (typeof parameters.width === "undefined" ? 0 : parameters.width),
|
||||
height = (typeof parameters.height === "undefined" ? 0 : parameters.height),
|
||||
internalFillColor = (typeof parameters.internalFillColor === "undefined" ? "#00979C" : parameters.internalFillColor),
|
||||
internalLineWidth = (typeof parameters.internalLineWidth === "undefined" ? 2 : parameters.internalLineWidth),
|
||||
internalStrokeColor = (typeof parameters.internalStrokeColor === "undefined" ? "#00979C" : parameters.internalStrokeColor),
|
||||
externalLineWidth = (typeof parameters.externalLineWidth === "undefined" ? 2 : parameters.externalLineWidth),
|
||||
externalStrokeColor = (typeof parameters.externalStrokeColor === "undefined" ? "#0097BC" : parameters.externalStrokeColor),
|
||||
autoReturnToCenter = (typeof parameters.autoReturnToCenter === "undefined" ? true : parameters.autoReturnToCenter);
|
||||
|
||||
// Create Canvas element and add it in the Container object
|
||||
var objContainer = document.getElementById(container);
|
||||
var canvas = document.createElement("canvas");
|
||||
canvas.id = title;
|
||||
if(width === 0) { width = objContainer.clientWidth; }
|
||||
if(height === 0) { height = objContainer.clientHeight; }
|
||||
canvas.width = width;
|
||||
canvas.height = height;
|
||||
objContainer.appendChild(canvas);
|
||||
var context=canvas.getContext("2d");
|
||||
|
||||
var isPressing = 0;
|
||||
var isMoving = 0;
|
||||
var isRelease = 0;
|
||||
|
||||
var circumference = 2 * Math.PI;
|
||||
var internalRadius = (canvas.width-((canvas.width/2)+10))/2;
|
||||
var maxMoveStick = internalRadius + 5;
|
||||
var externalRadius = internalRadius + 30;
|
||||
var centerX = canvas.width / 2;
|
||||
var centerY = canvas.height / 2;
|
||||
var directionHorizontalLimitPos = canvas.width / 10;
|
||||
var directionHorizontalLimitNeg = directionHorizontalLimitPos * -1;
|
||||
var directionVerticalLimitPos = canvas.height / 10;
|
||||
var directionVerticalLimitNeg = directionVerticalLimitPos * -1;
|
||||
// Used to save current position of stick
|
||||
var movedX = centerX;
|
||||
var movedY = centerY;
|
||||
|
||||
|
||||
|
||||
// Check if the device support the touch or not
|
||||
if("ontouchstart" in document.documentElement)
|
||||
{
|
||||
|
||||
canvas.addEventListener("touchstart", onTouchStart, true);
|
||||
canvas.addEventListener("touchmove", onTouchMove, true);
|
||||
document.addEventListener("touchend", onTouchEnd, true);
|
||||
/*
|
||||
canvas.addEventListener("dragstart", onDragStart, false);
|
||||
canvas.addEventListener("dragend", onDragEnd, false);
|
||||
canvas.addEventListener("touchcancel", onTouchCancel, false);
|
||||
*/
|
||||
}
|
||||
else
|
||||
{
|
||||
canvas.addEventListener("mousedown", onMouseDown, true);
|
||||
canvas.addEventListener("mousemove", onMouseMove, true);
|
||||
document.addEventListener("mouseup", onMouseUp, true);
|
||||
}
|
||||
// Draw the object
|
||||
drawExternal();
|
||||
drawInternal();
|
||||
|
||||
/**
|
||||
* @desc Draw the external circle used as reference position
|
||||
*/
|
||||
function drawExternal()
|
||||
{
|
||||
context.beginPath();
|
||||
context.arc(centerX, centerY, externalRadius, 0, circumference, false);
|
||||
context.lineWidth = externalLineWidth;
|
||||
context.strokeStyle = externalStrokeColor;
|
||||
context.stroke();
|
||||
}
|
||||
|
||||
/**
|
||||
* @desc Draw the internal stick in the current position the user have moved it
|
||||
*/
|
||||
function drawInternal()
|
||||
{
|
||||
context.beginPath();
|
||||
if(movedX<internalRadius) { movedX=maxMoveStick; }
|
||||
if((movedX+internalRadius) > canvas.width) { movedX = canvas.width-(maxMoveStick); }
|
||||
if(movedY<internalRadius) { movedY=maxMoveStick; }
|
||||
if((movedY+internalRadius) > canvas.height) { movedY = canvas.height-(maxMoveStick); }
|
||||
context.arc(movedX, movedY, internalRadius, 0, circumference, false);
|
||||
// create radial gradient
|
||||
var grd = context.createRadialGradient(centerX, centerY, 5, centerX, centerY, 200);
|
||||
// Light color
|
||||
grd.addColorStop(0, internalFillColor);
|
||||
// Dark color
|
||||
grd.addColorStop(1, internalStrokeColor);
|
||||
context.fillStyle = grd;
|
||||
context.fill();
|
||||
context.lineWidth = internalLineWidth;
|
||||
context.strokeStyle = internalStrokeColor;
|
||||
context.stroke();
|
||||
}
|
||||
|
||||
function postCoordinate()
|
||||
{
|
||||
joyX = (100*((movedX - centerX)/maxMoveStick)).toFixed();
|
||||
joyY = ((100*((movedY - centerY)/maxMoveStick))*-1).toFixed();
|
||||
|
||||
send_data();
|
||||
}
|
||||
|
||||
function releaseControl()
|
||||
{
|
||||
joyX = 0;
|
||||
joyY = 0;
|
||||
|
||||
send_data();
|
||||
}
|
||||
|
||||
/**
|
||||
* @desc Events for manage touch
|
||||
*/
|
||||
function noTouch(event)
|
||||
{
|
||||
isPressing = 0;
|
||||
isMoving = 0;
|
||||
isRelease = 0;
|
||||
}
|
||||
|
||||
function onTouchStart(event)
|
||||
{
|
||||
isPressing = 1;
|
||||
isMoving = 0;
|
||||
isRelease = 0;
|
||||
}
|
||||
|
||||
function onTouchMove(event)
|
||||
{
|
||||
// Prevent the browser from doing its default thing (scroll, zoom)
|
||||
event.preventDefault();
|
||||
if(isPressing === 1 && event.targetTouches[0].target === canvas)
|
||||
{
|
||||
isMoving = 1;
|
||||
isRelease = 0;
|
||||
|
||||
movedX = event.targetTouches[0].pageX;
|
||||
movedY = event.targetTouches[0].pageY;
|
||||
// Manage offset
|
||||
if(canvas.offsetParent.tagName.toUpperCase() === "BODY")
|
||||
{
|
||||
movedX -= canvas.offsetLeft;
|
||||
movedY -= canvas.offsetTop;
|
||||
}
|
||||
else
|
||||
{
|
||||
movedX -= canvas.offsetParent.offsetLeft;
|
||||
movedY -= canvas.offsetParent.offsetTop;
|
||||
}
|
||||
// Delete canvas
|
||||
context.clearRect(0, 0, canvas.width, canvas.height);
|
||||
// Redraw object
|
||||
drawExternal();
|
||||
drawInternal();
|
||||
|
||||
postCoordinate();
|
||||
}
|
||||
}
|
||||
|
||||
function onTouchEnd(event)
|
||||
{
|
||||
//event.preventDefault();
|
||||
isPressing = 0;
|
||||
isMoving = 0;
|
||||
isRelease = 1;
|
||||
|
||||
// If required reset position store variable
|
||||
if(autoReturnToCenter)
|
||||
{
|
||||
movedX = centerX;
|
||||
movedY = centerY;
|
||||
}
|
||||
// Delete canvas
|
||||
context.clearRect(0, 0, canvas.width, canvas.height);
|
||||
// Redraw object
|
||||
drawExternal();
|
||||
drawInternal();
|
||||
//canvas.unbind('touchmove');
|
||||
|
||||
releaseControl();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @desc Events for manage mouse
|
||||
*/
|
||||
function noMouse(event)
|
||||
{
|
||||
isPressing = 0;
|
||||
isMoving = 0;
|
||||
isRelease = 0;
|
||||
}
|
||||
|
||||
function onMouseDown(event)
|
||||
{
|
||||
isPressing = 1;
|
||||
isMoving = 0;
|
||||
isRelease = 0;
|
||||
}
|
||||
|
||||
function onMouseMove(event)
|
||||
{
|
||||
if(isPressing === 1)
|
||||
{
|
||||
isMoving = 1;
|
||||
isRelease = 0;
|
||||
|
||||
movedX = event.pageX;
|
||||
movedY = event.pageY;
|
||||
// Manage offset
|
||||
if(canvas.offsetParent.tagName.toUpperCase() === "BODY")
|
||||
{
|
||||
movedX -= canvas.offsetLeft;
|
||||
movedY -= canvas.offsetTop;
|
||||
}
|
||||
else
|
||||
{
|
||||
movedX -= canvas.offsetParent.offsetLeft;
|
||||
movedY -= canvas.offsetParent.offsetTop;
|
||||
}
|
||||
// Delete canvas
|
||||
context.clearRect(0, 0, canvas.width, canvas.height);
|
||||
// Redraw object
|
||||
drawExternal();
|
||||
drawInternal();
|
||||
|
||||
postCoordinate();
|
||||
}
|
||||
}
|
||||
|
||||
function onMouseUp(event)
|
||||
{
|
||||
isPressing = 0;
|
||||
isMoving = 0;
|
||||
isRelease = 1;
|
||||
|
||||
// If required reset position store variable
|
||||
if(autoReturnToCenter)
|
||||
{
|
||||
movedX = centerX;
|
||||
movedY = centerY;
|
||||
}
|
||||
// Delete canvas
|
||||
context.clearRect(0, 0, canvas.width, canvas.height);
|
||||
// Redraw object
|
||||
drawExternal();
|
||||
drawInternal();
|
||||
//canvas.unbind('mousemove');
|
||||
|
||||
releaseControl();
|
||||
}
|
||||
|
||||
|
||||
this.GetX = function ()
|
||||
{
|
||||
return (100*((movedX - centerX)/maxMoveStick)).toFixed();
|
||||
};
|
||||
|
||||
/**
|
||||
* @desc Normalizzed value of Y move of stick
|
||||
* @return Integer from -100 to +100
|
||||
*/
|
||||
this.GetY = function ()
|
||||
{
|
||||
return ((100*((movedY - centerY)/maxMoveStick))*-1).toFixed();
|
||||
};
|
||||
|
||||
|
||||
});
|
||||
|
||||
|
||||
var joy1Param = { "title": "1" };
|
||||
var Joy1 = new JoyStick('joy1Div', joy1Param);
|
||||
var joy1X = document.getElementById("joy1X");
|
||||
var joy1Y = document.getElementById("joy1Y");
|
||||
setInterval(function()
|
||||
{
|
||||
joyX = Joy1.GetX();
|
||||
joyY = Joy1.GetY();
|
||||
send_data();
|
||||
}, 150);
|
||||
|
||||
|
||||
</script>
|
||||
|
||||
|
||||
</body>
|
||||
</html>
|
||||
|
||||
|
||||
|
||||
)=====";
|
||||
@@ -0,0 +1,14 @@
|
||||
# SPDX-License-Identifier: GPL-2.0-or-later
|
||||
#
|
||||
# Example OpenOCD configuration file for ESP32-WROVER-KIT board.
|
||||
#
|
||||
# For example, OpenOCD can be started for ESP32 debugging on
|
||||
#
|
||||
# openocd -f board/esp32-wrover-kit-3.3v.cfg
|
||||
#
|
||||
|
||||
# Source the JTAG interface configuration file
|
||||
source [find interface/ftdi/esp32_devkitj_v1.cfg]
|
||||
set ESP32_FLASH_VOLTAGE 3.3
|
||||
# Source the ESP32 configuration file
|
||||
source [find target/esp32.cfg]
|
||||
@@ -0,0 +1,19 @@
|
||||
{
|
||||
"name":"Arduino on ESP32",
|
||||
"toolchainPrefix":"xtensa-esp32-elf",
|
||||
"svdFile":"esp32.svd",
|
||||
"request":"attach",
|
||||
"postAttachCommands":[
|
||||
"set remote hardware-watchpoint-limit 2",
|
||||
"monitor reset halt",
|
||||
"monitor gdb_sync",
|
||||
"thb setup",
|
||||
"c"
|
||||
],
|
||||
"overrideRestartCommands":[
|
||||
"monitor reset halt",
|
||||
"monitor gdb_sync",
|
||||
"thb setup",
|
||||
"c"
|
||||
]
|
||||
}
|
||||
File diff suppressed because it is too large.
Load diff
@@ -0,0 +1,189 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// Copyright (c) 2024 Mu Shibo
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
// THE SOFTWARE.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#include "robot.h"
|
||||
|
||||
Wrobot wrobot;
|
||||
RobotProtocol::RobotProtocol(uint8_t len)
|
||||
{
|
||||
_len = len;
|
||||
_now_buf = new uint8_t[_len];
|
||||
_old_buf = new uint8_t[_len];
|
||||
|
||||
for(int i=0;i<_len;i++) {
|
||||
_now_buf[i] = 0;
|
||||
}
|
||||
|
||||
_now_buf[0] = 0xAA;
|
||||
_now_buf[1] = 0x55;
|
||||
}
|
||||
|
||||
RobotProtocol::~RobotProtocol()
|
||||
{
|
||||
delete [] _now_buf;
|
||||
delete [] _old_buf;
|
||||
}
|
||||
|
||||
void RobotProtocol::spinOnce(void)
|
||||
{
|
||||
int flag = checkBufRefresh();
|
||||
if(flag)
|
||||
{
|
||||
//UART_WriteBuf(); //这个会将web端的控制信息转成串口协议发出
|
||||
|
||||
//Serial.println(wrobot.dir);//测试数据获取
|
||||
//Serial.println("date have send\n");
|
||||
//Serial.printf("height:%d\n", wrobot.height);
|
||||
//Serial.printf("roll:%d\n", wrobot.pitch);
|
||||
//Serial.printf("linear:%d\n", wrobot.linear);
|
||||
// Serial.printf("\n");
|
||||
// Serial.printf("joy_X:%d\n", wrobot.joyx);
|
||||
// Serial.printf("joy_Y:%d\n", wrobot.joyy);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**************如下同时用于串口输出控制协议***************/
|
||||
|
||||
void RobotProtocol::UART_WriteBuf(void)
|
||||
{
|
||||
for(int i=0; i<_len; i++) {
|
||||
Serial.write(_now_buf[i]);
|
||||
}
|
||||
}
|
||||
|
||||
int RobotProtocol::checkBufRefresh(void)
|
||||
{
|
||||
int ret = 0;
|
||||
for(int i=0; i<_len; i++)
|
||||
{
|
||||
if(_now_buf[i] != _old_buf[i]) {
|
||||
ret = 1;
|
||||
break;
|
||||
}else {
|
||||
ret = 0;
|
||||
}
|
||||
}
|
||||
|
||||
for(int i=0; i<_len; i++) {
|
||||
_old_buf[i]= _now_buf[i];
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
void RobotProtocol::parseBasic(StaticJsonDocument<300> &doc)
|
||||
{
|
||||
|
||||
_now_buf[2] = BASIC;
|
||||
|
||||
String dir = doc["dir"];
|
||||
if(dir == "stop") {
|
||||
_now_buf[3] = STOP;
|
||||
wrobot.dir = STOP;
|
||||
} else {
|
||||
if(dir == "jump") {
|
||||
_now_buf[3] = JUMP;
|
||||
wrobot.dir = JUMP;
|
||||
}else if(dir == "forward") {
|
||||
_now_buf[3] = FORWARD;
|
||||
wrobot.dir = FORWARD;
|
||||
}else if(dir == "back") {
|
||||
_now_buf[3] = BACK;
|
||||
wrobot.dir = BACK;
|
||||
}else if(dir == "left") {
|
||||
_now_buf[3] = LEFT;
|
||||
wrobot.dir = LEFT;
|
||||
}else if(dir == "right") {
|
||||
_now_buf[3] = RIGHT;
|
||||
wrobot.dir = RIGHT;
|
||||
}else{
|
||||
_now_buf[3] = STOP;
|
||||
wrobot.dir = STOP;
|
||||
}
|
||||
}
|
||||
|
||||
int height = doc["height"];
|
||||
_now_buf[4] = height;
|
||||
wrobot.height = height;
|
||||
|
||||
int roll = doc["roll"];
|
||||
wrobot.roll = roll;
|
||||
if(roll >= 0) {
|
||||
_now_buf[5] = 0;
|
||||
}else {
|
||||
_now_buf[5] = 1;
|
||||
}
|
||||
_now_buf[6] = abs(roll);
|
||||
|
||||
int linear = doc["linear"];
|
||||
wrobot.linear = linear;
|
||||
if(linear >= 0) {
|
||||
_now_buf[7] = 0;
|
||||
}else {
|
||||
_now_buf[7] = 1;
|
||||
}
|
||||
_now_buf[8] = abs(linear);
|
||||
|
||||
int angular = doc["angular"];
|
||||
wrobot.angular = angular;
|
||||
if(angular >= 0) {
|
||||
_now_buf[9] = 0;
|
||||
}else {
|
||||
_now_buf[9] = 1;
|
||||
}
|
||||
_now_buf[10] = abs(angular);
|
||||
|
||||
|
||||
int stable = doc["stable"];
|
||||
wrobot.go = stable;
|
||||
if(stable) {
|
||||
_now_buf[11] = 1;
|
||||
}else {
|
||||
_now_buf[11] = 0;
|
||||
}
|
||||
|
||||
int joy_x = doc["joy_x"];
|
||||
wrobot.joyx = joy_x;
|
||||
if(joy_x >= 0) {
|
||||
_now_buf[12] = 0;
|
||||
}else {
|
||||
_now_buf[12] = 1;
|
||||
}
|
||||
_now_buf[13] = abs(joy_x);
|
||||
|
||||
|
||||
int joy_y = doc["joy_y"];
|
||||
wrobot.joyy = joy_y;
|
||||
if(joy_y >= 0) {
|
||||
_now_buf[14] = 0;
|
||||
}else {
|
||||
_now_buf[14] = 1;
|
||||
}
|
||||
_now_buf[15] = abs(joy_y);
|
||||
}
|
||||
@@ -0,0 +1,74 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// Copyright (c) 2024 Mu Shibo
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
// THE SOFTWARE.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <WiFi.h>
|
||||
#include <ArduinoJson.h>
|
||||
|
||||
typedef struct {
|
||||
int height = 38;
|
||||
int roll;
|
||||
int linear;
|
||||
int angular;
|
||||
int dir;
|
||||
int dir_last;
|
||||
int joyy;
|
||||
int joyy_last;
|
||||
int joyx;
|
||||
int joyx_last;
|
||||
bool go;
|
||||
}Wrobot;
|
||||
|
||||
extern Wrobot wrobot;
|
||||
|
||||
// 机器人运动状态枚举
|
||||
typedef enum {
|
||||
FORWARD = 0,
|
||||
BACK,
|
||||
RIGHT,
|
||||
LEFT,
|
||||
STOP,
|
||||
JUMP,
|
||||
} QR_State_t;
|
||||
|
||||
|
||||
|
||||
// 机器人模式枚举类型
|
||||
typedef enum {
|
||||
BASIC = 0,
|
||||
} Robot_Mode_t;
|
||||
|
||||
class RobotProtocol
|
||||
{
|
||||
public:
|
||||
RobotProtocol(uint8_t len);
|
||||
~RobotProtocol();
|
||||
void spinOnce(void);
|
||||
void parseBasic(StaticJsonDocument<300> &doc);
|
||||
private:
|
||||
uint8_t *_now_buf;
|
||||
uint8_t *_old_buf;
|
||||
uint8_t _len;
|
||||
void UART_WriteBuf(void);
|
||||
int checkBufRefresh(void);
|
||||
};
|
||||
@@ -0,0 +1,66 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// Copyright (c) 2024 Mu Shibo
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
// THE SOFTWARE.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#include "wifi.h"
|
||||
|
||||
// 配置AP（热点）模式相关参数
|
||||
const char *AP_SSID = "WLROBOT";
|
||||
const char *AP_PSW = "12345678";
|
||||
|
||||
IPAddress AP_IP(192, 168, 1, 11);
|
||||
IPAddress AP_GATEWAY(192, 168, 1, 11);
|
||||
IPAddress AP_SUBNET(255, 255, 255, 0);
|
||||
|
||||
|
||||
// 配置STA模式相关参数
|
||||
char *sta_ssid = "MUJITECH";
|
||||
char *sta_password = "mujitech";
|
||||
|
||||
|
||||
|
||||
void WiFi_SetAP(void)
|
||||
{
|
||||
WiFi.mode(WIFI_AP);
|
||||
WiFi.softAPConfig(AP_IP, AP_GATEWAY, AP_SUBNET);
|
||||
WiFi.softAP(AP_SSID, AP_PSW);
|
||||
// Serial.println();
|
||||
// Serial.print("AP IP address = ");
|
||||
// Serial.println(WiFi.softAPIP());
|
||||
}
|
||||
|
||||
|
||||
// ESP-01S AP模式接入WiFi网络
|
||||
void set_sta_wifi()
|
||||
{
|
||||
|
||||
WiFi.begin(sta_ssid, sta_password);
|
||||
// 等待连接
|
||||
while(WiFi.status()!=WL_CONNECTED)
|
||||
{
|
||||
//Serial.print(".");
|
||||
delay(500);
|
||||
}
|
||||
// 打印ESP-01S的IP地址
|
||||
Serial.println("");
|
||||
Serial.print("IP Address: ");
|
||||
Serial.println(WiFi.localIP());
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// Copyright (c) 2024 Mu Shibo
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
// THE SOFTWARE.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <WiFi.h>
|
||||
|
||||
void WiFi_SetAP(void);
|
||||
void set_sta_wifi(); //STA模式接入WiFi网络
|
||||
@@ -0,0 +1,569 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// Copyright (c) 2024 Mu Shibo
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
// THE SOFTWARE.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
//机器人控制头文件
|
||||
#include <MPU6050_tockn.h>
|
||||
#include "Servo_STS3032.h"
|
||||
#include <SimpleFOC.h>
|
||||
#include <Arduino.h>
|
||||
|
||||
//wifi控制数据传输头文件
|
||||
#include <WebSocketsServer.h>
|
||||
#include <ArduinoJson.h>
|
||||
#include <WebServer.h>
|
||||
#include <WiFi.h>
|
||||
#include <FS.h>
|
||||
#include "basic_web.h"
|
||||
#include "robot.h"
|
||||
#include "wifi.h"
|
||||
#include "esp_adc_cal.h"
|
||||
|
||||
/************实例定义*************/
|
||||
|
||||
//电机实例
|
||||
BLDCMotor motor1 = BLDCMotor(7);
|
||||
BLDCMotor motor2 = BLDCMotor(7);
|
||||
BLDCDriver3PWM driver1 = BLDCDriver3PWM(32,33,25,22);
|
||||
BLDCDriver3PWM driver2 = BLDCDriver3PWM(26,27,14,12);
|
||||
|
||||
//编码器实例
|
||||
TwoWire I2Cone = TwoWire(0);
|
||||
TwoWire I2Ctwo = TwoWire(1);
|
||||
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
||||
MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
|
||||
|
||||
//PID控制器实例
|
||||
PIDController pid_angle {.P = 1, .I = 0, .D = 0, .ramp = 100000, .limit = 8};
|
||||
PIDController pid_gyro {.P = 0.06, .I = 0, .D = 0, .ramp = 100000, .limit = 8};
|
||||
PIDController pid_distance {.P = 0.5, .I = 0, .D = 0, .ramp = 100000, .limit = 8};
|
||||
PIDController pid_speed {.P = 0.7, .I = 0, .D = 0, .ramp = 100000, .limit = 8};
|
||||
PIDController pid_yaw_angle {.P = 1.0, .I = 0, .D = 0, .ramp = 100000, .limit = 8};
|
||||
PIDController pid_yaw_gyro {.P = 0.04, .I = 0, .D = 0, .ramp = 100000, .limit = 8};
|
||||
PIDController pid_lqr_u {.P = 1, .I = 15, .D = 0, .ramp = 100000, .limit = 8};
|
||||
PIDController pid_zeropoint {.P = 0.002,.I = 0, .D = 0, .ramp = 100000, .limit = 4};
|
||||
PIDController pid_roll_angle{.P = 8, .I = 0, .D = 0, .ramp = 100000, .limit = 450};
|
||||
|
||||
//低通滤波器实例
|
||||
LowPassFilter lpf_joyy{.Tf = 0.2};
|
||||
LowPassFilter lpf_zeropoint{.Tf = 0.1};
|
||||
LowPassFilter lpf_roll{.Tf = 0.3};
|
||||
|
||||
// commander通信实例
|
||||
Commander command = Commander(Serial);
|
||||
|
||||
void StabAngle(char* cmd) { command.pid(&pid_angle, cmd); }
|
||||
void StabGyro(char* cmd) { command.pid(&pid_gyro, cmd); }
|
||||
void StabDistance(char* cmd) { command.pid(&pid_distance, cmd); }
|
||||
void StabSpeed(char* cmd) { command.pid(&pid_speed, cmd); }
|
||||
void StabYawAngle(char* cmd) { command.pid(&pid_yaw_angle, cmd); }
|
||||
void StabYawGyro(char* cmd) { command.pid(&pid_yaw_gyro, cmd); }
|
||||
void lpfJoyy(char* cmd) { command.lpf(&lpf_joyy, cmd); }
|
||||
void StabLqrU(char* cmd) { command.pid(&pid_lqr_u, cmd); }
|
||||
void StabZeropoint(char* cmd) { command.pid(&pid_zeropoint, cmd); }
|
||||
void lpfZeropoint(char* cmd) { command.lpf(&lpf_zeropoint, cmd); }
|
||||
void StabRollAngle(char* cmd) { command.pid(&pid_roll_angle, cmd);}
|
||||
void lpfRoll(char* cmd) { command.lpf(&lpf_roll, cmd); }
|
||||
|
||||
//void Stabtest_zeropoint(char* cmd) { command.pid(&test_zeropoint, cmd); }
|
||||
|
||||
//WebServer实例
|
||||
WebServer webserver; // server服务器
|
||||
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
||||
RobotProtocol rp(20);
|
||||
int joystick_value[2];
|
||||
|
||||
//STS舵机实例
|
||||
SMS_STS sms_sts;
|
||||
|
||||
//MPU6050实例
|
||||
MPU6050 mpu6050(I2Ctwo);
|
||||
|
||||
/************参数定义*************/
|
||||
#define pi 3.1415927
|
||||
|
||||
//LQR自平衡控制器参数
|
||||
float LQR_angle = 0;
|
||||
float LQR_gyro = 0;
|
||||
float LQR_speed = 0;
|
||||
float LQR_distance = 0;
|
||||
float angle_control = 0;
|
||||
float gyro_control = 0;
|
||||
float speed_control = 0;
|
||||
float distance_control = 0;
|
||||
float LQR_u = 0;
|
||||
float angle_zeropoint = -2.25;
|
||||
float distance_zeropoint = -256.0; //轮部位移零点偏置（-256为一个不可能的位移值，将其作为未刷新的标志）
|
||||
|
||||
//YAW轴控制数据
|
||||
float YAW_gyro = 0;
|
||||
float YAW_angle = 0;
|
||||
float YAW_angle_last = 0;
|
||||
float YAW_angle_total = 0;
|
||||
float YAW_angle_zero_point = -10;
|
||||
float YAW_output = 0;
|
||||
|
||||
//腿部舵机控制数据
|
||||
byte ID[2];
|
||||
s16 Position[2];
|
||||
u16 Speed[2];
|
||||
byte ACC[2];
|
||||
|
||||
//逻辑处理标志位
|
||||
float robot_speed = 0; //记录当前轮部转速
|
||||
float robot_speed_last = 0; //记录上一时刻的轮部转速
|
||||
int wrobot_move_stop_flag = 0; //记录摇杆停止的标识
|
||||
int jump_flag = 0; //跳跃时段标识
|
||||
float leg_position_add = 0; //roll轴平衡控制量
|
||||
int uncontrolable = 0; //机身倾角过大导致失控
|
||||
|
||||
//电压检测
|
||||
uint16_t bat_check_num = 0;
|
||||
int BAT_PIN = 35; // select the input pin for the ADC
|
||||
static esp_adc_cal_characteristics_t adc_chars;
|
||||
static const adc1_channel_t channel = ADC1_CHANNEL_7;
|
||||
static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
|
||||
static const adc_atten_t atten = ADC_ATTEN_DB_11;
|
||||
static const adc_unit_t unit = ADC_UNIT_1;
|
||||
|
||||
//电量显示LED
|
||||
#define LED_BAT 13
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);//通讯串口
|
||||
Serial2.begin(1000000);//腿部sts舵机
|
||||
|
||||
//Wifi初始化
|
||||
//WiFi_SetAP();
|
||||
set_sta_wifi(); // ESP-01S STA模式接入WiFi网络
|
||||
webserver.begin();
|
||||
webserver.on("/", HTTP_GET, basicWebCallback);
|
||||
websocket.begin();
|
||||
websocket.onEvent(webSocketEventCallback);
|
||||
|
||||
//舵机初始化
|
||||
//舵机有效行程450
|
||||
//左侧舵机[2048+12+50,2048+12+450]
|
||||
//左侧舵机[2048-12-50,2048-12-450]
|
||||
sms_sts.pSerial = &Serial2;
|
||||
ID[0] = 1;
|
||||
ID[1] = 2;
|
||||
ACC[0] = 30;
|
||||
ACC[1] = 30;
|
||||
Speed[0] = 300;
|
||||
Speed[1] = 300;
|
||||
Position[0] = 2148;
|
||||
Position[1] = 1948;
|
||||
//舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
//电压检测
|
||||
adc_calibration_init();
|
||||
adc1_config_width(width);
|
||||
adc1_config_channel_atten(channel, atten);
|
||||
esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars);
|
||||
|
||||
//电量显示LED
|
||||
pinMode(LED_BAT,OUTPUT);
|
||||
|
||||
// 编码器设置
|
||||
I2Cone.begin(19,18, 400000UL);
|
||||
I2Ctwo.begin(23,5, 400000UL);
|
||||
sensor1.init(&I2Cone);
|
||||
sensor2.init(&I2Ctwo);
|
||||
|
||||
//mpu6050设置
|
||||
mpu6050.begin();
|
||||
mpu6050.calcGyroOffsets(true);
|
||||
|
||||
//连接motor对象与编码器对象
|
||||
motor1.linkSensor(&sensor1);
|
||||
motor2.linkSensor(&sensor2);
|
||||
|
||||
//速度环PID参数
|
||||
motor1.PID_velocity.P = 0.05;
|
||||
motor1.PID_velocity.I = 1;
|
||||
motor1.PID_velocity.D = 0;
|
||||
|
||||
motor2.PID_velocity.P = 0.05;
|
||||
motor2.PID_velocity.I = 1;
|
||||
motor2.PID_velocity.D = 0;
|
||||
|
||||
// 驱动器设置
|
||||
motor1.voltage_sensor_align = 6;
|
||||
motor2.voltage_sensor_align = 6;
|
||||
driver1.voltage_power_supply = 8;
|
||||
driver2.voltage_power_supply = 8;
|
||||
driver1.init();
|
||||
driver2.init();
|
||||
|
||||
//连接motor对象与驱动器对象
|
||||
motor1.linkDriver(&driver1);
|
||||
motor2.linkDriver(&driver2);
|
||||
|
||||
motor1.torque_controller = TorqueControlType::voltage;
|
||||
motor2.torque_controller = TorqueControlType::voltage;
|
||||
motor1.controller = MotionControlType::torque;
|
||||
motor2.controller = MotionControlType::torque;
|
||||
|
||||
// monitor相关设置
|
||||
motor1.useMonitoring(Serial);
|
||||
motor2.useMonitoring(Serial);
|
||||
//电机初始化
|
||||
motor1.init();
|
||||
motor1.initFOC();
|
||||
motor2.init();
|
||||
motor2.initFOC();
|
||||
|
||||
// 映射电机到commander
|
||||
command.add('A', StabAngle, "pid angle");
|
||||
command.add('B', StabGyro, "pid gyro");
|
||||
command.add('C', StabDistance, "pid distance");
|
||||
command.add('D', StabSpeed, "pid speed");
|
||||
command.add('E', StabYawAngle, "pid yaw angle");
|
||||
command.add('F', StabYawGyro, "pid yaw gyro");
|
||||
command.add('G', lpfJoyy, "lpf joyy");
|
||||
command.add('H', StabLqrU, "pid lqr u");
|
||||
command.add('I', StabZeropoint, "pid zeropoint");
|
||||
command.add('J', lpfZeropoint, "lpf zeropoint");
|
||||
command.add('K', StabRollAngle, "pid roll angle");
|
||||
command.add('L', lpfRoll, "lpf roll");
|
||||
|
||||
//command.add('M', Stabtest_zeropoint, "test_zeropoint");
|
||||
|
||||
|
||||
|
||||
delay(500);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
bat_check(); //电压检测
|
||||
web_loop(); //Web数据更新
|
||||
mpu6050.update(); //IMU数据更新
|
||||
lqr_balance_loop(); //lqr自平衡控制
|
||||
yaw_loop(); //yaw轴转向控制
|
||||
leg_loop(); //腿部动作控制
|
||||
|
||||
//将自平衡计算输出转矩赋给电机
|
||||
motor1.target = (-0.5)*(LQR_u + YAW_output);
|
||||
motor2.target = (-0.5)*(LQR_u - YAW_output);
|
||||
|
||||
//倒地失控后关闭输出
|
||||
if( abs(LQR_angle) > 25.0f )
|
||||
{
|
||||
uncontrolable = 1;
|
||||
}
|
||||
if( uncontrolable != 0 )//扶起后延时恢复
|
||||
{
|
||||
if( abs(LQR_angle) < 10.0f )
|
||||
{
|
||||
uncontrolable++;
|
||||
}
|
||||
if( uncontrolable > 200 )//200次程序循环的延时时间
|
||||
{
|
||||
uncontrolable = 0;
|
||||
}
|
||||
}
|
||||
|
||||
//关停输出（遥控停止或角度过大失控）
|
||||
if(wrobot.go==0 || uncontrolable!=0)
|
||||
{
|
||||
motor1.target = 0;
|
||||
motor2.target = 0;
|
||||
leg_position_add = 0;
|
||||
}
|
||||
|
||||
//记录上一次的遥控数据数据
|
||||
wrobot.dir_last = wrobot.dir;
|
||||
wrobot.joyx_last = wrobot.joyx;
|
||||
wrobot.joyy_last = wrobot.joyy;
|
||||
|
||||
//迭代计算FOC相电压
|
||||
motor1.loopFOC();
|
||||
motor2.loopFOC();
|
||||
|
||||
//设置轮部电机输出
|
||||
motor1.move();
|
||||
motor2.move();
|
||||
|
||||
command.run();
|
||||
}
|
||||
|
||||
//lqr自平衡控制
|
||||
void lqr_balance_loop(){
|
||||
//LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试
|
||||
//QR_u = LQR_k1*(LQR_angle - angle_zeropoint) + LQR_k2*LQR_gyro + LQR_k3*(LQR_distance - distance_zeropoint) + LQR_k4*LQR_speed;
|
||||
|
||||
//给负值是因为按照当前的电机接线，正转矩会向后转
|
||||
LQR_distance = (-0.5) *(motor1.shaft_angle + motor2.shaft_angle);
|
||||
LQR_speed = (-0.5) *(motor1.shaft_velocity + motor2.shaft_velocity);
|
||||
LQR_angle = (float)mpu6050.getAngleY();
|
||||
LQR_gyro = (float)mpu6050.getGyroY();
|
||||
//Serial.println(LQR_distance);
|
||||
|
||||
//计算自平衡输出
|
||||
angle_control = pid_angle(LQR_angle - angle_zeropoint);
|
||||
gyro_control = pid_gyro(LQR_gyro);
|
||||
|
||||
//运动细节优化处理
|
||||
if(wrobot.joyy != 0)//有前后方向运动指令时的处理
|
||||
{
|
||||
distance_zeropoint = LQR_distance;//位移零点重置
|
||||
pid_lqr_u.error_prev = 0; //输出积分清零
|
||||
}
|
||||
|
||||
if( (wrobot.joyx_last!=0 && wrobot.joyx==0) || (wrobot.joyy_last!=0 && wrobot.joyy==0) )//运动指令复零时的原地停车处理
|
||||
{
|
||||
wrobot_move_stop_flag = 1;
|
||||
}
|
||||
if( (wrobot_move_stop_flag==1) && (abs(LQR_speed)<0.5) )
|
||||
{
|
||||
distance_zeropoint = LQR_distance;//位移零点重置
|
||||
wrobot_move_stop_flag = 0;
|
||||
}
|
||||
|
||||
if( abs(LQR_speed)>15 )//被快速推动时的原地停车处理
|
||||
{
|
||||
distance_zeropoint = LQR_distance;//位移零点重置
|
||||
}
|
||||
|
||||
//计算位移控制输出
|
||||
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
||||
speed_control = pid_speed(LQR_speed- 0.1*lpf_joyy(wrobot.joyy) );
|
||||
|
||||
//轮部离地检测
|
||||
robot_speed_last = robot_speed; //记录连续两次的轮部转速
|
||||
robot_speed = LQR_speed;
|
||||
if( abs(robot_speed-robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) //若轮部角速度、角加速度过大或处于跳跃后的恢复时期，认为出现轮部离地现象，需要特殊处理
|
||||
{
|
||||
distance_zeropoint = LQR_distance; //位移零点重置
|
||||
LQR_u = angle_control + gyro_control; //轮部离地情况下，对轮部分量不输出；反之，正常状态下完整输出平衡转矩
|
||||
pid_lqr_u.error_prev = 0; //输出积分清零
|
||||
}
|
||||
else
|
||||
{
|
||||
LQR_u = angle_control + gyro_control + distance_control + speed_control;
|
||||
}
|
||||
|
||||
//触发条件：遥控器无信号输入、轮部位移控制正常介入、不处于跳跃后的恢复时期
|
||||
if( abs(LQR_u)<5 && wrobot.joyy == 0 && abs(distance_control)<4 && (jump_flag == 0))
|
||||
{
|
||||
|
||||
LQR_u = pid_lqr_u(LQR_u);//补偿小转矩非线性
|
||||
//Serial.println(LQR_u);
|
||||
angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control));//重心自适应
|
||||
}
|
||||
else
|
||||
{
|
||||
pid_lqr_u.error_prev = 0; //输出积分清零
|
||||
}
|
||||
|
||||
//平衡控制参数自适应
|
||||
if(wrobot.height < 50)
|
||||
{
|
||||
pid_speed.P = 0.7;
|
||||
}
|
||||
else if(wrobot.height < 64)
|
||||
{
|
||||
pid_speed.P = 0.6;
|
||||
}
|
||||
else
|
||||
{
|
||||
pid_speed.P = 0.5;
|
||||
}
|
||||
}
|
||||
|
||||
//腿部动作控制
|
||||
void leg_loop(){
|
||||
jump_loop();
|
||||
if(jump_flag == 0)//不处于跳跃状态
|
||||
{
|
||||
//机身高度自适应控制
|
||||
ACC[0] = 8;
|
||||
ACC[1] = 8;
|
||||
Speed[0] = 200;
|
||||
Speed[1] = 200;
|
||||
float roll_angle = (float)mpu6050.getAngleX() + 2.0;
|
||||
//leg_position_add += pid_roll_angle(roll_angle);
|
||||
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));//test
|
||||
Position[0] = 2048 + 12 + 8.4*(wrobot.height-32) - leg_position_add;
|
||||
Position[1] = 2048 - 12 - 8.4*(wrobot.height-32) - leg_position_add;
|
||||
if( Position[0]<2110 )
|
||||
Position[0]=2110;
|
||||
else if( Position[0]>2510 )
|
||||
Position[0]=2510;
|
||||
if( Position[1]<1586 )
|
||||
Position[1]=1586;
|
||||
else if( Position[1]>1986 )
|
||||
Position[1]=1986;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
}
|
||||
|
||||
//跳跃控制
|
||||
void jump_loop(){
|
||||
if( (wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0) )
|
||||
{
|
||||
ACC[0] = 0;
|
||||
ACC[1] = 0;
|
||||
Speed[0] = 0;
|
||||
Speed[1] = 0;
|
||||
Position[0] = 2048 + 12 + 8.4*(80-32);
|
||||
Position[1] = 2048 - 12 - 8.4*(80-32);
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
jump_flag = 1;
|
||||
}
|
||||
if( jump_flag > 0 )
|
||||
{
|
||||
jump_flag++;
|
||||
if( (jump_flag > 30) && (jump_flag < 35) )
|
||||
{
|
||||
ACC[0] = 0;
|
||||
ACC[1] = 0;
|
||||
Speed[0] = 0;
|
||||
Speed[1] = 0;
|
||||
Position[0] = 2048 + 12 + 8.4*(40-32);
|
||||
Position[1] = 2048 - 12 - 8.4*(40-32);
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
jump_flag = 40;
|
||||
}
|
||||
if(jump_flag > 200)
|
||||
{
|
||||
jump_flag = 0;//已准备好再次跳跃
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//yaw轴转向控制
|
||||
void yaw_loop(){
|
||||
//YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro);
|
||||
yaw_angle_addup();
|
||||
|
||||
YAW_angle_total += wrobot.joyx*0.002;
|
||||
float yaw_angle_control = pid_yaw_angle(YAW_angle_total);
|
||||
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
|
||||
YAW_output = yaw_angle_control + yaw_gyro_control;
|
||||
}
|
||||
|
||||
//Web数据更新
|
||||
void web_loop(){
|
||||
webserver.handleClient();
|
||||
websocket.loop();
|
||||
rp.spinOnce();//更新web端回传的控制信息
|
||||
}
|
||||
|
||||
//yaw轴角度累加函数
|
||||
void yaw_angle_addup() {
|
||||
YAW_angle = (float)mpu6050.getAngleZ();;
|
||||
YAW_gyro = (float)mpu6050.getGyroZ();
|
||||
|
||||
if(YAW_angle_zero_point == (-10))
|
||||
{
|
||||
YAW_angle_zero_point = YAW_angle;
|
||||
}
|
||||
|
||||
float yaw_angle_1,yaw_angle_2,yaw_addup_angle;
|
||||
if(YAW_angle > YAW_angle_last)
|
||||
{
|
||||
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
||||
yaw_angle_2 = YAW_angle - YAW_angle_last - 2*PI;
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
||||
yaw_angle_2 = YAW_angle - YAW_angle_last + 2*PI;
|
||||
}
|
||||
|
||||
if(abs(yaw_angle_1)>abs(yaw_angle_2))
|
||||
{
|
||||
yaw_addup_angle=yaw_angle_2;
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw_addup_angle=yaw_angle_1;
|
||||
}
|
||||
|
||||
YAW_angle_total = YAW_angle_total + yaw_addup_angle;
|
||||
YAW_angle_last = YAW_angle;
|
||||
}
|
||||
|
||||
void basicWebCallback(void)
|
||||
{
|
||||
webserver.send(300, "text/html", basic_web);
|
||||
}
|
||||
|
||||
void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
|
||||
{
|
||||
if(type == WStype_TEXT)
|
||||
{
|
||||
String payload_str = String((char*) payload);
|
||||
StaticJsonDocument<300> doc;
|
||||
DeserializationError error = deserializeJson(doc, payload_str);
|
||||
|
||||
String mode_str = doc["mode"];
|
||||
if(mode_str == "basic")
|
||||
{
|
||||
rp.parseBasic(doc);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//电压检测初始化
|
||||
void adc_calibration_init()
|
||||
{
|
||||
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK) {
|
||||
printf("eFuse Two Point: Supported\n");
|
||||
} else {
|
||||
printf("eFuse Two Point: NOT supported\n");
|
||||
}
|
||||
//Check Vref is burned into eFuse
|
||||
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK) {
|
||||
printf("eFuse Vref: Supported\n");
|
||||
} else {
|
||||
printf("eFuse Vref: NOT supported\n");
|
||||
}
|
||||
}
|
||||
|
||||
//电压检测
|
||||
void bat_check()
|
||||
{
|
||||
if(bat_check_num > 1000)
|
||||
{
|
||||
//电压读取
|
||||
uint32_t sum = 0;
|
||||
sum= analogRead(BAT_PIN);
|
||||
uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars);
|
||||
double battery=(voltage*3.97)/1000.0;
|
||||
|
||||
Serial.println(battery);
|
||||
//电量显示
|
||||
if(battery>7.8)
|
||||
digitalWrite(LED_BAT,HIGH);
|
||||
else
|
||||
digitalWrite(LED_BAT,LOW);
|
||||
|
||||
bat_check_num = 0;
|
||||
}
|
||||
else
|
||||
bat_check_num++;
|
||||
}
|
||||
Reference in new issue
Block a user