You mean to simulate system in matlab? I'm only 16, so I don't think I can do it
i would develop the sim in C on my laptop
while it takes time to develop a simulation, they are almost always easier to develop code with than on the target, save time and provide a better understanding of the system
simulations can often avoid damaging the target, as well
Could you give me a brief overview of how I should proceed? I've never tried anything like this.
i'm not sure and have distracted you from what you've been working on and the help you've gotten from others. ( i apologize to everyone. less than willing to read 100+ posts)
i didn't realize you're 16. such a project seems ambitious, even for me (and i have some aero knowledge). i've had to help experienced SW developers use PID for timing synchronization. i'm an EE
part of the question of asking if a simulation was done was to see how serious an effort was being made.
i could provide a Pid simulation i have. but as already mentioned, the key is developing a model for the object being controlled. in general, it involves an understanding of the forces affecting it and the mass/inertia that affects the accelerations (F=ma). not only are the linear motions, but rotational motions as well in 3 dimensions
and it's not obvious to me what you're using Pid to control on a multi prop drone. i heard that gyros are commonly used to stabilize behavior but have no knowledge of how. presumably there are tutorials
The Kp element appears to be written correctly. I would think that it could be perfectly functional with the right kP and min/max. The graph in the #107 video, with the (5 sec?) of observations of the drone look like it oscillates at about 0.5Hz. It looks like integral windup oscillations, since maybe the propellers reverse at 90° pitch rather than near 0° pitch.
I am concerned about your test tether frame and the interaction of the bungee cords with the IMU. Maybe a system like Quadcopter restraint system testing x525 flight test - YouTube would help align the bungee system with vertical axis and minimize bungee-induced horizontal oscillations and accelerations.
Also, looking at your video at 25% speed, I wonder if you have any throttle, or is the PID attempting to control the motors around thrust=0 +/- 30 rather than the drone supporting its own weight with thrust and controlling around, for example, thrust=40 +/- 30 I'd think the 0+/-30 range would have more non-linear thrust behavior around zero and be harder to control. What I like about the vertical tether system is that you could run at `thrust=40+/-??" and the floor tether would counterbalance the static thrust, and the tethers tend to balance the system. (I don't know if the bottom tether in that video was a springy bungee, or a stiffer static line.)
What's it like with Kp=0.3, Ki=0, Kd=0 with the +/-30 output limits?
The tether frame in the video looks nice. I was planning to use a similar type, but didn't want to drill a hole in the ceiling. But unless I get better results, I'll try changing the frame.
In the video, I had the throttle at around 20%, so the final engine speed may have been so low at some points that the engines were not moving. I tested at this low value because using a higher value would be dangerous with behaviour on video.
Today I re-tuned the kalman filter values and got even better results, which probably led to better test flights. video - Kp = 0.1, Ki = 0.04, Kd = 38
i looked at your code, the coefficients and am surprised that Kd, at least starts at zero. Kd controls oscillations and i believe is essential to reaching and remaining at to target
here are series of plots i generated from a simple simulation. they show the PID output in yellow, the error in red, and value being controlled, in this case a (pitch) angle, "deg" in green
this first plot is with Kp = 2.0, Ki = 0 and Kd = 1.4. as you can see, the control value, "deg" increases to a target value of 30, overshoots but then stabilizes around it
this next plot uses a Kd = 1.0 and the following plot a Kd=0
the code is below. it models the drone as a mass (moment) on which a force (the Pid output) is applied resulting in a rotational acceleration (Newton's equation, F=ma) . that acceleration is integrated (summed) resulting in a rotational velocity and that velocity integrated resulting in a position, angle.
yes the moment of the drone is a guess, and the PID out is not directly a force applied to the moment.
i don't know how much of this to explain, so ask questions if interested
i don't expect you to run this code. i hope you'll understand the need for a proper (?) Kd value, but it depends on the moment of the drone and the relationship between the PID out and resulting force
struct Pid {
double Kp;
double Ki;
double Kd;
double Max;
double Min;
const char *label;
double errLst;
double dErr;
double sum;
double out;
};
Pid pid [] = {
{ 2.0, 0.0, 0.0, 100.0, 100.0, "Pitch 0.0" },
{ 2.0, 0.0, 0.4, 100.0, 100.0, "Pitch 0.4" },
{ 2.0, 0.0, 1.0, 100.0, 100.0, "Pitch 1.0" },
{ 2.0, 0.0, 1.4, 100.0, 100.0, "Pitch 1.4" },
};
int idx = 0;
// -----------------------------------------------------------------------------
double
calcPid (
unsigned long dMsec,
double inp,
double setPt,
Pid *p )
{
double err = setPt - inp;
p->sum += err * dMsec;
p->sum = p->sum > p->Max ? p->Max : (p->sum < p->Min ? p->Min : p->sum);
if (0 != p->errLst)
p->dErr = (err - p->errLst);
p->out = p->Kp * err
+ p->Ki * p->sum
+ p->Kd * p->dErr / (dMsec / 1000.);
p->errLst = err;
// limit
// p->out = p->out > p->Max ? p->Max : (p->out < p->Min ? p->Min : p->out);
return p->out;
}
// -----------------------------------------------------------------------------
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <getopt.h>
char *progname;
unsigned int debug = 0;
unsigned int flag = 0;
// --------------------------------------------------------------------
double
motion (
int msec,
double force,
double moment )
{
static double deg;
static double degPsec;
double degPsecPsec = force / moment;
degPsec += degPsecPsec * msec / 1000;
deg += degPsec * msec / 1000;
return deg;
}
// --------------------------------------------------------------------
void
application (char *filename) {
FILE *fp;
char s[BUFSIZ];
printf ("%s: %s\n", progname, filename);
if ( (fp = fopen (filename, "rb")) == NULL) {
perror ("app - fopen input");
exit (1);
}
unsigned long msecTot = 0;
unsigned long msec;
double out;
double deg;
double targ;
while (fgets (s, BUFSIZ, fp) != NULL) {
printf ("%s", s);
int val;
sscanf (s, "%*s %d", &val);
if (strstr (s, "pid")) {
printf (" title %s\n", pid [idx].label);
while (val--) {
if (! (msecTot % 1000))
printf (" res %6s %6s %6s %6s %6s %6s\n", "msec", "targ",
"deg", "out", "err", "dErr");
out = calcPid (msec, deg, targ, & pid [idx]);
deg = motion (msec, out, 1);
printf (" res");
printf (" %6.2f", msecTot / 1000.0);
printf (" %6.2f", targ);
printf (" %6.2f", deg);
printf (" %6.2f", out);
printf (" %6.2f", pid [idx].errLst);
printf (" %6.2f", pid [idx].dErr);
// printf (" %6.2f", pid [0].Kp);
printf ("\n");
msecTot += msec;
}
printf (" params %6.3f %6.3f %6.3f\n",
pid [idx].Kp, pid [idx].Ki, pid [idx].Kd);
}
else if (strstr (s, "idx"))
idx = val;
else if (strstr (s, "kd"))
pid [idx].Kd = val;
else if (strstr (s, "msec"))
msec = val;
else if (strstr (s, "targ"))
targ = val;
}
}
// --------------------------------------------------------------------
void help (void) {
printf (" Usage: %s \n", progname);
}
// --------------------------------------------------------------------
int main (int argc, char **argv) {
int c;
progname = *argv;
while ((c = getopt(argc, argv, "D:d:i:")) != -1) {
switch (c) {
case 'D':
debug = atoi (optarg);
break;
case 'd':
pid [idx].Kd = atof (optarg);
break;
case 'i':
idx = atoi (optarg);
break;
default:
printf ("Error: unknown option '%c'\n", optopt);
help();
break;
};
}
application (argv[optind]);
return 0;
}
the program is driven by command line options and a command file executed thru a (unix) shell on a windows PC running cygwin
msec 50
targ 30
pid 200
Cool program. It took a tick to figure out how to invoke it.
drf@V51292 home % ./drone_pid ./drone_opts.txt
./drone_pid: ./drone_opts.txt
msec 50
targ 30
pid 200
title Pitch 0.0
res msec targ deg out err dErr
res 0.00 30.00 0.15 60.00 30.00 0.00
res 0.05 30.00 0.45 59.70 29.85 -0.15
res 0.10 30.00 0.90 59.10 29.55 -0.30
res 0.15 30.00 1.49 58.21 29.10 -0.45
res 0.20 30.00 2.22 57.02 28.51 -0.59
res 0.25 30.00 3.10 55.55 27.78 -0.74
res 0.30 30.00 4.11 53.80 26.90 -0.87
res 0.35 30.00 5.24 51.79 25.89 -1.01
res 0.40 30.00 6.51 49.51 24.76 -1.14
res 0.45 30.00 7.89 46.99 23.49 -1.26
res 0.50 30.00 9.37 44.23 22.11 -1.38
res 0.55 30.00 10.97 41.25 20.63 -1.49
res 0.60 30.00 12.66 38.06 19.03 -1.59
res 0.65 30.00 14.43 34.69 17.34 -1.69
...
I might adopt your drone 'motion()' simulation into one of my demos on PID with simulated heater or motor
Thank you for your contribution. There is a big difference compared to my Kd value. I tuned my pid according to Joop Brokking. I think I have understood the need for the correct Kd value. Today I disassembled the drone and using 3d printed parts, I tried to place the parts so that the center of gravity is closest to the center, because I found out, that it wasn't correct before. Then tomorrow I plan to find out the max and min values of the PID controller again and try to reconfigure it. I also spent some time trying to run the simulation you posted, but was unable to get it to work.
Joop Brokking's project looks interesting:
The trick on @gcjr's simulation I needed to figure out was to make a drone_opts.txt file with the configuration commands:
msec 50
targ 30
pid 200
And after compiling it, I ran it like this:
it then prints out the config file, the title, and then a run f the pid process for 200 steps of 50ms long.
You might try it with 4ms steps to match your code.
After adding the txt file, I still get the same errors and warnings as before when I wanted to compile the program:
$ gcc -o drone_pid drone_pid.c
drone_pid.c:16:1: error: unknown type name ‘Pid’; use ‘struct’ keyword to refer to the type
16 | Pid pid [] = {
| ^~~
| struct
drone_pid.c:17:5: warning: braces around scalar initializer
17 | { 2.0, 0.0, 0.0, 100.0, 100.0, "Pitch 0.0" },
| ^
drone_pid.c:17:5: note: (near initialization for ‘pid[0]’)
drone_pid.c:17:13: warning: excess elements in scalar initializer
17 | { 2.0, 0.0, 0.0, 100.0, 100.0, "Pitch 0.0" },
| ^~~
drone_pid.c:17:13: note: (near initialization for ‘pid[0]’)
drone_pid.c:17:19: warning: excess elements in scalar initializer
17 | { 2.0, 0.0, 0.0, 100.0, 100.0, "Pitch 0.0" },
| ^~~
drone_pid.c:17:19: note: (near initialization for ‘pid[0]’)
drone_pid.c:17:25: warning: excess elements in scalar initializer
17 | { 2.0, 0.0, 0.0, 100.0, 100.0, "Pitch 0.0" },
| ^~~~~
drone_pid.c:17:25: note: (near initialization for ‘pid[0]’)
drone_pid.c:17:32: warning: excess elements in scalar initializer
17 | { 2.0, 0.0, 0.0, 100.0, 100.0, "Pitch 0.0" },
| ^~~~~
drone_pid.c:17:32: note: (near initialization for ‘pid[0]’)
drone_pid.c:17:40: warning: excess elements in scalar initializer
17 | { 2.0, 0.0, 0.0, 100.0, 100.0, "Pitch 0.0" },
| ^~~~~~~~~~~
drone_pid.c:17:40: note: (near initialization for ‘pid[0]’)
drone_pid.c:18:5: warning: braces around scalar initializer
18 | { 2.0, 0.0, 0.4, 100.0, 100.0, "Pitch 0.4" },
| ^
drone_pid.c:18:5: note: (near initialization for ‘pid[1]’)
drone_pid.c:18:13: warning: excess elements in scalar initializer
18 | { 2.0, 0.0, 0.4, 100.0, 100.0, "Pitch 0.4" },
| ^~~
drone_pid.c:18:13: note: (near initialization for ‘pid[1]’)
drone_pid.c:18:19: warning: excess elements in scalar initializer
18 | { 2.0, 0.0, 0.4, 100.0, 100.0, "Pitch 0.4" },
| ^~~
drone_pid.c:18:19: note: (near initialization for ‘pid[1]’)
drone_pid.c:18:25: warning: excess elements in scalar initializer
18 | { 2.0, 0.0, 0.4, 100.0, 100.0, "Pitch 0.4" },
| ^~~~~
drone_pid.c:18:25: note: (near initialization for ‘pid[1]’)
drone_pid.c:18:32: warning: excess elements in scalar initializer
18 | { 2.0, 0.0, 0.4, 100.0, 100.0, "Pitch 0.4" },
| ^~~~~
drone_pid.c:18:32: note: (near initialization for ‘pid[1]’)
drone_pid.c:18:40: warning: excess elements in scalar initializer
18 | { 2.0, 0.0, 0.4, 100.0, 100.0, "Pitch 0.4" },
| ^~~~~~~~~~~
drone_pid.c:18:40: note: (near initialization for ‘pid[1]’)
drone_pid.c:19:5: warning: braces around scalar initializer
19 | { 2.0, 0.0, 1.0, 100.0, 100.0, "Pitch 1.0" },
| ^
drone_pid.c:19:5: note: (near initialization for ‘pid[2]’)
drone_pid.c:19:13: warning: excess elements in scalar initializer
19 | { 2.0, 0.0, 1.0, 100.0, 100.0, "Pitch 1.0" },
| ^~~
drone_pid.c:19:13: note: (near initialization for ‘pid[2]’)
drone_pid.c:19:19: warning: excess elements in scalar initializer
19 | { 2.0, 0.0, 1.0, 100.0, 100.0, "Pitch 1.0" },
| ^~~
drone_pid.c:19:19: note: (near initialization for ‘pid[2]’)
drone_pid.c:19:25: warning: excess elements in scalar initializer
19 | { 2.0, 0.0, 1.0, 100.0, 100.0, "Pitch 1.0" },
| ^~~~~
drone_pid.c:19:25: note: (near initialization for ‘pid[2]’)
drone_pid.c:19:32: warning: excess elements in scalar initializer
19 | { 2.0, 0.0, 1.0, 100.0, 100.0, "Pitch 1.0" },
| ^~~~~
drone_pid.c:19:32: note: (near initialization for ‘pid[2]’)
drone_pid.c:19:40: warning: excess elements in scalar initializer
19 | { 2.0, 0.0, 1.0, 100.0, 100.0, "Pitch 1.0" },
| ^~~~~~~~~~~
drone_pid.c:19:40: note: (near initialization for ‘pid[2]’)
drone_pid.c:20:5: warning: braces around scalar initializer
20 | { 2.0, 0.0, 1.4, 100.0, 100.0, "Pitch 1.4" },
| ^
drone_pid.c:20:5: note: (near initialization for ‘pid[3]’)
drone_pid.c:20:13: warning: excess elements in scalar initializer
20 | { 2.0, 0.0, 1.4, 100.0, 100.0, "Pitch 1.4" },
| ^~~
drone_pid.c:20:13: note: (near initialization for ‘pid[3]’)
drone_pid.c:20:19: warning: excess elements in scalar initializer
20 | { 2.0, 0.0, 1.4, 100.0, 100.0, "Pitch 1.4" },
| ^~~
drone_pid.c:20:19: note: (near initialization for ‘pid[3]’)
drone_pid.c:20:25: warning: excess elements in scalar initializer
20 | { 2.0, 0.0, 1.4, 100.0, 100.0, "Pitch 1.4" },
| ^~~~~
drone_pid.c:20:25: note: (near initialization for ‘pid[3]’)
drone_pid.c:20:32: warning: excess elements in scalar initializer
20 | { 2.0, 0.0, 1.4, 100.0, 100.0, "Pitch 1.4" },
| ^~~~~
drone_pid.c:20:32: note: (near initialization for ‘pid[3]’)
drone_pid.c:20:40: warning: excess elements in scalar initializer
20 | { 2.0, 0.0, 1.4, 100.0, 100.0, "Pitch 1.4" },
| ^~~~~~~~~~~
drone_pid.c:20:40: note: (near initialization for ‘pid[3]’)
drone_pid.c:31:5: error: unknown type name ‘Pid’
31 | Pid *p )
| ^~~
drone_pid.c: In function ‘application’:
drone_pid.c:105:45: error: request for member ‘label’ in something not a structure or union
105 | printf (" title %s\n", pid [idx].label);
| ^
drone_pid.c:111:23: warning: implicit declaration of function ‘calcPid’ [-Wimplicit-function-declaration]
111 | out = calcPid (msec, deg, targ, & pid [idx]);
| ^~~~~~~
drone_pid.c:118:44: error: request for member ‘errLst’ in something not a structure or union
118 | printf (" %6.2f", pid [idx].errLst);
| ^
drone_pid.c:119:44: error: request for member ‘dErr’ in something not a structure or union
119 | printf (" %6.2f", pid [idx].dErr);
| ^
drone_pid.c:128:26: error: request for member ‘Kp’ in something not a structure or union
128 | pid [idx].Kp, pid [idx].Ki, pid [idx].Kd);
| ^
drone_pid.c:128:40: error: request for member ‘Ki’ in something not a structure or union
128 | pid [idx].Kp, pid [idx].Ki, pid [idx].Kd);
| ^
drone_pid.c:128:54: error: request for member ‘Kd’ in something not a structure or union
128 | pid [idx].Kp, pid [idx].Ki, pid [idx].Kd);
| ^
drone_pid.c:135:22: error: request for member ‘Kd’ in something not a structure or union
135 | pid [idx].Kd = val;
| ^
drone_pid.c: In function ‘main’:
drone_pid.c:163:22: error: request for member ‘Kd’ in something not a structure or union
163 | pid [idx].Kd = atof (optarg);
| ^
rename it drone.cpp
Thanks, now it works fine!