// ---- 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; } }