///

/////

//////
///
////
///
///
///
///
///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, 0x92, 0x9e, 0x92, 0x63,
0x00, 0xff, 0x69, 0x6e, 0x64, 0x65, 0x78, 0x2e, 0x68, 0x74, 0x6d, 0x6c, 0x2e, 0x67,
0x7a, 0x00, 0xad, 0x58, 0xff, 0x6f, 0xda, 0x38, 0x14, 0xff, 0x19, 0xfe, 0x0a, 0x9f,
0xa7, 0xd3, 0x82, 0x28, 0x5f, 0x42, 0xbf, 0xd1, 0x94, 0x20, 0x75, 0x5b, 0x77, 0x77,
0xd2, 0x6e, 0x9d, 0xda, 0x4e, 0xd3, 0x69, 0x9a, 0x90, 0x49, 0x5c, 0xf0, 0x35, 0xd8,
0x28, 0x71, 0x60, 0x5c, 0xd5, 0xff, 0x7d, 0xcf, 0x76, 0x42, 0x42, 0x1b, 0x42, 0x40,
0x17, 0x84, 0x48, 0xec, 0xf7, 0x79, 0x9f, 0xf7, 0xec, 0x8f, 0xed, 0x47, 0x06, 0xbf,
0x7d, 0xb8, 0x79, 0x7f, 0xff, 0xcf, 0x97, 0x6b, 0x34, 0x95, 0xb3, 0x60, 0x58, 0x1f,
0xa4, 0x3f, 0x94, 0xf8, 0xf0, 0x33, 0xa3, 0x92, 0x20, 0x4e, 0x66, 0xd4, 0xc5, 0x0b,
0x46, 0x97, 0x73, 0x11, 0x4a, 0x8c, 0x3c, 0xc1, 0x25, 0xe5, 0xd2, 0xc5, 0x4b, 0xe6,
0xcb, 0xa9, 0xeb, 0xd3, 0x05, 0xf3, 0x68, 0x4b, 0x3f, 0x1c, 0x21, 0xc6, 0x99, 0x64,
0x24, 0x68, 0x45, 0x1e, 0x09, 0xa8, 0x6b, 0x63, 0x70, 0x12, 0xc9, 0x55, 0x40, 0x87,
0xf5, 0x76, 0x14, 0x30, 0x9f, 0x2a, 0x30, 0x61, 0x9c, 0x86, 0xe8, 0xa9, 0x8e, 0x90,
0x06, 0x39, 0xc8, 0xee, 0x76, 0x7f, 0xbf, 0xac, 0x3f, 0xd7, 0x13, 0x1b, 0xd3, 0xd7,
0x5a, 0xd2, 0xf1, 0x23, 0x93, 0x2d, 0x32, 0x9f, 0x53, 0x12, 0x12, 0xee, 0x51, 0x07,
0x71, 0xc1, 0xe9, 0x25, 0xf4, 0xcd, 0x48, 0x38, 0x61, 0xbc, 0x15, 0xd0, 0x07, 0xe9,
0xd8, 0x0a, 0x8b, 0xd0, 0x5c, 0x44, 0xc0, 0x2c, 0xb8, 0x83, 0xc8, 0x38, 0x12, 0x41,
0x2c, 0xb5, 0xa1, 0xa4, 0x3f, 0xc1, 0x43, 0xc0, 0x26, 0xd0, 0xee, 0x41, 0xd0, 0x34,
0x54, 0xad, 0x09, 0xed, 0xb9, 0x41, 0x4e, 0x29, 0x9b, 0x4c, 0xa5, 0x83, 0x7a, 0xa7,
0xf3, 0x9f, 0xea, 0x79, 0x4c, 0xbc, 0xc7, 0x49, 0x28, 0x62, 0xee, 0x3b, 0xe8, 0x8d,
0x7f, 0xac, 0x3e, 0xaa, 0x59, 0xc4, 0x32, 0x80, 0xc0, 0xb3, 0x20, 0xc4, 0x9c, 0x78,
0x4c, 0xae, 0x1c, 0xd4, 0x6d, 0x9f, 0xab, 0xe7, 0x34, 0x60, 0x09, 0xc1, 0xa6, 0xb1,
0xb4, 0x7b, 0x91, 0x0e, 0x23, 0xd7, 0x94, 0xc0, 0x4c, 0x57, 0x2d, 0x97, 0xb5, 0x33,
0x15, 0x8b, 0x24, 0xf7, 0xb5, 0x6b, 0x3b, 0x3f, 0x2c, 0x8e, 0x93, 0x52, 0x98, 0xe7,
0x96, 0x9c, 0xc6, 0xb3, 0xf1, 0xce, 0xc1, 0x2a, 0x6a, 0x4b, 0x46, 0x20, 0x4d, 0xb9,
0x74, 0x08, 0xba, 0x27, 0x57, 0x57, 0x67, 0x1f, 0x54, 0xb3, 0x17, 0x87, 0x91, 0x08,
0x1d, 0x18, 0x6b, 0x66, 0x86, 0x72, 0x23, 0xb6, 0x99, 0xf8, 0xaf, 0x05, 0x34, 0x13,
0x9a, 0x8b, 0xeb, 0x7f, 0xe3, 0x69, 0x8f, 0x63, 0x29, 0x61, 0xf8, 0xf4, 0x18, 0x3d,
0xd5, 0x6b, 0x39, 0x68, 0xcb, 0x13, 0x81, 0xb2, 0x7e, 0x73, 0xf2, 0xfe, 0xea, 0xe3,
0x69, 0x57, 0x3b, 0x30, 0x2d, 0x93, 0x90, 0x52, 0xae, 0xd0, 0x83, 0x4e, 0x22, 0xc2,
0x41, 0x27, 0x51, 0xf6, 0x58, 0xf8, 0xab, 0x61, 0x1d, 0x84, 0x6e, 0x0f, 0x7b, 0xe8,
0xd3, 0xf5, 0x1f, 0xe8, 0xf6, 0xe6, 0xdd, 0xcd, 0x3d, 0x74, 0xdb, 0xaa, 0xd5, 0x67,
0x0b, 0xe4, 0x05, 0x24, 0x8a, 0x5c, 0xbc, 0xa9, 0x5a, 0xa5, 0x67, 0x82, 0x98, 0x0f,
0xeb, 0x81, 0x04, 0xa0, 0xee, 0x3b, 0xdb, 0x19, 0x74, 0xc8, 0x70, 0xc0, 0xf8, 0x3c,
0x96, 0x48, 0xae, 0xe6, 0xb0, 0x52, 0xf4, 0x18, 0x60, 0x34, 0x63, 0xdc, 0xc5, 0xc7,
0x5d, 0xb8, 0x21, 0x3f, 0x5d, 0xdc, 0x3f, 0xc3, 0x08, 0x20, 0x31, 0xf4, 0x9f, 0x43,
0x5b, 0xde, 0x79, 0x88, 0xb5, 0xc3, 0xd9, 0xea, 0x56, 0x01, 0xf5, 0x92, 0x19, 0x87,
0x43, 0xfd, 0x5d, 0x53, 0xf5, 0x80, 0xaa, 0x57, 0x4e, 0x75, 0x96, 0x52, 0xd9, 0x3d,
0xb8, 0x4b, 0xb8, 0xec, 0x6e, 0xbf, 0x8c, 0x0c, 0xdc, 0x2a, 0x1e, 0x43, 0xb8, 0x26,
0x3b, 0x06, 0xb2, 0xe3, 0x72, 0xb2, 0xd3, 0x35, 0x99, 0xba, 0x4b, 0xc8, 0x2e, 0x4a,
0x13, 0x03, 0xaf, 0x05, 0x5c, 0x27, 0x78, 0x48, 0x46, 0xa1, 0x56, 0x45, 0x29, 0xa1,
0xbd, 0xe6, 0xcb, 0x72, 0xc3, 0x28, 0x92, 0x74, 0xee, 0xe2, 0x6e, 0x1b, 0x6e, 0xb7,
0x13, 0x03, 0x45, 0x01, 0xf1, 0x29, 0x1e, 0x2e, 0xaa, 0x10, 0x67, 0xc3, 0xda, 0xcf,
0x32, 0x3d, 0x29, 0xcd, 0x14, 0x5c, 0x17, 0x10, 0x9e, 0x41, 0x6b, 0xa9, 0x54, 0x20,
0x33, 0x93, 0x4f, 0x9a, 0x6b, 0x3f, 0xcb, 0x55, 0x8d, 0x32, 0xc8, 0x7a, 0x2b, 0xa5,
0x72, 0xfe, 0x9a, 0xf2, 0x7c, 0x07, 0xa5, 0x4a, 0x63, 0x93, 0x52, 0x4d, 0x61, 0x45,
0x4a, 0xe5, 0xfc, 0x35, 0x65, 0x7f, 0x57, 0x96, 0x65, 0x94, 0x6a, 0x65, 0x94, 0x51,
0x2a, 0xe7, 0xaf, 0x29, 0x2f, 0x94, 0x84, 0xf4, 0x51, 0xb0, 0x4b, 0x41, 0x39, 0xb9,
0x14, 0x88, 0xa9, 0x8c, 0x19, 0x38, 0x0a, 0x98, 0xed, 0xae, 0x12, 0xd1, 0x6e, 0x6a,
0xa5, 0xa1, 0xcd, 0xa4, 0x37, 0xc4, 0x04, 0x51, 0x94, 0x71, 0x2b, 0x96, 0x22, 0x72,
0xd8, 0x29, 0xc6, 0xb1, 0xf0, 0xca, 0xa9, 0x5b, 0xaf, 0xe7, 0x58, 0xb5, 0x24, 0xd4,
0x6a, 0x36, 0x4a, 0xa9, 0x81, 0xa3, 0x88, 0x1a, 0xf6, 0x8d, 0x77, 0x84, 0x8f, 0xbc,
0x29, 0xe1, 0xe5, 0xec, 0x10, 0xfb, 0xcb, 0xcc, 0x33, 0xf6, 0xde, 0x0e, 0xf2, 0x82,
0xdd, 0x09, 0x4e, 0x81, 0x51, 0x18, 0xf3, 0x44, 0x64, 0xe6, 0x4c, 0x40, 0x82, 0x7b,
0x01, 0xf3, 0x1e, 0xc1, 0x09, 0xe5, 0xfe, 0xc7, 0x98, 0x7b, 0xea, 0xa0, 0xb5, 0x1a,
0x78, 0x78, 0xfb, 0xf5, 0xf3, 0xa0, 0x63, 0x8c, 0x52, 0x4f, 0x2f, 0x10, 0x24, 0x96,
0x22, 0x8f, 0xb8, 0xfa, 0x7a, 0x7f, 0xb3, 0x86, 0x94, 0x9b, 0xf7, 0x52, 0xfb, 0x5e,
0x0e, 0xd0, 0x81, 0x93, 0x43, 0x1d, 0x20, 0x1d, 0x73, 0xbe, 0x0c, 0x22, 0x2f, 0x64,
0x73, 0x39, 0x84, 0x23, 0x49, 0x5f, 0x70, 0x8e, 0x44, 0x12, 0x7d, 0xbb, 0x1b, 0x7d,
0xbd, 0xfd, 0x84, 0x5c, 0x84, 0x97, 0x91, 0xd3, 0xe9, 0x60, 0xd4, 0x84, 0x73, 0x92,
0xfb, 0x62, 0xd9, 0x0e, 0x84, 0x47, 0x94, 0xef, 0xf6, 0x54, 0x80, 0x5d, 0x13, 0x61,
0xa7, 0xdf, 0xc3, 0xea, 0x40, 0x4b, 0xa1, 0xcb, 0x08, 0x60, 0x9c, 0x2e, 0xd1, 0x37,
0x3a, 0xbe, 0x13, 0xde, 0x23, 0x95, 0x96, 0xf1, 0xd6, 0x30, 0x56, 0x0b, 0x12, 0x22,
0x3d, 0x01, 0xa3, 0xc4, 0xee, 0x2a, 0x0c, 0xc9, 0xca, 0xca, 0xf5, 0xea, 0xb1, 0x2f,
0xec, 0x35, 0xb8, 0xf6, 0x3c, 0x8e, 0xa6, 0x96, 0x2f, 0xbc, 0x78, 0x06, 0xf5, 0x52,
0x7b, 0x42, 0xe5, 0x75, 0x40, 0xd5, 0xed, 0xbb, 0xd5, 0x5f, 0xbe, 0x95, 0x9d, 0x52,
0x8d, 0x03, 0x50, 0xbd, 0x83, 0x50, 0xc7, 0x07, 0xa1, 0x4e, 0x0e, 0x42, 0x9d, 0x1e,
0x84, 0x3a, 0xd3, 0xa8, 0x7d, 0x10, 0xe7, 0x07, 0xf1, 0xf4, 0xf7, 0xe6, 0xb9, 0xd8,
0x1b, 0x01, 0xeb, 0x73, 0x6f, 0x48, 0xa6, 0x06, 0xa3, 0xae, 0x1d, 0x30, 0x5d, 0x3a,
0xed, 0x8b, 0xc8, 0xb4, 0x53, 0x15, 0x91, 0xe9, 0xa6, 0x2a, 0x22, 0xd3, 0x4c, 0x55,
0x44, 0xa6, 0x97, 0xaa, 0x08, 0xa3, 0x95, 0x5a, 0x55, 0xf3, 0x54, 0x28, 0x55, 0xed,
0x8d, 0x44, 0x2a, 0xbb, 0x37, 0xfa, 0xa8, 0x3c, 0x6b, 0x46, 0x1b, 0x95, 0xcd, 0xcd,
0x2c, 0xef, 0x25, 0xa5, 0xfd, 0xa7, 0x39, 0x07, 0x79, 0x10, 0xa1, 0x15, 0x50, 0x89,
0x18, 0x6c, 0x6f, 0x5d, 0x74, 0xc9, 0x06, 0x76, 0xf7, 0x92, 0x35, 0x9b, 0x0d, 0xdd,
0xa9, 0xfe, 0x8e, 0x98, 0xcb, 0x04, 0xf4, 0x9d, 0xfd, 0x68, 0x0b, 0x6e, 0x4e, 0x2e,
0x17, 0x3d, 0xac, 0x0f, 0x81, 0xb5, 0x19, 0xfc, 0xcb, 0xa8, 0xb1, 0x07, 0x8b, 0xb9,
0xee, 0x71, 0xa3, 0x0e, 0x0f, 0x26, 0x20, 0x05, 0x63, 0x1c, 0xfe, 0x0a, 0xfc, 0x79,
0xff, 0x37, 0xec, 0xe1, 0x38, 0xad, 0x5e, 0x71, 0x53, 0x4e, 0x59, 0xd4, 0xd6, 0x46,
0x30, 0x01, 0xea, 0x28, 0xb2, 0x70, 0x84, 0x9b, 0x2c, 0xd7, 0xde, 0xb8, 0x44, 0xf0,
0x67, 0x84, 0x06, 0x11, 0x45, 0xc6, 0xf1, 0x49, 0x89, 0xe3, 0xb4, 0x3a, 0x3d, 0xc8,
0x71, 0x5f, 0x39, 0xde, 0x16, 0xb0, 0x2e, 0x58, 0x0e, 0x72, 0x7b, 0xb1, 0xdd, 0xad,
0xa9, 0x83, 0xf6, 0xf3, 0x9a, 0x4d, 0x49, 0x36, 0xe1, 0x2f, 0xfc, 0x22, 0x7c, 0x87,
0x9b, 0x16, 0x6b, 0xda, 0x8d, 0x26, 0x7e, 0x11, 0x73, 0x1e, 0xbb, 0x9d, 0x28, 0xb3,
0x59, 0x03, 0x9e, 0xf5, 0xdd, 0x73, 0x22, 0xcc, 0xef, 0x76, 0x77, 0xa7, 0x10, 0x0a,
0xc2, 0x54, 0xa8, 0x8d, 0x38, 0x55, 0x2d, 0x86, 0x2b, 0x04, 0x39, 0xcd, 0x1b, 0x40,
0x88, 0x85, 0x01, 0xa6, 0xa1, 0xd9, 0x07, 0x85, 0x06, 0xa8, 0x8d, 0xd0, 0xc4, 0xc3,
0x43, 0x44, 0xe5, 0x68, 0x9c, 0x94, 0x6c, 0x55, 0xa2, 0x7c, 0xac, 0x12, 0x65, 0x2d,
0x8d, 0x08, 0x6d, 0xd6, 0x51, 0x4f, 0x75, 0xe3, 0x44, 0xfa, 0x58, 0x41, 0xeb, 0xaa,
0x76, 0x11, 0x01, 0x85, 0xf2, 0x66, 0x62, 0xf5, 0x60, 0xad, 0x6e, 0x43, 0x42, 0x49,
0x95, 0x42, 0x83, 0x2d, 0xc8, 0x5a, 0x7d, 0x8d, 0xdc, 0xac, 0xf6, 0xd4, 0x6a, 0x35,
0xd0, 0xd0, 0x40, 0x6b, 0xb5, 0x3c, 0xf8, 0x14, 0xaa, 0xa9, 0x33, 0x4d, 0x9d, 0x16,
0x42, 0x50, 0x77, 0xca, 0x7b, 0x78, 0x3d, 0x74, 0xa4, 0xef, 0xee, 0x00, 0x7a, 0xa4,
0x5d, 0xde, 0xb3, 0x19, 0x85, 0x77, 0x3d, 0x97, 0x75, 0xb5, 0x93, 0x80, 0x0b, 0x4e,
0x35, 0xc3, 0x28, 0xa4, 0xe0, 0x0c, 0x5e, 0x3e, 0xa8, 0x0a, 0xec, 0xfb, 0x8f, 0xa4,
0x7b, 0x4a, 0xbd, 0xc7, 0xf7, 0x6b, 0x1b, 0xe8, 0xb1, 0x1a, 0xc8, 0x1d, 0x26, 0xd3,
0x12, 0x52, 0x19, 0x87, 0x5c, 0x17, 0x5a, 0x5f, 0x42, 0x31, 0x63, 0x11, 0xb5, 0xac,
0xc4, 0xcb, 0x11, 0x74, 0xfe, 0x0b, 0xa0, 0x9c, 0xb5, 0xba, 0xd8, 0x03, 0xb2, 0x96,
0x51, 0x3b, 0x84, 0x77, 0x14, 0xab, 0x3b, 0x49, 0x24, 0x45, 0xae, 0xeb, 0x66, 0xb5,
0x5e, 0xfb, 0xe6, 0xcb, 0xf5, 0xe7, 0xc6, 0x8b, 0x49, 0x4f, 0x3c, 0xa6, 0x55, 0x5c,
0x26, 0x70, 0x75, 0xe9, 0xb5, 0xbb, 0x69, 0x5f, 0x94, 0x92, 0xd9, 0x65, 0x9f, 0x5e,
0x04, 0xf7, 0xfc, 0xca, 0xa5, 0x6a, 0x81, 0x77, 0x3e, 0x10, 0x22, 0xf1, 0xfd, 0xeb,
0x05, 0x6c, 0xc3, 0x9f, 0x18, 0x94, 0xf7, 0x20, 0x35, 0xeb, 0xad, 0x98, 0x53, 0xfe,
0xf6, 0x68, 0x63, 0x00, 0x0a, 0xa9, 0x60, 0x93, 0xbe, 0x26, 0xde, 0xd4, 0x0a, 0x95,
0x5d, 0x08, 0xb9, 0x26, 0xe1, 0x37, 0xea, 0x79, 0x3e, 0x75, 0x91, 0x68, 0xc5, 0xbd,
0xb5, 0xea, 0x8d, 0x30, 0x7d, 0x22, 0x49, 0x3a, 0x02, 0x64, 0x49, 0xd8, 0xab, 0x39,
0x48, 0xc7, 0x01, 0x62, 0xcc, 0x00, 0xe9, 0xbc, 0xaf, 0x35, 0x0c, 0x2f, 0x84, 0x92,
0xca, 0x1c, 0x5e, 0xf9, 0xe8, 0x57, 0x9e, 0xbf, 0x00, 0x6e, 0x94, 0xe4, 0x5d, 0x0a,
0x15, 0x00, 0x00 };
#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] = { 168, 168, 169, 15, 20, 172, 172, 172, 15,
20 }; // max change in p per step
float amax[NumServos] = { 9.8, 9.8, 9.8, 0.1, 0.1, 10.5, 10.6, 10.6, 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_ = 14.6;
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 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;
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 -
if(part==dui_trai)
{
dir=-dir;
}
target[part]+=angle*dir;
target[part]=constrain(target[part],angle_min[part],angle_max[part]);
}
void move_right(int part,int angle,int dir)
{
// de -> tien
if(part==dui_phai)dir=-dir;
target[part]-=angle*dir;
target[part]=constrain(target[part],angle_min[part],angle_max[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 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)) {
Serial.println("run4");
run_4 = 0;
run_first = 1;
group[0] = 1;
//target[0] -= offset_ + offset_2;
vmax[0] = last_v[0];
amax[0] = last_a[0];
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]);
first_foot = 7 - first_foot;
dir_ = first_foot == 1 ? 1 : -1;
delay(1500);
}
}
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();
last_v[1] = vmax[5];
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();
last_v[0] = vmax[0];

return;

} else if (reg == 9) {
amax[0] = dataInS.toInt();
last_a[0] = amax[0];
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;
vmax[0] = last_v[0];
amax[0] = last_a[0];
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;
vmax[0] = last_v[0];
amax[0] = last_a[0];

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 (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;
}
}
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();
}
}
}
//MoveServos();