int DaqDevice::readData(int subdev,int channel,int range_idx,int aref){
if (comedi_data_read(_dev, subdev, channel, range_idx, aref, &_an_input[channel]) < 0){
return COMEDI_ERROR;
} else {
return COMEDI_OK;
}
}
double readpin(int chan)
{
comedi_t *it;
lsampl_t data, maxdata;
comedi_range *rang;
int readres;
double outval;
if((it=comedi_open("/dev/comedi0"))==NULL) {
printf("fail open");
}
comedi_set_global_oor_behavior(COMEDI_OOR_NUMBER);
readres=comedi_data_read(it,subdev,chan,0,aref, & data);
if(!readres) {
comedi_perror("comedi_data_read: ");
}
rang = comedi_get_range(it, subdev, chan, 0);
maxdata = comedi_get_maxdata(it, subdev, chan);
outval = comedi_to_phys(data, rang, maxdata);
printf("data=%d\noutval=%g\nreadres=%d\n",data, outval, readres);
return outval;
}
int DaqDevice::readCounter(int subdev,int channel)
{
if (comedi_data_read(_dev, subdev, channel, 0, 0, &_counter[channel]) < 0){
return COMEDI_ERROR;
} else {
return COMEDI_OK;
}
}
/**
* @brief read_sample reads the sapmle from the input channel; corrects it using soft calibration and returns the true value
* @return softcalibrated value
*/
lsampl_t read_sample()
{
lsampl_t data;
comedi_data_read(it, subdevice,Channel,0,0,&data);
double d=TheSlope*data;
d+=TheIntercept;
return d;
}
//READ THE data from the card here
int32 ATIDAQHardwareInterface::ReadSingleSample( float64 buffer[] )
{
int32 retVal = 0; /*the return code*/
unsigned long numRawSamples; /*the number of raw gauge value samples*/
numRawSamples = m_uiNumChannels * m_uiAveragingSize;
unsigned int i, j; /*loop/array indices*/
float64 * rawBuffer = new float64[numRawSamples]; /*the buffer which holds the raw, unaveraged gauge values*/
int res = 0;
// retVal = comedi_do_insnlist(comediDev,&il);
// if(retVal < 0){
// comedi_perror("");
// return retVal;
// }
for (unsigned int ch = 0; ch < m_uiNumChannels; ch++) {
// buffer[ch] = comedi_to_phys(dataArray[ch], &range, maxdata);
// old single read
res = comedi_data_read(comediDev, 0, ch, 0, AREF_DIFF , &data);
// if (res == 1){
buffer[ch] = comedi_to_phys(data, &range, maxdata);
}
delete []rawBuffer;
return retVal;
// float64 timeOut = ( numRawSamples / m_f64SamplingFrequency ) + 1; /*allow a full second
// for Windows timing inaccuracies (definitely overkill, but whatever)*/
// int32 read; /*number samples read*/
// retVal = DAQmxReadAnalogF64( *m_thDAQTask, m_uiAveragingSize, timeOut, DAQmx_Val_GroupByScanNumber,
// rawBuffer, numRawSamples, &read, NULL );
//
// for ( i = 0; i < m_uiNumChannels; i++ )
// {
// /*sum the raw values, storing the sum in the first raw data point*/
// for ( j = 1; j < m_uiAveragingSize; j++ )
// {
// rawBuffer[i] += rawBuffer[ i + ( j * m_uiNumChannels ) ];
// }
// /*store the average values in the output buffer*/
// buffer[i] = rawBuffer[i] / m_uiAveragingSize;
// }
//
// delete []rawBuffer; /*gotta keep up with those unmanaged pointers*/
}
void com_data_read(unsigned int index, uint64_t* value)
{
lsampl_t data;
comedi_data_read(it,
Comedi_subdev_ai,
index,
0,//unsigned int range,
AREF_GROUND,//unsigned int aref,
&data);
*value = (uint64_t)data;
}
int
main ( int argc, char *argb[] ) {
comedi_t *it;
lsampl_t data;
it = comedi_open("/dev/comedi0");
int i = 0;
for ( i = 0; i < 16; i++ ) {
comedi_data_read(it, subdev, i, range, aref, &data);
printf("%d\n", data);
}
}
double PIDFlowController::readVolts(bool *ok) const
{
lsampl_t samp = 0;
if (ok) *ok = true;
if (daq_ai) {
samp = daq_ai->getSample(params.chan_ai);
} else if ( comedi_data_read(dev_ai, params.subdev_ai, params.chan_ai, 0, 0, &samp) < 1 ) {
if (ok) *ok = false;
return 0.0;
}
return ais2v(samp);
}
static void data_read( void )
{
comedi_t *dev = lookup_dev( *sp++ );
unsigned int subdev = *sp++;
unsigned int chan = *sp++;
unsigned int range = *sp++;
unsigned int aref = *sp++;
lsampl_t data;
int c = comedi_data_read( dev, subdev, chan, range, aref, &data );
if( c < 1 ) comedi_perror( "@daq " );
*--sp = data;
}
int get_dio_bit(int chan) {
lsampl_t data;
int retval;
DIO_ERROR = FALSE;
retval = comedi_data_read(it, subdev_dio, chan, range_dio, aref_dio, &data);
if (retval < 0) {
comedi_perror("comedi_data_read in get_dio_bits");
DIO_ERROR = TRUE;
return 0;
}
if (data != 0) data = 1;
return data;
}
int comedi_data_read_delayed(comedi_t * dev, unsigned int subdev,
unsigned int chan, unsigned int range, unsigned int aref,
lsampl_t * data, unsigned int nano_sec)
{
int retval;
retval = comedi_data_read_hint(dev, subdev, chan, range, aref);
if (retval < 0)
return retval;
comedi_udelay((nano_sec + 999) / 1000);
return comedi_data_read(dev, subdev, chan, range, aref, data);
}
double ComediChan::dataRead(bool *ok) const
{
lsampl_t samp;
bool myok = true;
double ret = 0.;
if (daq) {
samp = daq->readSample(m_chan, &myok);
} else if ( comedi_data_read(dev(), m_subdev, m_chan, m_range, 0, &samp) < 1 ) {
err = "Read error.";
myok = false;
return 0.;
}
if (!myok) err = "Read error.";
else ret = (samp/static_cast<double>(m_maxdata) * (m_rangeMax-m_rangeMin)) + m_rangeMin;
if (ok) *ok = myok;
return ret;
}
double read_volts(int subdev, int chan, int range) {
lsampl_t data, maxdata;
int readres;
comedi_range *rang;
double outval;
comedi_set_global_oor_behavior(COMEDI_OOR_NUMBER);
readres=comedi_data_read(it,subdev,chan,range,aref, & data);
if(!readres) {
comedi_perror("comedi_data_read: ");
}
rang = comedi_get_range(it, subdev, chan, range);
maxdata = comedi_get_maxdata(it, subdev, chan);
outval = comedi_to_phys(data, rang, maxdata);
//printf("data=%d\noutval=%g\nreadres=%d\n",data, outval, readres);
return outval;
}
int setup_read_wave(int subd, int chan, int range, int npnts) {
// assert(it!=NULL);
printf("setup_read_wave...");fflush(stdout);
int ret;
int wv_n=++cmd->chanlist_len, wv_i=wv_n-1;
lsampl_t data;
//printf("wv_i %d \n", wv_i);
///assert(cmd!=NULL);
if(cmd->chanlist==NULL) {
cmd->chanlist=malloc(sizeof(unsigned int));
} else {
if(realloc(cmd->chanlist, (wv_n)*sizeof(unsigned int))==NULL)
{ printf("realloc cmd fail \n"); return -1;}
}
cmd->chanlist[wv_i]= CR_PACK(chan,range,aref);
if(npnts>cmd->stop_arg) cmd->stop_arg=npnts;
cmd->scan_end_arg=wv_n;
//test_board_read(it);
comedi_command_test(it,cmd);
ret=comedi_command_test(it,cmd);
if(ret!=0){
comedi_perror("comedi_command_test fail on read");
return -1;
}
//test_board_read(it);
inp_res[wv_i]=malloc(npnts*sizeof(double));
inp_rang[wv_i]=comedi_get_range(it, subd, chan, range);
inp_maxdata[wv_i]=comedi_get_maxdata(it, subd, chan);
comedi_data_read(it,subd,chan,range,aref, & data);
//printf("found sdtype %d \n", comedi_find_subdevice_by_type(it, COMEDI_SUBD_AI,0));
//printf("setup subd %d chan %d rng %d: data=%d outval=%g\n",subd, chan, range, data, comedi_to_phys(data, inp_rang[wv_i], inp_maxdata[wv_i]));
printf("done\n");fflush(stdout);
return wv_i;
}
static void inout(scicos_block *block)
{
struct ADCOMDev * comdev = (struct ADCOMDev *) (*block->work);
lsampl_t data;
double x = 0;
double term = 1;
double *y = block->outptr[0];
comedi_data_read(comdev->dev, comdev->subdev, comdev->channel, comdev->range, comdev->aref, &data);
if (comdev->use_softcal) {
unsigned i;
for(i = 0; i <= 4; ++i) {
x += comdev->coefficients[i+1] * term;
term *= data - comdev->coefficients[0];
}
} else {
x = comdev->range_min + data*(comdev->range_max - comdev->range_min)/comdev->maxdata;
}
y[0] = x;
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
double *y = ssGetOutputPortRealSignal(S,0);
#ifndef MATLAB_MEX_FILE
unsigned int channel = (unsigned int)COMEDI_CHANNEL;
unsigned int range = (unsigned int)COMEDI_RANGE;
unsigned int aref = (unsigned int)COMEDI_REFERENCE - 1;
void *dev = (void *)ssGetPWork(S)[0];
int subdev = ssGetIWork(S)[0];
double range_min = ssGetRWork(S)[0];
double range_max = ssGetRWork(S)[1];
lsampl_t data, maxdata = comedi_get_maxdata(dev, subdev, COMEDI_CHANNEL);
double x;
comedi_data_read(dev, subdev, channel, range, aref, &data);
x = data;
x /= maxdata;
x *= (range_max - range_min);
x += range_min;
*y = x;
#endif
}
double ComediAnalogInputHardCal::read()
{
lsampl_t sample;
comedi_data_read(m_device, m_subdevice, m_channels[0], m_range, m_aref, &sample);
return comedi_to_phys(sample, m_dataRange, m_maxData) * m_inputConversionFactor;
}
double ComediAnalogInputSoftCal::read()
{
lsampl_t sample;
comedi_data_read(m_device, m_subdevice, m_channels[0], m_range, m_aref, &sample);
return comedi_to_physical(sample, &m_converter) * m_inputConversionFactor;
}
/* -------------------------------------------------------------------- */
static long dsetRead_devAiSyncComedi(aiRecord *pai)
{
CONTEXT *p_myContext;
int mySubdevice;
int myChannel;
int myRange = 0;
int myAnalogReference;
int myPinNumber;
char p_myDeviceFile[BUFLEN];
comedi_t *p_myComediFileHandle;
lsampl_t data;
int maxdata;
int ret;
dsetLog(3, __FILE__ "[%d] -> %s(%s)\n", __LINE__, __func__, pai->name );
p_myContext = pai->dpvt;
p_myComediFileHandle = p_myContext->p_comediFileHandle;
myChannel = p_myContext->channel;
myRange = p_myContext->range;
myAnalogReference = p_myContext->analogReference;
mySubdevice = p_myContext->subdevice;
myPinNumber = p_myContext->pinNumber;
strcpy(p_myDeviceFile, p_myContext->p_deviceFile);
#if 0
dsetLog(7,__FILE__ "[%d] Subdevice = %d PinNumber =%d\n",
__LINE__, mySubdevice, myPinNumber);
dsetLog(7,__FILE__ "[%d] AnalogReference = %d Channel=%d\n",
__LINE__, myAnalogReference, myChannel);
dsetLog(7, __FILE__ "[%d] -> Range is %d\n", __LINE__, myRange);
#endif
maxdata = comedi_get_maxdata(p_myComediFileHandle, mySubdevice, myChannel);
#if 0
dsetLog(7,__FILE__ "[%d] maxdata = %d pn=%d\n",
__LINE__, maxdata, myPinNumber);
dsetLog(7,__FILE__ "[%d] Data %d -> device %s\n", __LINE__, data, pai->name);
dsetLog(7,__FILE__ "[%d] New Range = %lg\n",
comedi_get_range(p_myComediFileHandle, mySubdevice,
myChannel, myRange));
#endif
ret = comedi_data_read(p_myComediFileHandle, mySubdevice, myChannel,
myRange, myAnalogReference, &data);
if (ret < 0) {
comedi_perror(p_myDeviceFile);
dsetLog(1, __FILE__ "[%d] Error: Couldn't read hardware\n", __LINE__);
sleep(SLEEPTIME_ERROR);
return(ERROR);
}
pai->val = comedi_to_phys(data,
comedi_get_range(p_myComediFileHandle, mySubdevice,
myChannel, myRange),
maxdata);
pai->udf = isnan(pai->val);
dsetLog(7,__FILE__ "[%d] pai->udf is %d \n", __LINE__, pai->udf);
if (pai->udf) {
pai->val = 10;
dsetLog(7,__FILE__ "[%d] Error: Cable is disconnected, please verify\n", __LINE__);
}
dsetLog(7,__FILE__ "[%d] val,rval=%lg,%d -> %s\n",
__LINE__, pai->val, pai->rval, pai->name);
dsetLog(3,__FILE__ "[%d] <- %s\n", __LINE__, __func__);
return(NO_AUTOMATIC_CONVERSION);
/*return(DO_AUTOMATIC_CONVERSION); */
}
//*******************************************************************************
int pid_loop()
//Modified on May 8 to take into account a moving average, and a moving variance
//and also to remove the retraction of the piezo except on the first pass.
{
//This is the function to output a PID loop
//PID algorithm taken from Control System Desgin, by Karl Johan Astrom
//Chapter 6
//This algorithm is supposed to include integral wind-up and bumpless transition
int m;
lsampl_t data_to_card, data_from_card;
static comedi_t * dev_output, * dev_input;
static double bi, ad, bd; //PID coefficients
static double Pcontrib, Icontrib, Dcontrib; //individual PID contributions
static double FeedbackReading; //Readings of the error chann
static double v; //u is the actuator output, and v is the calculated output
static int j = 0;
static double LastDiffContrib;
static double Error;
static double LastError =0;
static double SecondLastError =0;
static double LastOutput =0;
//static double SummedPIDOutput; //Summed PID Output
static double SummedFeedbackReading; //Summed FeedbackReading
//static double SummedVariance;
static double M2_n;
static double delta;
static double alpha;
static struct queue PIDOutput_queue;//these are two queues to calculate the moving mean and variance
static struct queue FeedbackReadingVar_queue;
static struct queue FeedbackReading_queue;
static int NumbFirstSteps;
static double InitialStepSizeVoltage = 0.1;
static double InitialVoltageStep;
double last_mean, last_var, new_var; //popped values of mean and variance
//Initialize the queues
init_queue(&PIDOutput_queue);
init_queue(&FeedbackReadingVar_queue);
init_queue(&FeedbackReading_queue);
//rt_printk("Control channel device name is %s \n",device_names[ControlChannel.board_number]);
//rt_printk("Control channel subdevice %d and channel %d \n", ControlChannel.subdevice, ControlChannel.channel);
//rt_printk("Feedback channel device name is %s \n",device_names[FeedbackChannel.board_number]);
//rt_printk("Feedback channel subdevice %d and channel %d \n", FeedbackChannel.subdevice, FeedbackChannel.channel);
//dev_output is the channel that is to be controlled
dev_output = comedi_open(device_names[ControlChannel.board_number]);
//dev_input is the channel from which the error signal is read
dev_input = comedi_open(device_names[FeedbackChannel.board_number]);
//initialize the task
if(!(PIDloop_Task = rt_task_init_schmod(nam2num( "PIDLoop" ), // Name
0, // Priority
0, // Stack Size
0, //, // max_msg_size
SCHED_FIFO, // Policy
CPUMAP ))) // cpus_allowed
{
rt_printk("ERROR: Cannot initialize PIDLoop task\n");
exit(1);
}
//specify that this is to run on one CPU
rt_set_runnable_on_cpuid(PIDloop_Task, 0);
//lock memory and make hard real time
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_make_hard_real_time();
//Convert PIDLoop_time, which is in nanoseconds, to tick time (sampling_interval, in counts)
sampling_interval =nano2count(PIDLoop_Time);
//sampling_interval =nano2count_cpuid(PIDLoop_Time, 0);
// Let's make this task periodic..
expected = rt_get_time() + 100*sampling_interval;
//expected = rt_get_time_cpuid(0) + 100*sampling_interval;
rt_task_make_periodic(PIDloop_Task, expected, sampling_interval); //period in counts
pid_loop_running = 1; //set the pid loop running flag to FALSE
//retract the tip completely, if it is the first PID pass
if(FirstPIDPass)
{
//data_to_card = (lsampl_t) 0;
//MaxZVoltage corresponds to the fully retracted piezo
//rt_printk("MaxZVoltage is %f \n", MaxZVoltage);
//rt_printk("MinZVoltage is %f \n", MinZVoltage);
//rt_printk("MinOutputVoltage is %f \n", MinOutputVoltage);
//rt_printk("PIDOutput is %f \n", PIDOutput);
//rt_printk("AmplifierGainSign is %i \n", AmplifierGainSign);
//rt_printk("OutputPhase is %i \n", OutputPhase);
NumbFirstSteps = (nearbyint((MaxZVoltage-PIDOutput)/InitialStepSizeVoltage))-1;
//rt_printk("NumbFirstSteps is %i \n", NumbFirstSteps);
//NumbFirstSteps = ((MaxZVoltage - PIDOutput)/InitialStepSizeVoltage)); //-1 to be safe
//Set the direction of the voltage step
//PIDOutput = CurrentZVoltage;
if (MaxZVoltage>=PIDOutput)
{InitialVoltageStep=InitialStepSizeVoltage;}
else {InitialVoltageStep=-InitialStepSizeVoltage;};
if (NumbFirstSteps>1)
{
for(j=0;j<NumbFirstSteps;j++)
{ PIDOutput+=InitialVoltageStep;
data_to_card = (lsampl_t) nearbyint(((PIDOutput - MinOutputVoltage)/OutputRange)*MaxOutputBits);
//rt_printk("Data_to_card is %i \n", data_to_card);
comedi_lock(dev_output, ControlChannel.subdevice);
m=comedi_data_write(dev_output, ControlChannel.subdevice, ControlChannel.channel, AO_RANGE, AREF_DIFF, data_to_card);
comedi_unlock(dev_output, ControlChannel.subdevice);
// And wait until the end of the period.
rt_task_wait_period();
}
}
//Initialize the errors
LastError = 0;
SecondLastError = 0;
LastOutput = PIDOutput;
LastDiffContrib =0;
Dcontrib = 0;
Icontrib = 0;
AveragedPIDOutput=LastOutput; //This is what the main program will actually read
FirstPIDPass = 0;
}
//rt_printk("AntiWindup time is %f \n", AntiWindup_Time);
bi = PropCoff*PIDLoop_Time/IntTime; //integral gain
//rt_printk("PropCoff is %f \n", PropCoff);
//rt_printk("IntTime is %f \n", IntTime);
//in Astrom's article, ad is defined as below in the code, but the actual
//derivation gives the coefficient we actually use
//ad = (2*DiffTime- PID_cutoff_N*PIDLoop_Time)/(2*DiffTime+PID_cutoff_N*PIDLoop_Time);
ad = (DiffTime)/(DiffTime+PID_cutoff_N*PIDLoop_Time);
//rt_printk("DiffTime is %f \n", DiffTime);
//same comment about bd
//bd = 2*PropCoff*PID_cutoff_N*DiffTime/(2*DiffTime + PID_cutoff_N*PIDLoop_Time); //derivative gain
bd = PropCoff*PID_cutoff_N*DiffTime/(DiffTime + PID_cutoff_N*PIDLoop_Time);
//rt_printk("MaxZVoltage is %f \n", MaxZVoltage);
//Now calculate the initial means and variances
//SummedPIDOutput = 0; //initialize parameters if we take averages
//First means
SummedFeedbackReading =0;
//j=1;
alpha = ((float) 1)/(PID_averages+1);
for (j=0;j<PID_averages;j++)
{
//make a first reading
comedi_lock(dev_input, FeedbackChannel.subdevice);
m = comedi_data_read(dev_input, FeedbackChannel.subdevice, FeedbackChannel.channel, AI_RANGE, AREF_DIFF, &data_from_card);
comedi_unlock(dev_input, FeedbackChannel.subdevice);
//Convert to a voltage reading
SummedFeedbackReading += ((((float) data_from_card)/MaxInputBits)*InputRange + MinInputVoltage);
}
AveragedFeedbackReading =SummedFeedbackReading/PID_averages;
//Since we are not changing the output, the mean has not changed, and the variance is 0
M2_n = 0;
PIDOutputVariance = 0;
//Initialize the circular buffers
for (j=0; j<PID_averages; j++)
{
push_queue(&FeedbackReading_queue, AveragedFeedbackReading);
push_queue(&FeedbackReadingVar_queue, PIDOutputVariance);
push_queue(&PIDOutput_queue, LastOutput);
}
//Now do the regular loop
while(pid_loop_running)
{
//rt_printk("Got here 1 \n");
//check to see if the PID parameters have changed
if(PIDParametersChanged)
{
//update the PID coefficients
bi = PropCoff*PIDLoop_Time/IntTime; //integral gain
ad = (DiffTime)/(DiffTime+PID_cutoff_N*PIDLoop_Time);
bd = PropCoff*PID_cutoff_N*DiffTime/(DiffTime + PID_cutoff_N*PIDLoop_Time);
PIDParametersChanged = 0;
} //end of if(PIDParametersChanged)
//continue with the rest of the loop
//Read the input reading
comedi_lock(dev_input, FeedbackChannel.subdevice);
m = comedi_data_read(dev_input, FeedbackChannel.subdevice, FeedbackChannel.channel, AI_RANGE, AREF_DIFF, &data_from_card);
comedi_unlock(dev_input, FeedbackChannel.subdevice);
//Convert to a voltage reading
FeedbackReading = ((((float) data_from_card)/MaxInputBits)*InputRange + MinInputVoltage);
//rt_printk("Data from card is %d \n", data_from_card);
//rt_printk("Feedback reading is %f \n", FeedbackReading);
//rt_printk("Input m is %d \n", m);
delta = (FeedbackReading - AveragedFeedbackReading);
//AveragedFeedbackReading = alpha*FeedbackReading+(1-alpha)*AveragedFeedbackReading; //running averange
//PIDOutputVariance = alpha*(delta*delta) + (1-alpha)*PIDOutputVariance;
//Venkat changed the following line to add logarithmic averaging on January 10, 2012
if(Logarithmic){
Error = AmplifierGainSign*OutputPhase*log10(fabs(FeedbackReading/SetPoint));
}
else {
Error = AmplifierGainSign*OutputPhase*(SetPoint - FeedbackReading);//multiply by OutputPhase+AmplifierGainSign
}
//Error = AmplifierGainSign*OutputPhase*(SetPoint - FeedbackReading);//multiply by OutputPhase+AmplifierGainSign
Pcontrib = PropCoff*(Error - LastError);
//Not sure of sign of second contribution in line below...should it be - ?
Dcontrib = ad*LastDiffContrib - bd*(Error - 2*LastError + SecondLastError);
v = LastOutput + Pcontrib + Icontrib + Dcontrib;
//next, take care of saturation of the output....anti-windup
PIDOutput = v;
PIDOutput =(PIDOutput>MaxOutputVoltage)? MaxOutputVoltage:PIDOutput;
PIDOutput =(PIDOutput<MinOutputVoltage)? MinOutputVoltage:PIDOutput;
//Calculate the averaged quantities
pop_queue(&FeedbackReading_queue, &last_mean);
AveragedFeedbackReading += (FeedbackReading - last_mean)/PID_averages;
push_queue(&FeedbackReading_queue, FeedbackReading);
pop_queue(&FeedbackReadingVar_queue, &last_var);
new_var = delta*delta;
PIDOutputVariance += (new_var - last_var)/PID_averages;
push_queue(&FeedbackReadingVar_queue, new_var);
//send the control signal
//rt_printk("FeedbackReading is %f \n", FeedbackReading);
//rt_printk("v is %f \n", v);
//rt_printk("PID output should be %f \n", PIDOutput);
data_to_card = (lsampl_t) nearbyint(((PIDOutput - MinOutputVoltage)/OutputRange)*MaxOutputBits);
//data_to_card = (lsampl_t) 0;
comedi_lock(dev_output, ControlChannel.subdevice);
m=comedi_data_write(dev_output, ControlChannel.subdevice, ControlChannel.channel, AO_RANGE, AREF_DIFF, data_to_card);
comedi_unlock(dev_output, ControlChannel.subdevice);
//rt_printk("Output m is %d \n", m);
//Update the integral contribution after the loop
Icontrib = bi*Error;
//Update parameters
LastError = Error;
SecondLastError = LastError;
LastDiffContrib = Dcontrib;
LastOutput = PIDOutput;
//rt_printk("PContrib is %f \n", Pcontrib);
//rt_printk("IContrib is %f \n", Icontrib);
//rt_printk("DContrib is %f \n", Dcontrib);
//rt_printk("PIDOutput is %f \n", PIDOutput);
//Next part is to take the averaged PID output for recording if j>PID_averages and PID_averages>1
//SummedPIDOutput+=PIDOutput;
//SummedFeedbackReading += FeedbackReading;
//j++;
//AveragedPIDOutput=((PID_averages>1)&&(j>PID_averages))?(SummedPIDOutput/PID_averages):AveragedPIDOutput;
//AveragedFeedbackReading=((PID_averages>1)&&(j>PID_averages))?(SummedFeedbackReading/PID_averages):AveragedFeedbackReading;
//SummedPIDOutput=(j>PID_averages)? 0:SummedPIDOutput;
//SummedFeedbackReading=(j>PID_averages)? 0:SummedFeedbackReading;
//j=(j>PID_averages)? 1:j;
//Calculate moving exponential averages and variance
//delta = PIDOutput - AveragedPIDOutput;
//AveragedPIDOutput = alpha*PIDOutput + (1-alpha)*AveragedPIDOutput;
//PIDOutputVariance = alpha*(delta*delta) + (1-alpha)*PIDOutputVariance;
//PIDOutputVariance = alpha*abs(delta) + (1-alpha)*PIDOutputVariance;
pop_queue(&PIDOutput_queue, &last_mean);
AveragedPIDOutput += (PIDOutput - last_mean)/PID_averages;
push_queue(&PIDOutput_queue, PIDOutput);
// And wait until the end of the period.
rt_task_wait_period();
}
//rt_printk("Got here 3 \n");
//rt_printk("pid_loop_running is %d \n", pid_loop_running);
rt_make_soft_real_time();
comedi_close(dev_input);
comedi_close(dev_output);
rt_task_delete(PIDloop_Task); //Self termination at end.
pthread_exit(NULL);
return 0;
}
