[{"id":"1427a597-b584-44ff-9f44-eb79417dd80d","type":"code","data":{"code":"#include \n#include \n#include \n#include \n\n// WiFi 설정\nconst char* ssid = \"blueberry\";\nconst char* password = \"blueberry\";\n\nWebServer server(80);\n\n// MPU9250 주소\nconst int MPU9250_ADDR = 0x68;\n\n// 모터 제어 핀 설정\nconst int MOTOR_A = 23;\nconst int MOTOR_B = 19;\nconst int MOTOR_C = 18;\nconst int MOTOR_D = 26;\nconst int MOTOR_FREQ = 5000;\nconst int MOTOR_RESOLUTION = 10;\n\n// 드론 상태\nfloat throttle = 0;\nfloat pitch = 0;\nfloat roll = 0;\nfloat yaw = 0;\n\n// 센서 데이터\nfloat accelX, accelY, accelZ;\nfloat gyroX, gyroY, gyroZ;\nfloat angleX, angleY, angleZ;\n\n// PID 제어 상수\nstruct PIDConstants {\n float Kp, Ki, Kd;\n};\n\nPIDConstants pidRoll = {3.0, 0.02, 1.5};\nPIDConstants pidPitch = {3.0, 0.02, 1.5};\nPIDConstants pidYaw = {1.0, 0.01, 0.5};\n\n// PID 변수\nstruct PIDVariables {\n float error, previousError, integral;\n};\n\nPIDVariables pidVarsRoll, pidVarsPitch, pidVarsYaw;\n\nunsigned long lastSensorUpdate = 0;\nconst long sensorUpdateInterval = 50; // 50ms마다 센서 데이터 업데이트\n\nunsigned long previousTime = 0;\nfloat elapsedTime;\n\nAdafruit_BMP280 bmp;\n\n// HTML 웹 페이지 (1번 코드와 동일)\nconst char index_html[] PROGMEM = R\"rawliteral(\n\n\n\n \n \n 드론 제어 패널\n \n\n\n

드론 제어 패널

\n \n 0\n

\n Throttle\n

\n \n 0\n

\n Pitch\n

\n \n 0\n

\n Roll\n

\n \n 0\n

\n Yaw\n

센서 데이터

가속도 (g) - X: 0, Y: 0, Z: 0

자이로 (deg/s) - X: 0, Y: 0, Z: 0

\n \n\n\n)rawliteral\";\n\nvoid setup() {\n Serial.begin(115200);\n \n // Wi-Fi AP 모드 설정\n WiFi.softAP(ssid, password);\n\n IPAddress IP = WiFi.softAPIP();\n Serial.print(\"AP IP address: \");\n Serial.println(IP);\n\n // I2C 초기화\n Wire.begin();\n \n // MPU9250 초기화\n initMPU9250();\n\n // BMP280 초기화\n if (!bmp.begin(0x76)) {\n Serial.println(\"Could not find a valid BMP280 sensor, check wiring!\");\n while (1);\n }\n\n // 모터 핀 설정\n ledcSetup(0, MOTOR_FREQ, MOTOR_RESOLUTION);\n ledcSetup(1, MOTOR_FREQ, MOTOR_RESOLUTION);\n ledcSetup(2, MOTOR_FREQ, MOTOR_RESOLUTION);\n ledcSetup(3, MOTOR_FREQ, MOTOR_RESOLUTION);\n ledcAttachPin(MOTOR_A, 0);\n ledcAttachPin(MOTOR_B, 1);\n ledcAttachPin(MOTOR_C, 2);\n ledcAttachPin(MOTOR_D, 3);\n\n // 웹 서버 라우트 설정\n server.on(\"/\", HTTP_GET, handleRoot);\n server.on(\"/update\", HTTP_GET, handleUpdate);\n server.on(\"/getSensorData\", HTTP_GET, handleGetSensorData);\n\n server.begin();\n Serial.println(\"HTTP server started\");\n}\n\nvoid initMPU9250() {\n Wire.beginTransmission(MPU9250_ADDR);\n Wire.write(0x6B); // PWR_MGMT_1 register\n Wire.write(0); // MPU9250 동작 시작\n Wire.endTransmission(true);\n\n Wire.beginTransmission(MPU9250_ADDR);\n Wire.write(0x1C); // ACCEL_CONFIG register\n Wire.write(0x00); // +-2g 범위 설정\n Wire.endTransmission(true);\n\n Wire.beginTransmission(MPU9250_ADDR);\n Wire.write(0x1B); // GYRO_CONFIG register\n Wire.write(0x00); // +-250 degrees/s 범위 설정\n Wire.endTransmission(true);\n\n Serial.println(\"MPU9250 initialized\");\n}\n\nvoid updateSensors() {\n Wire.beginTransmission(MPU9250_ADDR);\n Wire.write(0x3B); // 가속도계 데이터 시작 레지스터\n Wire.endTransmission(false);\n Wire.requestFrom(MPU9250_ADDR, 14, true); // 가속도계 + 온도 + 자이로스코프 데이터 요청\n \n int16_t accelerometer_x = Wire.read() << 8 | Wire.read();\n int16_t accelerometer_y = Wire.read() << 8 | Wire.read();\n int16_t accelerometer_z = Wire.read() << 8 | Wire.read();\n \n int16_t temperature = Wire.read() << 8 | Wire.read();\n \n int16_t gyro_x = Wire.read() << 8 | Wire.read();\n int16_t gyro_y = Wire.read() << 8 | Wire.read();\n int16_t gyro_z = Wire.read() << 8 | Wire.read();\n \n accelX = accelerometer_x / 16384.0;\n accelY = accelerometer_y / 16384.0;\n accelZ = accelerometer_z / 16384.0;\n \n gyroX = gyro_x / 131.0;\n gyroY = gyro_y / 131.0;\n gyroZ = gyro_z / 131.0;\n\n // 각도 계산 (간단한 방법, 실제로는 더 복잡한 알고리즘 사용 권장)\n angleX = atan2(accelY, accelZ) * 180 / PI;\n angleY = atan2(-accelX, sqrt(accelY * accelY + accelZ * accelZ)) * 180 / PI;\n angleZ += gyroZ * elapsedTime; // 요 각도는 자이로 데이터만 사용\n}\n\nfloat calculatePID(PIDVariables &vars, PIDConstants &constants, float current, float target) {\n float error = target - current;\n vars.integral += error * elapsedTime;\n float derivative = (error - vars.previousError) / elapsedTime;\n float output = constants.Kp * error + constants.Ki * vars.integral + constants.Kd * derivative;\n vars.previousError = error;\n return output;\n}\n\nvoid handleRoot() {\n server.send(200, \"text/html\", index_html);\n}\n\nvoid handleUpdate() {\n if (server.hasArg(\"throttle\")) throttle = server.arg(\"throttle\").toFloat();\n if (server.hasArg(\"pitch\")) pitch = server.arg(\"pitch\").toFloat();\n if (server.hasArg(\"roll\")) roll = server.arg(\"roll\").toFloat();\n if (server.hasArg(\"yaw\")) yaw = server.arg(\"yaw\").toFloat();\n \n server.send(200, \"text/plain\", \"Updated\");\n}\n\nvoid handleGetSensorData() {\n String json = \"{\";\n json += \"\\\"accelX\\\":\" + String(accelX, 2) + \",\";\n json += \"\\\"accelY\\\":\" + String(accelY, 2) + \",\";\n json += \"\\\"accelZ\\\":\" + String(accelZ, 2) + \",\";\n json += \"\\\"gyroX\\\":\" + String(gyroX, 2) + \",\";\n json += \"\\\"gyroY\\\":\" + String(gyroY, 2) + \",\";\n json += \"\\\"gyroZ\\\":\" + String(gyroZ, 2);\n json += \"}\";\n \n server.send(200, \"application/json\", json);\n}\n\nvoid controlMotors() {\n float pidOutputRoll = calculatePID(pidVarsRoll, pidRoll, angleX, roll);\n float pidOutputPitch = calculatePID(pidVarsPitch, pidPitch, angleY, pitch);\n float pidOutputYaw = calculatePID(pidVarsYaw, pidYaw, angleZ, yaw);\n\n int m1 = constrain(throttle + pidOutputPitch + pidOutputRoll - pidOutputYaw, 0, 1023);\n int m2 = constrain(throttle + pidOutputPitch - pidOutputRoll + pidOutputYaw, 0, 1023);\n int m3 = constrain(throttle - pidOutputPitch - pidOutputRoll - pidOutputYaw, 0, 1023);\n int m4 = constrain(throttle - pidOutputPitch + pidOutputRoll + pidOutputYaw, 0, 1023);\n\n ledcWrite(0, m1);\n ledcWrite(1, m2);\n ledcWrite(2, m3);\n ledcWrite(3, m4);\n\n Serial.printf(\"Motor values: %d, %d, %d, %d\\n\", m1, m2, m3, m4);\n}\n\nvoid loop() {\n server.handleClient();\n \n unsigned long currentMillis = millis();\n elapsedTime = (currentMillis - previousTime) / 1000.0;\n previousTime = currentMillis;\n\n if (currentMillis - lastSensorUpdate >= sensorUpdateInterval) {\n updateSensors();\n controlMotors();\n lastSensorUpdate = currentMillis;\n }\n}\n\n","language":"cpp"}}]

코풀 커뮤니티

drone_report