Scribd Logo
Upload

Welcome to Scribd!

  • 28 views

    1.listing Program Ardino (Pitct Terbang Keatas)

    Uploaded by

    Al Mtdrs
    This Arduino code controls 4 servo motors to balance a quadcopter using a PID controller. It reads acceleration and gyroscope sensor data, calculates the current angle, compares it to the desired angle, and determines the PWM output for each motor based on the PID error correction. The PID values and motor PWM outputs are adjusted each loop to minimize the error and balance the quadcopter at the desired angle.

    Copyright:

    © All Rights Reserved
    Download as DOCX, PDF, TXT or read online from Scribd

    1.listing Program Ardino (Pitct Terbang Keatas)

    Uploaded by

    Al Mtdrs
    28 views7 pages
    This Arduino code controls 4 servo motors to balance a quadcopter using a PID controller. It reads acceleration and gyroscope sensor data, calculates the current angle, compares it to the desired angle, and determines the PWM output for each motor based on the PID error correction. The PID values and motor PWM outputs are adjusted each loop to minimize the error and balance the quadcopter at the desired angle.

    Original Description:

    okay

    Original Title

    1.Listing program ardino (pitct terbang keatas)

    Copyright

    © © All Rights Reserved

    Available Formats

    DOCX, PDF, TXT or read online from Scribd

    Did you find this document useful?

    Is this content inappropriate?

    This Arduino code controls 4 servo motors to balance a quadcopter using a PID controller. It reads acceleration and gyroscope sensor data, calculates the current angle, compares it to the desired angle, and determines the PWM output for each motor based on the PID error correction. The PID values and motor PWM outputs are adjusted each loop to minimize the error and balance the quadcopter at the desired angle.

    Copyright:

    © All Rights Reserved
    Download as DOCX, PDF, TXT or read online from Scribd
    Download as docx, pdf, or txt
    28 views7 pages

    1.listing Program Ardino (Pitct Terbang Keatas)

    Uploaded by

    Al Mtdrs
    This Arduino code controls 4 servo motors to balance a quadcopter using a PID controller. It reads acceleration and gyroscope sensor data, calculates the current angle, compares it to the desired angle, and determines the PWM output for each motor based on the PID error correction. The PID values and motor PWM outputs are adjusted each loop to minimize the error and balance the quadcopter at the desired angle.

    Copyright:

    © All Rights Reserved
    Download as DOCX, PDF, TXT or read online from Scribd
    Download as docx, pdf, or txt
    of 7

    Listing program ardino :

    #include <Wire.h>

    #include <Servo.h>

    Servo right_prop;

    Servo left_prop;

    Servo back_prop;

    Servo front_prop;

    int16_t Acc_rawX, Acc_rawY, Acc_rawZ, Gyr_rawX, Gyr_rawY, Gyr_rawZ;

    float Acceleration_angle[2];

    float Gyro_angle[2];

    float Total_angle[2];
    float elapsedTime, time, timePrev;

    int i;

    float rad_to_deg = 200 / 3.141592654;

    float PID, pwmRight, pwmLeft, pwmback, pwmfront ,error, previous_error;

    float pid_p = 0;

    float pid_i = 0;

    float pid_d = 0;

    /////////////////PID CONSTANTS/////////////////

    double kp = 0;

    double ki = 0;

    double kd = 0;

    double throttle = 1300; //initial value of throttle to the motors

    float desired_angle = 0; //This is the angle in which we whant the

    //balance to stay steady

    void setup() {

    Wire.begin(); //begin the wire comunication

    Wire.beginTransmission(0x68);

    Wire.write(0x6B);

    Wire.write(0);
    Wire.endTransmission(true);

    Serial.begin(9600);

    right_prop.attach(8); //attatch the right motor to pin 8 motor 1

    left_prop.attach(5); //attatch the left motor to pin 5 motor 2

    back_prop.attach(3); //attatch the right motor to pin 3 motor 2

    front_prop.attach(2);//attatch the left motor to pin 2 motor 4

    time = millis(); //Start counting time in milliseconds

    right_prop.writeMicroseconds(1000);

    left_prop.writeMicroseconds(1000);

    back_prop.writeMicroseconds(1000);

    front_prop.writeMicroseconds(1000);

    delay(7000);

    void loop() {

    timePrev = time; // the previous time is stored before the actual time read

    time = millis(); // actual time read

    elapsedTime = (time - timePrev) / 1000;

    Wire.beginTransmission(0x68);

    Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX

    Wire.endTransmission(false);

    Wire.requestFrom(0x68, 6, true);

    Acc_rawX = Wire.read() << 8 | Wire.read(); //each value needs two registres

    Acc_rawY = Wire.read() << 8 | Wire.read();

    Acc_rawZ = Wire.read() << 8 | Wire.read();


    Acceleration_angle[0] = atan((Acc_rawY / 16384.0) / sqrt(pow((Acc_rawX / 16384.0), 2) +
    pow((Acc_rawZ / 16384.0), 2))) * rad_to_deg;

    Acceleration_angle[1] = atan(-1 * (Acc_rawX / 16384.0) / sqrt(pow((Acc_rawY / 16384.0),


    2) + pow((Acc_rawZ / 16384.0), 2))) * rad_to_deg;

    Wire.beginTransmission(0x68);

    Wire.write(0x43); //Gyro data first adress

    Wire.endTransmission(false);

    Wire.requestFrom(0x68, 4, true); //Just 4 registers

    Gyr_rawX = Wire.read() << 8 | Wire.read(); //Once again we shif and sum

    Gyr_rawY = Wire.read() << 8 | Wire.read();

    Gyro_angle[0] = Gyr_rawX / 131.0;

    Gyro_angle[1] = Gyr_rawY / 131.0;

    Total_angle[0] = 0.98 * (Total_angle[0] + Gyro_angle[0] * elapsedTime) + 0.02 *


    Acceleration_angle[0];

    Total_angle[1] = 0.98 * (Total_angle[1] + Gyro_angle[1] * elapsedTime) + 0.02 *


    Acceleration_angle[1];

    Serial.print("sudut = ");

    Serial.println(Total_angle[1]);

    //Serial.println(pwmRight);
    //Serial.println(pwmLeft);

    //Serial.println(pwmBack);

    //Serial.println(pwmFront);

    error = Total_angle[1] - desired_angle;

    pid_p = kp * error;

    if (-3 < error < 3)

    pid_i = pid_i + (ki * error);

    pid_d = kd * ((error - previous_error) / elapsedTime);

    PID = pid_p + pid_i + pid_d;

    if (PID < -1000)

    PID = -1000;

    if (PID > 1000)

    PID = 1000;

    pwmRight = throttle - PID;

    pwmLeft = throttle + PID;

    pwmback = throttle + PID;

    pwmfront = throttle - PID;

    ///MOTOR 1

    if (pwmRight < 1200)


    {

    pwmRight = 1200;

    if (pwmRight > 1200)

    pwmRight = 1200;

    ///MOTOE 2

    if (pwmLeft < 1380)

    pwmLeft = 1380;

    if (pwmLeft > 1380)

    pwmLeft = 1380;

    ///MOTO 3

    if (pwmback < 1580)

    pwmback = 1580;

    if (pwmback > 1580)

    pwmback = 1580;

    }
    ///MOTOR 4

    if (pwmfront < 1100)

    pwmfront = 1100;

    if (pwmfront > 1100)

    pwmfront = 1100;

    right_prop.writeMicroseconds(pwmRight);

    left_prop.writeMicroseconds(pwmLeft);

    back_prop.writeMicroseconds(pwmback);

    front_prop.writeMicroseconds(pwmfront);

    previous_error = error; //Remember to store the previous error.

    You might also like