[{"id":"9ec3a023-53d2-440c-9389-ebcbc533eea1","type":"code","data":{"code":"#include \n#include \n#include \n#include \n\nconst char* ssid = \"DroneControlAP\";\nconst char* password = \"dronepassword\";\n\nWebServer server(80);\n\n// 모터 제어 핀 설정\nconst int MOTOR_A = 23;\nconst int MOTOR_B = 19;\nconst int MOTOR_C = 18;\nconst int MOTOR_D = 26;\n\n// PWM 설정\nconst uint32_t PWM_FREQ = 5000; // 5 kHz\nconst uint8_t PWM_RESOLUTION = 10; // 10-bit resolution, 0-1023\n\n// MPU9250 설정\nconst int MPU9250_ADDR = 0x68;\nconst int GYRO_CONFIG = 0x1B;\nconst int ACCEL_CONFIG = 0x1C;\nconst int GYRO_XOUT_H = 0x43;\nconst int ACCEL_XOUT_H = 0x3B;\n\n// 센서 스케일 팩터\nconst float GYRO_SCALE_FACTOR = 131.0; // for ±250dps\nconst float ACCEL_SCALE_FACTOR = 16384.0; // for ±2g\n\n// 각도 및 센서 데이터 변수\nfloat angleX = 0, angleY = 0, angleZ = 0;\nfloat gyroX, gyroY, gyroZ;\nfloat accelX, accelY, accelZ;\nfloat altitude, targetAltitude;\n\n// 시간 관련 변수\nunsigned long previousTime = 0;\nfloat elapsedTime;\n\n// 최대 허용 기울기 (도)\nconst float MAX_TILT_ANGLE = 3.0;\nconst float MIN_TILT_ANGLE = 0.0;\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};\nPIDConstants pidAltitude = {1.0, 0.1, 0.5};\n\n// PID 변수\nstruct PIDVariables {\n float error, previousError, integral;\n};\n\nPIDVariables pidVarsRoll, pidVarsPitch, pidVarsYaw, pidVarsAltitude;\n\n// 목표 각도 (호버링을 위해 0으로 설정)\nfloat targetX = 0, targetY = 0, targetZ = 0;\n\n// 센서 오프셋\nfloat gyroXoffset = 0, gyroYoffset = 0, gyroZoffset = 0;\nfloat accelXoffset = 0, accelYoffset = 0, accelZoffset = 0;\n\n// 모터 속도 변화 제한\nconst int MAX_MOTOR_SPEED_CHANGE = 10; // 한 루프당 최대 모터 속도 변화\n\n// 이전 모터 속도 저장 변수\nint prevMotorA = 0, prevMotorB = 0, prevMotorC = 0, prevMotorD = 0;\n\n// BMP280 객체 생성\nAdafruit_BMP280 bmp;\n\n// 드론 제어 변수\nbool isDroneOn = false;\nint throttle = 400; // 기본 throttle 값\nint yaw = 0;\nint pitch = 0;\nint roll = 0;\n\n// 트림 변수\nint trimRoll = 0;\nint trimPitch = 0;\nint trimYaw = 0;\n\n// HTML 웹 페이지\nconst char index_html[] PROGMEM = R\"rawliteral(\n\n\n\n Drone Control\n \n \n\n\n

Drone Control

Status: Off

\n \n \n

Use arrow keys to control the drone

\n \n \n \n \n

\n \n \n

Throttle:

\n\n \n\n\n)rawliteral\";\n\nvoid initMPU9250() {\n Wire.beginTransmission(MPU9250_ADDR);\n Wire.write(0x6B); // PWR_MGMT_1 레지스터\n Wire.write(0x00); // MPU9250 켜기\n Wire.endTransmission(true);\n \n // 자이로스코프 설정 (±250dps)\n Wire.beginTransmission(MPU9250_ADDR);\n Wire.write(GYRO_CONFIG);\n Wire.write(0x00); // 0x00 = ±250dps\n Wire.endTransmission(true);\n \n // 가속도계 설정 (±2g)\n Wire.beginTransmission(MPU9250_ADDR);\n Wire.write(ACCEL_CONFIG);\n Wire.write(0x00); // 0x00 = ±2g\n Wire.endTransmission(true);\n\n calibrateSensors();\n}\n\nvoid calibrateSensors() {\n float sumGyroX = 0, sumGyroY = 0, sumGyroZ = 0;\n float sumAccelX = 0, sumAccelY = 0, sumAccelZ = 0;\n int numSamples = 1000;\n\n Serial.println(\"센서 캘리브레이션 중...\");\n \n for (int i = 0; i < numSamples; i++) {\n readSensorData();\n sumGyroX += gyroX; sumGyroY += gyroY; sumGyroZ += gyroZ;\n sumAccelX += accelX; sumAccelY += accelY; sumAccelZ += accelZ;\n delay(1);\n }\n\n gyroXoffset = sumGyroX / numSamples;\n gyroYoffset = sumGyroY / numSamples;\n gyroZoffset = sumGyroZ / numSamples;\n accelXoffset = sumAccelX / numSamples;\n accelYoffset = sumAccelY / numSamples;\n accelZoffset = sumAccelZ / numSamples - 1.0; // 중력 가속도 보정\n\n Serial.println(\"센서 캘리브레이션 완료\");\n Serial.printf(\"Gyro 오프셋: X=%.2f, Y=%.2f, Z=%.2f\\n\", gyroXoffset, gyroYoffset, gyroZoffset);\n Serial.printf(\"Accel 오프셋: X=%.2f, Y=%.2f, Z=%.2f\\n\", accelXoffset, accelYoffset, accelZoffset);\n\n // 각도 초기화\n angleX = 0; angleY = 0; angleZ = 0;\n}\n\nvoid readSensorData() {\n Wire.beginTransmission(MPU9250_ADDR);\n Wire.write(ACCEL_XOUT_H);\n Wire.endTransmission(false);\n Wire.requestFrom(MPU9250_ADDR, 14, true);\n \n int16_t accelX_raw = Wire.read() << 8 | Wire.read();\n int16_t accelY_raw = Wire.read() << 8 | Wire.read();\n int16_t accelZ_raw = Wire.read() << 8 | Wire.read();\n int16_t temp_raw = Wire.read() << 8 | Wire.read();\n int16_t gyroX_raw = Wire.read() << 8 | Wire.read();\n int16_t gyroY_raw = Wire.read() << 8 | Wire.read();\n int16_t gyroZ_raw = Wire.read() << 8 | Wire.read();\n \n gyroX = (gyroX_raw / GYRO_SCALE_FACTOR) - gyroXoffset;\n gyroY = (gyroY_raw / GYRO_SCALE_FACTOR) - gyroYoffset;\n gyroZ = (gyroZ_raw / GYRO_SCALE_FACTOR) - gyroZoffset;\n \n accelX = (accelX_raw / ACCEL_SCALE_FACTOR) - accelXoffset;\n accelY = (accelY_raw / ACCEL_SCALE_FACTOR) - accelYoffset;\n accelZ = (accelZ_raw / ACCEL_SCALE_FACTOR) - accelZoffset;\n \n altitude = bmp.readAltitude(1013.25); // 해수면 기준 기압을 인자로 전달\n}\n\nvoid updateAngles() {\n unsigned long currentTime = millis();\n elapsedTime = (currentTime - previousTime) / 1000.0;\n \n // 자이로스코프 적분\n angleX += gyroX * elapsedTime;\n angleY += gyroY * elapsedTime;\n angleZ += gyroZ * elapsedTime;\n \n // 가속도계를 이용한 각도 계산\n float accelAngleX = atan2(accelY, accelZ) * 180 / PI;\n float accelAngleY = atan2(-accelX, sqrt(accelY * accelY + accelZ * accelZ)) * 180 / PI;\n \n // 상보필터\n const float alpha = 0.96;\n angleX = alpha * (angleX + gyroX * elapsedTime) + (1 - alpha) * accelAngleX;\n angleY = alpha * (angleY + gyroY * elapsedTime) + (1 - alpha) * accelAngleY;\n \n previousTime = currentTime;\n}\n\nfloat calculatePID(PIDVariables &vars, PIDConstants &constants, float current, float target, float elapsedTime) {\n vars.error = target - current;\n \n // 적분항 윈드업 방지\n if (abs(vars.error) < 5) { // 오차가 작을 때만 적분항 누적\n vars.integral += vars.error * elapsedTime;\n }\n vars.integral = constrain(vars.integral, -50, 50); // 적분항 제한\n \n float derivative = (vars.error - vars.previousError) / elapsedTime;\n \n float output = constants.Kp * vars.error + constants.Ki * vars.integral + constants.Kd * derivative;\n \n vars.previousError = vars.error;\n \n // PID 출력 제한\n output = constrain(output, -100, 100);\n \n return output;\n}\n\nvoid applyPIDControl() {\n float outputRoll = calculatePID(pidVarsRoll, pidRoll, angleX, targetX, elapsedTime);\n float outputPitch = calculatePID(pidVarsPitch, pidPitch, angleY, targetY, elapsedTime);\n float outputYaw = calculatePID(pidVarsYaw, pidYaw, angleZ, targetZ, elapsedTime);\n float outputAltitude = calculatePID(pidVarsAltitude, pidAltitude, altitude, targetAltitude, elapsedTime);\n \n // 0-3도 범위 유지를 위한 보정\n if (abs(angleX) < MIN_TILT_ANGLE) {\n outputRoll *= 0.5; // 0도에 가까울 때 출력 감소\n } else if (abs(angleX) > MAX_TILT_ANGLE) {\n outputRoll *= 1.5; // 3도 이상일 때 출력 증가\n }\n \n if (abs(angleY) < MIN_TILT_ANGLE) {\n outputPitch *= 0.5; // 0도에 가까울 때 출력 감소\n } else if (abs(angleY) > MAX_TILT_ANGLE) {\n outputPitch *= 1.5; // 3도 이상일 때 출력 증가\n }\n \n// 트림 값 적용\n outputRoll += trimRoll;\n outputPitch += trimPitch;\n outputYaw += trimYaw;\n \n // 사용자 입력 반영\n outputRoll += roll;\n outputPitch += pitch;\n outputYaw += yaw;\n \n // 모터 속도 계산\n int baseSpeed = throttle; // throttle을 기본 속도로 사용\n int motorA = baseSpeed - outputRoll - outputPitch + outputYaw + outputAltitude;\n int motorB = baseSpeed + outputRoll - outputPitch - outputYaw + outputAltitude;\n int motorC = baseSpeed + outputRoll + outputPitch + outputYaw + outputAltitude;\n int motorD = baseSpeed - outputRoll + outputPitch - outputYaw + outputAltitude;\n \n // 모터 속도 변화 제한\n motorA = constrain(motorA, prevMotorA - MAX_MOTOR_SPEED_CHANGE, prevMotorA + MAX_MOTOR_SPEED_CHANGE);\n motorB = constrain(motorB, prevMotorB - MAX_MOTOR_SPEED_CHANGE, prevMotorB + MAX_MOTOR_SPEED_CHANGE);\n motorC = constrain(motorC, prevMotorC - MAX_MOTOR_SPEED_CHANGE, prevMotorC + MAX_MOTOR_SPEED_CHANGE);\n motorD = constrain(motorD, prevMotorD - MAX_MOTOR_SPEED_CHANGE, prevMotorD + MAX_MOTOR_SPEED_CHANGE);\n \n // 모터 속도 제한 (최소 속도 증가)\n motorA = constrain(motorA, 200, 800);\n motorB = constrain(motorB, 200, 800);\n motorC = constrain(motorC, 200, 800);\n motorD = constrain(motorD, 200, 800);\n \n // 모터 속도 적용\n ledcWrite(MOTOR_A, motorA);\n ledcWrite(MOTOR_B, motorB);\n ledcWrite(MOTOR_C, motorC);\n ledcWrite(MOTOR_D, motorD);\n \n // 현재 모터 속도 저장\n prevMotorA = motorA;\n prevMotorB = motorB;\n prevMotorC = motorC;\n prevMotorD = motorD;\n \n // 디버깅을 위한 출력\n Serial.printf(\"Angles: X=%.2f, Y=%.2f, Z=%.2f, Alt=%.2f\\n\", angleX, angleY, angleZ, altitude);\n Serial.printf(\"Motors: A=%d, B=%d, C=%d, D=%d\\n\", motorA, motorB, motorC, motorD);\n}\n\nvoid handleRoot() {\n server.send(200, \"text/html\", index_html);\n}\n\nvoid handleControl() {\n String action = server.arg(\"action\");\n String response = \"{\\\"status\\\":\\\"\";\n \n if (action == \"start\") {\n isDroneOn = true;\n response += \"On\";\n } else if (action == \"stop\") {\n isDroneOn = false;\n yaw = 0;\n pitch = 0;\n roll = 0;\n response += \"Off\";\n } else if (isDroneOn) {\n if (action == \"forward\") pitch = 10;\n else if (action == \"backward\") pitch = -10;\n else if (action == \"left\") roll = -10;\n else if (action == \"right\") roll = 10;\n else if (action == \"trimUp\") trimPitch += 1;\n else if (action == \"trimDown\") trimPitch -= 1;\n else if (action == \"trimLeft\") trimRoll -= 1;\n else if (action == \"trimRight\") trimRoll += 1;\n else if (action == \"throttleUp\") {\n throttle = min(throttle + 10, 800); // 최대 800으로 제한\n targetAltitude = altitude; // 현재 고도를 목표 고도로 설정\n }\n else if (action == \"throttleDown\") {\n throttle = max(throttle - 10, 200); // 최소 200으로 제한\n targetAltitude = altitude; // 현재 고도를 목표 고도로 설정\n }\n else {\n pitch = 0;\n roll = 0;\n }\n response += \"Moving: \" + action;\n } else {\n response += \"Drone is off\";\n }\n \n response += \"\\\", \\\"throttle\\\":\";\n response += String(throttle);\n response += \"}\";\n server.send(200, \"application/json\", response);\n}\n\nvoid stabilizeHover() {\n // 현재 고도와 목표 고도의 차이 계산\n float altitudeError = targetAltitude - altitude;\n \n // 고도 오차에 따른 throttle 조정\n throttle += altitudeError * 5; // 5는 조정 계수, 필요에 따라 조정 가능\n \n // throttle 값 제한\n throttle = constrain(throttle, 200, 800);\n \n // Roll, Pitch, Yaw 각도 보정\n trimRoll -= angleX * 0.1; // 0.1은 조정 계수, 필요에 따라 조정 가능\n trimPitch -= angleY * 0.1;\n trimYaw -= angleZ * 0.1;\n \n // 트림 값 제한\n trimRoll = constrain(trimRoll, -20, 20);\n trimPitch = constrain(trimPitch, -20, 20);\n trimYaw = constrain(trimYaw, -20, 20);\n}\n\nvoid setup() {\n Serial.begin(115200);\n Wire.begin();\n \n // 모터 핀을 PWM에 연결\n ledcAttach(MOTOR_A, PWM_FREQ, PWM_RESOLUTION);\n ledcAttach(MOTOR_B, PWM_FREQ, PWM_RESOLUTION);\n ledcAttach(MOTOR_C, PWM_FREQ, PWM_RESOLUTION);\n ledcAttach(MOTOR_D, PWM_FREQ, PWM_RESOLUTION);\n \n // MPU9250 초기화 및 캘리브레이션\n initMPU9250();\n \n // BMP280 초기화\n if (!bmp.begin(0x76)) { // BMP280의 I2C 주소\n Serial.println(\"BMP280 센서를 찾을 수 없습니다. 연결을 확인하세요.\");\n while (1);\n }\n \n // 초기 목표 고도 설정\n targetAltitude = bmp.readAltitude(1013.25);\n \n // AP 모드 설정\n WiFi.softAP(ssid, password);\n IPAddress IP = WiFi.softAPIP();\n Serial.print(\"AP IP address: \");\n Serial.println(IP);\n\n // 웹 서버 라우트 설정\n server.on(\"/\", HTTP_GET, handleRoot);\n server.on(\"/control\", HTTP_GET, handleControl);\n server.begin();\n \n Serial.println(\"드론 제어 시스템 준비 완료\");\n \n // 모터 초기화 (최소 속도로 설정)\n ledcWrite(MOTOR_A, 200);\n ledcWrite(MOTOR_B, 200);\n ledcWrite(MOTOR_C, 200);\n ledcWrite(MOTOR_D, 200);\n \n delay(2000); // 시스템 안정화를 위한 대기\n}\n\nvoid loop() {\n server.handleClient();\n \n if (isDroneOn) {\n readSensorData();\n updateAngles();\n stabilizeHover(); // 호버링 안정화 함수 호출\n applyPIDControl();\n \n // 각도가 임계값을 초과하면 경고\n if (abs(angleX) > 30 || abs(angleY) > 30) {\n Serial.println(\"경고: 각도가 30도를 초과했습니다!\");\n }\n \n // 고도 변화가 크면 경고\n static float lastAltitude = altitude;\n if (abs(altitude - lastAltitude) > 0.5) { // 0.5m 이상 고도 변화시\n Serial.println(\"경고: 급격한 고도 변화 감지!\");\n }\n lastAltitude = altitude;\n } else {\n // 드론이 꺼져 있을 때 모터 정지\n ledcWrite(MOTOR_A, 0);\n ledcWrite(MOTOR_B, 0);\n ledcWrite(MOTOR_C, 0);\n ledcWrite(MOTOR_D, 0);\n }\n \n // 배터리 전압 체크 (가정: ADC를 통해 배터리 전압을 읽는다고 가정)\n int batteryVoltage = analogRead(34); // ADC 핀 번호는 실제 연결에 맞게 조정 필요\n if (batteryVoltage < 3.5 * 4096 / 3.3) { // 3.5V 이하로 떨어지면 경고 (4S 배터리 가정)\n Serial.println(\"경고: 배터리 전압 낮음!\");\n }\n \n delay(10); // 루프 주기 설정 (약 100Hz)\n}","language":"cpp"}},{"id":"bda2f259-934d-4417-8510-cb1eeac1e1fe","type":"image","data":{"images":["https://storage.googleapis.com/copool-prod-bucket/73751a82-01a1-4124-b432-e23f06d6bb92-KakaoTalk_20240809_130943222.jpg","https://storage.googleapis.com/copool-prod-bucket/33f10f8c-3da3-4ebf-8f83-cf464dea5443-KakaoTalk_20240809_130943222_01.jpg"]}},{"id":"2898d3fb-49b5-41c4-a58a-31da14834760","type":"text","data":"Arduino ESP32, MPU9250 자이로 센서를 사용하여 현재 기울어진 값을 확인하고 PID를 통해 자체 제어를 하여 호버링이 이루어지도록 한다. 웹에서 드론을 조작할 수 있도록 만들고, 상황마다 알맞은 PID값을 입력시켜 결론적으로 자체제어가 이루어지는 것이 목표이다."}]

코풀 커뮤니티

ESP32 drone pid code