+l

sim/gp

@@ -1,4 +1,5 @@
 set term x1
 set key autotitle columnheader
-set yrange [-30:50]
-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
+set yrange [-3:12]
+#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

sim/sim.cpp

@@ -4,7 +4,7 @@
 int main(){
   float sim_time = 0.0;
   float sim_step = 0.00001;
-  float sim_end_time = 0.2;
+  float sim_end_time = 0.03;
 
   srand(14235);
 
@@ -14,14 +14,15 @@ int main(){
   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;
-  mot.mech_spec.damping = 0.0000426;
-  mot.mech_spec.inertia = 0.0000268;
+  mot.mech_spec.friction = 0.0;//21;//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;
 
@@ -42,7 +43,7 @@ int main(){
 
   cmd_c cmd;
   cmd.reset();
-  cmd.periode = 0.2;
+  cmd.periode = 0.5;
   cmd.amplitude = 2;
   cmd.wave = cmd_c::SQUARE;
   cmd.type = cmd_c::POS;
@@ -51,37 +52,124 @@ int main(){
   cmd.acc_res = 0.01;
 
   drive_c drive;
-  drive.reset();
-  drive.dc = 50;
+  drive.dc = 5;
   drive.pwm_scale = 0.9;
   drive.pwm_res = 8400;
   drive.pid_periode = 0.001;
   drive.mot = &mot;
   drive.in = &cmd;
-  drive.input = input;
+  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;
+  double max_torq_pos = 0.0;
+  double max_acc_pos = 0.0;
+  double max_dvel_pos = 0.0;
 
+  double max_volt_neg = 0.0;
+  double max_cur_neg = 0.0;
+  double max_torq_neg = 0.0;
+  double max_acc_neg = -2;
+  double max_dvel_neg = 0.0;
+
+  double vel = 0.0;
+  double pos = 0.0;
+
+  int dir = 1;
+
+  double e_pos = 0.0;
+  double e_neg = 0.0;
+  drive.est.load = 0.0;
+
+  double volt = 0.0;
+  double cur = 0.0;
+  double torq = 0.0;
+
+  cout << flush << "time vel accp accn e" << endl;
+
+  while(vel >= -mot.mech_spec.max_rps){
+    ind = vel * drive.mot->elec_spec.v_rps;
+
+    max_volt_pos = (drive.dc * drive.pwm_scale - drive.dc * (1 - drive.pwm_scale)) - ind;
+    max_cur_pos += (max_volt_pos - max_cur_pos * mot.elec_spec.r) / mot.elec_spec.l * sim_step;
+    //max_cur_pos = max_volt_pos / drive.mot->elec_spec.r;
+    max_torq_pos = max_cur_pos * drive.mot->elec_spec.nm_a;
+    max_torq_pos -= drive.est.friction;
+    max_torq_pos -= drive.est.load;
+    max_torq_pos -= drive.est.damping * vel;
+    max_acc_pos = max_torq_pos / drive.est.inertia;
+
+    max_volt_neg = (drive.dc * (1 - drive.pwm_scale) - drive.dc * drive.pwm_scale) - ind;
+    max_cur_neg += (max_volt_neg - max_cur_neg * mot.elec_spec.r) / mot.elec_spec.l * sim_step;
+    //max_cur_neg = max_volt_neg / drive.mot->elec_spec.r;
+    max_torq_neg = max_cur_neg * drive.mot->elec_spec.nm_a;
+    max_torq_neg += drive.est.friction;
+    max_torq_neg -= drive.est.load;
+    max_torq_neg -= drive.est.damping * vel;
+    max_acc_neg = max_torq_neg / drive.est.inertia;
+
+    if(dir == 1){
+      vel += max_acc_pos * sim_step;
+    }
+    else {
+      vel += max_acc_neg * sim_step;
+    }
+    pos += vel * sim_step;
+
+    if(vel >= mot.mech_spec.max_rps){
+      dir = -1;
+    }
+
+    volt = drive.dc * drive.pwm_scale - drive.dc * (1 - drive.pwm_scale);
+    cur = volt / drive.mot->elec_spec.r;
+    torq = cur * drive.mot->elec_spec.nm_a;
+    //e_pos = torq / drive.est.inertia * exp(-sim_time * drive.dc / drive.mot->elec_spec.v_rps );
+    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);
+
+    //dp
+    //v0
+    //v1
+
+    //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;
+
+    sim_time += sim_step;
+  }
+  system("gnuplot --persist gp");
+
+  return(0);
+*/
   //cout << flush << "time cmd pos est_pos sin_avg sin_scale cos_avg cos_scale" << endl;
-  cout << flush << "time cmd ctr volt ind cur vel pos err" << endl;
+  cout << flush << "time curr acc vel" << endl;
 
   int count = 0;
   int pid_count = drive.pid_periode / sim_step;
 
   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.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);
 
     //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 << ", " << drive.in->get_pos() * 5 - 15<< ", " << drive.state.ctr << ", " << drive.mot->state.volt << ", " << drive.mot->state.ind << ", " << drive.mot->state.cur << ", " << drive.mot->state.vel << ", " << drive.mot->state.pos * 5 - 15 << ", " << minus_(drive.in->get_pos(), drive.est.pos) * (-100) - 15 << 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 << ", " << mot.state.cur << ", " << mot.state.acc / 100 << ", " << mot.state.vel / 10 << endl;
   }
 
   system("gnuplot --persist gp");

sim/sim.h

@@ -83,6 +83,7 @@ public:
     double max_i;
     double i;
     double r;
+    double l;
     double nm_a;
     double v_rps;
     double slip;
@@ -166,7 +167,9 @@ void mot_c::step(double periode){
   }
 
   state.ind = state.vel * elec_spec.v_rps;
-  state.cur = (state.volt - state.ind) / elec_spec.r;
+  double v = (state.volt - state.ind);
+  state.cur += (state.volt - state.ind - state.cur * elec_spec.r) / elec_spec.l * periode;
+  //state.cur = (state.volt - state.ind) / elec_spec.r;
   state.torq = state.cur * elec_spec.nm_a - (mech_spec.damping + load.damping) * state.vel + load.load;
   if(abs(state.torq) < mech_spec.friction + load.friction){
     state.torq = 0.0;
@@ -323,6 +326,11 @@ public:
     double acc;
     double cur;
 
+    double friction;
+    double load;
+    double damping;
+    double inertia;
+
     double p;
     double v;
     double a;
@@ -340,7 +348,8 @@ public:
   void reset();
   void step(double periode);
 
-  void (*input)(drive_c* drv, double periode);
+  void (*input_cmd)(drive_c* drv, double periode);
+  void (*input_feedback)(drive_c* drv, double periode);
   void (*pid)(drive_c* drv, double periode);
   void (*output)(drive_c* drv, double periode);
 
@@ -363,6 +372,11 @@ void drive_c::reset(){
   est.cos_avg = 0.0;
   est.res_var = 0.0;
 
+  est.friction = mot->mech_spec.friction;
+  est.load = 0.0;
+  est.damping = mot->mech_spec.damping;
+  est.inertia = mot->mech_spec.inertia;
+
   est.pos = 0.0;
   est.vel = 0.0;
   est.acc = 0.0;
@@ -374,12 +388,48 @@ void drive_c::reset(){
 }
 
 void drive_c::step(double periode){
-  input(this, periode);
+  input_cmd(this, periode);
+  input_feedback(this, periode);
   pid(this, periode);
   output(this, periode);
 }
 
-void input(drive_c* drv, double periode){
+void input_cmd(drive_c* drv, double periode){
+  double tp, tv;
+
+  tp = drv->cmd.pos;
+  tv = drv->cmd.vel;
+
+  switch(drv->in->type){
+    case cmd_c::POS:
+      drv->cmd.pos = drv->in->get_pos();
+      drv->cmd.vel = (drv->cmd.pos - tp) / periode;
+      drv->cmd.acc = (drv->cmd.vel - tv) / periode;
+      break;
+    case cmd_c::POS_VEL:
+      drv->cmd.pos = drv->in->get_pos();
+      drv->cmd.vel = drv->in->get_vel();;
+      drv->cmd.acc = (drv->cmd.vel - tv) / periode;
+      break;
+    case cmd_c::POS_VEL_ACC:
+      drv->cmd.pos = drv->in->get_pos();
+      drv->cmd.vel = drv->in->get_vel();;
+      drv->cmd.acc = drv->in->get_acc();
+      break;
+    case cmd_c::VEL:
+      drv->cmd.pos = drv->cmd.pos + drv->cmd.vel * periode;
+      drv->cmd.vel = drv->in->get_vel();;
+      drv->cmd.acc = (drv->cmd.vel - tv) / periode;
+      break;
+    case cmd_c::VEL_ACC:
+      drv->cmd.pos = drv->cmd.pos + drv->cmd.vel * periode;
+      drv->cmd.vel = drv->in->get_vel();;
+      drv->cmd.acc = drv->in->get_acc();
+      break;
+  }
+}
+
+void input_feedback(drive_c* drv, double periode){
   double t1, t2;
   static double sin_avg_amp = 1.0;
   static double cos_avg_amp = 1.0;
@@ -397,9 +447,6 @@ void input(drive_c* drv, double periode){
   static double vel_hist[hist_size];
   static double acc_hist[hist_size];
 
-
-
-
   t1 = drv->est.pos;
 
   switch(drv->mot->feedback.type){
@@ -480,44 +527,22 @@ void input(drive_c* drv, double periode){
   // t2 = drv->est.vel;
   // drv->est.vel = (drv->est.pos - t1) / periode;
   // drv->est.acc = (drv->est.vel - t2) / periode;
+}
 
-  switch(drv->in->type){
-    case cmd_c::POS:
-      drv->cmd.pos = drv->in->get_pos();
-      drv->cmd.vel = 0.0;
-      drv->cmd.acc = 0.0;
-      break;
-    case cmd_c::POS_VEL:
-      drv->cmd.pos = drv->in->get_pos();
-      drv->cmd.vel = drv->in->get_vel();;
-      drv->cmd.acc = 0.0;
-      break;
-    case cmd_c::POS_VEL_ACC:
-      drv->cmd.pos = drv->in->get_pos();
-      drv->cmd.vel = drv->in->get_vel();;
-      drv->cmd.acc = drv->in->get_acc();
-      break;
-    case cmd_c::VEL:
-      drv->cmd.pos = 0.0;
-      drv->cmd.vel = drv->in->get_vel();;
-      drv->cmd.acc = 0.0;
-      break;
-    case cmd_c::VEL_ACC:
-      drv->cmd.pos = 0.0;
-      drv->cmd.vel = drv->in->get_vel();;
-      drv->cmd.acc = drv->in->get_acc();
-      break;
-  }
+void input_feedback_real(drive_c* drv, double periode){
+  drv->est.pos = drv->mot->state.pos;
+  drv->est.vel = drv->mot->state.vel;
+  drv->est.acc = drv->mot->state.acc;
 }
 
 void pid(drive_c* drv, double periode){
-  double p = 10;              // kp
-  double i = 4 * periode;    // ki
-  double d = 0.01 / periode; // kd
+  double p = 180;              // kp
+  double i = 15000 * periode;    // ki
+  double d = 0.1 / periode; // kd
   double dd = 0.0 / periode;  // rel. vel. kd
-  double in = 0.1;            // ind. kp
+  double in = 0.0;            // ind. kp
 
-  double dc_scale = 50.0 / drv->dc; // dc voltage scale
+  double dc_scale = 1.0 / drv->dc; // dc voltage scale
   double ind = drv->mot->state.vel * drv->mot->elec_spec.v_rps / drv->dc;
 
   double e = minus_(drv->cmd.pos, drv->est.pos);
@@ -525,6 +550,7 @@ void pid(drive_c* drv, double periode){
   static double i_sum = 0;
 
   i_sum += e;
+  //i_sum -=
   drv->state.ctr = p * e;
   drv->state.ctr += i * i_sum;
   drv->state.ctr += d * (e - e_old);
@@ -535,14 +561,43 @@ void pid(drive_c* drv, double periode){
   drv->state.ctr += in * ind;
 
   drv->state.ctr = CLAMP(drv->state.ctr, -1, 1);
+  // if(ABS(drv->est.vel) >= drv->mot->mech_spec.max_rps * 0.7){
+  //   drv->state.ctr = 0.0;
+  // }
 
   if(abs(drv->state.ctr) >= 0.99){
     i_sum -= e;
   }
+  i_sum = CLAMP(i_sum, -1/i, 1/i);
 
   e_old = e;
 }
 
+void pid2(drive_c* drv, double periode){
+  double ind = drv->est.vel * drv->mot->elec_spec.v_rps;
+  double max_volt_pos = drv->dc * drv->pwm_scale - ind;
+  double max_cur_pos = max_volt_pos / drv->mot->elec_spec.r;
+  double max_torq_pos = max_cur_pos * drv->mot->elec_spec.nm_a;
+  max_torq_pos -= drv->est.friction;
+  max_torq_pos -= drv->est.load;
+  max_torq_pos -= drv->est.damping * drv->est.vel;
+  double max_acc_pos = max_torq_pos / drv->est.inertia;
+  double max_dvel_pos = max_acc_pos * periode;
+
+  double max_volt_neg = -drv->dc * drv->pwm_scale - ind;
+  double max_cur_neg = max_volt_neg / drv->mot->elec_spec.r;
+  double max_torq_neg = max_cur_neg * drv->mot->elec_spec.nm_a;
+  max_torq_neg -= drv->est.friction;
+  max_torq_neg += drv->est.load;
+  max_torq_neg -= drv->est.damping * drv->est.vel;
+  double max_acc_neg = max_torq_neg / drv->est.inertia;
+  double max_dvel_neg = max_acc_neg * periode;
+
+
+
+  drv->state.ctr = 0.0;
+}
+
 void output(drive_c* drv, double periode){
   double u, v, w;