singleservo_init.cpp

一个很棒的网络控制系统仿真软件

#define S_FUNCTION_NAME singleservo_init

#include "ttkernel.cpp"

class PID_Data {
public:
  struct { // states
    double u, Iold, Dold, yold; 
  } s;
  
  struct { // params
    double K, Ti, Td, N, h;
    int rChan, yChan, uChan; 
  } p;
};

// calculate control signal
void pidcalc(PID_Data* d, double r, double y) {
  double P = d->p.K*(r-y);
  double I = d->s.Iold;
  double D = d->p.Td/(d->p.N*d->p.h+d->p.Td)*d->s.Dold+d->p.N*d->p.K*d->p.Td/(d->p.N*d->p.h+d->p.Td)*(d->s.yold-y); 

  d->s.u = P + I + D;
  d->s.Iold = d->s.Iold + d->p.K*d->p.h/d->p.Ti*(r-y);
  d->s.Dold = D;
  d->s.yold = y;
};


// --------- Code function ----------
double pid(int seg, void* data) {

  PID_Data* d = (PID_Data*) data;

  switch (seg) {
  case 1:  
    pidcalc(d, ttAnalogIn(d->p.rChan), ttAnalogIn(d->p.yChan)); 
    return 0.002;
  case 2:    
    ttAnalogOut(d->p.uChan, d->s.u);
    return FINISHED;
  }
}

#define NBROFINPUTS 2
#define NBROFOUTPUTS 1
#define DISPATCHER FP

PID_Data *d;

void init() {
  
  ttInitKernel(NBROFINPUTS,NBROFOUTPUTS,DISPATCHER);

  d = new PID_Data;
  d->p.K = 0.96;
  d->p.Ti = 0.12;
  d->p.Td = 0.049;
  d->p.N = 10;
  d->p.h = 0.006;
  d->s.u = 0.0;
  d->s.Iold = 0.0;
  d->s.Dold = 0.0;
  d->s.yold = 0.0;
  d->p.rChan = 1;
  d->p.yChan = 2;
  d->p.uChan = 1;

  ttCreatePeriodicTask("pid_task", 0.0, 0.006, 2, pid, d);
}

void cleanup() {
 
  delete d;
}