www.pudn.com > truetime-1.2-compiled.zip > controller_init.cpp

// Distributed control system: controller node
//
// Receives messages from the sensor node, computes control signal
// and sends it to the actuator node. Also contains a high-priority
// disturbing task.

#define S_FUNCTION_NAME controller_init

#include "ttkernel.cpp"

// PID data structure
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(); // get sensor value
    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(2, m, 10); // Send 10 bytes to node 2 (actuator)
    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() {

  // Initialize TrueTime kernel  
  ttInitKernel(1, 0, FP); // nbrOfInputs, nbrOfOutputs, fixed priority
  
  // Create task data (local memory)
  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
  double deadline = 0.010;
  double prio = 2.0;
  ttCreateTask("pid_task", deadline, prio, ctrl_code, data);
  ttCreateJob("pid_task");

  // Disturbance task (uncomment to add disturbance task)
  // double offset = 0.0002;
  // double period = 0.007;
  // prio = 1.0;
  // ttCreatePeriodicTask("dummy", offset, period, prio, dummy_code);

  // Initialize network
  ttCreateInterruptHandler("msgRcv", prio, msgRcvhandler);
  ttInitNetwork(4, "msgRcv"); // node #4 in the network
  
  ttCreateEvent("packet");
}

void cleanup() {

  delete data;
}