controller_init.cpp

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

#define S_FUNCTION_NAME controller_init

#include "ttkernel.cpp"

// Event-driven controller node 

class PD_Data {
public:
  // ctrl params
  double K, Td, N, h, ad, bd;
  
  // ctrl states
  double yold, Dold, u;
};

// controller code function
double ctrl_code(int seg, void *data)
{
  double *m;
  double r, y, P, D;
  PD_Data* d = (PD_Data*) data;

  switch(seg) {
  case 1:
    ttWait("packet");
    return 0.0;

  case 2:
    m = (double*) ttGetMsg(1); // get sensor value on network 1
    y = *m;
    delete m; // delete message
    r = ttAnalogIn(1);
    
    P = d->K*(r-y);
    D = d->ad*d->Dold + d->bd*(d->yold-y);
    d->u = P + D;
    d->Dold = D;
    d->yold = y;
    return 0.0005;

  case 3:
    m = new double;
    *m = d->u;
    ttSendMsg(1, 2, m, 10); // Send 10 bytes to node 2 (actuator) on network 1
    ttSetNextSegment(1); // loop and wait for new packet
    return 0.0;
  }
}

// interfering task code function
double dummy_code(int seg, void *data)
{
  switch (seg) {
  case 1:
    return 0.004;
  case 2:
    return FINISHED;
  }
}

double msgRcvhandler(int seg, void *data)
{
  ttNotifyAll("packet");
  return FINISHED;
}


PD_Data* data;

void init() {

  ttInitKernel(1, 0, FP);
  
  data = new PD_Data;
  data->K = 1.5;
  data->Td = 0.035;
  data->N = 100000.0;
  data->h = 0.010;
  data->ad = data->Td/(data->N*data->h+data->Td);
  data->bd = data->N*data->K*data->Td/(data->N*data->h+data->Td);
  data->yold = 0.0;
  data->Dold = 0.0;
  data->u = 0.0;

  // Controller task
  ttCreateTask("pid_task", 0.006, 10, ctrl_code, data);
  ttCreateJob(0.0, "pid_task");

  // Disturbing task
  // ttCreatePeriodicTask("dummy", 0.0002, 0.007, 1, dummy_code);

  // Initialize network
  ttCreateInterruptHandler("msgRcv", 1, msgRcvhandler);
  ttInitNetwork(4, "msgRcv"); // I am node 4!
  ttCreateEvent("packet");
}

void cleanup() {

  delete data;
}