I try to modify one's source code of esp8266 flight controller, but I can't access the IP address and web page interface to update PID tuning not showing at the IP address I decided.
I can ping via windows cmd. It replied.
Please let me know what makes it error (not showing anything). It is the third day and perhaps, the code goes terrible. Please help
#include <Wire.h>
#include <ESP8266WiFi.h>
#include <ESP8266WebServer.h>
#include <WiFiUdp.h>
WiFiUDP UDP;
char packet[7];
boolean recvState;
IPAddress apIP(192, 168, 1, 1);
ESP8266WebServer server (80);
const char* ssid = "QuadX";
const char* password = "12345678";
#define D5 14
#define D6 13
#define D7 12
#define D8 15
// Minimize unstable drone in yaw
double desired_yaw = 0.0; // The target yaw angle
double current_yaw = 0.0; // The actual yaw angle measured by the sensor
double last_yaw_error = 0.0;
double integrateYawError(double yaw_error) {
static double yaw_error_sum = 0;
yaw_error_sum += yaw_error;
return yaw_error_sum;
}
int ESCpin_1, ESCpin_2, ESCpin_3, ESCpin_4;
int input_PITCH = 50;
int input_ROLL = 50;
int input_YAW;
int input_THROTTLE = 1050;
int state1, state2, state3, state4;
// IMU Variables
int16_t gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z, temperature, acc_total_vector;
float angle_pitch, angle_roll, angle_yaw;
boolean set_gyro_angles;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch_output, angle_roll_output;
float elapsedTime;
long Time, timePrev, time2;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
// PID Variables
float pitch_PID, roll_PID, yaw_PID;
float roll_error, roll_previous_error, pitch_error, pitch_previous_error, yaw_error;
float roll_pid_p, roll_pid_d, roll_pid_i, pitch_pid_p, pitch_pid_i, pitch_pid_d, yaw_pid_p, yaw_pid_i;
float roll_desired_angle, pitch_desired_angle, yaw_desired_angle;
// PID Parameters
double twoX_kp = 5;
double twoX_ki = 0.003;
double twoX_kd = 2;
double yaw_kp = 3;
double yaw_ki = 0.002;
double yaw_kd = 0.0;
void setup() {
pinMode(D5, OUTPUT);
pinMode(D6, OUTPUT);
pinMode(D7, OUTPUT);
pinMode(D8, OUTPUT);
delay(1300);
Serial.begin(115200);
// WiFi.softAPConfig(apIP, apIP, IPAddress(255, 255, 255, 0));
WiFi.softAP(ssid,password);
IPAddress myIP = WiFi.softAPIP();
while (WiFi.status() != WL_CONNECTED) {
GPOS = (1 << 14); GPOS = (1 << 12); GPOS = (1 << 13); GPOS = (1 << 15);
delayMicroseconds(1000);
GPOC = (1 << 14); GPOC = (1 << 12); GPOC = (1 << 13); GPOC = (1 << 15);
delayMicroseconds(1000);
yield();
}
// Set up routes
server.on("/", handleRoot);
server.on("/updatePID", handleUpdatePID);
Serial.println(WiFi.softAPIP());
UDP.begin(9999);
Wire.begin();
Wire.setClock(400000);
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x08);
Wire.endTransmission();
delay(1000);
for (int cal_int = 0; cal_int < 2000; cal_int++) {
if (cal_int % 125 == 0) Serial.print(".");
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
acc_x = Wire.read() << 8 | Wire.read();
acc_y = Wire.read() << 8 | Wire.read();
acc_z = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gyro_x = Wire.read() << 8 | Wire.read();
gyro_y = Wire.read() << 8 | Wire.read();
gyro_z = Wire.read() << 8 | Wire.read();
gyro_x_cal += gyro_x;
gyro_y_cal += gyro_y;
gyro_z_cal += gyro_z;
GPOS = (1 << 14); GPOS = (1 << 12); GPOS = (1 << 13); GPOS = (1 << 15);
delayMicroseconds(1000);
GPOC = (1 << 14); GPOC = (1 << 12); GPOC = (1 << 13); GPOC = (1 << 15);
delayMicroseconds(1000);
yield();
}
gyro_x_cal /= 2000;
gyro_y_cal /= 2000;
gyro_z_cal /= 2000;
Time = micros();
}
void loop() {
GPOS = (1 << 14); GPOS = (1 << 12); GPOS = (1 << 13); GPOS = (1 << 15);
timePrev = Time;
Time = micros();
elapsedTime = (float)(Time - timePrev) / (float)1000000;
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
acc_x = Wire.read() << 8 | Wire.read();
acc_y = Wire.read() << 8 | Wire.read();
acc_z = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gyro_x = Wire.read() << 8 | Wire.read();
gyro_y = Wire.read() << 8 | Wire.read();
gyro_z = Wire.read() << 8 | Wire.read();
gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;
angle_pitch += gyro_x * elapsedTime * 0.01526717557;
angle_roll += gyro_y * elapsedTime * 0.01526717557;
angle_yaw += gyro_z * elapsedTime * 0.01526717557;
angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);
acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z));
angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296;
angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296;
angle_pitch_acc += 0;
angle_roll_acc += 0;
if (set_gyro_angles) {
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
} else {
angle_pitch = angle_pitch_acc;
angle_roll = angle_roll_acc;
set_gyro_angles = true;
}
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;
roll_desired_angle = 3 * ((float)input_ROLL / (float)10 - (float)5);
pitch_desired_angle = 3 * ((float)input_PITCH / (float)10 - (float)5);
yaw_desired_angle = 0;
roll_error = angle_roll_output - roll_desired_angle;
pitch_error = angle_pitch_output - pitch_desired_angle;
yaw_error = angle_yaw - yaw_desired_angle;
roll_pid_p = twoX_kp * roll_error;
roll_pid_i = roll_pid_i + (twoX_ki * roll_error);
roll_pid_d = twoX_kd * ((roll_error - roll_previous_error) / elapsedTime);
pitch_pid_p = twoX_kp * pitch_error;
pitch_pid_i = pitch_pid_i + (twoX_ki * pitch_error);
pitch_pid_d = twoX_kd * ((pitch_error - pitch_previous_error) / elapsedTime);
yaw_pid_p = yaw_kp * yaw_error;
yaw_pid_i = yaw_pid_i + (yaw_ki * yaw_error);
yaw_pid_i = constrain(yaw_pid_i, -100, 100);
yaw_pid_p = constrain(yaw_pid_p, -400, 400);
roll_PID = roll_pid_p + roll_pid_i + roll_pid_d;
pitch_PID = pitch_pid_p + pitch_pid_i + pitch_pid_d;
yaw_PID = yaw_pid_p + yaw_pid_i;
// Motor adjustments
state1 = input_THROTTLE - roll_PID - pitch_PID + yaw_PID;
state2 = input_THROTTLE - roll_PID + pitch_PID - yaw_PID;
state3 = input_THROTTLE + roll_PID + pitch_PID + yaw_PID;
state4 = input_THROTTLE + roll_PID - pitch_PID - yaw_PID;
state1 = constrain(state1, 1050, 2000);
state2 = constrain(state2, 1050, 2000);
state3 = constrain(state3, 1050, 2000);
state4 = constrain(state4, 1050, 2000);
ESCpin_1 = state1;
ESCpin_2 = state2;
ESCpin_3 = state3;
ESCpin_4 = state4;
analogWrite(D5, ESCpin_1);
analogWrite(D6, ESCpin_2);
analogWrite(D7, ESCpin_3);
analogWrite(D8, ESCpin_4);
roll_previous_error = roll_error;
pitch_previous_error = pitch_error;
GPOC = (1 << 14); GPOC = (1 << 12); GPOC = (1 << 13); GPOC = (1 << 15);
yield();
server.handleClient();
Serial.println(WiFi.localIP());
Serial.println(WiFi.softAPIP());
}
void handleRoot() {
String html = "<h1>PID Tuning Interface</h1>";
html += "<form action=\"/updatePID\">";
html += "twoX_kp: <input type=\"text\" name=\"twoX_kp\" value=\"" + String(twoX_kp) + "\"><br>";
html += "twoX_ki: <input type=\"text\" name=\"twoX_ki\" value=\"" + String(twoX_ki) + "\"><br>";
html += "twoX_kd: <input type=\"text\" name=\"twoX_kd\" value=\"" + String(twoX_kd) + "\"><br>";
html += "yaw_kp: <input type=\"text\" name=\"yaw_kp\" value=\"" + String(yaw_kp) + "\"><br>";
html += "yaw_ki: <input type=\"text\" name=\"yaw_ki\" value=\"" + String(yaw_ki) + "\"><br>";
html += "yaw_kd: <input type=\"text\" name=\"yaw_kd\" value=\"" + String(yaw_kd) + "\"><br>";
html += "<input type=\"submit\" value=\"Update PID\">";
html += "</form>";
server.send(200, "text/html", html);
}
void handleUpdatePID() {
// Update PID values based on input from the web page
if (server.hasArg("twoX_kp")) twoX_kp = server.arg("twoX_kp").toFloat();
if (server.hasArg("twoX_ki")) twoX_ki = server.arg("twoX_ki").toFloat();
if (server.hasArg("twoX_kd")) twoX_kd = server.arg("twoX_kd").toFloat();
if (server.hasArg("yaw_kp")) yaw_kp = server.arg("yaw_kp").toFloat();
if (server.hasArg("yaw_ki")) yaw_ki = server.arg("yaw_ki").toFloat();
if (server.hasArg("yaw_kd")) yaw_kd = server.arg("yaw_kd").toFloat();
String message = "PID values updated: \n";
message += "twoX_kp: " + String(twoX_kp) + "\n";
message += "twoX_ki: " + String(twoX_ki) + "\n";
message += "twoX_kd: " + String(twoX_kd) + "\n";
message += "yaw_kp: " + String(yaw_kp) + "\n";
message += "yaw_ki: " + String(yaw_ki) + "\n";
message += "yaw_kd: " + String(yaw_kd) + "\n";
server.send(200, "text/plain", message);
}