Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-02-06 02:02:34 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

zeug

Browse Source
This commit is contained in:
crinq
2014-10-01 19:39:19 +02:00
parent 8819c0275d
commit 058cd61d0f
3 changed files with 42 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
sim/gp
View File

@@ -1,5 +1,5 @@
set term x1
set key autotitle columnheader
set yrange [-3:100]
#set yrange [-3:400]
#set xrange [0.245:0.4]
plot "plot" every 1 using 1:2 w l, "plot" every 1 using 1:3 w l, "plot" every 1 using 1:4 w l, "plot" every 1 using 1:5 w l, "plot" every 1 using 1:6 w l, "plot" every 1 using 1:7 w l, "plot" every 1 using 1:8 w l, "plot" every 1 using 1:9 w l, "plot" every 1 using 1:10 w l

49
sim/sim.cpp
View File

@@ -50,7 +50,7 @@ int main(){
mot2.mech_spec.damping = 0.004236;
mot2.mech_spec.inertia = 0.0012;
mot2.elec_spec.max_i = 12;
mot2.elec_spec.max_i = 60;
mot2.elec_spec.i = 10;
mot2.elec_spec.nm_a = 0.36;
mot2.elec_spec.r = 0.67;
@@ -115,19 +115,19 @@ int main(){
cmd.acc_res = 0.01;
drive_c drive;
drive.dc = 110;
drive.dc = 150;
drive.pwm_scale = 0.9;
drive.pwm_res = 8400;
drive.pid_periode = 0.001;
drive.mot = &mot3;
drive.mot = &mot;
drive.in = &cmd;
drive.input_cmd = input_cmd;
drive.input_feedback = input_feedback_real;
drive.pid = pid;
drive.output = output;
drive.reset();
/*
/*
double ind = 0.0;
double max_volt_pos = 0.0;
double max_cur_pos = 0.0;
@@ -199,14 +199,21 @@ int main(){
//v0
//v1
//-> a
double dp;
double v0;
double v1;
double a;
//v = 1/a * maxa * exp(-sim_time * a)
//e_pos = 30 * torq / drive.est.inertia * (sim_time - 0.15) * (sim_time - 0.15) + 1;
//e_pos = CLAMP(torq / drive.est.inertia * 0.8 + torq / drive.est.inertia * (-22) * sim_time * 0.5, 0, torq / drive.est.inertia);
cout << sim_time << ", " << vel << ", " << max_cur_pos << ", " << endl;//", " << max_acc_pos << ", " << max_acc_neg << ", " << e_pos << endl;
cout << sim_time << ", " << vel << ", " << max_acc_pos << ", " << e_pos << ", " << pos << endl;//", " << max_acc_pos << ", " << max_acc_neg << ", " << e_pos << endl;
sim_time += sim_step;
}
@@ -215,11 +222,33 @@ int main(){
return(0);
*/
//cout << flush << "time cmd pos est_pos sin_avg sin_scale cos_avg cos_scale" << endl;
cout << flush << "time curr acc vel" << endl;
cout << flush << "time curr acc vel acc0 acc1 y" << 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;
for(sim_time = 0.0; sim_time < sim_end_time; sim_time += sim_step){
//drive.in->step(sim_step);
//if(count == 0){
@@ -231,10 +260,14 @@ int main(){
drive.output(&drive, sim_step);
drive.mot->step(sim_step);
//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.cur << ", " << drive.mot->state.acc / 100 << ", " << drive.mot->state.vel << endl;
cout << sim_time/*drive.mot->state.vel*/ << ", " << drive.mot->state.cur << ", " << drive.mot->state.acc << ", " << drive.mot->state.vel << ", " << acc0 << ", " << acc1 << ", " << y << endl;
}
system("gnuplot --persist gp");

1
sim/sim.h
View File

@@ -167,7 +167,6 @@ void mot_c::step(double periode){
}
state.ind = state.vel * elec_spec.v_rps;
double v = (state.volt - state.ind);
state.cur += (state.volt - state.ind - state.cur * elec_spec.r) / elec_spec.l * periode;
state.cur = CLAMP(state.cur, -elec_spec.max_i, elec_spec.max_i);
//state.cur = (state.volt - state.ind) / elec_spec.r;