next topic »

[on:]

Hello dear members,

I believe that the time has come, where I have to ask you guys for some support over here.
And I also hope that this will help other people with similar projects.

However, I’m up for doing a tricopter. And right now I’m in the phase where I need you guys to confirm if my algorithm for stabilizing is correct.

I’ll post a picture to illustrate my thought. As they say, a picture can tell you more a thousand words (If you'd like to see the picture, go to the end of the post).

1. Read values from transmitter (throttle will give from 0 - ~180, the rest of the sticks gives ~90 in middle)
2. Read values from sensor (180 degrees if plan)
3. PID CONTROL
4. Combine PID outputs with throttle value
5. Output the values to the motors

For the PID configs

Code:

PID myPID_Roll_Plus(   &Input_Roll_Plus,    &Output_Roll_Plus,    &Setpoint_Roll_Plus,   1.5, 0.1, 0.5, DIRECT);
PID myPID_Roll_Minus(  &Input_Roll_Minus,   &Output_Roll_Minus,   &Setpoint_Roll_Minus,  1.5, 0.1, 0.5, REVERSE);
PID myPID_Pitch_Plus(  &Input_Pitch_Plus,   &Output_Pitch_Plus,   &Setpoint_Pitch_Plus,  1.5, 0.1, 0.5, DIRECT);
PID myPID_Pitch_Minus( &Input_Pitch_Minus,  &Output_Pitch_Minus,  &Setpoint_Pitch_Minus, 1.5, 0.1, 0.5, REVERSE);

Setting the values for the variables (OBS. setpoint is set to 90, because the sticks from the transmitter outputs 90 if not touched)

Code:

//////////////////////////////////////////////
/////          PID SETTINGS              /////
/////////////////////////////////////////////
void PID_SETTINGS(){
  myPID_Pitch_Plus.SetMode(AUTOMATIC);
  myPID_Pitch_Plus.SetOutputLimits(0,180);
  Setpoint_Pitch_Plus = 90;
  
  myPID_Pitch_Minus.SetMode(AUTOMATIC);
  myPID_Pitch_Minus.SetOutputLimits(0,180);
  Setpoint_Pitch_Minus = 90;
  
  myPID_Roll_Plus.SetMode(AUTOMATIC);
  myPID_Roll_Plus.SetOutputLimits(0,180);
  Setpoint_Roll_Plus = 90;
  
  myPID_Roll_Minus.SetMode(AUTOMATIC);
  myPID_Roll_Minus.SetOutputLimits(0,180);
  Setpoint_Roll_Minus = 90;
}

PID calculations (As I said before, the setpoint is set to 90. Thats why Im subtracting with 90. 180 - 90 = 90.

Code:

  Input_Pitch_Plus = kalAngleY-90;
   myPID_Pitch_Plus.Compute();
   Input_Pitch_Minus = kalAngleY-90;
   myPID_Pitch_Minus.Compute();
  
   Input_Roll_Plus = kalAngleX-90;  
   myPID_Roll_Plus.Compute();
   Input_Roll_Minus = kalAngleX-90;
   myPID_Roll_Minus.Compute();

The outputs for the motors

Code:

  THROTTLE_PID_BACK         = RECEIVER_THROTTLE_VAL + Output_Pitch_Plus;
   THROTTLE_PID_FRONT_LEFT   = RECEIVER_THROTTLE_VAL + Output_Roll_Plus + Output_Pitch_Minus;
   THROTTLE_PID_FRONT_RIGHT  = RECEIVER_THROTTLE_VAL + Output_Roll_Minus + Output_Pitch_Minus;

Here is the picture:
This will explain much better than the text above.


Ive also made a page on github, If ud like too see the whole code.
https://github.com/freak174/Tricopter--Build-phase-.git


Edit:
Forgot to mention that I am only trying to stabilize the tricopter by the throttle, which means Ive excluded all other sticks from the transmitter.

[Reply #1 on:]

Have you tried that code? What happened?

[Reply #2 on:]

I can't think of any sensible reason for having two PID instances for each degree of freedom.

I'd expect to see a PID for pitch, a PID for roll, some algorithm to control total vertical thrust, and a mixer function to combine the three control signals into three power demands for the three motors.

[Reply #3 on:]

PaulS:
I have tried this code. Ive successuly uploaded it to the arduino. But the last part is to set the correct kp,ki and kd values. But If it wouldnt work, I would never know where the problem is. If its with my algorithm or with the PID values.

PeterH:
From the beginning I had only one PID for the roll axid and one for the pitch axis.

But as you know the total output depends on the error.
Lets put up a scenario.
The setpoint is 180, and the input (sensor val) is higher than 180.
Error = setpoint - input -> this would generate a negative value, but in my code it only gives me a 0.

That is why I have two different PID for each axis. As one is:
DIRECT (plus) --> Will give me a value > 0 if sensor val < 180, but if sensor val > 180 I will get a 0.
REVERSE (minus) --> WIll give me a value  > 0 if sensor val > 180, but if sensor val < 180 I will get a 0.



You can also see this on the picture.

Isn't it supposed to look like this?

Cheers!

Edit:
Lets put up another scenario.
Make sure to look at the three last variables, that shall goto the motors (probably called the mixer), and remember 180 degree is stable.

Copter leaning forward (pitch) 200 degrees, roll are all stabe 180 degrees.

Error = Setpoint - Error
PID Roll:
Error (DIRECT- plus) = 180 - 180 = 0
Error (REVERSE - minus) = 180 - 180 = 0
--> No extra power from roll plus/minus.

PID Pitch:
Error (Direct - plus) = 180 - 200 < 0 = 0. Because there are no meaning to add extra power to the back motor, only the power from the throttle will be added.
Error (Direct - minus) = I believe it will be the opposite > 200 - 180 = 20. This will be added to the front two motors.

I hope I made it more clear for you guys now!

[Reply #4 on:]

Error = setpoint - input -> this would generate a negative value, but in my code it only gives me a 0.

Then your code is wrong - perhaps using unsigned instead of signed values. Using a separate PID instance for each direction is daft - the PIDs have state and you will just end up with a mess.

Design your sensor processing so that the error is positive in one direction and negative in the other, for each degree of freedom you want to control. Use one PID for each degree of freedom to output a control demand for that DoF.

Merge the control demands from each DoF to give you the total demand for each motor.

[Reply #5 on:]

I may have missed it, but how are you planning to control yaw?

[Reply #6 on:]

PeterH:
It may be due to this line.
Goes from 0 - 180, maybe I should change it to -180 - 180.

Code:

myPID_Pitch_Plus.SetOutputLimits(0,180);

However if that will fix it, should I make a IF statement (for the roll axis):
if output is negative -> make absolute value of it and add it to the right motor
else add it to the left motor?

Jabbado:
Actually you did not miss that part, cuz there are none yet. But I will implent a servo that will Control the back motor to rotate in the roll axis which will Control the yaw.

[Reply #7 on:]

It may be due to this line.
Goes from 0 - 180, maybe I should change it to -180 - 180.

Code:

myPID_Pitch_Plus.SetOutputLimits(0,180);

However if that will fix it, should I make a IF statement (for the roll axis)

You shouldn't need an if statement. But yes map the stick values so 0 degrees is center position. So you get positive or negative values when it's rolled to the left or right. And the same for pitch, yaw. Same with sensors - when the tri is perfectly horizontal read that as 0 deg pitch and roll.

[Reply #8 on:]

Jabbado:
Now I have two PID configs, for the pitch and roll. And they output negative or positive value depending if the sensor is outputting +- values.

But I just can't seem to put the equation together on the mixer.

THROTTLE_PID_BACK               = Receiver_throttle_val + PID_pitch_output (if the output is positive)
THROTTLE_PID_FRONT_LEFT     = Receiver_throttle_val + PID_roll_output (if the output is positive) + PID_pitch_output (if the output is negative)
THROTTLE_PID_FRONT_RIGHT   = Receiver_throttle_val + PID_roll_output (if the output is negative) + PID_pitch_output (if the output is negative)

I hope you understand my dilemma here. I mean I can't seem to make it fit without IF statements.

Cheers!

[Reply #9 on:]

Ok, take the pitch calculation. Now that you have +/- error values depending on whether the tri is pitching up or down, when that value is fed to the PID it's output will also change.

So in

Code:

THROTTLE_PID_BACK               = Receiver_throttle_val + PID_pitch_output

PID_pitch_output will sometimes be negative and sometimes positive depending on the pitch error. So there's no need for "if the output is positive" etc.

If you forget about whether it's a tri, quad, hex or whatever and just treat it as a generalized multicopter with pitch, roll, yaw (and vertical speed), you can do all the PID stuff in those three axes then map it to the motor commands at the end. Here's the motor command calc for a tricopter from the Aeroquad code:

Code:

void applyMotorCommand() {
  motorCommand[FRONT_LEFT]    = throttle + motorAxisCommandRoll - motorAxisCommandPitch*2/3;
  motorCommand[FRONT_RIGHT]   = throttle - motorAxisCommandRoll - motorAxisCommandPitch*2/3;
  motorCommand[REAR]          = throttle + motorAxisCommandPitch*4/3;
  const float yawMotorCommand = constrain(motorAxisCommandYaw,-MAX_RECEIVER_OFFSET-abs(receiverCommand[ZAXIS]),+MAX_RECEIVER_OFFSET+abs(receiverCommand[ZAXIS]));
  motorCommand[SERVO]         = constrain(TRI_YAW_MIDDLE + YAW_DIRECTION * yawMotorCommand, TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX);
}

Notice your pitch calc for the rear motor is the same apart from the scaling factor.
Your front motor calcs are almost correct except with one you add the roll command and the other you subtract the roll command, since one motor has to lift up while the other goes down.

[Reply #10 on:]

Jabbado:
Well now it makes sense to me, lol.

All I can do is to thank for the help, and I will try to put this together later cuz I burned an esc and Im pretty pissed of (not the first one).

Cheers!