threeservos_init.cpp

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

#define S_FUNCTION_NAME threeservos_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;
};


// --------- Generic 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 6
#define NBROFOUTPUTS 3
#define DISPATCHER RM

double periods[] = {0.006, 0.005, 0.004};
char* names[] = {"pid_task1", "pid_task2", "pid_task3"};
int rChans[] = {1, 3, 5};
int yChans[] = {2, 4, 6};
int uChans[] = {1, 2, 3};
PID_Data *d[3];

void init() {

  ttInitKernel(NBROFINPUTS,NBROFOUTPUTS,DISPATCHER);

  for (int i = 0; i < 3; i++) {
    d[i] = new PID_Data;
    d[i]->p.K = 0.96;
    d[i]->p.Ti = 0.12;
    d[i]->p.Td = 0.049;
    d[i]->p.N = 10;
    d[i]->p.h = periods[i];
    d[i]->s.u = 0.0;
    d[i]->s.Iold = 0.0;
    d[i]->s.Dold = 0.0;
    d[i]->s.yold = 0.0;
    d[i]->p.rChan = rChans[i];
    d[i]->p.yChan = yChans[i];
    d[i]->p.uChan = uChans[i];

    ttCreatePeriodicTask(names[i], 0.0, periods[i], 5, pid, d[i]);
  }
}

void cleanup() {

  for (int i = 0; i < 3; i++) {
    delete d[i];
  }
}