优化云台跟踪效果
This commit is contained in:
1 parent
aa0c27d499
commit
1b12e76fdb
1860 files changed
+537
-480293
No files matched your search
@@ -0,0 +1,274 @@
|
||||
<!DOCTYPE html>
|
||||
<html>
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||
<title>Wrobot</title>
|
||||
<link rel="stylesheet" href="{{url_for('static', filename='bootstrap/dist/css/bootstrap.min.css')}}">
|
||||
<link rel="stylesheet" href="{{url_for('static', filename='css/main.min.css')}}">
|
||||
<link rel="stylesheet" href="{{url_for('static', filename='font-awesome-4.7.0/css/font-awesome.min.css')}}">
|
||||
<script src="{{url_for('static', filename='js/jquery-3.6.0.min.js')}}"></script>
|
||||
<script src="{{url_for('static', filename='bootstrap/dist/js/bootstrap.bundle.min.js')}}"></script>
|
||||
<style>
|
||||
#nipple-log{height:auto;min-height:50px;max-height:200px;overflow-y:auto;border:1px solid #ddd;padding:5px}
|
||||
.joystick-container{width:300px;height:300px;margin:0 auto;border:2px solid #ddd;border-radius:2px;position:relative;background-color:#f8f9fa;cursor:grab;touch-action:none;user-select:none}
|
||||
.joystick-container:active{cursor:grabbing}
|
||||
.joystick-container::before{content:'';position:absolute;left:10%;top:50%;width:80%;height:1px;background-color:#ddd;transform:translateY(-50%);z-index:1}
|
||||
.joystick-container::after{content:'';position:absolute;left:50%;top:10%;width:1px;height:80%;background-color:#ddd;transform:translateX(-50%);z-index:1}
|
||||
.joystick-btn{width:50px;height:50px;background-color:#0d6efd;border-radius:50%;position:absolute;left:50%;top:50%;transform:translate(-50%,-50%);box-shadow:0 2px 8px rgba(0,0,0,.3);transition:background-color .2s;will-change:transform;z-index:2}
|
||||
.joystick-btn:hover{background-color:#0b5ed7}
|
||||
.joystick-btn.dragging{background-color:#0a58ca;box-shadow:0 4px 12px rgba(0,0,0,.4)}
|
||||
.angle-mark{position:absolute;color:#666;font-size:12px;font-weight:500;z-index:1}
|
||||
.mark-x-0{left:5px;top:50%;transform:translateY(-50%)}
|
||||
.mark-x-180{right:5px;top:50%;transform:translateY(-50%)}
|
||||
.mark-y-0{top:5px;left:50%;transform:translateX(-50%)}
|
||||
.mark-y-180{bottom:5px;left:50%;transform:translateX(-50%)}
|
||||
.angle-display{text-align:center;margin-top:15px;font-size:16px;color:#333;font-weight:500}
|
||||
.angle-item{margin:5px 0}
|
||||
#head_video{background-color:#f1f1f1;transition:opacity .1s ease}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<div class="container h-100">
|
||||
<!-- 设备&控制栏 -->
|
||||
<div class="row gx-3 my-3">
|
||||
<div class="col-12 d-flex align-items-center gap-3">
|
||||
<span class="text-secondary d-flex">设备ID:</span>
|
||||
<select class="form-select form-select-sm h3 blue1 w-auto " id="device_id">
|
||||
{% for k in device_list %}
|
||||
<option value="{{k}}" {{ 'selected' if k == device_id else '' }}>{{k}}</option>
|
||||
{% endfor %}
|
||||
</select>
|
||||
|
||||
<span class="text-secondary d-flex">跟踪:</span>
|
||||
<select class="form-select form-select-sm h3 blue1 w-auto" id="follow_flag">
|
||||
{% for k,v in follow_list.items() %}
|
||||
<option value="{{v}}" {{ 'selected' if k == follow_id else '' }}>{{k}}</option>
|
||||
{% endfor %}
|
||||
</select>
|
||||
|
||||
<button id="reset_btn" class="btn btn-outline-primary btn-sm">还原姿态</button>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- 摄像头+摇杆布局 -->
|
||||
<div class="row gx-3 justify-content-center align-items-center">
|
||||
<div class="col-md-6">
|
||||
<!-- 核心优化:直接指向实时流地址,浏览器自动刷新 -->
|
||||
<img class="img-fluid" id="head_video" src="/wrobot_cam/{{device_id}}" alt="摄像头视频" style="width:100%;height:auto;">
|
||||
</div>
|
||||
<div class="col-md-3">
|
||||
<div class="joystick-container" id="joystickContainer">
|
||||
<span class="angle-mark mark-x-0">180°</span>
|
||||
<span class="angle-mark mark-x-180">0°</span>
|
||||
<span class="angle-mark mark-y-0">180°</span>
|
||||
<span class="angle-mark mark-y-180">0°</span>
|
||||
<div class="joystick-btn" id="joystickBtn"></div>
|
||||
</div>
|
||||
<!-- 舵机角度显示 -->
|
||||
<div class="angle-display">
|
||||
<div class="angle-item">S1舵机(X轴):<span id="s1Angle">90</span>°</div>
|
||||
<div class="angle-item">S2舵机(Y轴):<span id="s2Angle">90</span>°</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- 日志框 -->
|
||||
<div class="d-flex gap-2 my-3">
|
||||
<div class="small w-100" id="nipple-log"></div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<script>
|
||||
// ===================== 全局配置 =====================
|
||||
const AJAX_SEND_INTERVAL = 100; // AJAX节流间隔(ms)
|
||||
const angleUpdateInterval = 500; // 角度显示节流间隔(ms)
|
||||
let isDragging = false;
|
||||
let lastSendTime = 0;
|
||||
let lastAngleUpdateTime = 0;
|
||||
let hasSetTrackOff = false; // 新增:标记是否已设置跟踪为关闭,避免重复触发
|
||||
|
||||
// ===================== DOM元素缓存 =====================
|
||||
const $deviceId = $('#device_id');
|
||||
const $followFlag = $('#follow_flag');
|
||||
const $resetBtn = $('#reset_btn');
|
||||
const $headVideo = $('#head_video');
|
||||
const $joystickContainer = $('#joystickContainer');
|
||||
const $joystickBtn = $('#joystickBtn');
|
||||
const $s1Angle = $('#s1Angle');
|
||||
const $s2Angle = $('#s2Angle');
|
||||
const $nippleLog = $('#nipple-log');
|
||||
|
||||
// 摇杆按钮尺寸缓存
|
||||
const btnHalfWidth = $joystickBtn.width() / 2;
|
||||
const btnHalfHeight = $joystickBtn.height() / 2;
|
||||
|
||||
// ===================== AJAX核心函数(所有交互) =====================
|
||||
function sendActionTxt(device_id, action_text) {
|
||||
const now = Date.now();
|
||||
if (now - lastSendTime < AJAX_SEND_INTERVAL) return false;
|
||||
lastSendTime = now;
|
||||
$.post("/wrobot/api/SendActionTxt", {
|
||||
device_id: device_id,
|
||||
action_text: action_text,
|
||||
ignoreMe: new Date().getTime()
|
||||
}, function (data, textstatus) {
|
||||
if (data['ret'] != 0) {
|
||||
return false;
|
||||
}
|
||||
addLogToNippleLog(data);
|
||||
})
|
||||
}
|
||||
|
||||
function sendVoiceTxt(device_id, voice_text) {
|
||||
$.post("/wrobot/api/SendVoiceTxt", {
|
||||
device_id: device_id,
|
||||
voice_text: voice_text,
|
||||
ignoreMe: new Date().getTime()
|
||||
}, function (data, textstatus) {
|
||||
if (data['ret'] != 0) {
|
||||
return false;
|
||||
}
|
||||
addLogToNippleLog(data);
|
||||
})
|
||||
|
||||
}
|
||||
|
||||
function setFollowFlag(device_id, follow_flag) {
|
||||
$.post("/wrobot/api/SetFollowFlag", {
|
||||
device_id: device_id,
|
||||
follow_flag: follow_flag,
|
||||
ignoreMe: new Date().getTime()
|
||||
}, function (data, textstatus) {
|
||||
if (data['ret'] != 0) {
|
||||
return false;
|
||||
}
|
||||
addLogToNippleLog(data);
|
||||
})
|
||||
}
|
||||
|
||||
// ===================== 摇杆控制逻辑 =====================
|
||||
function resetJoystick() {
|
||||
// 复位摇杆位置
|
||||
$joystickBtn.removeClass('dragging').css('transform', 'translate(-50%, -50%)');
|
||||
const resetAngle = 90;
|
||||
$s1Angle.text(resetAngle);
|
||||
$s2Angle.text(resetAngle);
|
||||
|
||||
// 发送复位指令
|
||||
const deviceId = $deviceId.val();
|
||||
sendActionTxt(deviceId, `S1:${resetAngle},S2:${resetAngle};`);
|
||||
setFollowFlag(deviceId, 'none');
|
||||
|
||||
addLogToNippleLog({ type: 'info', msg: '摇杆复位,舵机角度重置为90°' });
|
||||
}
|
||||
|
||||
// 摇杆按下
|
||||
$joystickBtn.on('mousedown touchstart', function(e) {
|
||||
e.preventDefault();
|
||||
isDragging = true;
|
||||
$joystickBtn.addClass('dragging');
|
||||
});
|
||||
|
||||
// 摇杆移动
|
||||
$joystickContainer.on('mousemove touchmove', function(e) {
|
||||
if (!isDragging) return;
|
||||
|
||||
// 适配触摸/鼠标事件
|
||||
const event = e.type.includes('touch') ? e.touches[0] : e;
|
||||
const clientX = event.clientX;
|
||||
const clientY = event.clientY;
|
||||
|
||||
// 计算容器位置和偏移
|
||||
const containerRect = $joystickContainer[0].getBoundingClientRect();
|
||||
const containerWidth = containerRect.width;
|
||||
const containerHeight = containerRect.height;
|
||||
const containerHalfWidth = containerWidth / 2;
|
||||
const containerHalfHeight = containerHeight / 2;
|
||||
|
||||
const offsetX = clientX - (containerRect.left + containerHalfWidth);
|
||||
const offsetY = clientY - (containerRect.top + containerHalfHeight);
|
||||
|
||||
// 边界限制
|
||||
const maxOffsetX = containerHalfWidth - btnHalfWidth;
|
||||
const maxOffsetY = containerHalfHeight - btnHalfHeight;
|
||||
const clampedOffsetX = Math.max(-maxOffsetX, Math.min(offsetX, maxOffsetX));
|
||||
const clampedOffsetY = Math.max(-maxOffsetY, Math.min(offsetY, maxOffsetY));
|
||||
|
||||
// 更新摇杆位置
|
||||
$joystickBtn.css('transform', `translate(calc(-50% + ${clampedOffsetX}px), calc(-50% + ${clampedOffsetY}px))`);
|
||||
|
||||
// 计算舵机角度
|
||||
const s1Angle = Math.round(180 - ((clampedOffsetX + maxOffsetX) / (2 * maxOffsetX)) * 180);
|
||||
const s2Angle = Math.round(180 - ((clampedOffsetY + maxOffsetY) / (2 * maxOffsetY)) * 180);
|
||||
|
||||
// 新增核心逻辑:拖动摇杆时自动切换跟踪模式为关闭(track_off)
|
||||
if (!hasSetTrackOff) {
|
||||
const trackOffVal = 'track_off'; // 请确认该值与follow_list中「关闭跟踪」对应的value一致
|
||||
// 如果当前不是关闭跟踪模式,则切换并触发change事件
|
||||
if ($followFlag.val() !== trackOffVal) {
|
||||
$followFlag.val(trackOffVal).trigger('change');
|
||||
addLogToNippleLog({ type: 'info', msg: '拖动摇杆,自动关闭跟踪模式' });
|
||||
}
|
||||
hasSetTrackOff = true; // 标记已设置,避免重复触发
|
||||
}
|
||||
|
||||
// 节流更新角度和发送指令
|
||||
const now = Date.now();
|
||||
if (now - lastAngleUpdateTime > angleUpdateInterval) {
|
||||
lastAngleUpdateTime = now;
|
||||
$s1Angle.text(s1Angle);
|
||||
$s2Angle.text(s2Angle);
|
||||
sendActionTxt($deviceId.val(), `S1:${s1Angle},S2:${s2Angle};`);
|
||||
}
|
||||
});
|
||||
|
||||
// 摇杆松开/离开
|
||||
$joystickContainer.on('mouseup mouseleave touchend touchcancel', function() {
|
||||
if (isDragging) {
|
||||
isDragging = false;
|
||||
$joystickBtn.removeClass('dragging');
|
||||
hasSetTrackOff = false; // 重置标记,下次拖动可再次触发
|
||||
// 如需松开复位,取消注释下面这行
|
||||
// resetJoystick();
|
||||
}
|
||||
});
|
||||
|
||||
// ===================== 日志辅助函数 =====================
|
||||
function addLogToNippleLog(logData) {
|
||||
const timeStr = new Date().toLocaleTimeString();
|
||||
const logHtml = `
|
||||
<div class="log-item ${logData.type}">
|
||||
<small>[${timeStr}]</small> ${logData.msg}
|
||||
${logData.data ? '<br>' + JSON.stringify(logData.data, null, 2) : ''}
|
||||
</div>
|
||||
`;
|
||||
$nippleLog.prepend(logHtml);
|
||||
|
||||
// 最多保留20条日志,避免性能问题
|
||||
if ($nippleLog.children().length > 20) {
|
||||
$nippleLog.children().last().remove();
|
||||
}
|
||||
$nippleLog.scrollTop(0);
|
||||
}
|
||||
|
||||
// ===================== 事件绑定 =====================
|
||||
$(function() {
|
||||
// 复位按钮
|
||||
$resetBtn.on('click', resetJoystick);
|
||||
|
||||
// 跟踪模式变更
|
||||
$followFlag.on('change', function() {
|
||||
setFollowFlag($deviceId.val(), $(this).val());
|
||||
});
|
||||
|
||||
// 设备ID变更时更新图像流地址
|
||||
$deviceId.on('change', function() {
|
||||
const deviceId = $(this).val();
|
||||
$headVideo.attr('src', `/wrobot_cam/${deviceId}`);
|
||||
});
|
||||
|
||||
});
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,28 @@
|
||||
from flask import Blueprint,session,render_template
|
||||
from app.glo import *
|
||||
|
||||
|
||||
wrobot = Blueprint('wrobot', __name__)
|
||||
|
||||
@wrobot.route('/wrobot/')
|
||||
def main():
|
||||
|
||||
device_list = GetUserDeviceIdList(session['user']['user_id'])
|
||||
|
||||
follow_list = {
|
||||
'face': 'track_face',
|
||||
# 'hand': 'track_hand',
|
||||
'none': 'track_off',
|
||||
}
|
||||
data = {
|
||||
"page_title": "%s" % ( "Wrobot") ,
|
||||
|
||||
"device_id": device_list[0],
|
||||
|
||||
"follow_id": 'face',
|
||||
|
||||
"device_list": device_list,
|
||||
"follow_list": follow_list,
|
||||
}
|
||||
|
||||
return render_template('/cn/aiot/wrobot.html', **data)
|
||||
@@ -0,0 +1,145 @@
|
||||
import threading
|
||||
import time
|
||||
import re
|
||||
import traceback
|
||||
import paho.mqtt.client as mqtt
|
||||
from flask import Blueprint, Response, session
|
||||
|
||||
from app.glo import *
|
||||
|
||||
# ==================== 全局配置 ====================
|
||||
mqtt_client = None
|
||||
MQTT_SUB_TOPIC = ""
|
||||
latest_frame_cache = {}
|
||||
cache_lock = threading.Lock()
|
||||
mqtt_sub_lock = threading.Lock()
|
||||
|
||||
wrobot_cam = Blueprint('wrobot_cam', __name__)
|
||||
|
||||
|
||||
# ==================== MQTT 核心逻辑 ====================
|
||||
def on_mqtt_connect(client, userdata, flags, rc):
|
||||
"""MQTT连接成功回调:此时暂不订阅(等第一次页面访问时再订阅)"""
|
||||
if rc == 0:
|
||||
print("MQTT连接成功(暂未订阅任何主题,等待页面访问)")
|
||||
else:
|
||||
print(f"MQTT连接失败 错误码: {rc}")
|
||||
|
||||
def on_mqtt_message(client, userdata, message):
|
||||
"""MQTT消息回调:仅处理已订阅的主题"""
|
||||
try:
|
||||
if not MQTT_SUB_TOPIC or message.topic != MQTT_SUB_TOPIC:
|
||||
return
|
||||
|
||||
device_id = message.topic.split('/')[0]
|
||||
if len(message.payload) < 100:
|
||||
return
|
||||
|
||||
with cache_lock:
|
||||
latest_frame_cache[device_id] = message.payload
|
||||
|
||||
except Exception as e:
|
||||
print(f"MQTT消息处理异常: {e}")
|
||||
traceback.print_exc()
|
||||
|
||||
def start_mqtt_background():
|
||||
"""启动MQTT客户端(仅连接,不订阅)"""
|
||||
global mqtt_client
|
||||
if mqtt_client is not None:
|
||||
return
|
||||
|
||||
mqtt_client = mqtt.Client()
|
||||
mqtt_client.on_connect = on_mqtt_connect
|
||||
mqtt_client.on_message = on_mqtt_message
|
||||
|
||||
try:
|
||||
mqtt_client.connect(MQTT_BROKER, MQTT_PORT, keepalive=60)
|
||||
mqtt_client.loop_start()
|
||||
print(f"MQTT客户端已启动 🚀(仅连接,未订阅主题)")
|
||||
except Exception as e:
|
||||
print(f"MQTT启动失败: {e}")
|
||||
traceback.print_exc()
|
||||
|
||||
def subscribe_first_topic(new_topic):
|
||||
"""第一次订阅主题(初始化订阅)"""
|
||||
global MQTT_SUB_TOPIC
|
||||
if not mqtt_client or not mqtt_client.is_connected():
|
||||
raise RuntimeError("MQTT客户端未连接")
|
||||
|
||||
with mqtt_sub_lock:
|
||||
MQTT_SUB_TOPIC = new_topic
|
||||
mqtt_client.subscribe(MQTT_SUB_TOPIC)
|
||||
print(f"MQTT首次订阅: {MQTT_SUB_TOPIC}")
|
||||
|
||||
def update_mqtt_subscription(new_topic):
|
||||
"""更新订阅主题(非首次)"""
|
||||
global MQTT_SUB_TOPIC
|
||||
if not mqtt_client or not mqtt_client.is_connected():
|
||||
raise RuntimeError("MQTT客户端未连接")
|
||||
|
||||
with mqtt_sub_lock:
|
||||
# 取消旧主题订阅
|
||||
mqtt_client.unsubscribe(MQTT_SUB_TOPIC)
|
||||
print(f"MQTT取消订阅: {MQTT_SUB_TOPIC}")
|
||||
|
||||
# 订阅新主题
|
||||
MQTT_SUB_TOPIC = new_topic
|
||||
mqtt_client.subscribe(MQTT_SUB_TOPIC)
|
||||
print(f"MQTT重新订阅: {MQTT_SUB_TOPIC}")
|
||||
|
||||
# ==================== Flask 路由核心逻辑 ====================
|
||||
def generate_video_frame(device_id):
|
||||
"""生成视频流"""
|
||||
blank_jpeg = b'\xff\xd8\xff\xe0\x00\x10JFIF\x00\x01\x01\x00\x00\x01\x00\x01\x00\x00\xff\xd9'
|
||||
|
||||
while True:
|
||||
try:
|
||||
with cache_lock:
|
||||
frame_data = latest_frame_cache.get(device_id, blank_jpeg)
|
||||
|
||||
yield (b'--frame\r\n'
|
||||
b'Content-Type: image/jpeg\r\n\r\n' + frame_data + b'\r\n')
|
||||
|
||||
time.sleep(0.01)
|
||||
|
||||
except GeneratorExit:
|
||||
break
|
||||
except Exception as e:
|
||||
print(f"视频流生成异常: {e}")
|
||||
yield (b'--frame\r\n'
|
||||
b'Content-Type: image/jpeg\r\n\r\n' + blank_jpeg + b'\r\n')
|
||||
time.sleep(0.1)
|
||||
|
||||
@wrobot_cam.route('/wrobot_cam/<string:device_id>')
|
||||
@wrobot_cam.route('/wrobot_cam')
|
||||
def main(device_id=None):
|
||||
"""核心逻辑:第一次访问时初始化订阅,后续访问时更新订阅"""
|
||||
# 1. 校验设备ID(无参数时直接报错,因为启动时未知默认设备)
|
||||
if not device_id:
|
||||
return Response("必须指定设备ID(访问方式:/wrobot_cam/设备ID)", status=400, mimetype='text/plain')
|
||||
|
||||
device_list = GetUserDeviceIdList(session['user']['user_id'])
|
||||
if device_id not in device_list:
|
||||
return Response(f"设备ID不合法", status=400, mimetype='text/plain')
|
||||
|
||||
|
||||
# 2. 构建真实主题
|
||||
new_mqtt_topic = f"{device_id}/head_video"
|
||||
|
||||
# 3. 区分「首次订阅」和「更新订阅」
|
||||
try:
|
||||
if not MQTT_SUB_TOPIC: # 初始值为空 → 第一次访问,初始化订阅
|
||||
threading.Thread(target=subscribe_first_topic, args=(new_mqtt_topic,), daemon=True).start()
|
||||
elif new_mqtt_topic != MQTT_SUB_TOPIC: # 非首次,更新订阅
|
||||
threading.Thread(target=update_mqtt_subscription, args=(new_mqtt_topic,), daemon=True).start()
|
||||
except RuntimeError as e:
|
||||
return Response(f"MQTT客户端异常:{str(e)}", status=500, mimetype='text/plain')
|
||||
|
||||
# 4. 返回视频流
|
||||
return Response(
|
||||
generate_video_frame(device_id),
|
||||
mimetype='multipart/x-mixed-replace; boundary=frame'
|
||||
)
|
||||
|
||||
# ==================== 启动初始化 ====================
|
||||
start_mqtt_background()
|
||||
@@ -43,9 +43,9 @@ void parseServoCommand(char* cmd) {
|
||||
int angle1 = atoi(s1Ptr + 3); // "S1:"占3个字符,跳过它们取数值
|
||||
int angle2 = atoi(s2Ptr + 3); // "S2:"占3个字符,跳过它们取数值
|
||||
|
||||
// 调用main.cpp中的舵机控制函数
|
||||
// Serial.println("angle1:" + String(angle1) + ", angle2:" + String(angle2));
|
||||
setServoAngles(angle1, angle2);
|
||||
// 调用摄像头舵机控制函数
|
||||
setCameraServoAngles(angle1, angle2);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -39,6 +39,8 @@ void parseSingleParam(char* cmd);
|
||||
|
||||
// 舵机控制函数声明
|
||||
void parseServoCommand(char* cmd);
|
||||
void setServoAngles(int s1, int s2);
|
||||
|
||||
void setCameraServoAngles(int s1, int s2);
|
||||
void setFrontierServoAngles(int servo_angle);
|
||||
|
||||
#endif // SERIAL_PARSER_H
|
||||
+83
-66
@@ -271,17 +271,22 @@ typedef struct
|
||||
Wrobot wrobot;
|
||||
|
||||
// 云台舵机控制引脚
|
||||
#define SERVO1_PIN 15 // 底部舵机 S1
|
||||
#define SERVO2_PIN 4 // 摄像头舵机 S2
|
||||
#define FRONTIER_SERVO_PIN 36 // 前档舵机 S1 4
|
||||
#define CAM_BOTTOM_SERVO_PIN 15 // 云台底座 S1 15
|
||||
#define CAM_FIX_SERVO_PIN 4 // 固定摄像头舵机 S2 4
|
||||
|
||||
// 创建两个舵机对象
|
||||
Servo servo1;
|
||||
Servo servo2;
|
||||
// 创建舵机对象
|
||||
Servo frontier_servo;
|
||||
Servo cam_bottom_servo;
|
||||
Servo cam_fix_servo;
|
||||
|
||||
// 舵机角度变量
|
||||
int servo1_angle = 90; // 云台底座舵机初始角度
|
||||
int servo2_angle = 90; // 摄像头旋转舵机初始角度
|
||||
int frontier_servo_angle = 0; // 前档舵机
|
||||
|
||||
int cam_bottom_servo_angle = 90; // 云台底座舵机初始角度 左右旋转 S1
|
||||
int cam_fix_servo_angle = 90; // 摄像头舵机初始角度 上下旋转 S2
|
||||
|
||||
// (暂时无用)
|
||||
float servo1_joyx_factor = 0.1f; // 舵机1 云台底座舵机 旋转 系数,水平的,所以是x
|
||||
float servo2_joyy_factor = 0.5f; // 舵机2 摄像头舵机 旋转 系数,上下的,所以是y
|
||||
|
||||
@@ -376,12 +381,17 @@ void setup()
|
||||
xboxControllerInit();
|
||||
|
||||
// 初始化云台两个舵机
|
||||
servo1.attach(SERVO1_PIN);
|
||||
servo2.attach(SERVO2_PIN);
|
||||
cam_bottom_servo.attach(CAM_BOTTOM_SERVO_PIN);
|
||||
cam_fix_servo.attach(CAM_FIX_SERVO_PIN);
|
||||
|
||||
// 初始位置:两个舵机都转到90度
|
||||
servo1.write(90); // 正面看90度正面,0度右手,180度左手
|
||||
servo2.write(90); // 摄像头朝上180度,朝下30度
|
||||
cam_bottom_servo.write(cam_bottom_servo_angle); // 正面看90度正面,0度右手,180度左手
|
||||
cam_fix_servo.write(cam_fix_servo_angle); // 摄像头朝上180度,朝下30度
|
||||
|
||||
// 前档舵机
|
||||
frontier_servo.attach(FRONTIER_SERVO_PIN);
|
||||
frontier_servo.write(frontier_servo_angle); // 摄像头朝上180度,朝下30度
|
||||
|
||||
delay(100);
|
||||
}
|
||||
|
||||
@@ -1093,7 +1103,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
}
|
||||
|
||||
// 左右转向 左摇杆X轴:先映射到-100~100,再按比例放大到-100~100
|
||||
if(!dragon_head_mode){ // 双手控制模式
|
||||
if(!dragon_head_mode){ // 不是龙头模式,左摇杆控制旋转
|
||||
convertedJ1PotX = constrain(
|
||||
map(remoteData.joyLHori, 0, 65535, -100, 100),
|
||||
-100, 100
|
||||
@@ -1107,7 +1117,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
wrobot.joyx = 0;
|
||||
}
|
||||
|
||||
}else{ // 龙头模式
|
||||
}else{ // 龙头模式 右摇杆控制旋转,左摇杆不控制旋转
|
||||
convertedJ2PotX = constrain(
|
||||
map(remoteData.joyRHori, 0, 65535, -100, 100),
|
||||
-100, 100
|
||||
@@ -1125,17 +1135,18 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
last_convertedJ1PotX = convertedJ1PotX;
|
||||
last_convertedJ2PotX = convertedJ2PotX;
|
||||
|
||||
// 控制摄像头舵机S2
|
||||
convertedJ1PotY = constrain(
|
||||
map(remoteData.joyLVert, 0, 65535, -100, 100),
|
||||
-100, 100
|
||||
);
|
||||
if(abs(convertedJ1PotY) > deadZone) { // 上下运动 -100 到 100
|
||||
servo2_angle = map( convertedJ1PotY, -100, 100, 160, 20);
|
||||
servo2_angle = constrain(servo2_angle, 0, 180);
|
||||
servo2.write(servo2_angle);
|
||||
if(dragon_head_mode){ // 龙头模式,左摇杆控制云台
|
||||
cam_bottom_servo_angle = constrain( // 左右
|
||||
map(remoteData.joyLHori, 0, 65535, 0, 180),
|
||||
0, 180
|
||||
);
|
||||
cam_fix_servo_angle = constrain(
|
||||
map(remoteData.joyLVert, 0, 65535, 180, 0),
|
||||
0, 180
|
||||
);
|
||||
setCameraServoAngles(cam_bottom_servo_angle, cam_fix_servo_angle);
|
||||
}
|
||||
|
||||
|
||||
// 智能计算wrobot.height ,根据前后速度,
|
||||
// convertedJ2PotY 范围-100到100,默认0,输出范围30(最矮)-50(最高), 默认 50
|
||||
// wrobot.height 0 最矮,50最高
|
||||
@@ -1146,55 +1157,46 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
// // 右摇摆
|
||||
if(remoteData.btnDirRight) {
|
||||
// roll_adjust += 0.20;
|
||||
|
||||
isExecutingAction = true; // 标记开始执行动作
|
||||
currentAction = &action_step_right; // 指定执行的动作
|
||||
stepStartTime = millis(); // 记录第一步开始时间
|
||||
currentStepIndex = 0; // 从第一步开始
|
||||
pitch_adjust += 0.20;
|
||||
// isExecutingAction = true; // 标记开始执行动作
|
||||
// currentAction = &action_step_right; // 指定执行的动作
|
||||
// stepStartTime = millis(); // 记录第一步开始时间
|
||||
// currentStepIndex = 0; // 从第一步开始
|
||||
|
||||
}
|
||||
// 左摇摆
|
||||
if(remoteData.btnDirLeft) {
|
||||
// roll_adjust -= 0.20;
|
||||
isExecutingAction = true; // 标记开始执行动作
|
||||
currentAction = &action_step_left; // 指定执行的动作
|
||||
stepStartTime = millis(); // 记录第一步开始时间
|
||||
currentStepIndex = 0; // 从第一步开始
|
||||
pitch_adjust -= 0.20;
|
||||
// isExecutingAction = true; // 标记开始执行动作
|
||||
// currentAction = &action_step_left; // 指定执行的动作
|
||||
// stepStartTime = millis(); // 记录第一步开始时间
|
||||
// currentStepIndex = 0; // 从第一步开始
|
||||
}
|
||||
|
||||
// 前倾
|
||||
// if(remoteData.btnDirRight) {
|
||||
// pitch_adjust += 0.20;
|
||||
// }
|
||||
// 后仰
|
||||
// if(remoteData.btnDirLeft) {
|
||||
// pitch_adjust -= 0.20;
|
||||
// }
|
||||
|
||||
|
||||
// 抬升
|
||||
if(remoteData.btnDirUp) {
|
||||
// leg_height_base -= 0.20;
|
||||
leg_height_base -= 0.20;
|
||||
// Serial.print("leg_height_base");
|
||||
// Serial.println(leg_height_base);
|
||||
|
||||
isExecutingAction = true; // 标记开始执行动作
|
||||
currentAction = &action_step_up; // 指定执行的动作
|
||||
stepStartTime = millis(); // 记录第一步开始时间
|
||||
currentStepIndex = 0; // 从第一步开始
|
||||
// isExecutingAction = true; // 标记开始执行动作
|
||||
// currentAction = &action_step_up; // 指定执行的动作
|
||||
// stepStartTime = millis(); // 记录第一步开始时间
|
||||
// currentStepIndex = 0; // 从第一步开始
|
||||
|
||||
}
|
||||
|
||||
// 下降
|
||||
if(remoteData.btnDirDown) {
|
||||
// leg_height_base += 0.20;
|
||||
leg_height_base += 0.20;
|
||||
// Serial.print("leg_height_base");
|
||||
// Serial.println(leg_height_base);
|
||||
|
||||
isExecutingAction = true; // 标记开始执行动作
|
||||
currentAction = &action_step_down; // 指定执行的动作
|
||||
stepStartTime = millis(); // 记录第一步开始时间
|
||||
currentStepIndex = 0; // 从第一步开始
|
||||
// isExecutingAction = true; // 标记开始执行动作
|
||||
// currentAction = &action_step_down; // 指定执行的动作
|
||||
// stepStartTime = millis(); // 记录第一步开始时间
|
||||
// currentStepIndex = 0; // 从第一步开始
|
||||
}
|
||||
|
||||
// 还原
|
||||
@@ -1213,7 +1215,17 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
|
||||
// 左扳机 rgb led 上键 闪烁6次:红红黄黄绿绿
|
||||
if(remoteData.trigLT && !isBlinking){
|
||||
startColorSequenceBlink();
|
||||
startColorSequenceBlink();
|
||||
}
|
||||
|
||||
// 控制前档舵机
|
||||
if(remoteData.trigLT){
|
||||
frontier_servo_angle = map(remoteData.trigLT, 0, 1023, 0, 180);
|
||||
setFrontierServoAngles(frontier_servo_angle);
|
||||
}
|
||||
if(!remoteData.trigLT){
|
||||
frontier_servo_angle = 0;
|
||||
setFrontierServoAngles(frontier_servo_angle);
|
||||
}
|
||||
|
||||
// 跳跃 右摇杆按键
|
||||
@@ -1533,21 +1545,26 @@ void obstacle_cancel()
|
||||
obstacle_mode = false; // 跳出越障模式
|
||||
}
|
||||
|
||||
// 添加舵机控制函数
|
||||
void setServoAngles(int s1, int s2) {
|
||||
// 控制摄像头舵机
|
||||
void setCameraServoAngles(int s1, int s2) {
|
||||
// 限制角度在0-180度范围内
|
||||
servo1_angle = constrain(s1, 0, 180);
|
||||
servo2_angle = constrain(s2, 0, 180);
|
||||
cam_bottom_servo_angle = constrain(s1, 0, 180);
|
||||
cam_fix_servo_angle = constrain(s2, 0, 180);
|
||||
|
||||
// 控制舵机转动
|
||||
servo1.write(servo1_angle);
|
||||
servo2.write(servo2_angle);
|
||||
cam_bottom_servo.write(cam_bottom_servo_angle);
|
||||
cam_fix_servo.write(cam_fix_servo_angle);
|
||||
|
||||
// 反馈执行结果
|
||||
Serial.print("执行成功:");
|
||||
Serial.print("舵机1转到 ");
|
||||
Serial.print(servo1_angle);
|
||||
Serial.print("度,舵机2转到 ");
|
||||
Serial.print(servo2_angle);
|
||||
Serial.println("度");
|
||||
}
|
||||
// Serial.print("舵机1转到 ");
|
||||
// Serial.print(cam_bottom_servo_angle);
|
||||
// Serial.print("度,舵机2转到 ");
|
||||
// Serial.print(cam_fix_servo_angle);
|
||||
// Serial.println("度");
|
||||
}
|
||||
|
||||
// 控制前档舵机
|
||||
void setFrontierServoAngles(int servo_angle) {
|
||||
servo_angle = constrain(servo_angle, 0, 180);
|
||||
frontier_servo.write(frontier_servo_angle);
|
||||
}
|
||||
Reference in new issue
Block a user