Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
1abf2d330This file is pinned to the selected release. Download the original to open it in your design tool.
#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);
}