Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-09-29 10:23:45 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

pid2 fix

Browse Source
This commit is contained in:
crinq
2015-02-24 01:43:05 +01:00
parent 05a36d2957
commit ee7d77ffbd
1 changed files with 52 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/comps/pid2.comp
+52 -50
View File
@@ -20,30 +20,29 @@
COMP(pid);
HAL_PIN(pos_ext_cmd) = 0.0;
HAL_PIN(pos_fb) = 0.0;
HAL_PIN(pos_cmd) = 0.0;
HAL_PIN(pos_error) = 0.0;
HAL_PIN(pos_ext_cmd) = 0.0; // cmd in (rad)
HAL_PIN(pos_fb) = 0.0; // feedback in (rad)
HAL_PIN(pos_error) = 0.0; // error out (rad)
HAL_PIN(vel_ext_cmd) = 0.0;
HAL_PIN(vel_fb) = 0.0;
HAL_PIN(vel_cmd) = 0.0;
HAL_PIN(vel_error) = 0.0;
HAL_PIN(vel_ext_cmd) = 0.0; // cmd in (rad/s)
HAL_PIN(vel_fb) = 0.0; // feedback in (rad/s)
HAL_PIN(vel_cmd) = 0.0; // cmd out (rad/s)
HAL_PIN(vel_error) = 0.0; // error out (rad/s)
HAL_PIN(acc_ext_cmd) = 0.0;
HAL_PIN(acc_cmd) = 0.0;
HAL_PIN(acc_ext_cmd) = 0.0; // cmd in (rad/s^2)
HAL_PIN(acc_cmd) = 0.0; // cmd out (rad/s^2)
HAL_PIN(force_ext_cmd) = 0.0;
HAL_PIN(force_cmd) = 0.0;
HAL_PIN(force_ext_cmd) = 0.0; // cmd in (Nm)
HAL_PIN(force_cmd) = 0.0; // cmd out (Nm)
HAL_PIN(cur_ext_cmd) = 0.0;
HAL_PIN(cur_fb) = 0.0;
HAL_PIN(cur_cmd) = 0.0;
HAL_PIN(cur_error) = 0.0;
HAL_PIN(cur_ext_cmd) = 0.0; // cmd in (A)
//HAL_PIN(cur_fb) = 0.0; // fb in (A)
HAL_PIN(cur_cmd) = 0.0; // cmd out (A)
//HAL_PIN(cur_error) = 0.0; // error out (A)
HAL_PIN(volt_cmd) = 0.0;
HAL_PIN(volt_cmd) = 0.0; // cmd out (V)
HAL_PIN(pwm_cmd) = 0.0;
HAL_PIN(pwm_cmd) = 0.0; // cmd out (V)
HAL_PIN(enable) = 0.0;
@@ -54,51 +53,51 @@ HAL_PIN(force_en) = 1.0;
HAL_PIN(cur_en) = 1.0;
HAL_PIN(time) = 0.0002;
HAL_PIN(time) = 0.0002; (s)
HAL_PIN(mot_r) = 30.0;
HAL_PIN(mot_l) = 0.01;
HAL_PIN(mot_j) = 0.001;
HAL_PIN(mot_km) = 30.0;
HAL_PIN(mot_r) = 23.7; // resistance (ohm)
HAL_PIN(mot_l) = 0.01; // inductance (henry)
HAL_PIN(mot_j) = 0.000026; // inertia (kgm^2)
HAL_PIN(mot_km) = 0.3724; // torque constant (Nm/A, V/rad/s)
HAL_PIN(mot_fr) = 30.0;
HAL_PIN(mot_fd) = 30.0;
HAL_PIN(mot_fl) = 30.0;
HAL_PIN(mot_fr) = 30.0; // friction (Nm)
HAL_PIN(mot_fd) = 30.0; // damping (Nm/rad/s)
HAL_PIN(mot_fl) = 30.0; // load (Nm)
HAL_PIN(pos_p) = 100.0;
HAL_PIN(vel_p) = 1.0;
HAL_PIN(vel_p) = 0.5;
HAL_PIN(vel_ff) = 1.0;
HAL_PIN(acc_p) = 0.1;
HAL_PIN(acc_pi) = 3.667;
HAL_PIN(acc_p) = 0.5;
HAL_PIN(acc_pi) = 0.0;
HAL_PIN(acc_ff) = 1.0;
HAL_PIN(force_p) = 3.667;
HAL_PIN(force_p) = 0.5;
HAL_PIN(force_ff) = 1.0;
HAL_PIN(cur_p) = 0.0;
HAL_PIN(cur_p) = 0.5;
HAL_PIN(cur_ff) = 1.0;
HAL_PIN(cur_kp) = 0.0;
HAL_PIN(cur_ki) = 0.0;
//HAL_PIN(cur_kp) = 0.0;
//HAL_PIN(cur_ki) = 0.0;
HAL_PIN(volt) = 130;
// user limits
HAL_PIN(vel_limit) = 1300.0;
HAL_PIN(vel_limit) = 1250.0;
HAL_PIN(acc_limit) = 300000.0;
HAL_PIN(force_limit) = 100.0;
HAL_PIN(cur_limit) = 10.0;
HAL_PIN(force_limit) = 3.0;
HAL_PIN(cur_limit) = 6.0;
HAL_PIN(volt_limit) = 400.0;
HAL_PIN(pwm_limit) = 0.9;
HAL_PIN(pwm_limit) = 0.95;
// max limits
HAL_PIN(max_vel) = 1300.0;
HAL_PIN(max_vel) = 1250.0;
HAL_PIN(max_acc) = 300000.0;
HAL_PIN(max_force) = 100.0;
HAL_PIN(max_cur) = 10.0;
HAL_PIN(max_force) = 3.0;
HAL_PIN(max_cur) = 6.0;
HAL_PIN(max_volt) = 400.0;
HAL_PIN(max_pwm) = 0.95;
@@ -148,9 +147,9 @@ float forcecmd;
float forcesat;
float curextcmd = PIN(cur_ext_cmd);
float curfb = PIN(cur_fb);
//float curfb = PIN(cur_fb);
float curcmd;
float curerr = 0.0;
//float curerr = 0.0;
float cursat;
float voltcmd;
@@ -166,11 +165,11 @@ if(r == 0.0){
float l = PIN(mot_l);
float j = PIN(mot_j);
if(j == 0.0){
j = 1.0;
j = 0.0001;
}
float m = PIN(mot_km);
if(m == 0.0){
m = 1.0;
m = 0.5;
}
float t = PIN(time);
if(t == 0.0){
@@ -178,6 +177,9 @@ if(t == 0.0){
}
float vlt = PIN(volt);
if(vlt == 0.0){
vlt = 0.1;
}
float velmax = MIN(PIN(vel_limit), PIN(max_vel));
float velmin = -velmax;
@@ -233,9 +235,12 @@ if(PIN(enable) > 0.0){
accp = 0.0;
force_error_sum = 0.0;
}
forcecmd = accp * acccmd / j + PIN(force_ff) * forceextcmd;
forcecmd = accp * acccmd * j + PIN(force_ff) * forceextcmd;
force_error_sum += velerr * t * accp * accpi;
force_error_sum = CLAMP(force_error_sum, forcemin_, forcemax_); //TODO
if(accpi == 0.0){
force_error_sum = 0.0;
}
forcecmd += force_error_sum;
forcesat = SAT2(forcecmd, forcemin_, forcemax_);
forcecmd = CLAMP(forcecmd, forcemin_, forcemax_);
@@ -252,14 +257,11 @@ if(PIN(enable) > 0.0){
if(PIN(cur_en) == 0.0){
curp = 0.0;
}
voltcmd = curp * curcmd / r + ind;
voltcmd = curp * curcmd * r + ind;
voltsat = SAT2(voltcmd, voltmin, voltmax);
voltcmd = CLAMP(voltcmd, voltmin, voltmax);
// volt -> pwm
if(vlt == 0.0){
pwmcmd = 0.0;
}
else{
pwmcmd = voltcmd / vlt;
}
@@ -290,7 +292,7 @@ PIN(vel_error) = velerr;
PIN(acc_cmd) = acccmd;
PIN(force_cmd) = forcecmd;
PIN(cur_cmd) = curcmd;
PIN(cur_error) = curerr;
//PIN(cur_error) = curerr;
PIN(volt_cmd) = voltcmd;
PIN(pwm_cmd) = pwmcmd;
PIN(vel_sat) = velsat;