Skip to content

Instantly share code, notes, and snippets.

@CarlosDanielDev

CarlosDanielDev/core-little-claw.ino

Created February 14, 2024 15:08
Show Gist options
  • Save CarlosDanielDev/34b071eeec5e62dd2f04cdfa5b36c4e6 to your computer and use it in GitHub Desktop.
Save CarlosDanielDev/34b071eeec5e62dd2f04cdfa5b36c4e6 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
core-little-claw.ino
#include <WiFi.h>
#include <AsyncTCP.h> // https://github.com/me-no-dev/AsyncTCP
#include <ESPAsyncWebServer.h> // https://github.com/me-no-dev/ESPAsyncWebServer
#include <ArduinoJson.h> // https://arduinojson.org/
#include <RoboCore_Vespa.h>
struct servo_angles {
uint8_t min;
uint8_t max;
uint8_t current;
};
AsyncWebServer server(80);
AsyncWebSocket ws("/ws");
const uint8_t PIN_LED = 15;
const uint8_t CLAW = 0;
const uint8_t RIGHT_SERVO = 1;
const uint8_t LEFT_SERVO = 2;
const uint8_t BASE_SERVO = 3;
const char *ALIAS_MOTOR_ANGLE = "angulo";
const char *ALIAS_ANGULO = "posicao";
const char *ALIAS_SERVO = "servo";
const char *ALIAS_VBAT = "vbat";
const char *ALIAS_VELOCIDADE = "velocidade";
VespaMotors motores;
VespaServo servos[4];
const uint16_t SERVO_MAX = 2500;
const uint16_t SERVO_MIN = 500;
enum Motor {
Base = 0,
Alcance,
Elevacao,
Garra
};
servo_angles serv_angles[4] = {
{ 0, 180, 90 },
{ 40, 180, 90 },
{ 80, 180, 90, },
{ 70, 160, 100 }
};
VespaBattery vbat;
uint8_t vbat_critic = 0xFF;
const uint32_t TEMPO_ATUALIZACAO_VBAT = 5000;
const uint32_t DISCONNECT_UPDATE_INTERVAL = 100; // [ms]
const uint8_t LED_VBAT_HIGH_INTERVAL = 1000; // [ms]
const uint8_t LED_VBAT_LOW_INTERVAL = 500; // [ms]
uint32_t timeout_vbat, timeout_disconnect, timeout_led_vbat;
bool allow_reset_motors = true;
const char html_busy[] PROGMEM = R"rawliteral(
<!DOCTYPE html>
<html>
<head>
<title>
Little hand
</title>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0, user-scalable=no" />
<style>
html, body {width: 100%; height: 100%; padding: 0; margin: 0; }
body {
overflow: hidden;
-moz-user-select: none;
-webkit-user-select: none;
-ms-user-select:none;
user-select:none;
-o-user-select:none;
}
.container {
height: 26px;
width: 50px;
position: relative;
}
.container * {
position: absolute;
}
</style>
</head>
<body style="height: 100%; font-family: 'Gill Sans', 'Gill Sans MT', Calibri, 'Trebuchet MS', sans-serif ;">
<div style="line-height: 26px; background-color: black; padding: 10px; padding-bottom: 0px;">
<div style="width: 100%; border: 0px solid red; text-align: center;">
<h1 style="color: #fff;">Little Hand</h1>
</div>
</div>
<div style="display: table; width:100%; height: calc(100% - 80px); border: 0px solid green;">
<div style="padding: 10px; background-color: yellow; text-align: center;">Outro usuário já está conectado neste robô</div>
</div>
</body>
</html>
)rawliteral";
// FIM DO HTML DE BUSY
//
const char index_html[] PROGMEM = R"rawliteral(,
<!DOCTYPE html>
<html>
<head>
<title>
LittleHand
</title>
<meta charset="UTF-8">
<meta name="viewport"
content="width=device-width, initial-scale=1.0, minimum-scale=1.0, maximum-scale=1.0, user-scalable=0">
<style>
html,
body {
width: 100%;
height: 100%;
padding: 0;
margin: 0;
overscroll-behavior: none;
}
body {
-moz-user-select: none;
-webkit-user-select: none;
-ms-user-select: none;
user-select: none;
-o-user-select: none;
}
.container {
height: 26px;
width: 50px;
position: relative;
}
.container * {
position: absolute;
}
.battery_warning {
top: 50%;
left: 50%;
transform: translate(-50%, -50%);
height: 20px;
width: 40px;
border: 2px solid black;
border-radius: 5px;
padding: 1px;
}
.battery_warning::before {
content: '';
position: absolute;
height: 13px;
width: 3px;
background: black;
left: 44px;
top: 50%;
transform: translateY(-50%);
border-radius: 0 3px 3px 0;
}
.battery {
top: 50%;
left: 50%;
transform: translate(-50%, -50%);
height: 20px;
width: 40px;
border: 2px solid #F1F1F1;
border-radius: 5px;
padding: 1px;
}
.battery::before {
content: '';
position: absolute;
height: 13px;
width: 3px;
background: #F1F1F1;
left: 44px;
top: 50%;
transform: translateY(-50%);
border-radius: 0 3px 3px 0;
}
.part {
background: #0F0;
top: 1px;
left: 1px;
bottom: 1px;
border-radius: 3px;
}
@keyframes animate {
0% {
width: 0%;
background: #F00;
}
50% {
width: 48%;
background: orange;
}
100% {
width: 95%;
background: #0F0;
}
}
.slidecontainer {
width: 100%;
/* Width of the outside container */
}
/* The slider itself */
.slider {
-webkit-appearance: none;
appearance: none;
width: calc(100% - 5px);
height: 50px;
background: #0f0f0;
outline: none;
}
.slider:hover {
opacity: 1;
}
.slider::-webkit-slider-thumb {
-webkit-appearance: none;
appearance: none;
width: 50px;
height: 50px;
background: lightgray;
cursor: pointer;
}
.slider::-moz-range-thumb {
width: 50px;
height: 50px;
background: #04AA6D;
cursor: pointer;
}
.slider_vertical_container {
height: 150px;
overflow: hidden;
border: 0px solid;
width: 50px;
position: relative;
}
.slider_vertical {
transform: rotate(270deg);
position: absolute;
top: 50px;
left: -50px;
width: 150px;
}
.main-container {
display: flex;
gap: 24px;
justify-content: space-around;
margin-top: 80px;
flex-direction: column;
align-items: center;
}
.monitor-container {
display: flex;
flex-direction: column;
}
.monitor-row {
display: flex;
}
@media (min-width: 900px) {
.main-container {
flex-direction: row;
}
}
</style>
</head>
<body style="height: 100%; font-family: 'Gill Sans', 'Gill Sans MT', Calibri, 'Trebuchet MS', sans-serif ;">
<div
style="display: none; width: 100%; height: 100%; position: absolute; align-content: center; justify-content: center; align-items: center; z-index: 10;"
onclick="reinicia();" id="ad">
Restart
</div>
<div
style="display: none; width: 100%; height: 100%; position: absolute; align-content: center; justify-content: center; align-items: center; z-index: 10;"
id="bat">
<div
style="background-color: yellow; width: 100%; display: flex; align-content: center; justify-content: center; align-items: center;">
<div class="container" style="margin-right: 10px;">
<div class="battery_warning">
<div class="part"></div>
</div>
</div>
<h1>Bateria fraca</h1>
</div>
</div>
<div style="line-height: 26px; background-color: black; padding: 10px; padding-bottom: 0px;">
<div class="container" style="float: right; margin-right: 10px;">
<div class="battery">
<div id="lbat" class="part"></div>
</div>
</div>
<div style="float: right; color: white; font-size: 18px; line-height: 26px; margin-right: 5px;">
<span id="vbat">0</span> V
</div>
<div style="width: 100%; border: 0px solid red; text-align: center;">
<h1 style="color: #fff; font-size: 24px; font-family: sans-serif;">LittleHand</h1>
</div>
</div>
<div class="main-container">
<div class="monitor-container">
<h2>Monitor</h2>
<div class="monitor-row">
<div>
<h3>Arm</h3>
<ul>
<li>
<b>Claw: <span id="ds1">0</span></b>
</li>
<li>
<b>Distance: <span title="motor right distance" id="ds2">0</span></b>
</li>
<li>
<b>Height: <span title="motor right distance" id="ds3">0</span></b>
</li>
<li>
<b>Base: <span title="motor right distance" id="ds4">0</span></b>
</li>
</ul>
</div>
<div>
<h3>Tank</h3>
<ul>
<li>
<b>Speed: <span id="speed">0</span>%</b>
</li>
<li>
<b>Angle: <span id="angle">0</span></b>
</li>
<li>
<b>Button: <span id="button">0</span></b>
</li>
</ul>
</div>
</div>
</div>
<div style="display: flex; flex-direction: column;">
<div
style="display: flex; width:100%; height: calc(100% - 80px); border: 0px solid green; font-size: 12px; color: rgb(157, 150, 142);">
<div style="display: flex; align-items: center; justify-content: center;">
<div
style="display: flex; align-items: center; justify-content: space-evenly; align-content: center; flex-direction: row; flex-wrap: wrap;">
<div class="slidecontainer"
style="display: flex; align-items: center; justify-content: space-evenly; align-content: center; flex-direction: row; flex-wrap: wrap; max-width: 260px; margin: 10px;">
<div>CLAW <br /> ↔</div>
<div style="width: 100%; white-space: nowrap;">
<input type="range" min="0" max="180" value="90" class="slider" style="width: calc(50% - 5px);" id="s1">
<input type="range" min="0" max="180" value="90" class="slider" style="width: calc(50% - 5px);"
id="s1c">
</div>
<div style="width: 50px; text-align: right;">HEIGHT <br /> ↕</div>
<div class="slider_vertical_container">
<input type="range" min="0" max="180" value="90" class="slider slider_vertical" id="s2">
</div>
<div class="slider_vertical_container">
<input type="range" min="0" max="180" value="90" class="slider slider_vertical"
style="transform: rotate(90deg);" id="s3">
</div>
<div style="width: 50px;">DISTANCE <br /> ↕</div>
<input type="range" min="0" max="180" value="90" class="slider" id="s4">
<div>BASE <br /> ↔</div>
</div>
</div>
</div>
</div>
<div>
<h2 id="status-current">Status connection: #####</h2>
</div>
</div>
<div style="display: flex;">
<div style="display: flex; width:100%; height: calc(100% - 80px); border: 0px solid green;">
<div style="display: flex; alig-items: center;">
<div
style="display: flex; align-items: center; justify-content: space-evenly; align-content: center; flex-direction: row; flex-wrap: wrap;">
<canvas id="canvas_joystick" style="border: 0px solid red;"></canvas>
</div>
</div>
</div>
</div>
</div>
<script>
let canvas_joystick, ctx_joystick;
let ctx_button;
let coord = {x: 0, y: 0};
let origin_joystick = {x: 0, y: 0};
let paint = false;
let movimento = 0;
let width, height, radius, button_size;
let origin_button = {x: 0, y: 0};
const width_to_radius_ratio = 0.04;
const width_to_size_ratio = 0.15;
const radius_factor = 7;
function updateStatusConnection(status) {
const statusElement = document.getElementById('status-current')
statusElement.innerText = `Status connection: ${status}`
}
function in_circle() {
const current_radius = Math.sqrt(Math.pow(coord.x - origin_joystick.x, 2) + Math.pow(coord.y - origin_joystick.y, 2));
if ((radius * radius_factor) >= current_radius) {
updateStatusConnection('inside circle')
console.log("INSIDE circle");
return true;
} else {
updateStatusConnection('outside circle')
console.log("OUTSIDE circle");
return false;
}
}
function getPosition_joystick(event) {
let mouse_x = event.clientX || event.touches[0].clientX || event.touches[1].clientX;
let mouse_y = event.clientY || event.touches[0].clientY || event.touches[1].clientY;
coord.x = mouse_x - canvas_joystick.offsetLeft;
coord.y = mouse_y - canvas_joystick.offsetTop;
}
var last_update = 0;
function Draw(event) {
if (paint) {
getPosition_joystick(event);
let angle_in_degrees, x, y, speed;
let angle = Math.atan2((coord.y - origin_joystick.y), (coord.x - origin_joystick.x));
if (in_circle()) {
x = coord.x - radius / 2;
y = coord.y - radius / 2;
} else {
x = radius * radius_factor * Math.cos(angle) + origin_joystick.x; // consider the outer circle
y = radius * radius_factor * Math.sin(angle) + origin_joystick.y; // consider the outer circle
}
speed = Math.round(100 * Math.sqrt(Math.pow(x - origin_joystick.x, 2) + Math.pow(y - origin_joystick.y, 2)) / (radius * radius_factor)); // consider the outer circle
if (speed > 100) {
speed = 100; // limit
}
if (Math.sign(angle) == - 1) {
angle_in_degrees = Math.round(- angle * 180 / Math.PI);
}
else {
angle_in_degrees = Math.round(360 - angle * 180 / Math.PI);
}
joystick(x, y);
document.getElementById("speed").innerText = speed;
document.getElementById("angle").innerText = angle_in_degrees;
if ((Date.now() - last_update) > 100) {
last_update = Date.now(); // update
send_joystick(speed, angle_in_degrees);
}
movimento = 1;
}
}
function joystick_background() {
ctx_joystick.clearRect(0, 0, canvas_joystick.width, canvas_joystick.height);
// draw the background circle
ctx_joystick.beginPath();
ctx_joystick.arc(origin_joystick.x, origin_joystick.y, radius * radius_factor, 0, Math.PI * 2, true);
ctx_joystick.fillStyle = 'gray';
ctx_joystick.fill();
//seta esquerda
ctx_joystick.beginPath();
ctx_joystick.moveTo(origin_joystick.x - (radius * radius_factor) - 50, origin_joystick.y);
ctx_joystick.lineTo(origin_joystick.x - (radius * radius_factor) - 25, origin_joystick.y + 25);
ctx_joystick.lineTo(origin_joystick.x - (radius * radius_factor) - 25, origin_joystick.y - 25);
ctx_joystick.fill();
//seta superior
ctx_joystick.beginPath();
ctx_joystick.moveTo(origin_joystick.x, origin_joystick.y - (radius * radius_factor) - 50);
ctx_joystick.lineTo(origin_joystick.x + 25, origin_joystick.y - (radius * radius_factor) - 25);
ctx_joystick.lineTo(origin_joystick.x - 25, origin_joystick.y - (radius * radius_factor) - 25);
ctx_joystick.fill();
//seta direita
ctx_joystick.beginPath();
ctx_joystick.moveTo(origin_joystick.x + (radius * radius_factor) + 50, origin_joystick.y);
ctx_joystick.lineTo(origin_joystick.x + (radius * radius_factor) + 25, origin_joystick.y + 25);
ctx_joystick.lineTo(origin_joystick.x + (radius * radius_factor) + 25, origin_joystick.y - 25);
ctx_joystick.fill();
//seta inferior
ctx_joystick.beginPath();
ctx_joystick.moveTo(origin_joystick.x, origin_joystick.y + (radius * radius_factor) + 50);
ctx_joystick.lineTo(origin_joystick.x + 25, origin_joystick.y + (radius * radius_factor) + 25);
ctx_joystick.lineTo(origin_joystick.x - 25, origin_joystick.y + (radius * radius_factor) + 25);
ctx_joystick.fill();
}
function joystick(x, y) {
// draw the background
joystick_background();
// draw the joystick circle
ctx_joystick.beginPath();
ctx_joystick.arc(x, y, radius * 3, 0, Math.PI * 2, true);
ctx_joystick.fillStyle = 'black';
ctx_joystick.fill();
ctx_joystick.strokeStyle = 'black';
ctx_joystick.lineWidth = 2;
ctx_joystick.stroke();
}
function resize() {
if (window.innerWidth > window.innerHeight) {
width = (window.innerWidth / 2) - 2; // half the window for two canvases
} else {
width = (window.innerWidth) - 2;
}
radius = width_to_radius_ratio * width;
button_size = width_to_size_ratio * width;
height = radius * radius_factor * 2 + 100; // use the diameter
// configure and draw the joystick canvas
ctx_joystick.canvas.width = width;
ctx_joystick.canvas.height = height;
origin_joystick.x = width / 2;
origin_joystick.y = height / 2;
joystick(origin_joystick.x, origin_joystick.y);
}
function startDrawing(event) {
paint = true;
getPosition_joystick(event);
if (in_circle()) {
joystick(coord.x, coord.y);
Draw(event);
}
}
function stopDrawing() {
paint = false; // reset
joystick(origin_joystick.x, origin_joystick.y);
document.getElementById("speed").innerText = 0;
document.getElementById("angle").innerText = 0;
// update the WebSocket client
if (movimento == 1) {
send_joystick(0, 0);
movimento = 0;
}
}
function sliderMirror(from, to) {
document.getElementById(to).value = 180 - document.getElementById(from).value
}
const conn = new WebSocket(`ws://${window.location.hostname}/ws`)
// const conn = new WebSocket(`ws://localhost/ws`)
let lbat = 0
function connectWebSocket() {
updateStatusConnection('trying to open')
conn.onopen = function () {
updateStatusConnection('Connection opened to ' + window.location.hostname)
}
conn.onerror = function (error) {
updateStatusConnection('WebSocket Error ' + JSON.stringify(error, null, 2)),
setTimeout(connectWebSocket, 5000)
}
conn.onmessage = function (e) {
console.log('Server: ' + e.data);
const data = JSON.parse(e.data);
// alert('data' + JSON.stringify(data, null, 0))
if (data["vbat"]) {
document.getElementById("vbat").innerText = (data["vbat"] / 1000).toFixed(1)
lbat = (data["vbat"] * 100 / 8400).toFixed(0)
if (lbat > 100) {lbat = 100}
if (lbat < 2) {lbat = 2}
console.log("lbat=" + lbat) // debug
document.getElementById("lbat").style.width = lbat + '%'
if (lbat < 20) {
document.getElementById("lbat").style.backgroundColor = "#F00"
} else if (lbat < 70) {
document.getElementById("lbat").style.backgroundColor = "orange"
} else {
document.getElementById("lbat").style.backgroundColor = "#0F0"
}
}
}
conn.onclose = function () {
updateStatusConnection('WebSocket connection closed')
connectWebSocket()
}
}
connectWebSocket()
function debounce(func, delay) {
let debounceTimer;
return function () {
const context = this;
const args = arguments;
clearTimeout(debounceTimer);
debounceTimer = setTimeout(() => func.apply(context, args), delay);
}
}
function send_slider(slider_id, value) {
var data = {'servo': slider_id, 'posicao': parseInt(value)};
data = JSON.stringify(data);
console.log('Slider data: ', data);
conn.send(data);
}
const debouncedSendSlider = debounce(function (slider_id, value) {
send_slider(slider_id, value);
}, 250);
function send_joystick(speed, angle) {
const data = {'velocidade': speed, 'angulo': angle};
console.log('Send joystick: ', data);
conn.send(JSON.stringify(data));
}
function handleGetValue(id) {
const element = document.getElementById(id)
return element.value
}
function handleAddEventListener(data) {
const { id, callback } = data;
const element = document.getElementById(id)
element.addEventListener('input', (event) => {
callback(event)
})
}
function setInnerHtml(text, id) {
const element = document.getElementById(id)
element.innerHTML = text
}
function handleDebug() {
document.addEventListener("input", function () {
setInnerHtml(handleGetValue('s1'), 'ds1')
setInnerHtml(handleGetValue('s2'), 'ds2')
setInnerHtml(handleGetValue('s3'), 'ds3')
setInnerHtml(handleGetValue('s4'), 'ds4')
}, false)
}
function handleSetupServMotors() {
const s1 = document.getElementById("s1")
handleAddEventListener({
id: 's1',
callback: (event) => {
sliderMirror('s1', 's1c');
debouncedSendSlider(1, event.target.value)
}
})
handleAddEventListener({
id: 's1c',
callback: (event) => {
debouncedSendSlider(1, s1.value)
sliderMirror('s1c', 's1')
}
})
handleAddEventListener({
id: 's2',
callback: (event) => {
debouncedSendSlider(2, event.target.value)
}
})
handleAddEventListener({
id: 's3',
callback: (event) => {
debouncedSendSlider(3, s1.value)
}
})
handleAddEventListener({
id: 's4',
callback: (event) => {
debouncedSendSlider(4, s1.value)
}
})
}
// document.getElementById('s1').addEventListener('input', (e) => {
// sliderMirror('s1', 's1c');
// debouncedSendSlider(1, e.target.value);
// });
// document.getElementById('s1c').addEventListener('input', (e) => {
// sliderMirror('s1c', 's1');
// debouncedSendSlider(1, s1.value);
// });
// document.getElementById('s2').addEventListener('input', (e) => {
// debouncedSendSlider(2, e.target.value);
// })
// document.getElementById('s3').addEventListener('input', (e) => {
// debouncedSendSlider(3, e.target.value);
// })
// document.getElementById('s4').addEventListener('input', (e) => {
// debouncedSendSlider(4, e.target.value);
// })
// document.addEventListener("input", function () {
// document.getElementById("ds1").innerHTML = s1.value;
// document.getElementById("ds2").innerHTML = s2.value;
// document.getElementById("ds3").innerHTML = s3.value;
// document.getElementById("ds4").innerHTML = s4.value;
// }, false);
handleSetupServMotors()
handleDebug()
window.addEventListener('load', () => {
canvas_joystick = document.getElementById('canvas_joystick');
ctx_joystick = canvas_joystick.getContext('2d');
resize();
canvas_joystick.addEventListener('mousedown', startDrawing);
canvas_joystick.addEventListener('mouseup', stopDrawing);
canvas_joystick.addEventListener('mousemove', Draw);
canvas_joystick.addEventListener('touchstart', startDrawing);
canvas_joystick.addEventListener('touchend', stopDrawing);
canvas_joystick.addEventListener('touchcancel', stopDrawing);
canvas_joystick.addEventListener('touchmove', Draw);
window.addEventListener('resize', resize);
document.getElementById("speed").innerText = 0;
document.getElementById("angle").innerText = 0;
document.getElementById("button").innerText = 0;
});
</script>
</body>
</html>
)rawliteral";
void setupWebServer(void);
void handleWebSocketMessage(void *, uint8_t *, size_t);
void onEvent(AsyncWebSocket *, AsyncWebSocketClient *, AwsEventType,
void *, uint8_t *, size_t);
void wifiSetup() {
Serial.print("Configurando a rede Wi-Fi... ");
const char *mac = WiFi.macAddress().c_str(); // obtem o MAC
char ssid[] = "little-hand"; // mascara do SSID (ate 63 caracteres)
char *senha = "12345678"; // senha padrao da rede (no minimo 8 caracteres)
// atualiza o SSID em funcao do MAC
for(uint8_t i=12 ; i < 16 ; i++){
ssid[i] = mac[i+12];
}
if(!WiFi.softAP(ssid, senha)){
Serial.println("ERRO");
// trava a execucao
while(1){
digitalWrite(PIN_LED, HIGH);
delay(100);
digitalWrite(PIN_LED, LOW);
delay(100);
}
}
Serial.println("OK");
Serial.printf("A rede \"%s\" foi gerada\n", ssid);
Serial.print("IP de acesso: ");
Serial.println(WiFi.softAPIP());
}
void littleHandSetup() {
servos[CLAW].attach(VESPA_SERVO_S1, SERVO_MIN, SERVO_MAX);
servos[RIGHT_SERVO].attach(VESPA_SERVO_S2, SERVO_MIN, SERVO_MAX);
servos[LEFT_SERVO].attach(VESPA_SERVO_S3, SERVO_MIN, SERVO_MAX);
servos[BASE_SERVO].attach(VESPA_SERVO_S4, SERVO_MIN, SERVO_MAX);
for(uint8_t i=0; i < 4; i++) {
servos[i].write(serv_angles[i].current);
}
}
void setupWebServer(void) {
ws.onEvent(onEvent);
server.addHandler(&ws);
server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
if(ws.count() == 0) {
request->send_P(200, "text/html", index_html);
} else {
request->send_P(200, "text/html", html_busy);
}
});
}
void setup(){
Serial.begin(115200);
Serial.println("Claw!");
pinMode(PIN_LED, OUTPUT);
digitalWrite(PIN_LED, LOW);
littleHandSetup();
wifiSetup();
setupWebServer();
server.begin();
Serial.println("Servidor iniciado\n");
}
void loop() {
if(millis() > timeout_vbat){
uint32_t tension = vbat.readVoltage();
if((tension < 7000) && (vbat_critic == 0xFF)){
Serial.printf("Tensao critica (%u mV)\n", tension);
vbat_critic = LOW;
digitalWrite(PIN_LED, vbat_critic);
timeout_led_vbat = millis() + LED_VBAT_LOW_INTERVAL;
} else if((tension >= 7000) && (vbat_critic < 0xFF)){
vbat_critic = 0xFF; // reset
// atualiza o estado do LED em funcao da conexao ativa
if(ws.count() > 0){
digitalWrite(PIN_LED, HIGH);
} else {
digitalWrite(PIN_LED, LOW);
}
}
if(ws.count() > 0){
const int json_tamanho = JSON_OBJECT_SIZE(1); // objeto JSON com um membro
StaticJsonDocument<json_tamanho> json;
json[ALIAS_VBAT] = tension;
size_t mensagem_comprimento = measureJson(json);
char mensagem[mensagem_comprimento + 1];
serializeJson(json, mensagem, (mensagem_comprimento+1));
mensagem[mensagem_comprimento] = 0; // EOS (mostly for debugging)
ws.textAll(mensagem, mensagem_comprimento);
Serial.printf("Tensao atualizada: %u mV\n", tension);
}
timeout_vbat = millis() + TEMPO_ATUALIZACAO_VBAT; // atualiza
}
if(millis() > timeout_led_vbat){
if(vbat_critic < 0xFF){
if(vbat_critic == LOW){
vbat_critic = HIGH;
timeout_led_vbat = millis() + LED_VBAT_HIGH_INTERVAL;
} else {
vbat_critic = LOW;
timeout_led_vbat = millis() + LED_VBAT_LOW_INTERVAL;
}
digitalWrite(PIN_LED, vbat_critic);
}
}
if(millis() > timeout_disconnect){
if((ws.count() == 0) && allow_reset_motors){
Serial.println("Reset dos motores");
motores.stop();
// atualiza os motores para as posicoes iniciais
for(uint8_t i=0 ; i < 4 ; i++){
servos[i].write(serv_angles[i].current);
}
allow_reset_motors = false; // reset
}
timeout_disconnect = millis() + DISCONNECT_UPDATE_INTERVAL; // atualiza
}
}
void handleWebSocketMessage(void *arg, uint8_t *data, size_t length) {
AwsFrameInfo *info = (AwsFrameInfo*)arg;
if (info->final && info->index == 0 && info->len == length && info->opcode == WS_TEXT) {
data[length] = 0;
Serial.printf("Incoming WS data: \"%s\"\n", (char*)data); // debug
const int json_tamanho = JSON_OBJECT_SIZE(2); // objeto JSON com dois membros
StaticJsonDocument<json_tamanho> json;
if(strstr(reinterpret_cast<char*>(data), ALIAS_VELOCIDADE) != nullptr) {
Serial.println("Velocidade");
DeserializationError erro = deserializeJson(json, data, length);
// extrai os valores do JSON
int16_t angulo = json[ALIAS_MOTOR_ANGLE]; // [0;360]
int16_t velocidade = json[ALIAS_VELOCIDADE]; // [0;100]
// debug
Serial.println("Motores: ");
Serial.print("Velocidade: ");
Serial.print(velocidade);
Serial.print(" | Angulo: ");
Serial.println(angulo);
//curva frente para a esquerda
if((angulo >= 90) && (angulo <= 180)){
motores.turn((velocidade * (135 - angulo) / 45), velocidade);
//curva frente para a direita
} else if((angulo >= 0) && (angulo < 90)){
motores.turn(velocidade, (velocidade * (angulo - 45) / 45));
//curva tras esquerda
} else if((angulo > 180) && (angulo <= 270)){
motores.turn((-1 * velocidade), (-1 * velocidade * (angulo - 225) / 45));
//curva tras direita
} else if(angulo > 270){
motores.turn((-1 * velocidade * (315 - angulo) / 45), (-1 * velocidade));
} else {
motores.stop();
}
}
else if(strstr(reinterpret_cast<char*>(data), ALIAS_SERVO) != nullptr){
DeserializationError erro = deserializeJson(json, data, length);
int16_t angulo = json[ALIAS_ANGULO]; // [0;180]
int16_t servo = json[ALIAS_SERVO]; // [1-4]
Serial.println("Servo: ");
Serial.println(servo);
Serial.println(" | Angulo: ");
Serial.println(angulo);
if((servo < 1) || (servo > 4)){
Serial.printf("Servo invalido (%u)\n", servo);
return;
}
if((angulo < 0) || (angulo > 180)){
Serial.printf("Angulo invalido (%u)\n", servo);
return;
}
servos[servo-1].write(angulo);
} else {
Serial.printf("Recebidos dados invalidos (%s)\n", data);
}
}
}
void onEvent(AsyncWebSocket *server, AsyncWebSocketClient *client, AwsEventType type, void *arg, uint8_t *data, size_t length) {
switch (type) {
case WS_EVT_CONNECT: {
digitalWrite(PIN_LED, HIGH); // acende o LED
if(ws.count() == 1){
Serial.printf("Cliente WebSocket #%u conectado de %s\n", client->id(), client->remoteIP().toString().c_str());
} else {
Serial.printf("Cliente WebSocket #%u de %s foi rejeitado\n", client->id(), client->remoteIP().toString().c_str());
ws.close(client->id());
}
break;
}
case WS_EVT_DISCONNECT: {
digitalWrite(PIN_LED, LOW);
if(ws.count() == 0){
digitalWrite(PIN_LED, LOW); // apaga o LED
}
Serial.printf("Cliente WebSocket #%u desconectado\n", client->id());
break;
}
case WS_EVT_DATA: {
handleWebSocketMessage(arg, data, length);
break;
}
case WS_EVT_PONG:
case WS_EVT_ERROR:
break;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment