0

I have two different programs written for my Arduino. One control the rockets thrust vector. The other one is for parachute.

I need to first run rotationmatrices() for a 4 second and then parachute.

How could i do that?

Here is my code:

//Libraries
#include <Servo.h>
#include <Wire.h>
#include <MPU6050_tockn.h>
#include <AFMotor.h>
#include <I2Cdev.h>

MPU6050 mpu6050(Wire);

const int MPU = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyroY, GyroZ;
double PIDX, PIDY, errorX, errorY, previous_errorX, previous_errorY,
       pwmX, pwmY, previouslog, OutX, OutY, OutZ, OreX, OreY, OreZ;
double PreviousGyroX, PreviousGyroY, PreviousGyroZ, IntGyroX,
       IntGyroY, IntGyroZ, DifferenceGyroX, DifferenceGyroY,
       DifferenceGyroZ, matrix1, matrix2, matrix3;
double matrix4, matrix5, matrix6, matrix7, matrix8, matrix9, Ax, Ay;

//Upright Angle of the Flight Computer-Dik durum açısı
int desired_angleX = 0;

//Offsets for tuning
int servoX_offset = 100;

//Position of servos through the startup function
int servoXstart = 80;

//The amount the servo moves by in the startup function
int servo_start_offset = 20;

//Ratio between servo gear and tvc mount
float servo_gear_ratio = 5.8;

double OrientationX = 0;
double OrientationY = 0;
double OrientationZ = 1;
double accAngleX;
double accAngleY;
double yaw;
double GyroX;
double gyroAngleX;
double gyroAngleY;
double pitch;

Servo servoX;

AF_DCMotor motor1(1); // create motor #1
AF_DCMotor motor2(2); // create motor #2
AF_DCMotor motor3(3); // create motor #1
AF_DCMotor motor4(4); // create motor #2

int buzzer = 3;

double dt, currentTime, previousTime, s, p;

//"P" Constants-Orantısal Değişken
float pidX_p = 0;
float pidY_p = 0;

//"I" Constants-Integral Değişken
float pidY_i = 0;
float pidX_i = 0;

//"D" Constants-Türevsel Değişken
float pidX_d = 0;
float pidY_d = 0;

int pos;

//PID Gains
double kp = 0.2;
double ki = 0.0;
double kd = 0.05;

int state = 0;

void setup() {
    Serial.begin(9600);
    Wire.begin();
    mpu6050.begin();
    gyrocalibrate();

    servoX.attach(10);

    motor1.setSpeed(255);
    motor2.setSpeed(255);
    motor3.setSpeed(255);
    motor4.setSpeed(255);

    pinMode(buzzer, OUTPUT);
}

void loop() {
    mpu6050.update();
    //Defining Time Variables-Zaman değişkeni tanımlanması
    previousTime = currentTime;
    currentTime = millis();
    dt = (currentTime - previousTime) / 1000;
    launchdetect();
    rotationmatrices();
    parachute();
}

void parachute() {
    motor1.run(BACKWARD);
    delay(1500);
    motor1.run(RELEASE);
    delay(20000);
}

void gyrocalibrate() {
    mpu6050.calcGyroOffsets(true);
}

void rotationmatrices(x) {
    mpu6050.update();
    PreviousGyroX = IntGyroX;
    PreviousGyroY = IntGyroY;
    PreviousGyroZ = IntGyroZ;

    IntGyroX = mpu6050.getGyroAngleX() * (PI / 180);
    IntGyroY = mpu6050.getGyroAngleY() * (PI / 180);
    IntGyroZ = mpu6050.getGyroAngleZ() * (PI / 180);

    DifferenceGyroX = (IntGyroX - PreviousGyroX);
    DifferenceGyroY = (IntGyroY - PreviousGyroY);
    DifferenceGyroZ = (IntGyroZ - PreviousGyroZ);

    OreX = OrientationX;
    OreY = OrientationY;
    OreZ = OrientationZ;

    //X Matrices
    matrix1 = (cos(DifferenceGyroZ) * cos(DifferenceGyroY));
    matrix2 = (((sin(DifferenceGyroZ) * -1) * cos(DifferenceGyroX)
            + (cos(DifferenceGyroZ)) * sin(DifferenceGyroY)
                * sin(DifferenceGyroX)));
    matrix3 = ((sin(DifferenceGyroZ) * sin(DifferenceGyroX)
            + (cos(DifferenceGyroZ)) * sin(DifferenceGyroY)
                * cos(DifferenceGyroX)));

    //Y Matrices
    matrix4 = sin(DifferenceGyroZ) * cos(DifferenceGyroY);
    matrix5 = ((cos(DifferenceGyroZ) * cos(DifferenceGyroX)
            + (sin(DifferenceGyroZ)) * sin(DifferenceGyroY)
                * sin(DifferenceGyroX)));
    matrix6 = (((cos(DifferenceGyroZ) * -1) * sin(DifferenceGyroX)
            + (sin(DifferenceGyroZ)) * sin(DifferenceGyroY)
                * cos(DifferenceGyroX)));

    //Z Matrices
    matrix7 = (sin(DifferenceGyroY)) * -1;
    matrix8 = cos(DifferenceGyroY) * sin(DifferenceGyroX);
    matrix9 = cos(DifferenceGyroY) * cos(DifferenceGyroX);

    OrientationX = ((OreX * matrix1)) + ((OreY * matrix2))
                 + ((OreZ * matrix3));
    OrientationY = ((OreX * matrix4)) + ((OreY * matrix5))
                 + ((OreZ * matrix6));
    OrientationZ = ((OreX * matrix7)) + ((OreY * matrix8))
                 + ((OreZ * matrix9));

    Serial.println(Ax);
    OutX = OrientationX * 60;
    Ax = asin(OrientationX) * (-180 / PI);
    pidcompute();
}

void servowrite() {
    servoX.write(pwmX);
}

void pidcompute() {
    previous_errorX = errorX;
    errorX = Ax - desired_angleX;

    //Defining "P"
    pidX_p = kp*errorX;

    //Defining "D"
    pidX_d = kd*((errorX - previous_errorX)/dt);

    //Defining "I"
    pidX_i = ki * (pidX_i + errorX * dt);

    PIDX = pidX_p + pidX_i + pidX_d;
    pwmX = ((PIDX * servo_gear_ratio) + servoX_offset);
    servowrite();
}

void launchdetect() {
    if (mpu6050.getAccZ() > 1) {
        state = 1;
    }
    if (state == 1) {
    }
}

void abortstart() {
    if (OrientationX > 40 || OrientationY > 40) {
        digitalWrite(buzzer, HIGH);
    }
    else if (OrientationX < 40 || OrientationY < 40) {
    }
}
  • Look at the BlinkWithoutDelay example. Learn how to use millis() or micros(). There are many tutorials about that online and its an imprtant principle to learn for Arduino programming. – chrisl Jul 11 at 15:13
  • @chrisl Thanks so much.I will look at them. – Polat Öztürk Jul 11 at 16:36
  • at first glance, it looks like your progran stops running when launch is detected ... the code listing is kind of messy and unfocused looking ... maybe it is an indication of the clarity of your thoughts about the project – jsotola Jul 11 at 18:21
  • here is an example of well formatted code ... it could use a blank line between the case statements, but overall, it should give you an idea of proper formatting ... arduino.stackexchange.com/questions/76834/… ... – jsotola Jul 11 at 18:42
0

you can do it like this using millis it's just an small portion of code just for your understanding you will require some editing for your code.

unsigned long milli1;
void setup()
{
}
void loop()
{  
     // code before rotationmatrices() function to be active.
     milli1= millis();
     while(millis()-milli1 <= 4000)  //4000milli sec = 4 sec
     {
      rotationmatrices();
     }
     parachute();
     
}

| improve this answer | |
  • 1
    That's not a proper use of millis, because it is blocking code as well. Take a look at BlinkWithoutDelay – Sim Son Jul 16 at 12:06

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.