I use JY901 and ESPAsyncWebServer to develop a system. JY901 reports data every 50ms. The system crashed while I was browsing the web. This situation is not easily reproducible, but it does happen every day. Has anyone encountered this problem?
Hi @david163888,
welcome to the arduino-forum.
I'm pretty sure that you agree and will follow the way how to solve your problem mimimum 200 minutes faster.
This requires to invest 20 minutes of your precious time to read how to speedup solving your problems.
Directly after registering you got presented informations how to speed up solving your problem.
You should really read it.
best regards Stefan
My car won't start. Has anyone encountered this problem?
You provide absolutely NO useful information that would enable anyone to provide any assistance. If it is "crashing", there is a bug in your code. Without seeing that code, how can anyone here possibly help?
webServer.cpp
/*
created on 2023_1020 by Richard
main features in the program
- read data from device
- send data to Google sheet
*/
#include <Arduino.h>
#include "init.h"
// //Include for send data to internet
// #include "WiFi.h"
// //#include <WebServer.h>
// #include <ESPAsyncWebServer.h>
// #include <SPIFFS.h>
// #include <HTTPClient.h>
// #include "time.h"
// #include <ESPmDNS.h>
String parameter_val;
// below settings for send data to internet
const char* host = "SP150";
const char *set_ntpServer = "pool.ntp.org";
const long set_gmtOffset_sec = 28800; //時區調整 +8
const int set_daylightOffset_sec = 0;
/* WiFi Client data */
// const char *field_ssid = "3E-4F-AP"; // change SSID
// const char *field_password = "0226805501"; // change password
const char *field_ssid = "3E-Guest"; // change SSID
const char *field_password = "0226805501"; // change password
// char* set_ssid = "simontai-2.4G"; // change SSID
// char* set_password = "12345678"; // change password
// const char *field_ssid = "Richard"; // change SSID
// const char *field_password = "jfkiw93134"; // change password
/* WiFi AP data */
IPAddress AP_local_ip(192,168,10,1);
IPAddress AP_gateway(192,168,10,1);
IPAddress AP_subnet(255,255,255,0);
IPAddress local_IP;
const char *AP_ssid = "SP150_AP";
const char *AP_password = "0226805501";
/* WebServer server data */
//WebServer server(80);
AsyncWebServer server(80);
int statusCode;
String content;
String room_id = "";
void init_WebServer(void) {
//void WebServer( void * pvParam ) {
//Serial.printf("WebServer program on core: %u\n", xPortGetCoreID());
/* sync server
// server.on("/", handleRoot);
// server.on("/about", []() {
// server.send(200, "text/html; charset=utf-8", "是在哈囉喔？");
// });
// server.onNotFound([]() {
// server.send(404, "text/plain", "File NOT found!");
// });
//
// server.begin();
*/
/* Async server */
server.serveStatic("/", SPIFFS, "/www/").setDefaultFile("index.html");
server.serveStatic("/img", SPIFFS, "/www/img/Poseidon.png");
server.serveStatic("/favicon.ico", SPIFFS, "/www/icon_result.ico");
// server.serveStatic("/favicon.ico", SPIFFS, "/www/favicon.ico");
server.serveStatic("/survival", SPIFFS, "/www/").setDefaultFile("survival.html");
// ================
server.on("/degree_quota", HTTP_GET, [](AsyncWebServerRequest * req)
{
int16_t val;
//Serial.print("get degree_quota\n");
if (req->hasParam("degree_quota")) {
AsyncWebParameter* p = req->getParam("degree_quota");
parameter_val = p->value();
//Serial.print("get degree_quota = " + parameter_val +"\n");
//degree_quota = (int16_t)parameter_val.toInt();
val = (int16_t)parameter_val.toInt();
if ((val >= 0) && (val <= 16))
{
reserved_data.degree_quota = (int16_t)parameter_val.toInt();
}
set_degree_quota();
}
});
server.on("/degree_quota_now", HTTP_GET, [](AsyncWebServerRequest * req) {
int16_t val = degree_quota;
//uint16_t val = 99;
//Serial.print("get degree_quota_now\n");
req->send(200, "text/plain", String(val));
//Serial.printf("web server on core: %u\n", xPortGetCoreID());
});
// ==================================================================
server.on("/local_IP", HTTP_GET, [](AsyncWebServerRequest * req) {
String return_ip;
//Serial.print("get local_IP\n");
return_ip = String(local_IP[0]) + String(".") + String(local_IP[1]) + String(".") + String(local_IP[2]) + String(".") + String(local_IP[3]);
//Serial.print(return_ip);
req->send(200, "text/plain", return_ip);
});
// ==================================================================
server.on("/pitch", HTTP_GET, [](AsyncWebServerRequest * req) {
int16_t val = Pitch_INT; // 讀取光敏電阻的數值
//val = 99;
//Serial.print("get pitch\n");
req->send(200, "text/plain", String(val));
});
server.on("/roll", HTTP_GET, [](AsyncWebServerRequest * req) {
int16_t val = Roll_INT; // 讀取光敏電阻的數值
//val = 66;
//Serial.print("get roll\n");
req->send(200, "text/plain", String(val));
});
// =================================================================
server.on("/reaction_mode", HTTP_GET, [](AsyncWebServerRequest * req)
{
//Serial.print("get reaction_mode\n");
if (req->hasParam("reaction_mode")) {
AsyncWebParameter* p = req->getParam("reaction_mode");
// parameter_val = p->value();
//Serial.print("get reaction_mode = " + parameter_val +"\n");
if (p->value() == "F") {
// reaction_mode = FOLLOWING_MODE;
reserved_data.reaction_mode = FOLLOWING_MODE;
//Serial.printf("reaction_mode = FOLLOWING_MODE\n");
} else if (p->value() == "C") {
// reaction_mode = COMPENSATION_MODE;
reserved_data.reaction_mode = COMPENSATION_MODE;
//Serial.printf("reaction_mode = COMPENSATION_MODE\n");
}
}
});
server.on("/reaction_mode_now", HTTP_GET, [](AsyncWebServerRequest * req)
{
//Serial.print("get reaction_mode_now\n");
if (reaction_mode == FOLLOWING_MODE) {
req->send(200, "text/plain", String("正向"));
//Serial.printf("reaction_mode = FOLLOWING_MODE\n");
} else if (reaction_mode == COMPENSATION_MODE) {
req->send(200, "text/plain", String("反向"));
//Serial.printf("reaction_mode = COMPENSATION_MODE\n");
}
req->send(200, "text/plain", "OK!");
});
// =================================================================
server.on("/step", HTTP_GET, [](AsyncWebServerRequest * req)
{
int16_t val;
//Serial.print("get step\n");
if (req->hasParam("step")) {
AsyncWebParameter* p = req->getParam("step");
parameter_val = p->value();
//Serial.print("get degree_quota = " + parameter_val +"\n");
// step = (int16_t)parameter_val.toInt();
val = (int16_t)parameter_val.toInt();
if ((val >= 1) && (val <= 25))
{
reserved_data.step = (int16_t)parameter_val.toInt();
}
}
});
server.on("/step_now", HTTP_GET, [](AsyncWebServerRequest * req) {
//Serial.print("get step_now\n");
int16_t val;
switch (reserved_data.step)
{
case 1 ... 2:
val = 1;
break;
case 3 ... 4:
val = 2;
break;
case 5 ... 7:
val = 3;
break;
case 8 ... 12:
val = 4;
break;
case 13 ... 25:
val = 5;
break;
}
req->send(200, "text/plain", String(val));
//Serial.printf("web server on core: %u\n", xPortGetCoreID());
});
// =================================================================
server.on("/z_quota", HTTP_GET, [](AsyncWebServerRequest * req) {
double val;
//Serial.print("get z_quota\n");
if (req->hasParam("z_quota"))
{
AsyncWebParameter* p = req->getParam("z_quota");
parameter_val = p->value();
//Serial.print("get z_quota = " + parameter_val + "\n");
// z_quota = parameter_val.toDouble();
val = parameter_val.toDouble();
if (( val >= 0) && (val <= 200.0))
{
reserved_data.z_quota = parameter_val.toDouble();
}
//Serial.print("get has z_quota = %f\n", z_quota); // can't be compiled passed, very strangy
}
});
server.on("/z_quota_now", HTTP_GET, [](AsyncWebServerRequest * req) {
double val = reserved_data.z_quota;
//uint16_t val = 99;
//Serial.print("get z_quota_now\n");
req->send(200, "text/plain", String(val));
});
// ==================================
server.on("/reset_default_parameter", HTTP_GET, [](AsyncWebServerRequest * req) {
//Serial.print("get reset_default_parameter\n");
reset_default_parameter();
});
server.on("/save_parameter", HTTP_GET, [](AsyncWebServerRequest * req) {
//Serial.print("get save_parameter\n");
save_parameter();
});
// ==================================
server.on("/show_angle", HTTP_GET, [](AsyncWebServerRequest * req) {
double val = rotation_quota;
//uint16_t val = 99;
//Serial.print("get show_angle\n");
req->send(200, "text/plain", String("OK"));
});
// ==================================
server.on("/survival_time", HTTP_GET, [](AsyncWebServerRequest * req) {
// uint32_t val = millis();
// //Serial.printf("survival time = %d\n", val);
// req->send(200, "text/plain", String(val));
req->send(200, "text/plain", String(millis()));
});
server.begin();
Serial.println("HTTP server ready!");
yield();
}
void init_WiFi(void) {
unsigned long start_Time;
/* WiFi settings */
Serial.println();
Serial.print("Connecting to wifi: ");
Serial.println(field_ssid);
Serial.flush();
WiFi.mode(WIFI_AP_STA);
WiFi.begin(field_ssid, field_password);
start_Time = millis();
while (true)
{
if (WiFi.status() == WL_CONNECTED)
{
//Serial.print("Connected");
local_IP = WiFi.localIP();
Serial.print("\nWIFI is connected, The local IP address is: ");
Serial.print(local_IP);
WiFi.softAPConfig(AP_local_ip, AP_gateway, AP_subnet); // must place after WiFi.begin
WiFi.softAP(AP_ssid, AP_password);
break;
}else
{
if (millis() - start_Time >= 5000)
{
Serial.print("\nWIFI Connect fail");
WiFi.mode(WIFI_AP); // should change mode, if WIFI_AP_STA fail
WiFi.softAPConfig(AP_local_ip, AP_gateway, AP_subnet);
WiFi.softAP(AP_ssid, AP_password);
// WiFi.config(IPAddress(192,168,0,100), IPAddress(192,168,0,100), IPAddress(255,255,255,0));
// local_IP = IPAddress(192,168,0,100);
break;
} else
{
delay(500);
Serial.print(".");
}
}
}
Serial.println();
Serial.print("AP: 192,168,10,1, subnet mask 255,255,255,0\n");
if (!MDNS.begin(host)) { //http://Richard.local
Serial.println("設置MDNS回應器時出錯了～");
while (1) {
delay(1000);
}
}
MDNS.setInstanceName("Cubie's ESP32"); // 設置實體名稱
MDNS.addService("http", "tcp", 80); // 設置服務描述
/* Init and get the time */
configTime(set_gmtOffset_sec, set_daylightOffset_sec, set_ntpServer);
}
balance_platfor_2024_0613.ino
// 2024_0316: finish all basic functions
#include <Arduino.h>
#include "init.h"
#include <SPIFFS.h>
#include <Commander.h>
#include "EEPROM.h"
#include <esp_task_wdt.h>
#include "esp_system.h"
// *********************************************************************************************************************************
// *********************************************************************************************************************************
// "1.00.01"; // add version number
// "1.00.02"; // add more delay to compromize the time of platform returning to original point
//"1.00.03"; // add survival time on web page, this version seems better that not easy crash when browse it
//"1.00.04"; // change to .svg vector img file for web page icon
//"1.00.05"; // remove index.html\window.setInterval(function () to improve crash problem significantly, but not really to resolve
const char *version = "1.00.06";
// 1. screen all varible data type conversions，
// 2. check varible at each begin of loop and web change data in the structure of reserved_data rather than varible immediately
// **********************************************************************************************************************************
// **********************************************************************************************************************************
// #include <esp_cpu.h>
//Serial使用GPIO1->TX0，GPIO3->RX0，通常用於USB通訊和寫程式debug使用，預設系統使用
//Serial1: platform,使用GPIO10->TX1，GPIO9->RX1，通常用於SPI flash上，預設系統未使用
//Serial2: JY901S,使用GPIO17->TX2，GPIO16->RX2，通常用於自行使用，大部分的人都會使用這個，預設系統未使用
// pitch 16, roll 14
// #define Z_AXIS_quota_FOR_ACCELERATION 60.0
// #define Z_AXIS_quota_FOR_PITCH_ROLL 140.0
//pitch 17, roll 15
//#define Z_AXIS_quota_FOR_ACCELERATION 50.0
//#define Z_AXIS_quota_FOR_PITCH_ROLL 150.0
//static const uint8_t LED_BUILTIN = 2;
//pitch 17, roll 15
#define Z_AXIS_quota_FOR_ACCELERATION 200.0 // over use for Z-axis //XXXXXXXXXXXXXXXXXXXXXXXXXXXXXx
#define Z_AXIS_quota_FOR_PITCH_ROLL 150.0
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
void playing_senarion(void);
TaskHandle_t handle_platform;
TaskHandle_t handle_webserver;
uint16_t stackSize_platform = 3000; // 分配給任務的記憶體大小
uint16_t stackSize_webserver = 4000; // 分配給任務的記憶體大小5
uint32_t chipId = 0;
uint16_t omission = 30;
int8_t reaction_mode = COMPENSATION_MODE; //compensation
double rotation_quota = Z_AXIS_quota_FOR_PITCH_ROLL;
uint8_t show_angle = 1;
uint8_t show_height = 0;
uint8_t show_length = 0;
int16_t step = 3;
// this toward_step is related to z_ratio, omission
// z_ratio omission toward_step deviation comment
// 10 30 0.9 +-8~+-30 best
double toward_step = 0.9;
uint8_t transmission_adjust = 1;
double z_quota = Z_AXIS_quota_FOR_ACCELERATION;
double z_quota_left;
uint16_t z_ratio = 10; // suggest use around "10" that is significant
int16_t degree_quota;
MyStructure reserved_data;
// UART1: balance_platform
// UART2: JY901S
//extern void init_serial(void);
uint16_t time_distance_ratio = 2;
uint16_t check_period = 50;
uint8_t before_loop = 1;
uint8_t serial0_received_bytes = 0;
uint8_t serial1_received_bytes = 0;
uint8_t serial2_received_bytes = 0;
uint16_t execution_speed = 50;
uint32_t step_length = 40; //
uint8_t new_JY901S_packet = 0;
// read current as base number
double base_Roll_F;
double base_Pitch_F;
double base_Yaw_F;
double Injection_Pitch_F;
double Injection_Roll_F;
double v0 = 0, v = 0;
double base_az_g = -1.002666 ; //4F = 0.995605, 18F = 0.999 ~ 1.000 //6F林口 = -0.994208
double previous_g = -1.003906;
double g_a = 9806.65; //9.80665 m/s², s = 1/2at^2
double delta_g_a = 0;
double previous_delta_g_a = 0;
double add_distance = 0, z_distance = 0, previous_z_distance = 0;
double z_current = 0;
double z_upper_bound = (Z_AXIS_quota_FOR_ACCELERATION / 2);
double z_lower_bound = (-1 * Z_AXIS_quota_FOR_ACCELERATION / 2);
uint32_t loop_delay_ms = 10;
uint8_t cmd_t0_CH9121_REACTION[] = {0x55, 0xAA, 0x00, 0x00, 0x14, 0x01, 0x00, 0x00, 0xFF, 0xFF,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x03, 0x20, //800
0x00, 0x02, 0xE3, 0x6B, 0x00, 0x02, 0xE3, 0x6B, 0x00, 0x02, // index of: #1 20~23 , #2 24~27 , #3 28~31
0xE3, 0x6B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
}; // 38 bytes
int32_t Height = 0;
int32_t base_height = 7430; //7430: 4F三峽; //28500: 6F林口
double base_height_F;
uint8_t i_Z_axis_adjust = 0;
int32_t z_Height, z_Height_previous;
// common variables
uint8_t *TH, *TL;
uint16_t T;
double T_F;
// 0x5551 variables
uint8_t *axH, *axL;
uint16_t ax;
int16_t *ax_p;
double ax_F;
uint8_t *ayH, *ayL;
uint16_t ay;
int16_t *ay_p;
double ay_F;
uint8_t *azH, *azL;
uint16_t az;
int16_t *az_p;
double az_F;
// 0x5552 variables
uint8_t *wxH, *wxL;
uint16_t wx;
double wx_F;
uint8_t *wyH, *wyL;
uint16_t wy;
double wy_F;
uint8_t *wzH, *wzL;
uint16_t wz;
double wz_F;
// 0x5553 variables
uint8_t *RollH, *RollL; // x axis, 滾轉角
uint16_t Roll;
double Roll_F;
volatile int16_t Roll_INT;
uint8_t *PitchH, *PitchL;// y axis, 俯仰角
uint16_t Pitch;
double Pitch_F;
volatile int16_t Pitch_INT;
uint8_t *YawH, *YawL; // z axis, 偏航角
uint16_t Yaw;
double Yaw_F;
// 0x5554 variables
uint8_t *HxH, *HxL;
uint16_t Hx;
double Hx_F;
uint8_t *HyH, *HyL;
uint16_t Hy;
double Hy_F;
uint8_t *HzH, *HzL;
uint16_t Hz;
double Hz_F;
double move_step;
double slope_X, slope_Y;
double length_X = 1000, length_y = 1000; //mm
double height_max = 20;
double height_need_Z = 0;
double height = 0; // +- 100mm
uint8_t action_status = 0;
double x_axis_length = 470;
double y_axis_length = 550;
int32_t base_length = 100;
int32_t average_length;
int32_t increasing_length;
int32_t length_1 = 100;
int32_t length_2 = 100;
int32_t length_3 = 100;
int32_t length_1_previous = 100;
int32_t length_2_previous = 100;
int32_t length_3_previous = 100;
int32_t length_1_total = 0;
int32_t length_2_total = 0;
int32_t length_3_total = 0;
int32_t length_1_remain = 0;
int32_t length_2_remain = 0;
int32_t length_3_remain = 0;
double pitch_limitation_positive;
double pitch_limitation_negtive;
double roll_limitation_positive;
double roll_limitation_negtive;
double pitch_plus_roll_limitation_positive;
double pitch_plus_roll_limitation_negtive;
//uint16_t max_roll = 0;
// Commander 接口构型
// - serial - 可选择接收 HardwareSerial 或 Stream 实例
// - eol - 可选择接收 eol 字符 - 默认另起一行： "\n"
// - echo - 可选的 echo 行结束符（命令行反馈） - 默认 false
//Commander commander = Commander(Serial, "\n", false);
Commander cmd;
uint16_t transmission_adjust_table[] = {1250, 1250, 1603, 1746, 1672, 1577, 1490, 1413, 1348, 1292, 1244 , 1203, 1167, 1136, 1110, 1089, 1071, 1059, 1051, 1050, 1250, 1250, 1250};
uint32_t length_value_table[] = {0, 0x30D4, 0x7D40, 0xCCB3, 0x10550, 0x1341D, 0x15D38, 0x18299, 0x1A562, 0x1C655, 0x1E5FC, 0x204C4, 0x22309, 0x24121, 0x25F63, 0x27E2F, 0x29DFC, 0x2BF6A, 0x2E36B, 0x30BAD, 0x3D0A4};
uint16_t length_1_ratio, length_2_ratio, length_3_ratio;
uint8_t index1, index2, index3;
uint8_t *execution_speed_p;
void setup() {
// char *sent_string;
uint32_t Freq = 0;
// pinMode(LED_BUILTIN, OUTPUT);
// 8 sec(delay) + 12 sec(Http server) + 4 sec(vertical movement) + 8 sec(rotation) + 3 sec(return to original) + 5 sec(delay) == 40 sec
delay(8000); //delay(ms) // this wait for unstable power source to stable, don't remove this !!!
delay(12000); // add more delay to compromize the time of platform returning original point
// digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
// delay(500); // wait for a second
// digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
init_serial();
initialiseCommander();
init_WiFi();
Serial.print("version = ");
Serial.println(version);
//xTaskCreatePinnedToCore(WebServer, "WebServer", 15000, NULL, 0, NULL, 0);
//xTaskCreate(WebServer, "WebServer", 2000, NULL, 0, NULL);
//Serial.println("掛載SPIFFS分區出錯啦～");
//Serial.flush(); // wait Serial FIFO to be empty and then spend almost no time processing it
//Serial.flush(); // wait Serial FIFO to be empty and then spend almost no time processing it
if (!SPIFFS.begin(true))
{
Serial.printf("mount SPIFFS fail\n");
return;
} else
{
Serial.printf("mount SPIFFS done\n");
}
init_JY901S();
// digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
// delay(500); // wait for a second
// digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
// delay(500);
// digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
// delay(500); // wait for a second
// digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
//calculate_angle_limitation(); //? do we need it here?
delay(1000); //delay(ms)
init_platform(); //lift to 100ms initially
// digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
// delay(500); // wait for a second
// digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
// delay(500);
// digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
// delay(500); // wait for a second
// digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
// delay(500);
// digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
// delay(500); // wait for a second
// digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
// for(int i=0; i<17; i=i+8) {
// chipId |= ((ESP.getEfuseMac() >> (40 - i)) & 0xff) << i;
// }
// Serial.printf("ESP32 Chip model = %s Rev %d\n", ESP.getChipModel(), ESP.getChipRevision());
// Serial.printf("This chip has %d cores\n", ESP.getChipCores());
// Serial.print("Chip ID: "); Serial.println(chipId);
//xTaskCreate(WebServer, "WebServer", 2000, NULL, 0, NULL);
// rad CPU clock, it works!!!
Freq = getCpuFrequencyMhz();
Serial.print("CPU Freq = ");
Serial.print(Freq);
Serial.println(" MHz");
// Serial.printf("setup finished!\n"); //around 160ms for a loop
// Serial.printf(" omission = %d, reaction_mode = %d, rotation_quota = %f, show_angle = %d, show_height = %d, show_length = %d, step = %d, toward_step = %f, transmission_adjust = %d, z_quota = %f, z_ratio = %d\n",
// omission,
// reaction_mode,
// rotation_quota,
// show_angle,
// show_height,
// show_length,
// step,
// toward_step,
// transmission_adjust,
// z_quota,
// z_ratio);
init_WebServer();
//WebServer(NULL);
//disableCore0WDT();
//xTaskCreate(WebServer, "WebServer", stackSize_platform, NULL, 1, &handle_webserver);
// new feature /////////////
playing_senarion();
// digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
// delay(500); // wait for a second
// digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
// delay(500);
// digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
// delay(500); // wait for a second
// digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
// delay(500);
// digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
// delay(500); // wait for a second
// digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
// delay(500);
// digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
// delay(500); // wait for a second
// digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
// delay(500);
// digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
// delay(500); // wait for a second
// digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
// new feature /////////////
// iniialize data to start
z_distance = 0;
length_1 = 100;
length_2 = 100;
length_3 = 100;
execution_speed = 50;
//z_Height_previous = z_Height;
if (!EEPROM.begin(EEPROM_SIZE))
{
Serial.println("failed to initialise EEPROM"); delay(1000000);
}
EEPROM.get(0, reserved_data);
omission = reserved_data.omission;
reaction_mode = reserved_data.reaction_mode;
rotation_quota = reserved_data.rotation_quota;
show_angle = reserved_data.show_angle;
show_height = reserved_data.show_height;
show_length = reserved_data.show_length;
step = reserved_data.step;
transmission_adjust = reserved_data.transmission_adjust;
toward_step = reserved_data.toward_step;
z_quota = reserved_data.z_quota;
z_ratio = reserved_data.z_ratio;
degree_quota = reserved_data.degree_quota;
z_quota_left = z_quota / 2;
Serial.printf("omission = %d, reaction_mode = %d, rotation_quota = %f, show_angle = %d, show_height = %d, show_length = %d, step = %d, toward_step = %f, transmission_adjust = %d, , z_ratio = %d\n",
omission,
reaction_mode,
rotation_quota,
show_angle,
show_height,
show_length,
step,
toward_step,
transmission_adjust,
z_quota,
z_ratio);
//xTaskCreate(task_platform, "task_platform", stackSize_platform, NULL, 1, &handle_platform); //stack = "4000" is very important, otherwise to execute certain command will cause crash
//xTaskCreatePinnedToCore(task_platform, "task_platform", stackSize_platform, NULL, 1, NULL, 0);
Serial.println( esp_get_minimum_free_heap_size());
Serial.println( esp_get_free_heap_size());
Serial.print("setup finished!\n");
}
// void loop()
// {
// //Serial.printf("main program on core: %u\n", xPortGetCoreID());
// //yield();
// // UBaseType_t m = stackSize_webserver - uxTaskGetStackHighWaterMark(handle_webserver);
// // Serial.printf("webserver stack %u byte\n", m);
// vTaskDelay(1000);
// }
// void task_platform(void * pvParam ) {
void loop()
{
uint8_t index;
UBaseType_t m;
omission = reserved_data.omission;
reaction_mode = reserved_data.reaction_mode;
rotation_quota = reserved_data.rotation_quota;
show_angle = reserved_data.show_angle;
show_height = reserved_data.show_height;
show_length = reserved_data.show_length;
step = reserved_data.step;
transmission_adjust = reserved_data.transmission_adjust;
toward_step = reserved_data.toward_step;
z_quota = reserved_data.z_quota;
z_ratio = reserved_data.z_ratio;
degree_quota = reserved_data.degree_quota;
z_quota_left = z_quota / 2;
//Serial.printf("out millis = %d\n", millis() ); //around 50ms for a loop
//Serial.println( esp_get_minimum_free_heap_size());
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
// while(1)
// {
// esp_task_wdt_reset();
// yield();
// m = stackSize_platform - uxTaskGetStackHighWaterMark(handle_platform);
// Serial.printf("task_platform stack %u byte\n", m);
//Serial.printf("no JY901S_packet ============================= millis = %d\n", millis() ); //around 160ms for a loop
before_loop = 0;
//Serial.printf("task_platform program on core: %u\n", xPortGetCoreID());
//Serial.printf("main program priority: %u\n", uxTaskPriorityGet());
if (new_JY901S_packet == 1)
{
vTaskDelay(1);
//delay(1);
//Serial.printf("millis = %d\n", millis() ); //around 50ms for a loop
new_JY901S_packet = 0;
// put your main code here, to run repeatedly:
//delay(loop_delay_ms); //delay(ms)
//delay(100); //delay(ms)
// delay is set a gap between command
//Serial.printf("new_JY901S_packet ============================= millis = %d\n", millis() ); //around 160ms for a loop
//if (degree_quota > 0)
//{
calculate_angle_limitation();
//Serial.printf("============================\n");
// Serial.printf("Original Roll = %f\n", Roll_F);
// Serial.printf("Original Pitch = %f\n", Pitch_F);
// // Serial.printf(" Yaw = %f\n", Yaw_F); // not support at platform at all
// Serial.printf("\n");
// read pitch
// read roll
// #1 axis: pitch
// #2 axis: roll up
// #3 axis: roll down
// test data
//Pitch_F = -1;
//Roll_F = 2;
length_1 = 100;
length_2 = 100;
length_3 = 100;
if (show_angle) {
//Serial.printf("Raw int Roll_F = %d, base_Roll_F = %d,, Pitch_F = %d, base_Pitch_F = %d\n", (uint16_t)(Roll_F), (uint16_t)(base_Roll_F), (uint16_t)(Pitch_F), (uint16_t)(base_Pitch_F));
//Serial.printf("Raw int Roll_F = %d, base_Roll_F = %d,, Pitch_F = %d, base_Pitch_F = %d, base_Yaw_F = %d, Yaw_F = %d\n", (int16_t)(Roll_F), (int16_t)(base_Roll_F), (int16_t)(Pitch_F), (int16_t)(base_Pitch_F), (int16_t)(base_Yaw_F), (int16_t)(Yaw_F));
}
// if (show_angle) {
// Serial.printf("Original Roll = %d, Pitch = %d\n", Roll_INT, Pitch_INT);
// }
normalize_angle();
if (show_angle) {
//Serial.printf("after normalized Roll_F = %d, base_Roll_F = %d,, Pitch_F = %d, base_Pitch_F = %d, base_Yaw_F = %d, Yaw_F = %d\n", (int16_t)(Roll_F), (int16_t)(base_Roll_F), (int16_t)(Pitch_F), (int16_t)(base_Pitch_F), (int16_t)(base_Yaw_F), (int16_t)(Yaw_F));
//Serial.printf("after normalized Roll = %d, Pitch = %d\n", Roll_INT, Pitch_INT);
}
fit_angle_limitation();
if (show_angle) {
//Serial.printf("after fit: Pitch = %d, Roll = %d\n", Pitch_INT, Roll_INT);
}
posture_adjust();
if (show_angle) {
//Serial.printf("============================= millis = %d\n", millis() ); //around 160ms for a loop
//Serial.printf("after posture_adjust action_status = %d, send data ==> length_1 = %d, length_2 = %d, length_3 = %d\n", action_status, length_1, length_2, length_3);
Serial.printf("after posture_adjust: Pitch = %d, Roll = %d, mills = %d\n", Pitch_INT, Roll_INT, millis());
}
Z_axis_adjust();
//Z_axis_height_JY901b();
if (show_height) {
if ( z_Height != 0 ) {
//Serial.printf("============================= millis = %d\n", millis() ); //around 160ms for a loop
Serial.printf("z_Height = %d, Height = %d, base_height = %d, base_height_F = %f\n", z_Height, Height, base_height, base_height_F);
Serial.printf("after fit: Pitch = %d, Roll = %d\n", Pitch_INT, Roll_INT);
}
}
if (show_length) {
Serial.printf("after Z_axis_height_JY901b action_status = %d, send data ==> length_1 = %d, length_2 = %d, length_3 = %d\n", action_status, length_1, length_2, length_3);
//Serial.printf(" action_status = %d, send data ==> length_1 = %x, length_2 = %x, length_3 = %x\n", action_status, length_1*1250, length_2*1250, length_3*1250);
//Serial.printf(" action_status = %d, send data ==> length_1 = %x, length_2 = %x, length_3 = %x\n", action_status, (length_1+increasing_length)*1250, (length_2+increasing_length)*1250, (length_3+increasing_length)*1250);
}
step_adjust();
//Serial.printf("after step_adjust action_status = %d, send data ==> length_1 = %d, length_2 = %d, length_3 = %d\n", action_status, length_1, length_2, length_3);
boundary_protection();
//Serial.printf("after boundary_protection action_status = %d, send data ==> length_1 = %d, length_2 = %d, length_3 = %d\n", action_status, length_1, length_2, length_3);
// filter noise
if (transmission_adjust == 1)
{
index1 = length_1 / 10;
index2 = length_2 / 10;
index3 = length_3 / 10;
//Serial.printf("length_1_ratio = %d, length_2_ratio = %d, length_3_ratio = %d\n", length_1_ratio, length_2_ratio, length_3_ratio);
length_1_ratio = transmission_adjust_table[index1];
length_2_ratio = transmission_adjust_table[index2];
length_3_ratio = transmission_adjust_table[index3];
transmit(length_1 * length_1_ratio, length_2 * length_2_ratio, length_3 * length_3_ratio );
}
else {
transmit(length_1 * 1250, length_2 * 1250, length_3 * 1250 );
}
if (show_length) {
//Serial.printf("base_height = %d, z_Height = %d, length_1 = %d, length_2 = %d, length_3 = %d\n",base_height, z_Height, length_1, length_2, length_3);
//Serial.printf(" action_status = %d, send data ==> length_1 = %d, length_2 = %d, length_3 = %d\n", action_status, length_1, length_2, length_3);
//Serial.printf(" action_status = %d, send data ==> length_1_previous = %d, length_2_previous = %d, length_3_previous = %d\n", action_status, length_1_previous, length_2_previous, length_3_previous);
}
//toward_to_new_base();
length_1_previous = length_1;
length_2_previous = length_2;
length_3_previous = length_3;
// transmit((length_1+increasing_length)*1250, (length_2+increasing_length)*1250, (length_3+increasing_length)*1250 );
}
else
{
//Serial.printf("No packet *********************************************************************************************\n");
//delay(10);
}
cmd.update();
//yield();
// esp_task_wdt_reset();
// }
} // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void boundary_protection(void)
{
if (length_1 < 0)
length_1 = 0;
if (length_1 >= 200)
length_1 = 200;
if (length_2 < 0)
length_2 = 0;
if (length_2 >= 200)
length_2 = 200;
if (length_3 < 0)
length_3 = 0;
if (length_3 >= 200)
length_3 = 200;
}
void calculate_angle_limitation(void)
{
//uint16_t x_axis_length = 470;
//uint16_t y_axis_length = 550;
pitch_limitation_positive = atan(rotation_quota / x_axis_length) * 180.0 / PI; // remember to use XXX.0 rather than XXX, otherwise will cause result to zero
pitch_limitation_negtive = atan(rotation_quota / x_axis_length) * (-1) * 180.0 / PI;
roll_limitation_positive = atan(rotation_quota / y_axis_length) * 180.0 / PI;
roll_limitation_negtive = atan(rotation_quota / y_axis_length) * (-1) * 180.0 / PI;
// Serial.printf("pitch_limitation_positive = %f, pitch_limitation_negtive = %f\n", pitch_limitation_positive, pitch_limitation_negtive);
// Serial.printf("roll_limitation_positive = %f, roll_limitation_negtive = %f\n", roll_limitation_positive, roll_limitation_negtive);
}
void fit_angle_limitation(void)
{
if ((Pitch_INT > 0 ) && (Pitch_INT > (int16_t)pitch_limitation_positive))
{
Pitch_INT = (int16_t)pitch_limitation_positive;
}
if ((Pitch_INT < 0 ) && (Pitch_INT < (int16_t)pitch_limitation_negtive))
{
Pitch_INT = (int16_t)pitch_limitation_negtive;
}
if ((Roll_INT > 0 ) && (Roll_INT > (int16_t)roll_limitation_positive))
{
Roll_INT = (int16_t)roll_limitation_positive;
}
if ((Roll_INT < 0 ) && (Roll_INT < (int16_t)roll_limitation_negtive))
{
Roll_INT = (int16_t)roll_limitation_negtive;
}
}
void normalize_angle(void)
{
if ((Pitch_F > 180) && (Pitch_F <= 360)) {
Pitch_F = Pitch_F - 360;
}
Pitch_INT = (uint16_t)(Pitch_F - base_Pitch_F);
if (Roll_F < 180.0) {
Roll_F = Roll_F + 360.0;
}
Roll_INT = (uint16_t)(base_Roll_F - Roll_F);
}
void posture_adjust(void)
{
if ((Pitch_INT >= 0) && ( Roll_INT >= 0 ))// rise #1 #2 ++
{
if (reaction_mode == FOLLOWING_MODE) {
action_status = 1;
} else {
Pitch_INT = Pitch_INT * (-1);
Roll_INT = Roll_INT * (-1);
action_status = 4;
}
} else if ((Pitch_INT >= 0) && ( Roll_INT < 0 ))// rise #1 #3 +-
{
//action_status = 2;
if (reaction_mode == FOLLOWING_MODE) {
action_status = 2;
} else {
Pitch_INT = Pitch_INT * (-1);
Roll_INT = Roll_INT * (-1);
action_status = 3;
}
} else if ((Pitch_INT < 0) && ( Roll_INT >= 0 ))// rise #2 #3 -+
{
//action_status = 3;
if (reaction_mode == FOLLOWING_MODE) {
action_status = 3;
} else {
Pitch_INT = Pitch_INT * (-1);
Roll_INT = Roll_INT * (-1);
action_status = 2;
}
} else if ((Pitch_INT < 0) && ( Roll_INT < 0 ))// rise #2 #3 --
{
//action_status = 4;
if (reaction_mode == FOLLOWING_MODE) {
action_status = 4;
} else {
Pitch_INT = Pitch_INT * (-1);
Roll_INT = Roll_INT * (-1);
action_status = 1;
}
}
length_1 = 0;
length_2 = 0;
length_3 = 0;
increasing_length = 0;
switch (action_status) { // pitch is priority than roll
case 1:
// (#1/#3) = pitchc:\Users\Public\Desktop\Logic 2.4.14.lnk
// x_axis_length*tan(pitch) = #1
length_1 = x_axis_length * tan(Pitch_INT * PI / 180); //100 // 0 degree length_1 = 0
length_2 = y_axis_length * tan(Roll_INT * PI / 180); //57
length_1_total = length_1 + (length_2) / 2; // 128
if (length_1_total >= length_2) {
if (length_1_total > rotation_quota) {
length_1_remain = rotation_quota - length_1;
length_2 = length_1_remain * 2;
length_1 = rotation_quota;
} else {
length_1 = length_1_total;
}
} else {
if (length_2 > rotation_quota) {
length_2_remain = rotation_quota - length_2;
length_1 = length_1_remain / 2 + length_1;
length_2 = rotation_quota;
} else {
length_1 = length_1_total;
}
}
//Serial.printf("in case 1: length_1 = %d, length_2 = %d, length_3 = %d\n", length_1, length_2, length_3);
average_length = (max(length_1, length_2)) / 2;
z_quota_left = 100 - average_length;
break;
case 2:
length_1 = x_axis_length * tan(Pitch_INT * PI / 180); // (#1/#3) = pitch
length_3 = y_axis_length * tan(Roll_INT * PI / 180) * (-1); // (#3/#2) = roll
length_1_total = length_1 + (length_3) / 2;
if (length_1_total >= length_2) {
if (length_1_total > rotation_quota) {
length_1_remain = rotation_quota - length_1;
length_3 = length_1_remain * 2;
length_1 = rotation_quota;
} else
{
length_1 = length_1_total;
}
} else {
if (length_3 > rotation_quota) {
length_3_remain = rotation_quota - length_3;
length_1 = length_1_remain / 2 + length_1;
length_2 = rotation_quota;
} else {
length_1 = length_1_total;
}
}
//Serial.printf("in case 1: length_1 = %d, length_2 = %d, length_3 = %d\n", length_1, length_2, length_3);
average_length = (max(length_1, length_3)) / 2;
z_quota_left = 100 - average_length;
break;
case 3:
// (#1/#3) = pitch
length_2 = x_axis_length * tan(Pitch_INT * PI / 180) * (-1); // 11 : 91
length_3 = length_2;
length_2_total = y_axis_length * tan(Roll_INT * PI / 180) + length_3; // 7: 68
if (length_2_total > rotation_quota) {
length_2 = rotation_quota;
} else {
length_2 = length_2_total;
}
length_1 = (length_2 - length_3) / 2; // 68/2 = 34 (34,91,160)
average_length = (length_2 + min(length_1, length_3)) / 2;
z_quota_left = 100 - average_length;
break;
case 4:
// (#1/#2) = pitch
length_2 = x_axis_length * tan(Pitch_INT * PI / 180) * (-1);
length_3 = length_2;
length_3_total = y_axis_length * tan(Roll_INT * PI / 180) * (-1) + length_2;
if (length_3_total > rotation_quota) {
length_3 = rotation_quota;
} else {
length_3 = length_3_total;
}
length_1 = (length_3 - length_2) / 2;
average_length = (length_3 + min(length_1, length_2)) / 2;
z_quota_left = 100 - average_length;
break;
}
increasing_length = base_length - average_length; // move object to central point
// Serial.printf("in case %d: shift_length_1 = %d, shift_length_2 = %d, shift_length_3 = %d\n", action_status, length_1 + increasing_length, length_2 + increasing_length, length_3 + increasing_length );
//Serial.printf("average_length = %d, z_quota_left = %f\n", average_length, z_quota_left);
// Serial.printf("after posture_adjust: length_1 = %d, length_2 = %d, length_3 = %d\n", length_1, length_2, length_3);
/*
// stepping
if (length_1 > step_length)
{
length_1 = step_length;
}
if (length_2 > step_length)
{
length_2 = step_length;
}
if (length_3 > step_length)
{
length_3 = step_length;
}
Serial.printf(" after stepping = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", action_status, length_1, length_2, length_3);
*/
//Serial.printf(" action_status = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", action_status, length_1, length_2, length_3);
//Serial.printf(" in case %d: shift_length_1 = %d, shift_length_2 = %d, shift_length_3 = %d\n", action_status, length_1 + increasing_length, length_2 + increasing_length, length_3 + increasing_length );
length_1 += increasing_length;
length_2 += increasing_length;
length_3 += increasing_length;
}
void step_adjust(void)
{
// change length , but not change angle
// up limitation
if ((length_1 - length_1_previous) > step)
{
length_1 = length_1_previous + step;
}
if ((length_2 - length_2_previous) > step)
{
length_2 = length_2_previous + step;
}
if ((length_3 - length_3_previous) > step)
{
length_3 = length_3_previous + step;
}
// down limitation
if ((length_1_previous - length_1) > step)
{
length_1 = length_1_previous - step;
}
if ((length_2_previous - length_2) > step)
{
length_2 = length_2_previous - step;
}
if ((length_3_previous - length_3) > step)
{
length_3 = length_3_previous - step;
}
}
void toward_to_new_base(void)
{
if (z_Height > 0)
{
base_height_F += toward_step;
base_height = (uint32_t)base_height_F;
//Serial.printf("toward_to_new_base+\n");
}
if (z_Height < 0)
{
base_height_F -= toward_step;
base_height = (uint32_t)base_height_F;
//Serial.printf("toward_to_new_base-\n");
}
}
void transmit(uint32_t length_1, uint32_t length_2, uint32_t length_3 )
{
/*
uint8_t cmd_t0_CH9121_REACTION[] = {0x55, 0xAA, 0x00, 0x00, 0x14, 0x01, 0x00, 0x00, 0xFF, 0xFF,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x03, 0x20,
0x00, 0x02, 0xE3, 0x6B, 0x00, 0x02, 0xE3, 0x6B, 0x00, 0x02, // index of: #1 20~23 , #2 24~27 , #3 28~31
0xE3, 0x6B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // 38 bytes
*/
uint8_t *length_1p, *length_2p, *length_3p;
uint8_t *temp_p;
uint8_t i;
uint8_t sent_bytes;
execution_speed_p = &(cmd_t0_CH9121_REACTION[18]);
length_1p = &(cmd_t0_CH9121_REACTION[20]);
length_2p = &(cmd_t0_CH9121_REACTION[24]);
length_3p = &(cmd_t0_CH9121_REACTION[28]);
*execution_speed_p = execution_speed / 256;
*(execution_speed_p + 1) = execution_speed % 256;
/* test data
length_1 = 0x01020304;
length_2 = 0x01020304;
length_3 = 0x01020304;
*/
// little endian to big endian
temp_p = (uint8_t *)&length_1;
*(length_1p + 0) = *(temp_p + 3);
*(length_1p + 1) = *(temp_p + 2);
*(length_1p + 2) = *(temp_p + 1);
*(length_1p + 3) = *(temp_p + 0);
temp_p = (uint8_t *)&length_2;
*(length_2p + 0) = *(temp_p + 3);
*(length_2p + 1) = *(temp_p + 2);
*(length_2p + 2) = *(temp_p + 1);
*(length_2p + 3) = *(temp_p + 0);
temp_p = (uint8_t *)&length_3;
*(length_3p + 0) = *(temp_p + 3);
*(length_3p + 1) = *(temp_p + 2);
*(length_3p + 2) = *(temp_p + 1);
*(length_3p + 3) = *(temp_p + 0);
/*
Serial.printf("\n new data: ");
for (i = 0; i < 38; i++ )
{
Serial.printf("%x", cmd_t0_CH9121_REACTION[i]); // fill it with characters A..Z
if (i < 37)
{
Serial.printf("-"); // fill it with characters A..Z
}
else {
Serial.printf("\n");
}
}
*/
sent_bytes = Serial1.write(cmd_t0_CH9121_REACTION, sizeof(cmd_t0_CH9121_REACTION)); // ESP32 TX FIFO is about 128 bytes, 125 bytes will fit fine
//bytesSent = Serial1.write(cmd_t0_CH9121_Up, sizeof(cmd_t0_CH9121_Up));
}
void Z_axis_adjust(void)
{
// double base_az_F = 0.995605;
// double g = 9806.65; //9.80665 m/s², s = 1/2at^2
// double delta_g = 0;
double vertical_a = 0;
double one_step_limitaion = 1000;
double ratio = z_ratio;
double t = 0.05; // 50ms, // don't fast than this rate, CPU can't afford to more than 50Hz/20ms
double z_max, z_min, z_tmp_max, z_tmp_min;
double real_add;
int32_t direction; //up down direction
//Serial.printf("Height = %d, base_height = %d, ax_F = %f, ay_F = %f, az_F = %f, Pitch_F = %f, Roll_F = %f\n", Height, base_height, ax_F, ay_F, az_F, Pitch_F, Roll_F);
if ((Height - base_height) > 0)
{
direction = 1;
} else if ((Height - base_height) < 0)
{
direction = -1;
} else
{
direction = 0;
}
z_upper_bound = (z_quota / 2);
z_lower_bound = (-1 * z_quota / 2);
vertical_a = az_F * cos((Pitch_F) * TWO_PI / 360.0) * cos((Roll_F) * TWO_PI / 360.0) - ax_F * sin((Pitch_F) * TWO_PI / 360.0) + ay_F * sin((Roll_F) * TWO_PI / 360.0);
//Serial.printf("vertical_a = %f, ax_F = %f, ay_F = %f, az_F = %f, Pitch_F = %f, Roll_F = %f\n", vertical_a, ax_F, ay_F, az_F, Pitch_F, Roll_F);
//Serial.printf("direction = %d\n", direction);
//Serial.printf("az_F = %f\n", az_F);
delta_g_a = 1 * (vertical_a - base_az_g) * g_a;
//Serial.printf("vertical_a = %f, delta_g_a = %f\n", vertical_a, delta_g_a);
if ( abs(delta_g_a) >= omission ) // eliminate noise: like as gap of Gravity difference, small motion, ...
{ // 50: 9880*0.005
//Serial.printf("delta_g_a = %f\n", delta_g_a);
//Serial.printf("v0 = %f\n", v0);
//if ((delta_g_a * previous_delta_g_a < 0 ) && ( abs(delta_g_a - previous_delta_g_a) > 8000.0)) {
/*
if (delta_g_a * previous_delta_g_a < 0 ) {
v = 0;
v0 = 0;
Serial.printf("reset v0\n");
}
else
*/
{
v = v0 + delta_g_a * t;
//z_distance = z_distance + v0*t + 0.5*delta_g_a*t*t;
z_distance = v0 * t + 0.5 * delta_g_a * t * t; //t : each loop cycle time
v0 = v;
}
//z_distance = z_distance + velocity*0.02*ratio; //0.02 : each loop cycle time
previous_delta_g_a = delta_g_a;
//Serial.printf("v0 = %f\n", v0);
//Serial.printf("z_distance = %f, previous_z_distance = %f\n", z_distance, previous_z_distance);
//Serial.printf("z_distance = %f, millis = %d\n", z_distance, millis() );
if (abs(previous_z_distance - z_distance) >= 100) {
z_distance = (previous_z_distance - z_distance) / 2;
}
//Serial.printf("add_distance = %f, millis = %d\n", add_distance, millis() );
// length_1 = 100 + add_distance;
// length_2 = 100 + add_distance;
// length_3 = 100 + add_distance;
// if (add_distance > 0){
// real_add = min(z_upper_bound, add_distance);
// } else {
// real_add = max(z_lower_bound, add_distance);
// }
//Serial.printf("z_upper_bound = %f, z_lower_bound = %f, add_distance = %f, real_add = %f\n", z_upper_bound, z_lower_bound, add_distance, real_add);
//Serial.printf("direction = %d, z_distance = %f, previous_z_distance = %f\n", direction, z_distance, previous_z_distance);
if (direction = 1) //same direction, that is effective force
//if (direction != 0) //same direction, that is effective force
{
//add_distance = -1 * z_distance;
//add_distance = z_distance*ratio;
z_Height = (int32_t)(z_distance * ratio);
if (z_Height > z_quota_left )
{
z_Height = z_quota_left;
}
if (z_Height < (-1 * z_quota_left))
{
z_Height = -1 * z_quota_left;
}
//Serial.printf("consistancy z_Height = %d\n", z_Height );
//Serial.printf("direction = %d, z_Height = %d\n", direction, z_Height);
if (reaction_mode == FOLLOWING_MODE) {
length_1 += z_Height;
length_2 += z_Height;
length_3 += z_Height;
} else {
length_1 -= z_Height;
length_2 -= z_Height;
length_3 -= z_Height;
}
}
else
{
z_distance = previous_z_distance;
//Serial.printf("direction = %d, z_Height = %d\n", direction, z_Height );
}
previous_z_distance = z_distance;
/*
i_Z_axis_adjust++;
if ((i_Z_axis_adjust%10)==0 )
{
//Serial.printf("add_distance = %f\n", add_distance );
Serial.printf("z_Height = %d\n", z_Height );
}
*/
//Serial.printf(" action_status = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", action_status, length_1, length_2, length_3);
//Serial.printf(" in case %d: shift_length_1 = %d, shift_length_2 = %d, shift_length_3 = %d\n", action_status, length_1 + increasing_length, length_2 + increasing_length, length_3 + increasing_length );
}
else {
v0 = 0;
}
}
void Z_axis_height_JY901b(void)
{
z_Height = Height - base_height;
if (z_Height != 0)
{
toward_to_new_base();
}
if (z_Height >= z_quota_left)
{
z_Height = z_quota_left;
}
if (z_Height <= (-1 * z_quota_left))
{
z_Height = -1 * z_quota_left;
}
//Serial.printf("z_Height = %d, z_Height_previous = %d\n",z_Height, z_Height_previous);
//Serial.printf("z_Height = %d, Pitch_INT = %d, Roll_INT = %d\n",z_Height, Pitch_INT, Roll_INT);
// if ((abs(z_Height_previous - z_Height) <= z_ratio+1) && (z_Height_previous == 0)) //try to filter noise
// {
// z_Height_previous = z_Height;
// z_Height = 0;
// Serial.printf("in z_Height = %d\n",z_Height);
// }
if (abs(z_Height) <= (omission)) //try to filter noise
//if (((abs(Pitch_INT) <= 0 ) && (abs(Roll_INT) <= 0))) //try to filter noise
//if ((Pitch_INT == 0 ) && (Roll_INT == 0)) //try to filter noise
//if (abs(z_Height) <= (omission) ) //try to filter noise
{
z_Height_previous = z_Height;
z_Height = 0;
//Serial.printf("inner z_Height = %d\n",z_Height);
}
else
{
//Serial.printf("move z_Height = %d\n",z_Height);
z_Height_previous = z_Height;
if (reaction_mode == FOLLOWING_MODE) {
length_1 += z_Height;
length_2 += z_Height;
length_3 += z_Height;
} else {
length_1 -= z_Height;
length_2 -= z_Height;
length_3 -= z_Height;
}
}
//Serial.printf(" action_status = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", action_status, length_1, length_2, length_3);
//Serial.printf("z_Height = %d, length_1 = %d, length_2 = %d, length_3 = %d\n",z_Height, length_1, length_2, length_3);
//Serial.printf("z_Height = %d\n",z_Height);
//z_Height_previous = z_Height;
}
void playing_senarion(void)
{
int kk;
omission = 30;
reaction_mode = FOLLOWING_MODE;
rotation_quota = 150;
step = 3;
transmission_adjust = 1;
toward_step = 1;
z_quota = 200;
z_ratio = 15;
degree_quota = 15;
z_quota_left = z_quota / 2;
// up to 200
// return to center
// roll to 15 step 5
// roll to -15 step 5
// pitch to 17 step 5
// pitch down -15 step 5
// run in sin wave a clcle
uint16_t delay_control = 800;
// Injection_Pitch_F = base_Pitch_F;
// Injection_Roll_F = base_Roll_F;
delay(1000);
// Pitch_F = base_Pitch_F;
// Pitch_F = base_Pitch_F;
// transmit(length_1*1250, length_2*1250, length_3*1250 );
execution_speed = delay_control + 1000;
transmit(length_value_table[20], length_value_table[20], length_value_table[20] );
// Serial.printf("cmd_t0_CH9121_Up200mm\n");
// for (kk = 0; kk < 38; kk++)
// {
// Serial.printf(" %x ", cmd_t0_CH9121_REACTION[kk]);
// }
// execution_speed_p = &(cmd_t0_CH9121_Up200mm[18]);
// *execution_speed_p = execution_speed / 256;
// *(execution_speed_p+1) = execution_speed % 256;
// Serial1.write(cmd_t0_CH9121_Up200mm, 38);
delay(delay_control);
execution_speed = delay_control;
transmit(length_value_table[0], length_value_table[0], length_value_table[0] );
// Serial.printf("cmd_t0_CH9121_Original\n");
// for (kk = 0; kk < 38; kk++)
// {
// Serial.printf(" %x ", cmd_t0_CH9121_REACTION[kk]);
// }
// execution_speed_p = &(cmd_t0_CH9121_Original[18]);
// *execution_speed_p = execution_speed / 256;
// *(execution_speed_p+1) = execution_speed % 256;
// Serial1.write(cmd_t0_CH9121_Original, 38);
delay(delay_control);
transmit(length_value_table[10], length_value_table[10], length_value_table[10] ); // 10 cm
// Serial.printf("cmd_to_100mm\n");
// for (kk = 0; kk < 38; kk++)
// {
// Serial.printf(" %x ", cmd_t0_CH9121_REACTION[kk]);
// }
// execution_speed_p = &(cmd_t0_CH9121_Up100mm[18]);
// *execution_speed_p = execution_speed / 256;
// *(execution_speed_p+1) = execution_speed % 256;
// Serial1.write(cmd_t0_CH9121_Up100mm, 38);
delay(delay_control);
Pitch_INT = 15;
Roll_INT = 0;
// Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
//Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1_previous = %d, length_2_previous = %d, length_3_previous = %d\n", Pitch_INT, Roll_INT, length_1_previous, length_2_previous, length_3_previous );
dancing_show();
//Serial.printf("pitch_limitation_positive = %f, pitch_limitation_negtive = %f, roll_limitation_positive = %f, roll_limitation_negtive = %f\n", pitch_limitation_positive, pitch_limitation_negtive, roll_limitation_positive, roll_limitation_negtive );
delay(delay_control);
Pitch_INT = 10;
Roll_INT = -10;
// Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
//Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1_previous = %d, length_2_previous = %d, length_3_previous = %d\n", Pitch_INT, Roll_INT, length_1_previous, length_2_previous, length_3_previous );
dancing_show();
//Serial.printf("pitch_limitation_positive = %f, pitch_limitation_negtive = %f, roll_limitation_positive = %f, roll_limitation_negtive = %f\n", pitch_limitation_positive, pitch_limitation_negtive, roll_limitation_positive, roll_limitation_negtive );
delay(delay_control);
Pitch_INT = 0;
Roll_INT = -15;
// Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
//Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1_previous = %d, length_2_previous = %d, length_3_previous = %d\n", Pitch_INT, Roll_INT, length_1_previous, length_2_previous, length_3_previous );
dancing_show();
//Serial.printf("pitch_limitation_positive = %f, pitch_limitation_negtive = %f, roll_limitation_positive = %f, roll_limitation_negtive = %f\n", pitch_limitation_positive, pitch_limitation_negtive, roll_limitation_positive, roll_limitation_negtive );
delay(delay_control);
Pitch_INT = -10;
Roll_INT = -10;
// Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
//Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1_previous = %d, length_2_previous = %d, length_3_previous = %d\n", Pitch_INT, Roll_INT, length_1_previous, length_2_previous, length_3_previous );
dancing_show();
//Serial.printf("pitch_limitation_positive = %f, pitch_limitation_negtive = %f, roll_limitation_positive = %f, roll_limitation_negtive = %f\n", pitch_limitation_positive, pitch_limitation_negtive, roll_limitation_positive, roll_limitation_negtive );
delay(delay_control);
Pitch_INT = -15;
Roll_INT = 0;
// Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
//Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1_previous = %d, length_2_previous = %d, length_3_previous = %d\n", Pitch_INT, Roll_INT, length_1_previous, length_2_previous, length_3_previous );
dancing_show();
delay(delay_control);
Pitch_INT = -10;
Roll_INT = 10;
// Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
dancing_show();
delay(delay_control);
Pitch_INT = 0;
Roll_INT = 15;
// Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
dancing_show();
delay(delay_control);
Pitch_INT = 10;
Roll_INT = 10;
// Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
//Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1_previous = %d, length_2_previous = %d, length_3_previous = %d\n", Pitch_INT, Roll_INT, length_1_previous, length_2_previous, length_3_previous );
dancing_show();
delay(delay_control);
Pitch_INT = 15;
Roll_INT = 0;
// Serial.printf("Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
dancing_show();
delay(delay_control);
execution_speed_p = &(cmd_t0_CH9121_Up100mm[18]);
*execution_speed_p = execution_speed / 256;
*(execution_speed_p + 1) = execution_speed % 256;
Serial1.write(cmd_t0_CH9121_Up100mm, 38);
delay(delay_control);
int ii;
execution_speed = 50;
// for (ii = 0; ii < 3; ii++ )
// {
transmit(length_value_table[20], length_value_table[20], length_value_table[20] ); // 10 cm
Serial.printf("cmd_t0_CH9121_Up200mm\n");
// for (kk = 0; kk < 38; kk++)
// {
// Serial.printf(" %x ", cmd_t0_CH9121_REACTION[kk]);
// }
// execution_speed_p = &(cmd_t0_CH9121_Up200mm[18]);
// *execution_speed_p = execution_speed / 256;
// *(execution_speed_p+1) = execution_speed % 256;
// Serial1.write(cmd_t0_CH9121_Up200mm, 38);
delay(1000);
transmit(length_value_table[10], length_value_table[10], length_value_table[10] ); // 10 cm
Serial.printf("cmd_t0_CH9121_Up100mm\n");
// for (kk = 0; kk < 38; kk++)
// {
// Serial.printf(" %x ", cmd_t0_CH9121_REACTION[kk]);
// }
// execution_speed_p = &(cmd_t0_CH9121_Up100mm[18]);
// *execution_speed_p = execution_speed / 256;
// *(execution_speed_p+1) = execution_speed % 256;
// Serial1.write(cmd_t0_CH9121_Up100mm, 38);
delay(1000);
transmit(length_value_table[17], length_value_table[17], length_value_table[17] ); // 10 cm
Serial.printf("cmd_t0_CH9121_Up170mm\n");
// for (kk = 0; kk < 38; kk++)
// {
// Serial.printf(" %x ", cmd_t0_CH9121_REACTION[kk]);
// }
// execution_speed_p = &(cmd_t0_CH9121_Up170mm[18]);
// *execution_speed_p = execution_speed / 256;
// *(execution_speed_p+1) = execution_speed % 256;
// Serial1.write(cmd_t0_CH9121_Up170mm, 38);
delay(1000);
transmit(length_value_table[10], length_value_table[10], length_value_table[10] ); // 10 cm
Serial.printf("cmd_t0_CH9121_Up100mm\n");
// for (kk = 0; kk < 38; kk++)
// {
// Serial.printf(" %x ", cmd_t0_CH9121_REACTION[kk]);
// }
// execution_speed_p = &(cmd_t0_CH9121_Up100mm[18]);
// *execution_speed_p = execution_speed / 256;
// *(execution_speed_p+1) = execution_speed % 256;
// Serial1.write(cmd_t0_CH9121_Up100mm, 38);
delay(1000);
transmit(length_value_table[13], length_value_table[13], length_value_table[13] ); // 10 cm
Serial.printf("cmd_t0_CH9121_Up130mm\n");
// for (kk = 0; kk < 38; kk++)
// {
// Serial.printf(" %x ", cmd_t0_CH9121_REACTION[kk]);
// }
// execution_speed_p = &(cmd_t0_CH9121_Up130mm[18]);
// *execution_speed_p = execution_speed / 256;
// *(execution_speed_p+1) = execution_speed % 256;
// Serial1.write(cmd_t0_CH9121_Up130mm, 38);
delay(1000);
transmit(length_value_table[10], length_value_table[10], length_value_table[10] ); // 10 cm
Serial.printf("cmd_t0_CH9121_Up100mm\n");
// for (kk = 0; kk < 38; kk++)
// {
// Serial.printf(" %x ", cmd_t0_CH9121_REACTION[kk]);
// }
// execution_speed_p = &(cmd_t0_CH9121_Up100mm[18]);
// *execution_speed_p = execution_speed / 256;
// *(execution_speed_p+1) = execution_speed % 256;
// Serial1.write(cmd_t0_CH9121_Up100mm, 38);
delay(1000);
// }
//////////////////////////////////
execution_speed = 1000;
transmit(length_value_table[10], length_value_table[10], length_value_table[10] ); // 10 cm
Serial.printf("cmd_t0_CH9121_Up100mm\n");
// execution_speed_p = &(cmd_t0_CH9121_Up100mm[18]);
// *execution_speed_p = execution_speed / 256;
// *(execution_speed_p+1) = execution_speed % 256;
Serial1.write(cmd_t0_CH9121_Up100mm, 38);
Serial.printf("cmd_t0_CH9121_Up100mm\n");
delay(1000);
Pitch_INT = 0;
Roll_INT = 0;
length_1_previous = 100;
length_2_previous = 100;
length_3_previous = 100;
Serial.printf("leave playing_senarion\n");
delay(2000);
}
void dancing_show(void) {
calculate_angle_limitation();
length_1 = 100;
length_2 = 100;
length_3 = 100;
// normalize_angle();
//Serial.printf("pitch_limitation_positive = %f, pitch_limitation_negtive = %f, roll_limitation_positive = %f, roll_limitation_negtive = %f\n", pitch_limitation_positive, pitch_limitation_negtive, roll_limitation_positive, roll_limitation_negtive );
//fit_angle_limitation();
Serial.printf("dancing_show fit_angle_limitation Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
posture_adjust();
// Serial.printf("dancing_show posture_adjust Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
// Z_axis_adjust();
//step_adjust();
//Serial.printf("dancing_show step_adjust Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
boundary_protection();
//Serial.printf("dancing_show boundary_protection Pitch_INT = %d: Roll_INT = %d, length_1 = %d, length_2 = %d, length_3 = %d\n", Pitch_INT, Roll_INT, length_1, length_2, length_3 );
if (transmission_adjust == 1)
{
index1 = length_1 / 10;
index2 = length_2 / 10;
index3 = length_3 / 10;
//Serial.printf("length_1_ratio = %d, length_2_ratio = %d, length_3_ratio = %d\n", length_1_ratio, length_2_ratio, length_3_ratio);
length_1_ratio = transmission_adjust_table[index1];
length_2_ratio = transmission_adjust_table[index2];
length_3_ratio = transmission_adjust_table[index3];
transmit(length_1 * length_1_ratio, length_2 * length_2_ratio, length_3 * length_3_ratio );
}
else {
transmit(length_1 * 1250, length_2 * 1250, length_3 * 1250 );
}
length_1_previous = length_1;
length_2_previous = length_2;
length_3_previous = length_3;
}
jy901s.cpp
#include <Arduino.h>
#include "init.h"
void onReceiveUart2_from_JY901S(void)
{
uint8_t index;
uint8_t *function_id_p;
uint16_t function_id;
uint8_t serial2_left_bytes;
uint8_t *command_p, checksum;
//Serial.printf(">>>>>>>>>>>>>>>>>>>>>>>>>\n");
//Serial.printf("millis = %d\n", millis() ); //around 160ms for a loop
// This is a callback function that will be activated on UART RX events
size_t available = Serial2.available();
serial2_received_bytes = available;
//Serial.printf("onReceive Callback:: There are %d bytes available: ", available);
index = 0;
while (available--) {
UART2_received[index] = (uint8_t)Serial2.read();
index++;
}
// for (uint8_t i = 0; i < serial2_received_bytes; i++)
// {
// Serial.printf("%x", UART2_received[i]); // fill it with characters A..Z
// if (i < serial2_received_bytes - 1)
// {
// Serial.printf("-"); // fill it with characters A..Z
// }
// }
// Serial.printf(" Serial2\n");
serial2_left_bytes = serial2_received_bytes;
command_p = &UART2_received[0];
while (serial2_left_bytes >= COMMAND_SEGMENT_LENGTH)
{
checksum = 0;
function_id_p = command_p;
for (uint8_t i = 0; i < COMMAND_SEGMENT_LENGTH-1; i++)
{
checksum += *(command_p+i);
}
if (checksum != *(command_p+COMMAND_SEGMENT_LENGTH-1))
{
return;
}
//Serial.printf(" function_id = %08x\n", *function_id_p); //don't print this line for 0x55
//Serial.printf(" function_id = %08x\n", *(function_id_p+1));
function_id = (*function_id_p<<8) + *(function_id_p+1);
// ax = (*ax_p<<8) + *(ax_p+1);
// ay = (*ay_p<<8) + *(ay_p+1);
// az = (*az_p<<8) + *(az_p+1);
// Serial.printf(" function_id = %08x\n", function_id);
// Serial.printf(" ax = %08x\n", ax);
// Serial.printf(" ay = %08x\n", ay);
// Serial.printf(" az = %08x\n", az);
switch (function_id)
{
case 0x5551:
//ax=((AxH<<8)|AxL)/32768*16g(g 为重力加速度，可取 9.8m/s^2)
//ay=((AyH<<8)|AyL)/32768*16g(g 为重力加速度，可取 9.8m/s^2)
//az=((AzH<<8)|AzL)/32768*16g(g 为重力加速度，可取 9.8m/s^2)
axL = command_p+2;
axH = command_p+3;
ax = ((uint16_t)(*axH<<8)|(uint16_t)*axL);
//Serial.printf(" ax = %d\n", ax);
ax_p = (int16_t*)&ax;
//Serial.printf(" *ax_p = %d\n", *ax_p);
ax_F = ((*ax_p)/32768.0f)*16.0f;
//Serial.printf(" ax_F = %f\n", ax_F);
ayL = command_p+4;
ayH = command_p+5;
ay = (uint16_t)(*ayH<<8)|(uint16_t)*ayL;
//Serial.printf(" ay = %d\n", ay);
ay_p = (int16_t*)&ay;
//Serial.printf(" *ay_p = %d\n", *ay_p);
ay_F = ((*ay_p)/32768.0f)*16.0f;
//Serial.printf(" ay_F = %f\n", ay_F);
azL = command_p+6;
azH = command_p+7;
az = (uint16_t)(*azH<<8)|(uint16_t)*azL;
//Serial.printf(" az = %d\n", az);
az_p = (int16_t*)&az;
//Serial.printf(" *az_p = %d\n", *az_p);
az_F = ((*az_p)/32768.0f)*16.0f;
//Serial.printf(" az_F = %f\n", az_F);
TL = command_p+8;
TH = command_p+9;
T = (uint16_t)(*TH<<8)|(uint16_t)*TL;
T_F = T/100.0f;
//Serial.printf(" T = %f\n", T_F);
break;
case 0x5552:
//wx=((wxH<<8)|wxL)/32768*2000(°/s)
//wy=((wyH<<8)|wyL)/32768*2000(°/s)
//wz=((wzH<<8)|wzL)/32768*2000(°/s)
wxL = command_p+2;
wxH = command_p+3;
wx = (uint16_t)(*wxH<<8)|(uint16_t)*wxL;
wx_F = (wx/32768.0f)*2000.0f;
//Serial.printf(" wxL = %d\n", *wxL);
//Serial.printf(" wxH = %d\n", *wxH);
//Serial.printf(" wx = %d\n", wx);
//Serial.printf(" wx = %f\n", wx_F);
wyL = command_p+4;
wyH = command_p+5;
wy = (uint16_t)(*wyH<<8)|(uint16_t)*wyL;
wy_F = (wy/32768.0f)*2000.0f;
//Serial.printf(" wy = %f\n", wy_F);
wzL = command_p+6;
wzH = command_p+7;
wz = (uint16_t)(*wzH<<8)|(uint16_t)*wzL;
wz_F = (wz/32768.0f)*2000.0f;
//Serial.printf(" wz = %f\n", wz_F);
TL = command_p+8;
TH = command_p+9;
T = (uint16_t)(*TH<<8)|(uint16_t)*TL;
T_F = T/100.0f;
//Serial.printf(" T = %f\n", T_F);
break;
case 0x5553:
// uint16_t usCRC16, usTemp, i, usData[4];
// usData[0] = ((uint16_t)s_ucWitDataBuff[3] << 8) | (uint16_t)s_ucWitDataBuff[2];
// fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f;
// fGyro[i] = sReg[GX+i] / 32768.0f * wz.0f;
// fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f;
// 滾轉角（x 軸）Roll=((RollH<<8)|RollL)/32768*180(°)
// 俯仰角（y 轴）Pitch=((PitchH<<8)|PitchL)/32768*180(°)
// 偏航角（z 轴）Yaw=((YawH<<8)|YawL)/32768*180(°)
// 温度计算公式：T=((TH<<8)|TL) /100 ℃
// 校验和：Sum=0x55+0x53+RollH+RollL+PitchH+PitchL+YawH+YawL+TH+TL
RollL = command_p+2;
RollH = command_p+3;
Roll = (uint16_t)(*RollH<<8)|(uint16_t)*RollL;
Roll_F = (Roll/32768.0f)*180.0f;
//Serial.printf(" RollL = %d\n", *RollL);
//Serial.printf(" RollH = %d\n", *RollH);
//Serial.printf(" Roll = %d\n", Roll);
//Serial.printf(" Roll = %f\n", Roll_F);
PitchL = command_p+4;
PitchH = command_p+5;
Pitch = (uint16_t)(*PitchH<<8)|(uint16_t)*PitchL;
Pitch_F = (Pitch/32768.0f)*180.0f;
//Serial.printf(" Pitch = %f\n", Pitch_F);
// YawL = command_p+6;
// YawH = command_p+7;
// Yaw = (uint16_t)(*YawH<<8)|(uint16_t)*YawL;
// Yaw_F = (Yaw/32768.0f)*180.0f;
if (before_loop == 1)
{
base_Pitch_F = Pitch_F;
if ((base_Pitch_F > 180.0) && (base_Pitch_F <= 360.0)) {
base_Pitch_F = base_Pitch_F - 360.0;
}
base_Roll_F = Roll_F;
// if ((base_Roll_F > 180.0) && (base_Roll_F <= 360.0)) {
// base_Roll_F = base_Roll_F - 360.0;
// }
if (base_Roll_F < 180.0) {
base_Roll_F = base_Roll_F + 360.0;
}
normalize_angle();
// base_Yaw_F = Yaw_F;
//vertical_a = az_F * cos((360.0-Pitch_F)*TWO_PI/360.0)* cos((Roll_F-180.0)*TWO_PI/360.0) - ax_F * sin((360.0-Pitch_F)*TWO_PI/360.0) + ay_F * sin((Roll_F-180.0)*TWO_PI/360.0);
//base_az_g = az_F * cos((360.0-Pitch_F)*TWO_PI/360.0)* cos((Roll_F-180.0)*TWO_PI/360.0) - ax_F * sin((360.0-Pitch_F)*TWO_PI/360.0) + ay_F * sin((Roll_F-180.0)*TWO_PI/360.0);
base_az_g = az_F * cos((Pitch_F)*TWO_PI/360.0)* cos((Roll_F)*TWO_PI/360.0) - ax_F * sin((Pitch_F)*TWO_PI/360.0) + ay_F * sin((Roll_F)*TWO_PI/360.0);
//Serial.printf("setup base_az_g = %f, ax_F = %f, ay_F = %f, az_F = %f, Pitch_F = %f, Roll_F = %f, Yaw_F = %f\n", base_az_g, ax_F, ay_F, az_F, Pitch_F, Roll_F, Yaw_F);
//Serial.printf("base_az_g = %f\n", base_az_g);
}
//Serial.printf(" Yaw = %f\n", Yaw_F);
// TL = command_p+8;
// TH = command_p+9;
// T = (uint16_t)(*TH<<8)|(uint16_t)*TL;
// T_F = T/100.0f;
//Serial.printf(" T = %f\n", T_F);
break;
case 0x5554:
//磁场（x 轴）Hx=(( HxH<<8)| HxL)
//磁场（y 轴）Hy=(( HyH <<8)| HyL)
//磁场（z 轴）Hz=(( HzH<<8)| HzL)
HxL = command_p+2;
HxH = command_p+3;
Hx = (uint16_t)(*HxH<<8)|(uint16_t)*HxL;
Hx_F = (Hx/32768.0f)*180.0f;
//Serial.printf(" HxL = %d\n", *HxL);
//Serial.printf(" HxH = %d\n", *HxH);
//Serial.printf(" Hx = %d\n", Hx);
//Serial.printf(" Hx = %f\n", Hx_F);
HyL = command_p+4;
HyH = command_p+5;
Hy = (uint16_t)(*HyH<<8)|(uint16_t)*HyL;
Hy_F = (Hy/32768.0f)*180.0f;
//Serial.printf(" Hy = %f\n", Hy_F);
HzL = command_p+6;
HzH = command_p+7;
Hz = (uint16_t)(*HzH<<8)|(uint16_t)*HzL;
Hz_F = (Hz/32768.0f)*180.0f;
//Serial.printf(" Hz = %f\n", Hz_F);
// TL = command_p+8;
// TH = command_p+9;
// T = (uint16_t)(*TH<<8)|(uint16_t)*TL;
// T_F = T/100.0f;
// Serial.printf(" T = %f\n", T_F);
break;
case 0x5556:
Height = ((uint32_t)(*(command_p+6)) + (uint32_t)(*(command_p+7)<<8) + (uint32_t)(*(command_p+8)<<16) + (uint32_t)(*(command_p+9)<<24))/10; //cm
Height = Height *z_ratio; // adjust ratio
//Serial.printf("H0 = %x, H1 = %x,H2 = %x,H3 = %x, Height = %d \n", *(command_p+6), *(command_p+7), *(command_p+8), *(command_p+9), Height);
if (before_loop == 1)
{
base_height = Height;
base_height_F = (double)base_height;
//Serial.printf("base_height = %d\n", base_height);
}
//Serial.printf("0x5556====== millis = %d\n", millis() ); //around 160ms for a loop
break;
}
serial2_left_bytes -= COMMAND_SEGMENT_LENGTH;
command_p += COMMAND_SEGMENT_LENGTH;
new_JY901S_packet = 1;
}
//Serial.printf("millis = %d\n", millis() ); //around 160ms for a loop
//Serial.printf("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n");
}
init.cpp
////
#include <Arduino.h>
#include "init.h"
// UART1: balance_platform
// UART2: JY901S
uint8_t UART1_received[DATA_SIZE + 1];
uint8_t UART2_received[DATA_SIZE + 1];
// balance_platform command
// 绝对时间指的是以第一条运动数据指令开始计算的时间值，单位是毫秒。L 表示四字节长度的整数。
// 电机的绝对位置，每个轴的位置数据都是 4 字节长度的，高位在前的，有符号整数。0xff ff ff ff 表示-1 位
uint8_t UART1_TX_COMMAND_1[] = { 0x55, 0xAA, 0x00, 0x00, 0x14, 0x01 , 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, // 16 per line
0x00, 0x00, 0x03, 0x20, 0x00, 0x02, 0xE3, 0x6B, 0x00, 0x02, 0xE3, 0x6B, 0x00, 0x02, 0xE3, 0x6B,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
// up 100mm
uint8_t cmd_t0_CH9121_Up100mm[] = {0x55, 0xAA, 0x00, 0x00, 0x14, 0x01, 0x00, 0x00, 0xFF, 0xFF,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x1F, 0x40, //8000
0x00, 0x01, 0xE5, 0xFC, 0x00, 0x01, 0xE5, 0xFC, 0x00, 0x01,
0xE5, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
}; // 38 bytes
uint8_t cmd_t0_CH9121_Up130mm[] = {0x55, 0xAA, 0x00, 0x00, 0x14, 0x01, 0x00, 0x00, 0xFF, 0xFF,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x1F, 0x40, //8000
0x00, 0x02, 0x41, 0x21, 0x00, 0x02, 0x41, 0x21, 0x00, 0x02,
0x41, 0x21, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
}; // 38 bytes
uint8_t cmd_t0_CH9121_Up170mm[] = {0x55, 0xAA, 0x00, 0x00, 0x14, 0x01, 0x00, 0x00, 0xFF, 0xFF,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x1F, 0x40, //8000
0x00, 0x02, 0xBF, 0x6A, 0x00, 0x02, 0xBF, 0x6A, 0x00, 0x02,
0xBF, 0x6A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
}; // 38 bytes
// up 200mm
uint8_t cmd_t0_CH9121_Up200mm[] = {0x55, 0xAA, 0x00, 0x00, 0x14, 0x01, 0x00, 0x00, 0xFF, 0xFF,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x1F, 0x40, //8000
0x00, 0x03, 0xD0, 0xA4, 0x00, 0x03, 0xD0, 0xA4, 0x00, 0x03,
0xD0, 0xA4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
}; // 38 bytes
// original
// uint8_t cmd_t0_CH9121_Original[] = {0x55, 0xAA, 0x00, 0x00, 0x14, 0x01, 0x00, 0x00, 0xFF, 0xFF,
// 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x13, 0x88, //5000
// 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
// 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // 38 bytes
uint8_t cmd_t0_CH9121_Original[] = {0x55, 0xAA, 0x00, 0x00, 0x14, 0x01, 0x00, 0x00, 0xFF, 0xFF,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x1F, 0x40, //8000
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
}; // 38 bytes
// JY901S command
// save config: { 0xFF, 0xAA, 0x00, SAVE, 0x00 };
/*
0：保持当前配置
1：恢复默认（出厂）配置并保存
*/
uint8_t UART2_UNLOCK_COMMAND_69[] = { 0xFF, 0xAA, 0x69, 0x88, 0xB5};
uint8_t UART2_SAVE_COMMAND_00[] = { 0xFF, 0xAA, 0x00, 0x00, 0x00};
// set report contain: { 0xFF, 0xAA, 0x02, RSWL, RSWH };
// RSWL
// 0x57, 0x56, 0x55, 0x54, 0x53, 0x52, 0x51, 0x50
// RSWH
// X, X, X, X, X, 0x5A, 0x59, 0x58
// 0x57 经纬度包 8
// 0x56 气压&高度包 4
// 0x55 端口状态 2
// 0x54 磁场信息包 1
// 0x53 角度信息包 8
// 0x52 角速度信息包 4
// 0x51 加速度信息包 2
// 0x50 时间信息包 1
// 0x5A 卫星定位精度 4
// 0x59 四元素输出包 2
// 0x58 地速数据包 1
uint8_t UART2_return_data_COMMAND_02[] = { 0xFF, 0xAA, 0x02, 0x4A, 0x00};
//
// set report rate: { 0xFF, 0xAA, 0x03, RATE, 0x00 };
/*
RATE：回传速率
0x00：0.1Hz // no such
0x01：0.2Hz
0x02：0.5Hz
0x03：1Hz
0x04：2Hz
0x05：5Hz
0x06：10Hz（默认） // fit 9600
0x07：20Hz
0x08：50Hz
0x09：100Hz // fit 115200
0x0a：无（保留）
0x0b：200Hz
0x0c：单次输出
*/
/*
RATE：return rate
0x01：0.1Hz
0x02：0.5Hz
0x03：1Hz
0x04：2Hz
0x05：5Hz
0x06：10Hz（default）
0x07：20Hz
0x08：50Hz // don't over this rate, CPU can't afford to more than 50Hz/20ms
0x09：100Hz <---
0x0a：125Hz
0x0b：200Hz
0x0c：Single
0x0d: Not output
*/
uint8_t UART2_REPORT_RATE_COMMAND_03[] = { 0xFF, 0xAA, 0x03, 0x07, 0x00};
// rate 00 is not work
/**********************************
set baudrate: {0xFF, 0xAA, 0x04, BAUD, 0x0}
BAUD：波特率设置
0x00：2400
0x01：4800
0x02：9600（默认）
0x03：19200
0x04：38400
0x05：57600
0x06：115200
0x07：230400 <<----------------------
0x08：460800
0x09：921600
************************************/
uint8_t UART2_REPORT_BAUD_RATE_COMMAND_04[] = { 0xFF, 0xAA, 0x04, 0x07, 0x00};
//Serial使用GPIO1->TX0，GPIO3->RX0，通常用於USB通訊和寫程式debug使用，預設系統使用
//Serial1使用GPIO10->TX1，GPIO9->RX1，通常用於SPI flash上，預設系統未使用
//Serial2使用GPIO17->TX2，GPIO16->RX2，通常用於自行使用，大部分的人都會使用這個，預設系統未使用
void init_serial(void) {
// UART0 will be used to log information into Serial Monitor
Serial.begin(115200);
//Serial.onReceive(onReceiveUart0_from_console, true); // sets a RX callback function for Serial 1
// Serial1 for platform
Serial1.begin(BAUD1, SERIAL_8N1, RX1PIN, TX1PIN); // Rx = 4, Tx = 5 will work for ESP32, S2, S3 and C3
Serial1.setRxTimeout(RXTIMEOUT);
Serial1.setRxFIFOFull(FIFIFULL);
//Serial1.onReceive(onReceiveUart1_from_balance_platform, true); // sets a RX callback function for Serial 1
// Serial2 for JY901S
Serial2.begin(230400, SERIAL_8N1, RX2PIN, TX2PIN); // Rx = 4, Tx = 5 will work for ESP32, S2, S3 and C3
//Serial2.begin(9600, SERIAL_8N1, RX2PIN, TX2PIN); // Rx = 4, Tx = 5 will work for ESP32, S2, S3 and C3
Serial2.setRxTimeout(RXTIMEOUT);
Serial2.setRxFIFOFull(FIFIFULL);
Serial2.onReceive(onReceiveUart2_from_JY901S, true); // sets a RX callback function for Serial 1
Serial.printf("init_serial done\n");
}
void init_platform(void) {
int bytesSent;
bytesSent = Serial1.write(cmd_t0_CH9121_Up100mm, sizeof(cmd_t0_CH9121_Up100mm));
//Serial.printf("1. bytesSent length = %d\n", bytesSent);
//cmd_t0_CH9121_Original
delay(5000);
//bytesSent = Serial1.write(cmd_t0_CH9121_Original, sizeof(cmd_t0_CH9121_Original));
//Serial.printf("2. bytesSent length = %d\n", bytesSent);
}
void init_JY901S(void) {
// set report rate
// 0xFF 0xAA 0x03 RATE 0x00
int bytesSent;
//bytesSent = Serial2.write(UART2_UNLOCK_COMMAND_69, sizeof(UART2_UNLOCK_COMMAND_69)); //JY901s use this command, JY61P not use this
//Serial.printf("bytesSent length of UART2_UNLOCK_COMMAND_69 = %x\n", 0x6988B5);
delay(1);
bytesSent = Serial2.write(UART2_return_data_COMMAND_02, sizeof(UART2_return_data_COMMAND_02));
//Serial.printf("bytesSent length of UART2_return_data_COMMAND_02 = %x\n", UART2_return_data_COMMAND_02[3]);
delay(1);
bytesSent = Serial2.write(UART2_REPORT_RATE_COMMAND_03, sizeof(UART2_REPORT_RATE_COMMAND_03));
//Serial.printf("bytesSent length of UART2_REPORT_RATE_COMMAND_03 = %x\n", UART2_REPORT_RATE_COMMAND_03[3]);
delay(1);
// must change init_serial()/Serial2.begin(BAUD2==>115200, after "reboot"
bytesSent = Serial2.write(UART2_REPORT_BAUD_RATE_COMMAND_04, sizeof(UART2_REPORT_BAUD_RATE_COMMAND_04));
//Serial.printf("bytesSent length of UART2_REPORT_BAUD_RATE_COMMAND_04 = %x\n", UART2_REPORT_BAUD_RATE_COMMAND_04[3]);
delay(1);
bytesSent = Serial2.write(UART2_SAVE_COMMAND_00, sizeof(UART2_SAVE_COMMAND_00));
//Serial.printf("bytesSent length of UART2_SAVE_COMMAND_00 = %d\n", UART2_SAVE_COMMAND_00[3]);
}
init.h
#include <Commander.h>
#include "WiFi.h"
//#include <WebServer.h>
#include <ESPAsyncWebServer.h>
#include <SPIFFS.h>
#include <HTTPClient.h>
#include "time.h"
#include <ESPmDNS.h>
// Serial
#define TX0PIN 1 // used by USB/UART/Serial
#define RX0PIN 3 // used by USB/UART/Serial
#define BAUD0 115200 // Any baudrate from 300 to 115200
//Serial1 // connect to ethernet module
#define TX1PIN 10 // used by USB/UART/Serial1
#define RX1PIN 9 // used by USB/UART/Serial1
#define BAUD1 115200//38400 //57600 //115200 // Any baudrate from 300～921600bps
//Serial2 // connect to JY901s
#define TX2PIN 17 // used by USB/UART/Serial2
#define RX2PIN 16 // used by USB/UART/Serial2
#define BAUD2 115200 // Any baudrate from 300 to 115200
#define RXTIMEOUT 50 //minisecond
#define FIFIFULL 32 // byte
#define DATA_SIZE 500
#define COMMAND_SEGMENT_LENGTH 11
#define FOLLOWING_MODE 1
#define COMPENSATION_MODE -1
#define MAX_LENGTH 200
#define EEPROM_SIZE 128
struct MyStructure {
uint16_t omission;
int8_t reaction_mode;
double rotation_quota;
uint8_t show_angle;
uint8_t show_height;
uint8_t show_length;
int16_t step;
double toward_step;
uint8_t transmission_adjust;
double z_quota;
uint16_t z_ratio;
int16_t degree_quota;
};
void init_serial(void);
void normalize_angle(void);
void onReceiveUart0_from_console(void);
void onReceiveUart1_from_balance_platform(void);
void onReceiveUart2_from_JY901S(void);
void init_JY901S(void);
void init_platform(void);
void initialiseCommander(void);
void init_WiFi(void);
void init_WebServer(void);
//void WebServer( void * pvParam );
//void task_platform(void * pvParam );
bool degree_quota_IntHandler(Commander &Cmdr);
bool getFloatHandler(Commander &Cmdr);
bool omission_IntHandler(Commander &Cmdr);
bool reaction_StringsHandler(Commander &Cmdr);
bool reset_default_parameter_structHandler(Commander &Cmdr);
bool rotation_quota_FloatHandler(Commander &Cmdr);
bool save_parameter_structHandler(Commander &Cmdr);
bool show_angle_IntHandler(Commander &Cmdr);
bool show_height_IntHandler(Commander &Cmdr);
bool show_length_IntHandler(Commander &Cmdr);
bool show_parameter_structHandler(Commander &Cmdr);
bool step_IntHandler(Commander &Cmdr);
bool transmission_adjust_IntHandler(Commander &Cmdr);
bool toward_step_FloatHandler(Commander &Cmdr);
bool z_quota_FloatHandler(Commander &Cmdr);
bool z_ratio_IntHandler(Commander &Cmdr);
bool helloHandler(Commander &Cmdr);
bool hiddenHandler(Commander &Cmdr);
bool setFloatHandler(Commander &Cmdr);
bool setFloatsHandler(Commander &Cmdr);
bool setIntHandler(Commander &Cmdr);
bool setIntsHandler(Commander &Cmdr);
bool setStringsHandler(Commander &Cmdr);
void set_degree_quota(void);
bool reset_default_parameter(void);
bool save_parameter(void);
//extern AsyncWebServer server(80);
extern MyStructure reserved_data;
extern uint16_t omission;
extern int8_t reaction_mode;
extern double rotation_quota;
extern uint8_t show_angle;
extern uint8_t show_height;
extern uint8_t show_length;
extern int16_t step;
extern double toward_step;
extern uint8_t transmission_adjust;
extern double z_quota;
extern uint16_t z_ratio;
extern int16_t degree_quota;
extern Commander cmd;
extern uint8_t new_JY901S_packet;
extern uint8_t UART0_received[DATA_SIZE + 1];
extern uint8_t UART1_received[DATA_SIZE + 1];
extern uint8_t UART2_received[DATA_SIZE + 1];
extern uint8_t cmd_t0_CH9121_REACTION[];
extern uint8_t cmd_t0_CH9121_Up100mm[];
extern uint8_t cmd_t0_CH9121_Up130mm[];
extern uint8_t cmd_t0_CH9121_Up170mm[];
extern uint8_t cmd_t0_CH9121_Up200mm[];
extern uint8_t cmd_t0_CH9121_Original[];
extern uint8_t serial0_received_bytes;
extern uint8_t serial1_received_bytes;
extern uint8_t serial2_received_bytes;
// read current as base number
extern double base_Roll_F;
extern double base_Pitch_F;
extern double base_Yaw_F;
extern int32_t Height;
extern int32_t base_height;
extern double base_height_F;
extern double toward_step;
extern uint8_t before_loop;
extern uint8_t *TH, *TL;
extern uint16_t T;
extern double T_F;
// 0x5551 variables
extern double base_az_g;
extern uint8_t *axH, *axL;
extern uint16_t ax;
extern int16_t *ax_p;
extern double ax_F;
extern uint8_t *ayH, *ayL;
extern uint16_t ay;
extern int16_t *ay_p;
extern double ay_F;
extern uint8_t *azH, *azL;
extern uint16_t az;
extern int16_t *az_p;
extern double az_F;
// 0x5552 variables
extern uint8_t *wxH, *wxL;
extern uint16_t wx;
extern double wx_F;
extern uint8_t *wyH, *wyL;
extern uint16_t wy;
extern double wy_F;
extern uint8_t *wzH, *wzL;
extern uint16_t wz;
extern double wz_F;
// 0x5553 variables
extern uint8_t *RollH, *RollL;
extern uint16_t Roll;
extern double Roll_F;
extern volatile int16_t Roll_INT;
extern uint8_t *PitchH, *PitchL;
extern uint16_t Pitch;
extern double Pitch_F;
extern volatile int16_t Pitch_INT;
extern uint8_t *YawH, *YawL;
extern uint16_t Yaw;
extern double Yaw_F;
// 0x5554 variables
extern uint8_t *HxH, *HxL;
extern uint16_t Hx;
extern double Hx_F;
extern uint8_t *HyH, *HyL;
extern uint16_t Hy;
extern double Hy_F;
extern uint8_t *HzH, *HzL;
extern uint16_t Hz;
extern double Hz_F;
masterCommander.cpp
this is my first time try using forum to help me to solve problem. if there any action not following the rule of forum, please let me know. The code is large that my browser told me memory is not enough, I didn't upload all my code. If there no sufficient code to realize the issue, I will upload rest code later.
Since you have no clue where it is failing, step one would be to put print statements at every few "steps" of the code, to see how far it is getting before crashing. Follow each print statement with Serial.flush() to make sure the message gets out BEFORE proceeding to the next step. The LAST message printed will tell you the last step that succeeded, and the problem will be between that print statement and the next one. Add/remove print statements to narrow the range of the search as needed.
Thanks! I think I've found the reason. When a crash occurs, error messages are of the following types:
- Assertion failed: tcp_sent /IDF/components/lwip/lwip/src/core/tcp.c:2050 (the socket status transmitting the callback is invalid)
- Assertion failed: block_locate_free tlsf.c:566 (block_size(block) >= size)
- Assertion failed: multi_heap_free multi_heap_poisoning.c:276 (head != NULL)
- Watchdog failure
I found that when the crash occurred, the ping response time became very large, even reaching 900ms. So I tried closing the connection to the local WiFi network(path: client browser --- office AP --- ESP32) and just letting the client browser web server connect directly(path: client browser --- ESP32) to the ESP32. The ping response time reduce to under 100ms. Then I don't see the crash happening again. I think the root cause is the response time to large,
While waiting for the ping response, call delay() or yield(), to keep the watchdog from firing. Also, provide a timeout in case the response NEVER comes. NEVER create a wait loop that does not contain some kind of escape if whatever it's waiting for never happens.
(anecdotally) I have had similar problems with a webserver on an ESP8266 and switching to LittleFS has been the solution.