PID help

Projects Programming
1

Hello!
I'm having some trouble conceptualizing how to go about implementing PID into my Drone. there are a few ways I'm thinking about implementing the pipeline:
Having one PID Object for each of the six motors,
Having one PID Object for each axis

How I would implement PIDs on a per-motor basis:

712×718 25 KB

How I would implement PIDs on a per-axis basis:

786×815 26.7 KB

I'm guessing that the second one will be the best course of action, as it can all be put into it's own loop, where the PID data will be available on-demand, as oppose to calculations it on the spot. It is object oriented, and runs in the background.

The first one has it's appeals, too. It eliminates some of the caluclations needed in order to find the acutal value. It is procedural, and very straight forward and performes on the spot.

What do you guys think?

2

Why do you think trigonometry is needed? There are 4 independent control loops, roll, pitch, yaw and thrust,
you then output those suitably weighted to the motors (thrust for instance goes with the same sign to all,
yaw with a sign depending on prop spin direction, etc). All linear, control loops like linear...

The tricky bit is if you saturate one of the motors, then the loops are no longer independent and
you crash!

And you want to arrange that the values passed to the motor are linear in thrust, ie you may need
a look-up table from linearized thrust to actual PWM drive level.

3

MarkT:
Why do you think trigonometry is needed?

see my other topic over here: Making a hexcopter with the arduino Due. (WIP) - Arduino Due - Arduino Forum
also, it's worth noting that I have 6 motors, so a simple sign flip will not work.

4

Aha! Now I understand why you want to use trigonometry.

The motors don't move, do they? Then pre-calculate the multiplier factors and don't do any trig while flying.

5

You don't really need any trig. The PID feedback loops generates Outputs: PitchUp, RollRight, YawCW. You apply those Outputs either positive or negative to the appropriate motors. The feedback loops will adjust the Outputs to get the Inputs to the Setpoints.

CW Front Left Motor +PitchUp +RollRight +YawCW
CCW Left Motor +RollRight -YawCW
CW Back Left Motor +PitchUp +RollRight +YawCW
CCW Front Right Motor -PitchUp -RollRight -YawCW
CW Right Motor -RollRight +YawCW
CCW Back Right Motor -PitchUp -RollRight -YawCW
6

I may need some help, in that case. Can I calculate the motor powers withour going from cartesian to polar on the input values? Can someone provide an example, without PID, for direct input to the motors, using throttle, pitch, yaw?

7

I'm surprised you can't figure it out from the table I provided. Of course without PID loops you will need to be quick on the controls. The inputs will need to be scaled so as not to overflow the math and the motor outputs should be constrained to the allowed limits.

void loop() {
// Control inputs:
// int PitchUp;  // 0 = level
// int RollRight; // 0 = level
// int YawRateCW;  // 0 = No Yaw
// unsigned int Throttle;  // 0 = motors stopped

  MotorSpeed(MOTOR_FL_CW ,   Throttle + PitchUp + RollRight + YawRateCW);
  MotorSpeed(MOTOR_L_CCW ,   Throttle                  + RollRight - YawRateCW);  
  MotorSpeed(MOTOR_BL_CW,     Throttle - PitchUp + RollRight + YawRateCW);
  MotorSpeed(MOTOR_FR_CCW , Throttle + PitchUp - RollRight - YawRateCW);
  MotorSpeed(MOTOR_R_CW,       Throttle                  - RollRight + YawRateCW);
  MotorSpeed(MOTOR_BR_CCW , Throttle - PitchUp - RollRight - YawRateCW);
}
8

johnwasser:
I'm surprised you can't figure it out from the table I provided. Of course without PID loops you will need to be quick on the controls. The inputs will need to be scaled so as not to overflow the math and the motor outputs should be constrained to the allowed limits.

void loop() {

// Control inputs:
// int PitchUp;  // 0 = level
// int RollRight; // 0 = level
// int YawRateCW;  // 0 = No Yaw
// unsigned int Throttle;  // 0 = motors stopped

MotorSpeed(MOTOR_FL_CW ,  Throttle + PitchUp + RollRight + YawRateCW);
  MotorSpeed(MOTOR_L_CCW ,  Throttle                  + RollRight - YawRateCW); 
  MotorSpeed(MOTOR_BL_CW,    Throttle - PitchUp + RollRight + YawRateCW);
  MotorSpeed(MOTOR_FR_CCW , Throttle + PitchUp - RollRight - YawRateCW);
  MotorSpeed(MOTOR_R_CW,      Throttle                  - RollRight + YawRateCW);
  MotorSpeed(MOTOR_BR_CCW , Throttle - PitchUp - RollRight - YawRateCW);
}

what do the parameters represent? the ones that you pass into MotorSpeed?
also, wouldn't this make the left/right motors all react the same roll? what are the units of yaw rate? should I just feed my pwms into Roll and pitch? is there a multiplier that goes with the pitch/roll?

9

tuskiomi:
What do the parameters represent, the ones that you pass into MotorSpeed?

They represent the control inputs. Positive values Pitch (the nose) Up, Roll Right, Yaw Clockwise, and Throttle Up. Negative values will Pitch/Roll/Yaw in the other direction. I would scale them to about +/-10% of the maximum output range. Throttle can be scaled from 0 to 100% of the maximum output value. The MotorSpeed() function should constrain the speed value to the allowed range: 0-255 for analogWrite(), 1000-2000 for servo.writeMicroseconds(), or 0-180 for servo.write().

tuskiomi:
also, wouldn't this make the left/right motors all react the same roll?

I don't know what you mean by "react the same roll". If the RollRight value is positive the three Right motors will slow down and the three Left motors will speed up. This causes a roll to the right. Similarly for PitchUp but since two of the motors are on the pitch axis (or near enough) they don't contribute to rotation on the pitch axis and are ignored. I may have CW and CCW signs mixed up on the Yaw.

tuskiomi:
what are the units of yaw rate?

Unitless. It's just a value to change the relative speed of CW and CCW motors. Scale it to whatever units give you the control you want. The higher the value the faster you can spin but the harder it will be to keep it pointed in any particular direction.

tuskiomi:
Should I just feed my PWMs into Roll and pitch? Is there a multiplier that goes with the pitch/roll?

It's not clear what you mean by "my PWM's". If you use pots directly your inputs will range from about 0 to 1023 with a rest position about 512. If you are getting pulse length information from an RC radio the range is about 1000 to 2000 with a rest position about 1500. I would start by mapping your input range to +/- 10% of your output range with the rest position being zero.

10

I suppose, I'm simply confused by your maming conventions. how would the code look if the motor vars were named 'motor[X]Degrees' where [X] was the measure from the front of the drone?

11

tuskiomi:
I suppose, I'm simply confused by your naming conventions. How would the code look if the motor vars were named 'motor[X]Degrees' where [X] was the measure from the front of the drone?

The motor naming convention is "L" for Left, "R" for Right, "F" for Front, and "B" for Back.
I'll assume you mean degrees clockwise.

  MotorSpeed(motor330Degrees ,   Throttle + PitchUp + RollRight + YawRateCW);
  MotorSpeed(motor270Degrees,   Throttle                  + RollRight - YawRateCW);  
  MotorSpeed(motor210Degrees,     Throttle - PitchUp + RollRight + YawRateCW);
  MotorSpeed(motor30Degrees, Throttle + PitchUp - RollRight - YawRateCW);
  MotorSpeed(motor90Degrees,       Throttle                  - RollRight + YawRateCW);
  MotorSpeed(motor150Degrees , Throttle - PitchUp - RollRight - YawRateCW);
12

johnwasser:
The motor naming convention is "L" for Left, "R" for Right, "F" for Front, and "B" for Back.
I'll assume you mean degrees clockwise.

  MotorSpeed(motor330Degrees ,   Throttle + PitchUp + RollRight + YawRateCW);

MotorSpeed(motor270Degrees,  Throttle                  + RollRight - YawRateCW); 
  MotorSpeed(motor210Degrees,    Throttle - PitchUp + RollRight + YawRateCW);
  MotorSpeed(motor30Degrees, Throttle + PitchUp - RollRight - YawRateCW);
  MotorSpeed(motor90Degrees,      Throttle                  - RollRight + YawRateCW);
  MotorSpeed(motor150Degrees , Throttle - PitchUp - RollRight - YawRateCW);

and a karma to you! this makes much more sense!

13

I came up with some test code, does it look correct?

#include <PID_v1.h>

double m1PWM,m2PWM,m3PWM,m4PWM,m5PWM,m6PWM;//m1 is at 30 deg, m6 is at 330.
double m1PitchK, m2PitchK, m3PitchK, m4PitchK, m5PitchK, m6PitchK;
double mo1RollK, mo2RollK, mo3RollK, mo4RollK, mo5RollK, mo6RollK;
double mot1YawK, mot2YawK, mot3YawK, mot4YawK, mot5YawK, mot6YawK;
double offset;
int pwmPitch,pwmRoll,pwmYaw,pwmThrottle;
// The sign used to reverse the axes, valid values are 1, and -1.
int PitchSign = 1, RollSign = 1, YawSign = 1;
double upperLimit= 10; //max: 10 degrees from normal
int minPitch=600, rangePitch=1200;
int minRoll=600, rangeRoll=1200;
int minYaw=600, rangeYaw=1200;
double outPitchPID, outRollPID, outYawPID;
double inPitchPID, inRollPID, inYawPID;
double tgtPitchPID, tgtRollPID, tgtYawPID;
PID PitchPID(&inPitchPID, &outPitchPID, &tgtPitchPID,0,0,0,DIRECT);
PID RollPID(&inRollPID, &outRollPID, &tgtRollPID,0,0,0,DIRECT);
PID YawPID(&inYawPID, &outYawPID, &tgtYawPID,0,0,0,DIRECT);



void setup() {
  PitchPID.SetMode(MANUAL);
  PitchPID.SetSampleTime(100);
  RollPID.SetMode(MANUAL);
  RollPID.SetSampleTime(100);
  YawPID.SetMode(MANUAL);
  YawPID.SetSampleTime(100);
  m1PitchK=cos(PI/3)*PitchSign;
  m2PitchK=cos(PI/2)*PitchSign;
  m3PitchK=cos(5*PI/6)*PitchSign;
  m4PitchK=cos(7*PI/6)*PitchSign;
  m5PitchK=cos(3*PI/2)*PitchSign;
  m6PitchK=cos(11*PI/6)*PitchSign;
  mo1RollK=sin(PI/3)*RollSign;
  mo2RollK=sin(PI/2)*RollSign;
  mo3RollK=sin(5*PI/6)*RollSign;
  mo4RollK=sin(7*PI/6)*RollSign;
  mo5RollK=sin(3*PI/2)*RollSign;
  mo6RollK=sin(11*PI/6)*RollSign;
  mot1YawK=-YawSign;
  mot2YawK=YawSign;
  mot3YawK=-YawSign;
  mot4YawK=YawSign;
  mot5YawK=-YawSign;
  mot6YawK=YawSign;
 

}

void loop() {
  inPitchPID = getPitch();
  inRollPID = getRoll();
  inYawPID = getYaw();
  tgtPitchPID = ((pwmPitch-minPitch)/rangePitch)*upperLimit;//((pwmPitch-minPitch)/rangePitch) gives a -1 to 1 value (percentage) and Upper limit turns that into a degree measurement.
  tgtRollPID = ((pwmRoll-minRoll)/rangeRoll)*upperLimit;
  tgtYawPID = ((pwmYaw-minYaw)/rangeYaw)*upperLimit;

  m1PWM =pwmThrottle + degToPWM(outPitchPID)*m1PitchK + degToPWM(outRollPID)*mo1RollK + degToPWM(outYawPID)*mot1YawK;
  m2PWM =pwmThrottle + degToPWM(outPitchPID)*m2PitchK + degToPWM(outRollPID)*mo2RollK + degToPWM(outYawPID)*mot2YawK;
  m3PWM =pwmThrottle + degToPWM(outPitchPID)*m3PitchK + degToPWM(outRollPID)*mo3RollK + degToPWM(outYawPID)*mot3YawK;
  m4PWM =pwmThrottle + degToPWM(outPitchPID)*m4PitchK + degToPWM(outRollPID)*mo4RollK + degToPWM(outYawPID)*mot4YawK;
  m5PWM =pwmThrottle + degToPWM(outPitchPID)*m5PitchK + degToPWM(outRollPID)*mo5RollK + degToPWM(outYawPID)*mot5YawK;
  m6PWM =pwmThrottle + degToPWM(outPitchPID)*m6PitchK + degToPWM(outRollPID)*mo6RollK + degToPWM(outYawPID)*mot6YawK;
  
  
}

int degToPWM(double d){
  return 0;
  }


//TODO: return sensor values
double getPitch(){
  return 0;
}
double getRoll(){
  return 0;
}
double getYaw(){
  return 0;
}
14

Teeny bump

15 
int degToPWM(double d){
  return 0;
  }

This looks wrong. It's going to null out all your PID outputs.

You also added a lot of complications with various trigonometry values and I suspect they are completely unnecessary.