*SOLVED* PID Stabilization code - Program abruptly stops.

Hi!

So, basically, what happens is that when the PID loop, or the programs main loop is running, it’s supposed to be taking gyro/accel data and use that to move servos. The thing is, usually after 5-10 seconds, the program stops working, the servo stops, and even if i move the gyro board around the servos do not respond.

I’m using an arduino 101 with one generic 9g servo connected to 5v on the arduino board.

Heres the code:

//Proportional Servo Control
#include <Servo.h>

    #include <CurieIMU.h>
#include <MadgwickAHRS.h>

Madgwick filter;
unsigned long microsPerReading, microsPrevious;
float accelScale, gyroScale;
Servo leftServo, rightServo;
int rolly, pitchy, heady;
float roll, pitch, heading;
//////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////PID STUFF//////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////

float pid_p_gain_roll = 1;               //Gain setting for the roll P-controller
float pid_i_gain_roll = 0.001;              //Gain setting for the roll I-controller
float pid_d_gain_roll = 0;              //Gain setting for the roll D-controller
int pid_max_roll = 90;                    //Maximum output of the PID-controller (+/-)
float pid_p_gain_pitch = 1;  //Gain setting for the pitch P-controller.
float pid_i_gain_pitch = 0.001;  //Gain setting for the pitch I-controller.
float pid_d_gain_pitch = 0;  //Gain setting for the pitch D-controller.
int pid_max_pitch = 90;  //Maximum output of the PID-controller (+/-)
//////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////PID STUFF//////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////


long acc_x, acc_y, acc_z, acc_total_vector;
unsigned long timer_channel_1, timer_channel_2, timer_channel_3, timer_channel_4, esc_timer, esc_loop_timer;
unsigned long timer_1, timer_2, timer_3, timer_4, current_time;
unsigned long loop_timer;
double gyro_pitch, gyro_roll, gyro_yaw;
double gyro_axis_cal[4];
float pid_error_temp;
float pid_i_mem_roll, pid_roll_setpoint, gyro_roll_input, pid_output_roll, pid_last_roll_d_error;
float pid_i_mem_pitch, pid_pitch_setpoint, gyro_pitch_input, pid_output_pitch, pid_last_pitch_d_error;
float pid_i_mem_yaw, pid_yaw_setpoint, gyro_yaw_input, pid_output_yaw, pid_last_yaw_d_error;
float angle_roll_acc, angle_pitch_acc, angle_pitch, angle_roll;
boolean gyro_angles_set;


int leftAngle;
int rightAngle;

void setup() {
  Serial.begin(9600);
leftServo.attach(9);
rightServo.attach(10);

/*leftServo.write(0);
delay(500);
leftServo.write(180);
delay(500);
leftServo.write(0);
delay(500);*/

  // start the IMU and filter
  CurieIMU.begin();
  CurieIMU.setGyroRate(25);
  CurieIMU.setAccelerometerRate(25);
  filter.begin(25);

  // Set the accelerometer range to 2G
  CurieIMU.setAccelerometerRange(2);
  // Set the gyroscope range to 250 degrees/second
  CurieIMU.setGyroRange(250);

  // initialize variables to pace updates to correct rate
  microsPerReading = 1000000 / 25;
  microsPrevious = micros();
}

void loop() {

  Serial.println("Test");

  int aix, aiy, aiz;
  int gix, giy, giz;
  float ax, ay, az;
  float gx, gy, gz;

  unsigned long microsNow;

  // check if it's time to read data and update the filter
  microsNow = micros();
  if (microsNow - microsPrevious >= microsPerReading) {

    // read raw data from CurieIMU
    CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);

    // convert from raw data to gravity and degrees/second units
    ax = convertRawAcceleration(aix);
    ay = convertRawAcceleration(aiy);
    az = convertRawAcceleration(aiz);
    gx = convertRawGyro(gix);
    gy = convertRawGyro(giy);
    gz = convertRawGyro(giz);

    // update the filter, which computes orientation
    filter.updateIMU(gx, gy, gz, ax, ay, az);

    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();

    
    rolly = (int) roll;
    pitchy = (int) pitch;
    heady = (int) heading;


    //leftServo.write(-pitchy + 90);
    //rightServo.write(rolly + 90);
    //delay(10);



    // increment previous time, so we keep proper pace
    microsPrevious = microsPrevious + microsPerReading;
  }
  
  calculate_pid();
  leftServoMethod();
  rightServoMethod();
  Serial.print("Roll ");
  Serial.print(roll);
  Serial.print(" Pitch ");
  Serial.print(pitch);
  Serial.print(" Yaw ");
  Serial.print(heading);
  Serial.print(" ");
  Serial.print("    Left Servo write    ");
  Serial.print(leftAngle);
  Serial.print("    Right Servo write    ");
  Serial.print(rightAngle);
  Serial.print("   PID pitch output:   ");
  Serial.print(pid_output_pitch);
  Serial.print("   PID roll output Roll:   ");
  Serial.println(pid_output_roll);
}

float convertRawAcceleration(int aRaw) {
  // since we are using 2G range
  // -2g maps to a raw value of -32768
  // +2g maps to a raw value of 32767
  
  float a = (aRaw * 2.0) / 32768.0;
  return a;
}

float convertRawGyro(int gRaw) {
  // since we are using 250 degrees/seconds range
  // -250 maps to a raw value of -32768
  // +250 maps to a raw value of 32767
  
  float g = (gRaw * 250.0) / 32768.0;
  return g;
}

void calculate_pid() {
  //Roll calculations
  pid_error_temp = roll - pid_roll_setpoint;
  pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;
  Serial.print("    FirstPID mem roll    ");
  Serial.println(pid_i_mem_roll);
  if (pid_i_mem_roll > pid_max_roll)pid_i_mem_roll = pid_max_roll;
  else if (pid_i_mem_roll < pid_max_roll * -1)pid_i_mem_roll = pid_max_roll * -1;

  pid_output_roll = pid_p_gain_roll * pid_error_temp + pid_i_mem_roll + pid_d_gain_roll * (pid_error_temp - pid_last_roll_d_error);
  if (pid_output_roll > pid_max_roll)pid_output_roll = pid_max_roll;
  else if (pid_output_roll < pid_max_roll * -1)pid_output_roll = pid_max_roll * -1;


  pid_last_roll_d_error = pid_error_temp;

  //Pitch calculations
  pid_error_temp = pitch - pid_pitch_setpoint;
  pid_i_mem_pitch += pid_i_gain_pitch * pid_error_temp;
  if (pid_i_mem_pitch > pid_max_pitch)pid_i_mem_pitch = pid_max_pitch;
  else if (pid_i_mem_pitch < pid_max_pitch * -1)pid_i_mem_pitch = pid_max_pitch * -1;

  pid_output_pitch = pid_p_gain_pitch * pid_error_temp + pid_i_mem_pitch + pid_d_gain_pitch * (pid_error_temp - pid_last_pitch_d_error);
  if (pid_output_pitch > pid_max_pitch)pid_output_pitch = pid_max_pitch;
  else if (pid_output_pitch < pid_max_pitch * -1)pid_output_pitch = pid_max_pitch * -1;

  pid_last_pitch_d_error = pid_error_temp;
  leftServoMethod();
  rightServoMethod();

  
}

void leftServoMethod() {
 
if (pid_output_roll+pid_output_pitch+90 >= 180) leftAngle = 180;
else leftAngle = pid_output_roll+pid_output_pitch+120;
leftServo.write(leftAngle);
}

void rightServoMethod() {
 
if (pid_output_pitch+pid_output_roll+90 >= 180) rightAngle = 180;
else rightAngle = (abs(abs(90-pid_output_pitch+pid_output_roll+90)-110));
rightServo.write(rightAngle);
}

Do not power servos or motors from the Arduino 5V port. You will destroy the Arduino that way.
Use a separate power supply for the servo and connect the grounds together.

The code is an absolute mess, and your description of the problem is unclear and unhelpful. What is this program supposed to do?

The only advice I can offer is to put debugging print statements in the code and make sure that the values taken on by the variables make sense.

Some things that jump out on a quick scan of the code:

  • This:

microsPerReading = 1000000 / 25;

is not going to give the result you expect. The divide will be done as ints, which have a
max value of 32767. Even with the correct value, you're taking a reading every 40 mSec.
That is a LONG time for a PID.

  • You're reading new data every, more or less, 40 mSec, but updating the PID EVERY time
    through loop(). That makes little sense. Why would the PID output change if the input
    hasn't changed?

  • You are updating the PID at somewhat random times, which will drive it crazy. A PID really
    wants to be updated at very regular intervals. Otherwise, tuning will be almost impossible.

  • Have you measured the calculation time? With all those float calculations, if you're doing this
    on an AVR, it's going to be VERY slow.

Regards,
Ray L.

RayLivingston:
microsPerReading = 1000000 / 25;
is not going to give the result you expect. The divide will be done as ints, which have a max value of 32767. Even with the correct value, you're taking a reading every 40 mSec. That is a LONG time for a PID.

Nah, the compiler is smart enough to figure out that 1000000 doesn't fit into an int so it will get promoted to long. Since it is a long the math will be done as a long.

void setup(){
  Serial.begin(9600);
  long var = 1000000 / 25;
  Serial.print(var);
}

void loop(){}

Gives as output:

40000

jremington:
The only advice I can offer is to put debugging print statements in the code and make sure that the values taken on by the variables make sense.

Yeah, did this to find out where the program was bugging through the initial PID calculations.

What it's supposed to do is use gyro/accel values to tell the servos to change the tilt/pitch of an aircraft and correct its attitude. It uses a PID feedback loop (is that what its called? not really a CS guy) to determine the servos angles, and thus how fast the aircraft will roll/pitch forward/back/left/right to correct its attitude.

RayLivingston:
Some things that jump out on a quick scan of the code:

  • This:

microsPerReading = 1000000 / 25;

is not going to give the result you expect. The divide will be done as ints, which have a
max value of 32767. Even with the correct value, you're taking a reading every 40 mSec.
That is a LONG time for a PID.

My friend did this part of the code since i'm not very familiar with arduino 101 - I had a version of this using an MPU 9025 (I think?) which worked fine but it's been a while since i worked on that program and this one. 40mSec IS pretty slow for a PID, how can i change the interval at the readings? The one I was working with before had around 5-10 ms between readings. I kind of want to know what the code means as well because i feel like that'd help me out in the future with the same program. I'll read it over tonight and study up, in the meantime if you have any tips or explanations about the code that'd be great.

RayLivingston:

  • You're reading new data every, more or less, 40 mSec, but updating the PID EVERY time
    through loop(). That makes little sense. Why would the PID output change if the input
    hasn't changed?

  • You are updating the PID at somewhat random times, which will drive it crazy. A PID really
    wants to be updated at very regular intervals. Otherwise, tuning will be almost impossible.

I had no idea. I really shouldve looked over this more.

I think the problem is that the arduino is powering the servos. I'll get an external 5v and just connect the signal PWM to the arduino board, does that sound about right?

I think the problem is that the arduino is powering the servos.

That just will not work at all...

Regards,
Ray L.

So i hooked up both the servos to an external power source and hooked up the grounds to the arduino. Works perfectly! Thanks so much to everyone here.

If anyone knows how i can speed up the PID loop please help me out. Can I just change the

MicrosPerReading = 100000/25 line to equal however ms i want, and then compensate the rest of the microsprev and etc line to reflect that?

Don't print so much. You are trying to print hundreds of bytes every time through the loop. At 9600 baud, that takes hundreds of milliseconds. You won't achieve your desired rate of 25 updates per second.

Also look at when it's doing things. The if(microsNow - ... statement means it will only do a maximum of 25 position updates per second. But the PID update is done at a different frequency. It will occur many more times per second. I would put that inside the same block so a PID update only occurs when there's a position update.

I moved the PID inside the if loop of the micros if statement. Everything seems to be working fine now! Thanks again. My final problem is that I dont know if i can change this

  microsPerReading = 1000000 / 25;
           microsPrevious = micros();

the microsPerReading into whatever number I want. The comments say that those two lines

“initialize variables to pace updates to correct rate”

but i guess I dont understand what that’s trying to tell me. More specifically, what I understand is it paces updates, meaning changes it to a set interval, and that set interval is somehow the “correct” interval/rate? Is there a correct rate or can we force an update whenever we want? I’m sure that the accelerometers and gyroscopes can get updates faster than every 40 ms. Most quadcopters/flight controllers run this at every 4ms. Can I just change 1000000 / 25 to 100/25? I think this might even have been a typo for 1000(ms)/250(corrections (as in 250 corrections a sec)). either way i’ll be trying microsperreading = 1000/250 later today and reporting back in case anyone else comes across this thread with the same problem.

OhItsTarik:
I dont know if i can change this

  microsPerReading = 1000000 / 25;

microsPrevious = micros();



Can I just change 1000000 / 25 to 100/25?

You are calculating a number of microseconds. You are dividing the number of microseconds in one second (one million) by the number of samples per second to get microseconds per sample. If you change the one million to one hundred you would be calculating "microseconds in one ten-thousandth of a second divided by samples per ten-thousandth of a second to get microseconds per sample". It would make much more sense to KEEP the microseconds per second unchanged and change the desired samples per second:

microsPerReading = 1000000 / 250000;

Are you sure your code is fast enough to process samples every 4 microseconds? Perhaps you wanted 4 milliseconds (250 samples per second)?

microsPerReading = 1000000 / 250;

johnwasser:
You are calculating a number of microseconds. You are dividing the number of microseconds in one second (one million) by the number of samples per second to get microseconds per sample. If you change the one million to one hundred you would be calculating "microseconds in one ten-thousandth of a second divided by samples per ten-thousandth of a second to get microseconds per sample". It would make much more sense to KEEP the microseconds per second unchanged and change the desired samples per second:

microsPerReading = 1000000 / 250000;

Are you sure your code is fast enough to process samples every 4 microseconds? Perhaps you wanted 4 milliseconds (250 samples per second)?

microsPerReading = 1000000 / 250;

Ah! I had no idea that the units were microseconds, makes sense since it's called "MICROSPerReading"... Then yes all thats missing is a 0 to the end of the 25 in the main code, like you said. I'll try this out.

EDIT: I've tried it out, works fine! To everyone else: Make sure you change

CurieIMU.setGyroRate(100);

to the proper frequency in hz as well. It can only be like 20, 50, 100, 200, etc but make sure you change that.

Also, make sure that the speed that you're performing your outputs is fast enough for your servos to stay up with,