#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 signalvoid 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 FPPID_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;}