mirror/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2024-12-20 23:52:15 +00:00
Code Issues Projects Releases Wiki Activity
motsim
stmbl/sim/sim.cpp
Rene Hopf 646e5fa81a tvd sim
2014-11-09 19:19:04 +01:00

411 lines
10 KiB
C++
Raw Permalink Blame History

#include "sim.h"
// Rcpp implementation of the 1D TVD algorithm in:
// Condat, L (2012) A Direct Algorithm for 1D Total Variation Denoising
// http://hal.inria.fr/docs/00/67/50/43/PDF/condat_killer_tv.pdf
// Accessed 11 August 2014.
// [[Rcpp::export]]
void tvd_1d_condat(std::vector<float> &y, std::vector<float> &x, int N, float lambda)
{
int k, k0, kn, kp, i;
float vmin, vmax, umin, umax;
k = k0 = kn = kp = 1;
vmin = y[0] - lambda;
vmax = y[0] + lambda;
umin = lambda;
umax = -lambda;
while (1)
{
// b
if (k == N)
{
x[k-1] = vmin + umin;
break;
}
if (y[k] + umin < vmin - lambda)
{
// b1
for (i = k0-1; i < kn; i++)
x[i] = vmin;
kn++;
k = k0 = kp = kn;
vmin = y[k-1];
vmax = y[k-1] + 2*lambda;
umin = lambda;
umax = -lambda;
}
else if (y[k] + umax > vmax + lambda)
{
// b2
for (i = k0-1; i < kp; i++)
x[i] = vmax;
kp++;
k = k0 = kn = kp;
vmin = y[k-1] - 2*lambda;
vmax = y[k-1];
umin = lambda;
umax = -lambda;
}
else
{
// b3
k++;
umin = umin + y[k-1] - vmin;
umax = umax + y[k-1] - vmax;
if (umin >= lambda)
{
// b31
vmin = vmin + (umin - lambda)/((float)(k - k0 + 1));
umin = lambda;
kn = k;
}
if (umax <= -lambda)
{
// b32
vmax = vmax + (umax + lambda)/((float)(k - k0 + 1));
umax = -lambda;
kp = k;
}
}
// c
if (k == N)
{
if (umin < 0)
{
// c1
for (i = k0-1; i < kn; i++)
x[i] = vmin;
kn++;
k = k0 = kn;
vmin = y[k-1];
umin = lambda;
umax = y[k-1] + lambda - vmax;
}
else if (umax > 0)
{
// c2
for (i = k0-1; i < kn; i++)
x[i] = vmax;
kp++;
k = k0 = kp;
vmax = y[k-1];
umax = -lambda;
umin = y[k-1] - lambda - vmin;
}
else
{
// c3
for (i = k0-1; i < N; i++)
x[i] = vmin + umin/((float)(k - k0 + 1));
break;
}
}
}
}
// Rcpp implementation of the 1D TVD algorithm in:
// Condat, L (2012) A Direct Algorithm for 1D Total Variation Denoising
// http://hal.inria.fr/docs/00/67/50/43/PDF/condat_killer_tv.pdf
// Accessed 11 August 2014.
// [[Rcpp::export]]
std::vector<double> tvd_1d_condat_worker(std::vector<double>& y, double lambda)
{
// NOTE: in wrapping R code, verify that y.size() <= 2^32-1. Although
// newer versions of R get around this index limit, Rcpp is still
// limited to int indices.
int N = y.size();
int k, k0, kn, kp, i;
double vmin, vmax, umin, umax;
std::vector<double> x(N);
k = k0 = kn = kp = 1;
vmin = y[0] - lambda;
vmax = y[0] + lambda;
umin = lambda;
umax = -lambda;
while (true)
{
// b
if (k == N)
{
x[k-1] = vmin + umin;
break;
}
if (y[k] + umin < vmin - lambda)
{
// b1
for (i = k0-1; i < kn; i++)
x[i] = vmin;
kn++;
k = k0 = kp = kn;
vmin = y[k-1];
vmax = y[k-1] + 2*lambda;
umin = lambda;
umax = -lambda;
}
else if (y[k] + umax > vmax + lambda)
{
// b2
for (i = k0-1; i < kp; i++)
x[i] = vmax;
kp++;
k = k0 = kn = kp;
vmin = y[k-1] - 2*lambda;
vmax = y[k-1];
umin = lambda;
umax = -lambda;
}
else
{
// b3
k++;
umin = umin + y[k-1] - vmin;
umax = umax + y[k-1] - vmax;
if (umin >= lambda)
{
// b31
vmin = vmin + (umin - lambda)/(static_cast<double>(k - k0 + 1));
umin = lambda;
kn = k;
}
if (umax <= -lambda)
{
// b32
vmax = vmax + (umax + lambda)/(static_cast<double>(k - k0 + 1));
umax = -lambda;
kp = k;
}
}
// c
if (k == N)
{
if (umin < 0)
{
// c1
for (i = k0-1; i < kn; i++)
x[i] = vmin;
kn++;
k = k0 = kn;
vmin = y[k-1];
umin = lambda;
umax = y[k-1] + lambda - vmax;
}
else if (umax > 0)
{
// c2
for (i = k0-1; i < kn; i++)
x[i] = vmax;
kp++;
k = k0 = kp;
vmax = y[k-1];
umax = -lambda;
umin = y[k-1] - lambda - vmin;
}
else
{
// c3
for (i = k0-1; i < N; i++)
x[i] = vmin + umin/(static_cast<double>(k - k0 + 1));
break;
}
}
}
return x;
}
int main(){
float sim_time = 0.0;
float sim_step = 0.0001;
float sim_end_time = 0.1;
srand(14235);
// e240
mot_c mot;
mot.reset();
mot.mech_spec.max_rps = 83.3;
mot.mech_spec.mot_type = mot_c::mech_spec_s::DC;
mot.mech_spec.pole_count = 1;
mot.mech_spec.friction = 0.021;//0.021;
mot.mech_spec.damping = 0.0000426;//0.0000426;
mot.mech_spec.inertia = 0.0000268;//0.0000268;
mot.elec_spec.max_i = 13.9;
mot.elec_spec.i = 1.9;
mot.elec_spec.nm_a = 0.135;
mot.elec_spec.r = 5.4;
mot.elec_spec.l = 0.0082;
mot.elec_spec.v_rps = 0.852;
mot.elec_spec.slip = 0;
mot.feedback.type = mot_c::feedback_s::RES;
mot.feedback.count = 1;
mot.feedback.res_offset = 0.0;
mot.noise.sin_scale = 1.0;
mot.noise.cos_scale = 1.0;
mot.noise.sin_offset = 0.0;
mot.noise.cos_offset = 0.0;
mot.noise.var = 0.0;
mot.load.friction = 0.0;
mot.load.load = 0.0;
mot.load.damping = 0.0;
mot.load.inertia = 0.0;
// bautz e728
mot_c mot2;
mot2.reset();
mot2.mech_spec.max_rps = 50;
mot2.mech_spec.mot_type = mot_c::mech_spec_s::DC;
mot2.mech_spec.pole_count = 1;
mot2.mech_spec.friction = 0.18;
mot2.mech_spec.damping = 0.004236;
mot2.mech_spec.inertia = 0.0012;
mot2.elec_spec.max_i = 60;
mot2.elec_spec.i = 10;
mot2.elec_spec.nm_a = 0.36;
mot2.elec_spec.r = 0.67;
mot2.elec_spec.l = 0.0011;
mot2.elec_spec.v_rps = 2.28;
mot2.elec_spec.slip = 0;
mot2.feedback.type = mot_c::feedback_s::RES;
mot2.feedback.count = 1;
mot2.feedback.res_offset = 0.0;
mot2.noise.sin_scale = 1.0;
mot2.noise.cos_scale = 1.0;
mot2.noise.sin_offset = 0.0;
mot2.noise.cos_offset = 0.0;
mot2.noise.var = 0.0;
mot2.load.friction = 0.0;
mot2.load.load = 0.0;
mot2.load.damping = 0.0;
mot2.load.inertia = 0.0;
// bosch
mot_c mot3;
mot3.reset();
mot3.mech_spec.max_rps = 16.6;
mot3.mech_spec.mot_type = mot_c::mech_spec_s::DC;
mot3.mech_spec.pole_count = 4;
mot3.mech_spec.friction = 0.065;//0.0;
mot3.mech_spec.damping = 0.0000826;//0.0;
mot3.mech_spec.inertia = 0.000141;
mot3.elec_spec.max_i = 4;
mot3.elec_spec.i = 2.2;
mot3.elec_spec.nm_a = 0.2727;
mot3.elec_spec.r = 15.2;
mot3.elec_spec.l = 0.0082;//0.030;
mot3.elec_spec.v_rps = 3.4;//5;
mot3.elec_spec.slip = 0;
mot3.feedback.type = mot_c::feedback_s::RES;
mot3.feedback.count = 1;
mot3.feedback.res_offset = 0.0;
mot3.noise.sin_scale = 1.0;
mot3.noise.cos_scale = 1.0;
mot3.noise.sin_offset = 0.0;
mot3.noise.cos_offset = 0.0;
mot3.noise.var = 0.01;
mot3.load.friction = 0.0;
mot3.load.load = 0.0;
mot3.load.damping = 0.0;
mot3.load.inertia = 0.0;
cmd_c cmd;
cmd.reset();
cmd.periode = 2;
cmd.amplitude = 1;
cmd.wave = cmd_c::CONST;
cmd.type = cmd_c::POS;
cmd.pos_res = 0.01;
cmd.vel_res = 0.01;
cmd.acc_res = 0.01;
drive_c drive;
drive.dc = 50;
drive.pwm_scale = 0.9;
drive.pwm_res = 8400;
drive.pid_periode = 0.001;
drive.mot = &mot3;
drive.in = &cmd;
drive.input_cmd = input_cmd;
drive.input_feedback = input_feedback_real;
drive.pid = pid;
drive.output = output;
drive.reset();
cout << flush << "time pos pos2 pos3" << endl;
int count = 0;
int pid_count = drive.pid_periode / sim_step;
double vel0 = MIN(drive.dc * (2 * drive.pwm_scale - 1) / drive.mot->elec_spec.v_rps, drive.mot->mech_spec.max_rps) * 0.7;
double ind0 = vel0 * drive.mot->elec_spec.v_rps;
double volt0 = drive.dc * (2 * drive.pwm_scale - 1);
double cur0 = (volt0 - ind0) / drive.mot->elec_spec.r;
double torq0 = cur0 * drive.mot->elec_spec.nm_a;
double acc0 = torq0 / drive.mot->mech_spec.inertia;
double vel1 = MIN(drive.dc * (2 * drive.pwm_scale - 1) / drive.mot->elec_spec.v_rps, drive.mot->mech_spec.max_rps) * 0.3;
double ind1 = vel1 * drive.mot->elec_spec.v_rps;
double volt1 = drive.dc * (2 * drive.pwm_scale - 1);
double cur1 = (volt1 - ind1) / drive.mot->elec_spec.r;
double torq1 = cur1 * drive.mot->elec_spec.nm_a;
double acc1 = torq1 / drive.mot->mech_spec.inertia;
double a, b, y;
a = (acc1 - acc0) / (vel1 - vel0);
b = acc0 - a * vel0;
std::vector<float> in,out,foo;
int i = 0;
for(sim_time = 0.0; sim_time < sim_end_time; sim_time += sim_step){
drive.in->step(sim_step);
if(count == 0){
drive.step(sim_step * pid_count);
}
count++;
count %= pid_count;
//drive.state.ctr = 1;
drive.output(&drive, sim_step);
drive.mot->step(sim_step);
in.push_back(drive.mot->get_sin()*drive.mot->get_polarity());
out.push_back(drive.mot->get_sin()*drive.mot->get_polarity());
foo.push_back(sin((drive.mot->state.pos + drive.mot->feedback.res_offset) * drive.mot->feedback.count));
//e_pos = (torq - drive.est.load) / drive.est.inertia * exp(-sim_time * mot.elec_spec.v_rps / drive.est.inertia * mot.elec_spec.nm_a / mot.elec_spec.r);
//y = a * drive.mot->state.vel + b;
//cout << sim_time << ", " << drive.in->get_pos() << ", " << drive.mot->state.pos << ", " << drive.est.pos << ", " << drive.est.sin_avg << ", " << drive.est.sin_scale << ", " << drive.est.cos_avg << ", " << drive.est.cos_scale << endl;
//cout << sim_time << ", " << drive.mot->state.pos << ", " << drive.mot->state.vel << ", " << drive.mot->state.acc << ", " << drive.est.p << ", " << drive.est.v << ", " << drive.est.a << endl;//", " << drive.state.ctr << ", " << minus_(drive.in->get_pos(), drive.est.pos) << endl;
//cout << sim_time << ", " << "-15, " << drive.in->get_pos() * 5 - 15<< ", " << drive.state.ctr * 10 << ", " << drive.mot->state.cur << ", " << drive.mot->state.vel / 5 << ", " << drive.mot->state.pos * 5 - 15 << ", " << minus_(drive.in->state.pos, drive.mot->state.pos) * (-100) - 15 << endl;//", " << drive.state.ctr << ", " << minus_(drive.in->get_pos(), drive.est.pos) << endl;
//cout << sim_time/*drive.mot->state.vel*/ << ", " << drive.est.pos << ", " << drive.mot->state.pos << endl;
}
//out = tvd_1d_condat_worker(in, 0.05);
tvd_1d_condat(in, out, in.size(),0.05);
for(sim_time = 0.0; sim_time < sim_end_time; sim_time += sim_step){
cout << sim_time/*drive.mot->state.vel*/ << ", " << in[i] << ", " << out[i] << ", " << foo[i] << endl;
i++;
}
system("gnuplot --persist gp");
return(0);
}
Reference in New Issue View Git Blame Copy Permalink