///

/////
//////
///
////
///
///
///
///
///quay trái, quay phải --> chân trái tiến->lùi + phải lùi tiến---> chân phải tiến
lùi -> chân trái lùi tiến
// đế chân phải vẫn chạm. Có thể chỉnh sau khi về gốc thì lại cho nó cúi gót tiếp
lần nữa rồi ok

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <WiFiManager.h> // https://github.com/tzapu/WiFiManager
#include <WiFi.h>
#include <ArduinoWebsockets.h>
#include "ESPAsyncWebServer.h"
using namespace websockets;
#define SERVOMIN 150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600 // This is the 'maximum' pulse length count (out of 4096)
600
#define USMIN 600 // This is the rounded 'minimum' microsecond length based on
the minimum pulse of 150
#define USMAX 2400 // This is the rounded 'maximum' microsecond length based on
the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
WebsocketsServer server;
AsyncWebServer webserver(80);
#define run_every(t) for (static uint16_t last_; \
(uint16_t)(uint16_t(millis()) - last_) >= (t); \
last_ += (t))
IPAddress ip_ap(192, 168, 1, 1);
IPAddress gateway_ap(192, 168, 1, 1);
IPAddress subnet_ap(255, 255, 255, 0);
int first_foot = 1;
const char* ssid = "30 PDG";
const char* password = "bktech1017";
const uint8_t index_html_gz[] = { 0x1f, 0x8b, 0x08, 0x08, 0x60, 0x5d, 0x9d, 0x63,
0x00, 0xff, 0x69, 0x6e, 0x64, 0x65, 0x78, 0x2e, 0x68, 0x74, 0x6d, 0x6c, 0x2e, 0x67,
0x7a, 0x00, 0xad, 0x58, 0x7d, 0x6f, 0xda, 0xba, 0x17, 0xfe, 0x1b, 0x3e, 0x85, 0x7f,
0x9e, 0xae, 0x16, 0x44, 0x79, 0x09, 0x2d, 0xa5, 0x4d, 0x09, 0x52, 0xb7, 0xb1, 0x17,
0xa9, 0x77, 0x9d, 0xda, 0x4e, 0xd3, 0xd5, 0x34, 0x55, 0x26, 0x71, 0xc1, 0xb7, 0xc1,
0x46, 0x89, 0x53, 0xc6, 0xad, 0xfa, 0xdd, 0x77, 0x6c, 0xe7, 0x8d, 0x36, 0x0d, 0x2f,
0xfa, 0x05, 0x21, 0x82, 0x7d, 0x9e, 0xf3, 0x1c, 0x1f, 0x3f, 0xb6, 0x4f, 0x32, 0xfc,
0xdf, 0x87, 0xcb, 0xf7, 0x37, 0xff, 0x7c, 0x1b, 0xa3, 0x99, 0x9c, 0x07, 0xa3, 0xfa,
0x30, 0xfd, 0xa1, 0xc4, 0x87, 0x9f, 0x39, 0x95, 0x04, 0x71, 0x32, 0xa7, 0x2e, 0x7e,
0x60, 0x74, 0xb9, 0x10, 0xa1, 0xc4, 0xc8, 0x13, 0x5c, 0x52, 0x2e, 0x5d, 0xbc, 0x64,
0xbe, 0x9c, 0xb9, 0x3e, 0x7d, 0x60, 0x1e, 0x6d, 0xe9, 0x3f, 0x07, 0x88, 0x71, 0x26,
0x19, 0x09, 0x5a, 0x91, 0x47, 0x02, 0xea, 0xda, 0x18, 0x9c, 0x44, 0x72, 0x15, 0xd0,
0x51, 0xbd, 0x1d, 0x05, 0xcc, 0xa7, 0x0a, 0x4c, 0x18, 0xa7, 0x21, 0x7a, 0xac, 0x23,
0xa4, 0x41, 0x0e, 0xb2, 0xbb, 0xdd, 0xbf, 0xce, 0xea, 0x4f, 0xf5, 0xc4, 0xc6, 0xf4,
0xb5, 0x96, 0x74, 0x72, 0xcf, 0x64, 0x8b, 0x2c, 0x16, 0x94, 0x84, 0x84, 0x7b, 0xd4,
0x41, 0x5c, 0x70, 0x7a, 0x06, 0x7d, 0x73, 0x12, 0x4e, 0x19, 0x6f, 0x05, 0xf4, 0x4e,
0x3a, 0xb6, 0xc2, 0x22, 0xb4, 0x10, 0x11, 0x30, 0x0b, 0xee, 0x20, 0x32, 0x89, 0x44,
0x10, 0x4b, 0x6d, 0x28, 0xe9, 0x6f, 0xf0, 0x10, 0xb0, 0x29, 0xb4, 0x7b, 0x10, 0x34,
0x0d, 0x55, 0x6b, 0x42, 0x3b, 0x30, 0xc8, 0x19, 0x65, 0xd3, 0x99, 0x74, 0x50, 0xaf,
0xbf, 0xf8, 0xad, 0xfe, 0x4f, 0x88, 0x77, 0x3f, 0x0d, 0x45, 0xcc, 0x7d, 0x07, 0xbd,
0xf1, 0x0f, 0xd5, 0x47, 0x35, 0x8b, 0x58, 0x06, 0x10, 0x78, 0x1e, 0x84, 0x58, 0x10,
0x8f, 0xc9, 0x95, 0x83, 0xba, 0xed, 0x81, 0xfa, 0x9f, 0x06, 0x2c, 0x21, 0xd8, 0x34,
0x96, 0x76, 0x2f, 0xd2, 0x61, 0x14, 0x9a, 0x12, 0x98, 0xe9, 0xaa, 0x15, 0x46, 0xed,
0xcc, 0xc4, 0x43, 0x32, 0xf6, 0xcc, 0xb5, 0x5d, 0x4c, 0x8b, 0xe3, 0xa4, 0x14, 0xe6,
0x7f, 0x4b, 0xce, 0xe2, 0xf9, 0x64, 0x63, 0xb2, 0xca, 0xda, 0x92, 0x0c, 0xa4, 0x43,
0xae, 0x4c, 0x41, 0xf7, 0xe8, 0xfc, 0xfc, 0xf8, 0x83, 0x6a, 0xf6, 0xe2, 0x30, 0x12,
0xa1, 0x03, 0xb9, 0x66, 0x26, 0x95, 0x6b, 0xb1, 0xcd, 0xc5, 0x7f, 0x2d, 0xa0, 0x99,
0xd2, 0x42, 0x5c, 0xff, 0x37, 0x9e, 0xf6, 0x24, 0x96, 0x12, 0xd2, 0xa7, 0x73, 0xf4,
0x58, 0xaf, 0x15, 0xa0, 0x2d, 0x4f, 0x04, 0xca, 0xfa, 0xcd, 0xd1, 0xfb, 0xf3, 0x8f,
0xfd, 0xae, 0x76, 0x60, 0x5a, 0xa6, 0x21, 0xa5, 0x5c, 0xa1, 0x87, 0x9d, 0x44, 0x84,
0xc3, 0x4e, 0xa2, 0xec, 0x89, 0xf0, 0x57, 0xa3, 0x3a, 0x08, 0xdd, 0x1e, 0xf5, 0xd0,
0xc5, 0xf8, 0x13, 0xba, 0xba, 0x7c, 0x77, 0x79, 0x03, 0xdd, 0xb6, 0x6a, 0xf5, 0xd9,
0x03, 0xf2, 0x02, 0x12, 0x45, 0x2e, 0x5e, 0x57, 0xad, 0xd2, 0x33, 0x41, 0xcc, 0x87,
0xf5, 0x40, 0x02, 0x50, 0xf7, 0xb5, 0xed, 0x0c, 0xec, 0x61, 0x87, 0x8c, 0x86, 0x8c,
0x2f, 0x62, 0x89, 0xe4, 0x6a, 0x01, 0x6b, 0x45, 0x67, 0x01, 0xa3, 0x39, 0xe3, 0x2e,
0x3e, 0xec, 0xc2, 0x0d, 0xf9, 0xed, 0xe2, 0x93, 0x63, 0x8c, 0x00, 0x14, 0x43, 0xff,
0xc0, 0xc6, 0x28, 0x92, 0x74, 0xe1, 0x62, 0xb8, 0x29, 0xf2, 0x84, 0x58, 0xfb, 0x9e,
0xaf, 0xae, 0x94, 0x07, 0xbd, 0x7a, 0x26, 0xe1, 0x48, 0x7f, 0x33, 0xd6, 0x1e, 0xb0,
0xf6, 0x40, 0xf5, 0xd5, 0xb4, 0xc7, 0x29, 0xad, 0xad, 0xee, 0x32, 0xb2, 0x24, 0x00,
0x40, 0x57, 0x11, 0x03, 0x85, 0xe2, 0x34, 0xe4, 0x19, 0xf1, 0x21, 0x10, 0x1f, 0x3a,
0x95, 0xac, 0xfd, 0x8c, 0x75, 0x00, 0x77, 0x19, 0x59, 0xaf, 0x8a, 0x0c, 0xdc, 0x96,
0x90, 0x1d, 0xe1, 0x11, 0xb9, 0x0d, 0xb5, 0x5a, 0x2a, 0x19, 0x61, 0x1c, 0x86, 0xb0,
0x9f, 0xf3, 0xa5, 0xe3, 0xed, 0xb6, 0x2b, 0x47, 0x09, 0x14, 0x25, 0xc4, 0x7d, 0x3c,
0x7a, 0xd8, 0x86, 0x38, 0x4f, 0xf0, 0x49, 0x3e, 0xd4, 0x23, 0xb8, 0x7d, 0x9d, 0x10,
0x5c, 0x97, 0x10, 0x1e, 0x43, 0x5a, 0x8f, 0x9d, 0x7e, 0xb7, 0x5a, 0x45, 0x30, 0xbc,
0x6c, 0x12, 0x8d, 0x9c, 0xf2, 0x01, 0xab, 0xac, 0x83, 0xe6, 0x5f, 0xe5, 0x05, 0x86,
0x12, 0xde, 0x01, 0xf0, 0x0e, 0x36, 0xf1, 0xaa, 0x01, 0xad, 0xf3, 0x9e, 0xe6, 0xa3,
0xdd, 0xc4, 0x0b, 0x0c, 0x25, 0xbc, 0x27, 0xc0, 0x7b, 0xe2, 0x1c, 0x55, 0xf3, 0xbe,
0xa0, 0xb5, 0xd5, 0x3a, 0x2a, 0x64, 0xb9, 0x8a, 0x17, 0x18, 0x4a, 0x78, 0x4f, 0x95,
0xa2, 0xf4, 0x89, 0xb1, 0x49, 0x50, 0x05, 0xf5, 0x94, 0x68, 0xab, 0x8a, 0x19, 0x38,
0x4a, 0x98, 0xed, 0xae, 0xd2, 0xd4, 0x66, 0xea, 0xb5, 0x95, 0xfa, 0x52, 0x5b, 0x10,
0x45, 0x15, 0xb7, 0x62, 0x29, 0x23, 0x87, 0x5d, 0x64, 0x12, 0x0b, 0x0f, 0x76, 0x8c,
0x2d, 0x33, 0x9e, 0x8d, 0xbb, 0x57, 0x5c, 0xc3, 0x1b, 0xc8, 0x81, 0xa5, 0x8c, 0x1c,
0x76, 0x92, 0x77, 0x84, 0xdf, 0x7a, 0x33, 0xc2, 0x9d, 0x7e, 0x25, 0x7f, 0x2b, 0x57,
0x78, 0x59, 0xe2, 0x37, 0x0d, 0xbe, 0x64, 0xcb, 0x82, 0x13, 0xe3, 0x36, 0x8c, 0x39,
0x74, 0x28, 0x5e, 0x73, 0x7e, 0x20, 0xc1, 0xbd, 0x80, 0x79, 0xf7, 0xe0, 0x84, 0x72,
0xff, 0x63, 0xcc, 0x3d, 0x75, 0x28, 0x5b, 0x0d, 0x3c, 0xba, 0xfa, 0xfe, 0x75, 0xd8,
0x31, 0x46, 0xa9, 0xa7, 0x67, 0x08, 0x12, 0x4b, 0x51, 0x44, 0x9c, 0x7f, 0xbf, 0xb9,
0xcc, 0x20, 0xd5, 0xe6, 0xbd, 0xd4, 0xbe, 0x57, 0x01, 0x50, 0x02, 0x51, 0x76, 0x17,
0xe3, 0x8f, 0x70, 0x10, 0xbd, 0x6a, 0xa6, 0xf7, 0x26, 0x1d, 0xf1, 0x97, 0x4f, 0x9f,
0x8b, 0x86, 0x1d, 0x38, 0xb5, 0xd4, 0xe1, 0xd5, 0x31, 0x67, 0xdb, 0x30, 0xf2, 0x42,
0xb6, 0x90, 0x23, 0x38, 0x0e, 0xf5, 0x05, 0x67, 0x58, 0x24, 0xd1, 0x8f, 0xeb, 0xdb,
0xef, 0x57, 0x17, 0xc8, 0x45, 0x78, 0x19, 0x39, 0x9d, 0x0e, 0x46, 0x4d, 0x38, 0xa3,
0xb9, 0x2f, 0x96, 0xed, 0x40, 0x78, 0x44, 0xc5, 0xda, 0x9e, 0x09, 0xb0, 0x6b, 0x22,
0xec, 0x9c, 0xf4, 0xb0, 0x3a, 0x4c, 0x53, 0xe8, 0x32, 0x02, 0x18, 0xa7, 0x4b, 0xf4,
0x83, 0x4e, 0xae, 0x85, 0x77, 0x4f, 0xa5, 0x65, 0xbc, 0x35, 0x8c, 0xd5, 0x03, 0x09,
0x91, 0x9e, 0xd1, 0xdb, 0xc4, 0xee, 0x3c, 0x0c, 0xc9, 0xca, 0x2a, 0xf4, 0xea, 0xb9,
0x2c, 0xed, 0x35, 0xb8, 0xf6, 0x22, 0x8e, 0x66, 0x96, 0x2f, 0xbc, 0x78, 0x0e, 0xb5,
0x5a, 0x7b, 0x4a, 0xe5, 0x38, 0xa0, 0xea, 0xf6, 0xdd, 0xea, 0x8b, 0x6f, 0xe5, 0xc7,
0x62, 0x63, 0x0f, 0x54, 0x6f, 0x2f, 0xd4, 0xe1, 0x5e, 0xa8, 0xa3, 0xbd, 0x50, 0xfd,
0xbd, 0x50, 0xc7, 0x1a, 0xb5, 0x0b, 0x62, 0xb0, 0x17, 0xcf, 0xc9, 0xce, 0x3c, 0xa7,
0x3b, 0x23, 0x60, 0x97, 0xd9, 0x19, 0x92, 0xab, 0xc1, 0xa8, 0x6b, 0x03, 0x4c, 0x97,
0x6d, 0xbb, 0x22, 0x72, 0xed, 0x6c, 0x8b, 0xc8, 0x75, 0xb3, 0x2d, 0x22, 0xd7, 0xcc,
0xb6, 0x88, 0x5c, 0x2f, 0xdb, 0x22, 0x8c, 0x56, 0x6a, 0xdb, 0x9a, 0xa7, 0x42, 0xd9,
0xd6, 0xde, 0x48, 0x64, 0x6b, 0xf7, 0x46, 0x1f, 0x5b, 0xcf, 0x9a, 0xd1, 0xc6, 0xd6,
0xe6, 0x66, 0x96, 0x77, 0x92, 0xd2, 0xee, 0xd3, 0x5c, 0x80, 0xdc, 0x89, 0xd0, 0x0a,
0xa8, 0x44, 0x0c, 0xb6, 0xb7, 0x2e, 0x3a, 0x63, 0x43, 0xbb, 0x7b, 0xc6, 0x9a, 0xcd,
0x86, 0xee, 0x54, 0x8f, 0x42, 0xe6, 0x32, 0x01, 0xfd, 0x64, 0xbf, 0xda, 0x82, 0x9b,
0xa3, 0xd0, 0x45, 0x77, 0xd9, 0xa1, 0x92, 0x99, 0xc1, 0x13, 0x4e, 0x8d, 0xdd, 0x59,
0xcc, 0x75, 0x0f, 0x1b, 0x75, 0xf8, 0x63, 0x02, 0x52, 0x30, 0xc6, 0xe1, 0x31, 0xe4,
0xf3, 0xcd, 0xdf, 0xb0, 0x87, 0xe3, 0xb4, 0x42, 0xc6, 0x4d, 0x39, 0x63, 0x51, 0x5b,
0x1b, 0xc1, 0x04, 0xa8, 0xa3, 0xcd, 0xc2, 0x11, 0x6e, 0xb2, 0x42, 0x7b, 0xe3, 0x0c,
0xc1, 0x83, 0x10, 0x0d, 0x22, 0x8a, 0x8c, 0xe3, 0xa3, 0x0a, 0xc7, 0x69, 0x05, 0xbc,
0x97, 0xe3, 0x13, 0xe5, 0xf8, 0xb5, 0x80, 0x75, 0x15, 0xb4, 0x97, 0xdb, 0xd3, 0xd7,
0xdd, 0x9a, 0xe2, 0x6a, 0x37, 0xaf, 0xf9, 0x94, 0xe4, 0x13, 0xfe, 0xcc, 0x2f, 0xc2,
0xd7, 0xb8, 0x69, 0xb1, 0xa6, 0xdd, 0x68, 0xe2, 0x67, 0x31, 0x17, 0xb1, 0xaf, 0x13,
0xe5, 0x36, 0x19, 0xe0, 0x49, 0xdf, 0x3d, 0x25, 0xc2, 0xfc, 0x69, 0x77, 0x37, 0x0a,
0xa1, 0x24, 0x4c, 0x85, 0x5a, 0x8b, 0x53, 0x15, 0x78, 0x78, 0x8b, 0x20, 0x67, 0x45,
0x03, 0x08, 0xb1, 0x34, 0xc0, 0x34, 0x34, 0x7b, 0xaf, 0xd0, 0x00, 0xb5, 0x16, 0x9a,
0xb8, 0xbb, 0x8b, 0xa8, 0xbc, 0x9d, 0x24, 0x55, 0xe0, 0x36, 0x51, 0xde, 0x6f, 0x13,
0x65, 0x2d, 0x8d, 0x08, 0xad, 0xd7, 0x65, 0x8f, 0x75, 0xe3, 0x44, 0xfa, 0x58, 0x41,
0xeb, 0xaa, 0x76, 0x11, 0x01, 0x85, 0xf2, 0x66, 0x6a, 0xf5, 0x60, 0xad, 0x3e, 0xd5,
0x33, 0xa0, 0x29, 0xb8, 0x40, 0x55, 0x06, 0x11, 0xc3, 0x96, 0xb1, 0xd6, 0x9f, 0x54,
0x5a, 0x6a, 0x99, 0x24, 0x16, 0xb0, 0xdc, 0x95, 0x45, 0x39, 0x37, 0x14, 0x79, 0xa9,
0xab, 0xe0, 0x15, 0xee, 0x5a, 0xee, 0x7c, 0xbd, 0xfe, 0x54, 0xeb, 0xdd, 0x40, 0x43,
0x03, 0xad, 0xd5, 0x8a, 0xe0, 0x3e, 0xd4, 0x63, 0xc7, 0x9a, 0x3a, 0x2d, 0xa5, 0xa0,
0x12, 0x96, 0x37, 0xf0, 0x72, 0xeb, 0x40, 0xdf, 0x5d, 0x03, 0xf4, 0x40, 0xbb, 0xbc,
0x61, 0x73, 0x0a, 0x6f, 0xaa, 0xce, 0xea, 0x6a, 0x2f, 0x02, 0x17, 0x9c, 0x6a, 0x86,
0xdb, 0x90, 0x82, 0x33, 0x78, 0x75, 0xa2, 0x6a, 0xb8, 0x9f, 0xbf, 0x92, 0xee, 0x19,
0xf5, 0xee, 0xdf, 0x67, 0x36, 0xd0, 0x63, 0x35, 0x90, 0x3b, 0x4a, 0x26, 0x36, 0xa4,
0x32, 0x0e, 0xb9, 0x2e, 0xd5, 0xbe, 0x85, 0x62, 0xce, 0x22, 0x6a, 0x59, 0x89, 0x97,
0x03, 0xe8, 0xfc, 0x17, 0x40, 0x05, 0x6b, 0x75, 0xb1, 0x3b, 0x64, 0x2d, 0xa3, 0x76,
0x08, 0x6f, 0x58, 0x56, 0xd7, 0x92, 0x48, 0x8a, 0x5c, 0xd7, 0xcd, 0xab, 0xc5, 0xf6,
0xe5, 0xb7, 0xf1, 0xd7, 0xc6, 0x33, 0xd9, 0x24, 0x1e, 0xd3, 0x3a, 0x30, 0x5f, 0x22,
0xea, 0xd2, 0xab, 0x7f, 0xdd, 0xbe, 0x6c, 0x48, 0x66, 0x9f, 0x7e, 0x7c, 0x16, 0xdc,
0xd3, 0x0b, 0x97, 0xaa, 0x05, 0xde, 0x58, 0x41, 0x88, 0xc4, 0xf7, 0xc7, 0x0f, 0xb0,
0x91, 0x5f, 0x30, 0x78, 0xe4, 0x00, 0xb1, 0x5a, 0x6f, 0xc5, 0x82, 0xf2, 0xb7, 0x07,
0x6b, 0x09, 0x28, 0xa5, 0x82, 0x6d, 0x7e, 0x4c, 0xbc, 0x99, 0x15, 0x2a, 0xbb, 0x10,
0xc6, 0x9a, 0x84, 0xdf, 0xa8, 0x17, 0xf9, 0xd4, 0x45, 0xa2, 0x15, 0xf7, 0xb2, 0x75,
0x63, 0xa4, 0xed, 0x13, 0x49, 0xd2, 0x0c, 0x90, 0x25, 0x61, 0x2f, 0xe6, 0x20, 0xcd,
0x03, 0xc4, 0x98, 0x03, 0xd2, 0x79, 0xcf, 0x56, 0x01, 0xbc, 0xce, 0x4a, 0x6a, 0x7b,
0x78, 0x61, 0xa5, 0x5f, 0xd8, 0xfe, 0x01, 0x7f, 0xcd, 0x0d, 0x2f, 0xc8, 0x15, 0x00,
0x00 };
#define de 0
#define goi 1
#define dui 2
#define de_trai 0
#define de_phai 5
#define goi_trai 1
#define goi_phai 6
#define dui_trai 2
#define dui_phai 7
#define tien 1
#define lui -1
#define rad_to_deg(x) (x * 180 / PI)
#define deg_to_rad(x) (x * PI / 180)
int cvt_angle(int pulse_) {
pulse_ = constrain(pulse_, 0, 180);
return map(pulse_, 0, 180, SERVOMIN, SERVOMAX);
}
volatile bool run_now = 0;
const byte NumServos = 10;
const int VelPeriod = 50;
int dir_ = 1;
const int angle_min[NumServos] = { 30, 60, 50, 70, 30, 35, 40, 0, 20, 45 };
// minimum pulse length (in uS)
const int angle_max[NumServos] = { 86, 160, 170, 120, 60, 85, 90, 130, 120, 95 };
// maximum pulse length (in uS) // how
often the servo position is recalculated
float vmax[NumServos] = { 173, 172, 172, 15, 20, 172, 172, 172, 15, 20 };
// max change in p per step
float amax[NumServos] = { 12.8, 12.2, 12.2, 0.1, 0.1, 12.6, 12.3, 12.3, 0.6, 0.1 };
// max change in v per step
byte group[NumServos] = { 1, 1, 1, 0, 0, 2, 2, 2, 0, 0 };
int angle_left_foot = 10;
float offset_ = 12.2;
int angle_right_foot = 10;
const int InitialPosition[NumServos] = {
71,
101,
102,
82,
39,
50,
70, //50-80
40, //60-90
46, //45-58
92 // 50-80
};

bool AtTargets = true;

float sign(float a) {
if (a < 0) return -1;
if (a > 0) return +1;
return 0;
}

//-----------------------------------------------------------
// sqr()
// square funtion
// it's a function not a macro like sq()
//-----------------------------------------------------------
float sqr(float a) {
return a * a;
}
volatile bool sit_bool = 0;
volatile bool turn_right_start = 0;
volatile bool turn_left_start = 0;
volatile bool run_first = 0;
volatile bool run_2 = 0;
volatile bool run_3 = 0;
volatile bool run_4 = 0;
volatile bool run_loop = 0;
volatile bool run_forever = 0;
int offset_2 = 4;
int right_offset = 5;
float target[NumServos]; // target position
float p[NumServos]; // current position
float v[NumServos]; // velocity: (change in p per step)
// 0 = move independently; 1..n = move together
float l[3];
float phi[2];
float a, b, alpha, u;
float first_phi[2] = { 20, 30 }; // test
float delta_phi[2];
float dir_phi[2];
int last_v[5];
int last_a[5];
float value_offset = 5.5;
void inverse_kinematic(float x, float y) {
u = acos((l[1] * l[1] + l[2] * l[2] - (x - l[0]) * (x - l[0]) - y * y) / (2 *
l[1] * l[2]));
a = l[2] - l[1] * cos(u);
b = l[1] * sin(u);
alpha = acos((b) / (sqrt(a * a + b * b)));
phi[1] = rad_to_deg(acos((y) / (sqrt(a * a + b * b))) + alpha);
phi[0] = rad_to_deg(u) - phi[1];
for (int i = 0; i < 2; i++) {
delta_phi[i] = phi[i] - first_phi[i];
}
// co phi[1] phi[0] thi ta do goc luc dau xem phi[1] phi[0](x=0 , y=0) la bao
nhieu -> lay phi[1](x)-phi[1](0) -> do dich chuyen -> cho servo quay goc do ()
}
void MoveServos() {
static unsigned long previousMillis = 0;
if (millis() - previousMillis < VelPeriod)
return;
previousMillis = millis();

float g[NumServos];
float v1, a1, tmax, sign1, p0;

for (byte grp = 0; grp < NumServos; grp++) {

tmax = 0;
for (byte servo = 0; servo < NumServos; servo++)
if (group[servo] == grp) {
g[servo] = abs(target[servo] - p[servo]) / vmax[servo];
if (amax[servo] > 0) {
sign1 = sign(sqr(v[servo]) + amax[servo] * 2 * (2 * abs(v[servo]) +
sign(v[servo]) * (p[servo] - target[servo])));
g[servo] = max(g[servo],
(float)(sign1 * abs(v[servo]) + sqrt(abs(2 * sign1 *
v[servo] * abs(v[servo]) + 4 * amax[servo] * (p[servo] - target[servo])))) /
amax[servo]);
}
tmax = max(tmax, g[servo]);
}
for (byte servo = 0; servo < NumServos; servo++)
if (group[servo] == grp) {
if (grp == 0)
tmax = g[servo];
if ((tmax < 2) or ((abs(v[servo]) <= amax[servo] + 0.0001) and
(abs(p[servo] + v[servo] - target[servo]) <= 1))) {
p[servo] = target[servo];
v[servo] = 0;
} else if (tmax > 0) {
g[servo] = g[servo] / tmax;
a1 = amax[servo] * sqr(g[servo]);

if (a1 == 0) {
v[servo] = (target[servo] - p[servo]) / tmax;
} else {
v1 = vmax[servo] * g[servo];
 p0 = 2 * v[servo] + abs(v[servo]) * v[servo] / (2 * a1);
a1 = sign(target[servo] - p[servo] - p0) * a1;
if (a1 * (target[servo] - p[servo]) < 0) {
a1 = sign(a1) * abs(v[servo]) * v[servo] / (2 * (target[servo] -
p[servo]));
a1 = constrain(a1, -amax[servo], amax[servo]);
}

v[servo] = v[servo] + a1;

v[servo] = constrain(v[servo], -v1, v1);
}

v[servo] = constrain(v[servo], -vmax[servo], vmax[servo]);

p[servo] = p[servo] + v[servo];
}
}
}
for (byte servo = 0; servo < NumServos; servo++) {
p[servo] = constrain(p[servo], angle_min[servo], angle_max[servo]);
//myservo[servo].writeMicroseconds(p[servo]);
pwm.setPWM(servo, 0, cvt_angle(p[servo]));
//Serial.println("servo[" + String(servo) + "]:" + String(p[servo]));
}
}
void move_left(int part, int angle, int dir) {
/// de - tien + -> goi - tien - -> dui tien +

target[part] += angle * dir;

target[part] = constrain(target[part], angle_min[part], angle_max[part]);
Serial.println(part);
}
void move_right(int part, int angle, int dir) {
// de -> tien
// part+=5;
target[part] -= angle * dir;
target[part] = constrain(target[part], angle_min[part], angle_max[part]);
Serial.println(part);
}
void check_run() {
for (byte servo = 0; servo < NumServos; servo++) {
if (p[servo] != target[servo]) {
Serial.println(String(p[servo]) + "..." + String(target[servo]));
return;
}
}
run_now = 0;
Serial.println("stop");
}
bool get_target(int t) {
return (abs(target[t] - p[t])) < 0.5 ? 1 : 0;
}
void sit_down() {
group[goi_trai] = 3;
group[de_trai] = 3;
group[goi_phai] = 3;
group[de_phai] = 3;
static bool sit_ = 1;
static bool end_ = 0;
if (sit_ == 1) {
 sit_ = 0;
move_left(goi_trai, offset_2, lui);
move_right(goi_phai, offset_2, lui);
move_left(de_trai, offset_2, tien);
move_right(de_phai, offset_2, tien);
}
if (get_target(goi_trai) == 1 && get_target(goi_phai) == 1 && end_ == 0) {
end_ = 1;
move_left(goi_trai, offset_2, tien);
move_right(goi_phai, offset_2, tien);
move_left(de_trai, offset_2, lui);
move_right(de_phai, offset_2, lui);
delay(1500);
}
if (end_ == 1 && get_target(goi_trai) == 1 && get_target(goi_phai) == 1) {
end_ = 0;
sit_bool = 0;
turn_left_start = 0;
group[goi_trai] = 1;
group[de_trai] = 1;
group[goi_phai] = 2;
group[de_phai] = 2;
}
MoveServos();
}
void turn_right(int angle) {

group[goi_trai] = 1;
group[dui_trai] = 1;
group[de_trai] = 1;
static bool set_angle = 1;
static bool end_ = 0;
if (set_angle == 1) {
move_left(goi_trai, angle - 2, lui);
move_left(de_trai, angle, tien);
move_left(dui_trai, angle, tien);
set_angle = 0;
}
if (get_target(goi_trai) && end_ == 0) {
move_left(goi_trai, angle - 2, tien);
move_left(de_trai, angle, lui);
move_left(dui_trai, angle, lui);
end_ = 1;
delay(200);
}
if (get_target(goi_trai)) {
set_angle = 1;
turn_right_start = 0;
end_ = 0;
}
MoveServos();
}
void turn_left(int angle) {

group[goi_phai] = 2;
group[dui_phai] = 2;
group[de_phai] = 2;
static bool set_angle = 1;
static bool end_ = 0;
 if (set_angle == 1) {
move_right(goi_phai, angle - 2, lui);
move_right(de_phai, angle, tien);
move_right(dui_phai, angle, tien);
set_angle = 0;
}
if (get_target(goi_phai) && end_ == 0) {
move_right(goi_phai, angle - 2, tien);
move_right(de_phai, angle, lui);
move_right(dui_phai, angle, lui);
end_ = 1;
delay(200);
}
if (get_target(goi_phai) && end_ == 1) {
set_angle = 1;
turn_left_start = 0;
end_ = 0;
sit_bool = 1;
}
MoveServos();
}
void move_count(bool forever = 0) {
if (run_first == 1) {
if (get_target(first_foot + 1)) {
//vmax[0] +=8;
// amax[0]+=1;
// target[0] += offset_ + offset_2;
Serial.println("target" + String(first_foot) + ":" +
String(target[first_foot]));
delay(100);
target[first_foot] += (offset_ * dir_ - right_offset);
target[first_foot + 1] -= offset_ * dir_ - right_offset;
if (first_foot == 1) {
move_right(de_phai, offset_ / 3, tien);
group[5] = 1;
} else {
move_left(de_trai, offset_ / 3, tien);
//target[0] += offset_ / 3;
group[0] = 2;
}
Serial.println("run2");
run_first = 0;
run_2 = 1;
}
}
if (run_2 == 1) {
if (get_target(first_foot + 1)) {
Serial.println("target" + String(first_foot) + ":" +
String(target[first_foot]));
if (first_foot == 1) {
// target[5] += offset_ / 3 - offset_ / 2;
move_right(de_phai, offset_ / 3, lui);
move_right(de_phai, offset_2, lui);
group[5] = 2;
} else {
// target[0] -= offset_ / 3;
move_left(de_trai, offset_ / 3, lui);
move_left(de_trai, offset_2, lui);
group[0] = 1;
 }
right_offset = first_foot == 1 ? value_offset : 0;
target[7 - first_foot] += offset_ * dir_ + right_offset;
target[7 - first_foot + 1] -= offset_ * dir_ + right_offset;
target[7 - first_foot] = constrain(target[7 - first_foot], angle_min[7 -
first_foot], angle_max[7 - first_foot]);
target[7 - first_foot + 1] = constrain(target[7 - first_foot + 1],
angle_min[7 - first_foot + 1], angle_max[7 - first_foot + 1]);
delay(100);
run_2 = 0;
run_3 = 1;
}
}
if (run_3 == 1) {
if (get_target(7 - first_foot + 1)) {
if (first_foot == 1) {
move_right(de_phai, offset_2, tien);
// target[5] -= -offset_ / 2;
group[5] = 2;
} else {
move_left(de_trai, offset_2, tien);
group[0] = 0;
}
Serial.println("target" + String(7 - first_foot) + ":" + String(target[7 -
first_foot]));
target[7 - first_foot] -= offset_ * dir_ + right_offset;
target[7 - first_foot + 1] += offset_ * dir_ + right_offset;
Serial.println("run3");
run_3 = 0;
run_4 = 1;
}
}
if (forever == 0) {
if (run_4 == 1) {
if (get_target(7 - first_foot + 1)) {
Serial.println("target" + String(7 - first_foot) + ":" + String(target[7 -
first_foot]));
Serial.println("run4");
run_4 = 0;
run_loop = 0;
first_foot = 7 - first_foot;
}
}
MoveServos();
}
}
void move_forever() {
move_count(1);
if (run_4 == 1) {
if (get_target(7 - first_foot + 1)) {
run_first = 1;

group[0] = 1;
//target[0] -= offset_ + offset_2;
run_4 = 0;
target[0] = constrain(target[0], angle_min[0], angle_max[0]);

first_foot = 7 - first_foot;
dir_ = first_foot == 1 ? 1 : -1;
 right_offset = first_foot == 1 ? 0 : value_offset;
target[first_foot] -= (offset_ * dir_ - right_offset);
target[first_foot] = constrain(target[first_foot], angle_min[first_foot],
angle_max[first_foot]);
target[first_foot + 1] += offset_ * dir_ - right_offset;
target[first_foot + 1] = constrain(target[first_foot + 1],
angle_min[first_foot + 1], angle_max[first_foot + 1]);
delay(1100);
}
}
MoveServos();
}
void initWiFi() {
WiFi.mode(WIFI_STA);
WiFi.begin(ssid, password);
Serial.print("Connecting to WiFi ..");
while (WiFi.status() != WL_CONNECTED) {
Serial.print('.');
delay(1000);
}
Serial.println(WiFi.localIP());
}
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);

WiFiManager wm;
bool res;
// res = wm.autoConnect(); // auto generated AP name from chipid
// res = wm.autoConnect("AutoConnectAP"); // anonymous ap
IPAddress _ip = IPAddress(192, 168, 1, 78);
IPAddress _gw = IPAddress(192, 168, 1, 1);
IPAddress _sn = IPAddress(255, 255, 255, 0);
//end-bloc
wm.setSTAStaticIPConfig(_ip, _gw, _sn);
wm.setConnectTimeout(60);
wm.setTimeout(60);
res = wm.autoConnect("2_leg_robot", "123456789"); // password protected ap
if (!res) {
Serial.println("Failed to connect");
delay(3000);
//reset and try again, or maybe put it to deep sleep
ESP.restart();
delay(5000);
// ESP.restart();
} else {
//if you get here you have connected to the WiFi
Serial.println("connected...yeey :)");
delay(100);
webserver.on("/", HTTP_GET, [](AsyncWebServerRequest* request) {
AsyncWebServerResponse* response = request->beginResponse_P(200, "text/html",
index_html_gz, sizeof(index_html_gz));
response->addHeader("Content-Encoding", "gzip");
request->send(response);
});
webserver.begin();
pwm.begin();
pwm.setOscillatorFrequency(27000000);
 pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates
for (byte servo = 0; servo < NumServos; servo++) {
p[servo] = InitialPosition[servo];

target[servo] = p[servo];
pwm.setPWM(servo, 0, cvt_angle(p[servo]));
// group[servo] = 0;
}
server.listen(82);
Serial.print("Is server live? ");
Serial.println(server.available());
}
}
void check_input(String dataIn) {
int index_ = dataIn.indexOf("s");
int reg;
if (index_ != -1) {
reg = dataIn[index_ + 1] - '0';
String dataInS = dataIn.substring(2, dataIn.length()); // Extract only the
number. E.g. from "s1120" to "120"
if (reg == 3) {
amax[5] = dataInS.toInt() - 0.01;
amax[6] = dataInS.toInt() - 0.01;
amax[7] = dataInS.toInt() - 0.01;
return;
} else if (reg == 4) {

vmax[5] = dataInS.toInt();

vmax[6] = dataInS.toInt();
vmax[7] = dataInS.toInt();
return;
} else if (reg == 8) {
vmax[0] = dataInS.toInt();
vmax[1] = dataInS.toInt();
vmax[2] = dataInS.toInt();

return;

} else if (reg == 9) {
amax[0] = dataInS.toInt();

amax[1] = dataInS.toInt();
amax[2] = dataInS.toInt();

return;
}
target[reg] = dataInS.toInt();
target[reg] = constrain(target[reg], angle_min[reg], angle_max[reg]);
Serial.println(target[reg]);
} else if (dataIn.indexOf("r") != -1) {
run_now = 1;
} else if (dataIn.indexOf("t") != -1) {
run_first = 1;
run_loop = 1;
group[0] = 1;
//target[0] -= offset_ + offset_2;

target[0] = constrain(target[0], angle_min[0], angle_max[0]);

 dir_ = first_foot == 1 ? 1 : -1;
right_offset = first_foot == 1 ? 0 : value_offset;
target[first_foot] -= (offset_ * dir_ - right_offset);
target[first_foot] = constrain(target[first_foot], angle_min[first_foot],
angle_max[first_foot]);
target[first_foot + 1] += offset_ * dir_ - right_offset;
target[first_foot + 1] = constrain(target[first_foot + 1], angle_min[first_foot
+ 1], angle_max[first_foot + 1]);
} else if (dataIn.indexOf("h") != -1) {
String dataInS = dataIn.substring(1, dataIn.length()); // Extract only the
number. E.g. from "s1120" to "120"
offset_ = dataInS.toInt();
} else if (dataIn.indexOf("k") != -1) {
String dataInS = dataIn.substring(1, dataIn.length()); // Extract only the
number. E.g. from "s1120" to "120"
offset_2 = dataInS.toInt();
} else if (dataIn.indexOf("l") != -1) {
if (run_forever == 0) {
group[0] = 1;
//target[0] -= offset_ + offset_2;
run_first = 1;
// run_loop = 1;
//target[0] -= offset_ + offset_2;
target[0] = constrain(target[0], angle_min[0], angle_max[0]);
dir_ = first_foot == 1 ? 1 : -1;
right_offset = first_foot == 1 ? 0 : value_offset;
target[first_foot] -= offset_ * dir_ - right_offset; // goi - tien goi - lui
target[first_foot] = constrain(target[first_foot], angle_min[first_foot],
angle_max[first_foot]);
target[first_foot + 1] += offset_ * dir_ - right_offset; // dui + tien dui -
lui
target[first_foot + 1] = constrain(target[first_foot + 1],
angle_min[first_foot + 1], angle_max[first_foot + 1]);
} else if (run_forever == 1) {
for (byte servo = 0; servo < NumServos; servo++) {
p[servo] = InitialPosition[servo];

target[servo] = p[servo];
pwm.setPWM(servo, 0, cvt_angle(p[servo]));
// group[servo] = 0;
}
}
run_forever = 1 - run_forever;
} else if (dataIn.indexOf("u") != -1) {
int reg_ = dataIn[dataIn.indexOf("u") + 1] - '0';
Serial.println(reg_);
if (reg_ == 1) {
turn_left_start = 1;
} else {
turn_right_start = 1;
}
}
}
void handle_message(WebsocketsMessage msg) {
String data_ = msg.data();
check_input(data_);
}
void loop() {
// put your main code here, to run repeatedly:
 auto client = server.accept();
client.onMessage(handle_message);
while (client.available()) {
client.poll();
if (run_now == 1) {
check_run();
MoveServos();
Serial.println("run");
}
if (run_loop == 1) {
move_count();
} else if (run_forever == 1) {
move_forever();
} else if (turn_right_start == 1) {
turn_right(offset_2 + 3);
} else if (turn_left_start == 1) {
sit_down();
}
}
}
//MoveServos();