// ---- PID code function for Implementation 3 ----
double pid_code3(int seg, void* data) {
double r, y;
TaskData* d = (TaskData*) data;
switch (seg) {
case 1:
d->t = ttCurrentTime();
return 0.0;
case 2:
r = ttAnalogIn(d->rChan);
y = ttAnalogIn(d->yChan);
pidcalc(d, r, y);
return 0.002;
case 3:
ttAnalogOut(d->uChan, d->u);
// Sleep
d->t += d->h;
ttSleepUntil(d->t);
return 0.0;
default:
ttSetNextSegment(2); // loop
return 0.0;
}
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
rtsys = getrtsys(); // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs != 1) {
TT_MEX_ERROR("Wrong number of input arguments!\nUsage: ttAnalogIn(inpChan)");
return;
}
if (!mxIsDoubleScalar(prhs[0])) {
TT_MEX_ERROR("ttAnalogIn: inpChan must be a number");
return;
}
int inpChan = (int) *mxGetPr(prhs[0]);
double retval = ttAnalogIn(inpChan);
plhs[0] = mxCreateDoubleScalar(retval);
}
// ---- PID code function for Implementation 1 ----
double pid_code1(int seg, void* data) {
double r, y;
TaskData* d = (TaskData*) data;
switch (seg) {
case 1:
r = ttAnalogIn(d->rChan);
y = ttAnalogIn(d->yChan);
pidcalc(d, r, y);
return 0.002;
default:
ttAnalogOut(d->uChan, d->u);
return FINISHED;
}
}
// 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:
ttTake("sem"); // wait for a message
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, 80); // Send 80 bits to node 2 (actuator)
ttSetNextSegment(1); // loop and wait for new packet
return 0.0;
}
return FINISHED; // to supress compilation warnings
}
// ---- PID code function for Implementation 2 ----
double pid_code2(int seg, void* data) {
double inp[2]; // controller block diagram inputs
double outp[2]; // controller block diagram outputs
double *d2 = (double *)data;
switch (seg) {
case 1:
inp[0] = ttAnalogIn(1);
inp[1] = ttAnalogIn(2);
ttCallBlockSystem(2, outp, 2, inp, "controller");
*d2 = outp[0]; // store control signal
return outp[1]; // execution time returned from block
default:
ttAnalogOut(1, *d2);
return FINISHED;
}
}
// ---- Sampler code function for Implementation 4 ----
double sampler_code(int seg, void* data) {
double y;
int* d = (int*) data;
switch (seg) {
case 1:
y = ttAnalogIn(*d);
ttTryPost("Samples", new double(y)); // put sample in mailbox
ttCreateJob("pid_task"); // trigger task job
return 0.0002;
default:
return FINISHED;
}
}
// code function
double sensor_code(int seg, void *data) {
static double *y;
switch (seg) {
case 1:
y = new double;
*y = ttAnalogIn(1);
return 0.0005;
case 2:
ttSendMsg(3, y, 80); // Send measurement (80 bits) to node 3 (controller)
return 0.0004;
case 3:
return FINISHED;
}
return FINISHED; // to supress compilation warnings
}
// ---- PID code function for Implementation 4 ----
double pid_code4(int seg, void* data) {
double r;
double *y;
TaskData* d = (TaskData*) data;
switch (seg) {
case 1:
r = ttAnalogIn(d->rChan);
y = (double*) ttTryFetch("Samples");
pidcalc(d, r, *y);
delete y;
return 0.0018;
default:
ttAnalogOut(d->uChan, d->u);
return FINISHED;
}
}
