static void end(scicos_block *block)
{
struct ADCOMDev * comdev = (struct ADCOMDev *) (*block->work);
if (comdev->dev) {
int index = comdev->index;
ComediDev_InUse[index]--;
ComediDev_AIInUse[index]--;
if (!ComediDev_AIInUse[index]) {
comedi_unlock(comdev->dev, comdev->subdev);
}
if (!ComediDev_InUse[index]) {
comedi_close(comdev->dev);
printf("\nCOMEDI /dev/comedi%d closed.\n\n", index);
ComediDev[index] = NULL;
}
}
free(comdev);
}
static void mdlTerminate(SimStruct *S)
{
#ifndef MATLAB_MEX_FILE
int index = (int)COMEDI_DEVICE - 1;
void *dev = (void *)ssGetPWork(S)[0];
int subdev = ssGetIWork(S)[0];
char *devname[4] = {"/dev/comedi0","/dev/comedi1","/dev/comedi2","/dev/comedi3"};
if (ssGetErrorStatus(S) == NULL) {
ComediDev_InUse[index]--;
ComediDev_AIInUse[index]--;
if (!ComediDev_AIInUse[index]) {
comedi_unlock(dev, subdev);
}
if (!ComediDev_InUse[index]) {
comedi_close(dev);
printf("\nCOMEDI %s closed.\n\n", devname[index]);
ComediDev[index] = NULL;
}
}
#endif
}
//*******************************************************************************
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;
}
int sineoutput()
{
//Initial test function to try out Real time stuff.
int m, i=0;
lsampl_t data_to_card;
static comedi_t * dev;
RTIME ElapsedTime;
dev = comedi_open(device_names[AnalogOutputChannel.board_number]);
if(!(Sinewaveloop_Task = rt_task_init_schmod(nam2num( "Sinewave" ), // Name
2, // Priority
0, // Stack Size
0, //, // max_msg_size
SCHED_FIFO, // Policy
CPUMAP ))) // cpus_allowed
{
printf("ERROR: Cannot initialize sinewave task\n");
exit(1);
}
//specify that this is to run on one CPU
//rt_set_runnable_on_cpuid(Sinewaveloop_Task, 0);
//Convert samp_time, which is in nanoseconds, to tick time
//sampling_interval = nano2count(SAMP_TIME); //Converts a value from
//nanoseconds to internal count units.
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_make_hard_real_time();
sampling_interval =nano2count_cpuid(SAMP_TIME, 0);
rt_printk("Sampling interval is %f12 \n",count2nano((float) sampling_interval));
// Let's make this task periodic..
expected = rt_get_time_cpuid(0) + 100*sampling_interval;
//Manan changed all the timer commands to _couid version on 10/22/2012 to see if it helps
//sampling_interval =nano2count_cpuid(SAMP_TIME,0);
// rt_printk("Sampling interval is %f12 \n",count2nano_cpuid((float) sampling_interval,0));
// Let's make this task periodic..
//expected = rt_get_time_cpuid(0) + 100*sampling_interval;
rt_task_make_periodic(Sinewaveloop_Task, expected, sampling_interval); //period in counts
//rt_task_resume(Sinewaveloop_Task);
sine_loop_running=1;
// Concurrent function Loop
rt_printk("SineWaveAmplitude is is %f \n",SineWaveAmplitude);
rt_printk("SineWaveFrequency is %f \n",SineWaveFrequency);
rt_printk("sine_loop_running is %d \n",sine_loop_running);
rt_printk("SAMP_TIME is %d \n",SAMP_TIME);
//start_time = (float)rt_get_time_ns_cpuid(0)/1E9; //in seconds
start_time = (float)rt_get_time_ns_cpuid(0)/1E9;
old_time = start_time;
rt_printk("AnalogOutputChannel board_it is %p \n",AnalogOutputChannel.board_id);
rt_printk("AnalogOutputChannel devicename is %p \n",*(AnalogOutputChannel.devicename));
rt_printk("AnalogOutputChannel boardname is %p \n",*(AnalogOutputChannel.boardname));
rt_printk("AnalogOutputChannel subdevice is %d \n",AnalogOutputChannel.subdevice);
rt_printk("AnalogOutputChannel channel is %d \n",AnalogOutputChannel.channel);
//OutputValue = 1;
//ElapsedTime = 0;
OutputValue = 0;
//sine_loop_running = 0; //set this to 0 for testing
while(sine_loop_running)
{
i++; // Count Loops.
current_time = (float)rt_get_time_ns_cpuid(0)/1E9;
//current_ticks = rt_get_time_cpuid(0);
//current_time = (float) (count2nano_cpuid(current_ticks,0)/1E9);
//current_time = (float)rt_get_time_ns_cpuid(0)/1E9;
//rt_printk("LOOP %d,-- Period time: %f12 %f12\n",i, current_time - old_time,count2nano((float)sampling_interval)/1E9);
OutputValue = SineWaveAmplitude*sin(2*PI*SineWaveFrequency*(current_time-start_time));
//OutputValue+=((SAMP_TIME*PI*2*SineWaveFrequency)/1E9)*cos(2*PI*SineWaveFrequency*((float)SAMP_TIME)/1E9);
//if (OutputValue>10.0)
//{OutputValue = -10;
//}
//OutputValue = SineWaveAmplitude*sin(2*PI*SineWaveFrequency*((float)ElapsedTime)/1E9);
ElapsedTime+=SAMP_TIME;
//OutputValue = -1*OutputValue;
//rt_printk("OutputValue is %f12 \n",OutputValue);
data_to_card = (lsampl_t) nearbyint(((OutputValue - MinOutputVoltage)/OutputRange)*MaxOutputBits);
//m=rt_comedi_command_data_write(AnalogOutputChannel.board_id, AnalogOutputChannel.subdevice, NCHAN, data_to_card);
comedi_lock(dev, AnalogOutputChannel.subdevice);
m=comedi_data_write(dev, AnalogOutputChannel.subdevice, AnalogOutputChannel.channel, AO_RANGE, AREF_DIFF, data_to_card);
comedi_unlock(dev, AnalogOutputChannel.subdevice);
// m=comedi_data_write(AnalogOutputChannel.board_id, AnalogOutputChannel.subdevice,
// AnalogOutputChannel.channel, AO_RANGE, AREF_GROUND, data_to_card);
//rt_printk("Data_to_card is %d; result from rt_comedi_command_data_write is %d \n",data_to_card, m);
//rt_printk("LOOP %d,-- Loop time: %f12 \n",i, (float)(current_time-old_time));
//rt_printk("LOOP %d,-- AO Out time: %f12 \n",i, (float)rt_get_time_ns()/1E9 - current_time);
//rt_printk("LOOP %d,-- AO Out time: %f12 \n",i, (float)rt_get_cpu_time_ns()/1E9 - current_time);
//rt_printk("Data_to_card is %d \n",data_to_card);
old_time = current_time;
/* if (i== 100000)
{
sine_loop_running = 0;
//printf("LOOP -- run: %d %d\n ",keep_on_running,&keep_on_running);
//printf("RTAI LOOP -- run: %d \n ",i);
break;
}
*/
rt_task_wait_period(); // And waits until the end of the period.
}
rt_make_soft_real_time();
comedi_close(dev);
rt_task_delete(Sinewaveloop_Task); //Self termination at end.
pthread_exit(NULL);
return 0;
}
int PLLReferenceGeneration()
{
//Initial test function to try out Real time stuff.
int m, i=0, err, n;
lsampl_t data_to_card;
static comedi_t * dev;
static int OutputFIFOBufferSize;
static int PLLGenerationBufferSize;
unsigned int maxdata;
unsigned int chanlist[16];
int ret;
static lsampl_t data[PLLGenerationBufferNPoints]; //this is the buffer used to send data points out
comedi_cmd cmd;
dev = comedi_open(device_names[PLLReferenceGenerationChannel.board_number]);
//Check the size of the output buffer
OutputFIFOBufferSize = comedi_get_buffer_size(dev, PLLReferenceGenerationChannel.subdevice);
rt_printk("OutputFIFO Buffer size is %i\n", OutputFIFOBufferSize);
//Set the actual buffer size that we will be using to half this and the number of data points to one fourth
//Now configure the output channel using a Comedi instruction
//BufferSize is initially set to be double the number of PLLGenerationBufferNPoints
PLLGenerationBufferSize = 2*PLLGenerationBufferNPoints;
maxdata = comedi_get_maxdata(dev, PLLReferenceGenerationChannel.subdevice, PLLReferenceGenerationChannel.channel);
rt_printk("PLL Reference channel max data is %i\n", maxdata);
offset = maxdata / 2;
amplitude = maxdata - offset;
memset(&cmd,0,sizeof(cmd));
cmd.subdev = PLLReferenceGenerationChannel.subdevice;
cmd.flags = CMDF_WRITE;
cmd.start_src = TRIG_INT;
cmd.start_arg = 0;
cmd.scan_begin_src = TRIG_TIMER;
cmd.scan_begin_arg = PLLGenMinPulseTime; //minimum update time for the
cmd.convert_src = TRIG_NOW;
cmd.convert_arg = 0;
cmd.scan_end_src = TRIG_COUNT;
cmd.scan_end_arg = NCHAN; //only one channel
cmd.stop_src = TRIG_NONE;
cmd.stop_arg = 0;
cmd.chanlist = chanlist;
cmd.chanlist_len = NCHAN;
chanlist[0] = CR_PACK(PLLReferenceGenerationChannel.channel, AO_RANGE, AREF_GROUND);
dds_init(PLLWaveformFrequency, PLLUpdateFrequency);
err = comedi_command_test(dev, &cmd);
if (err < 0) {
comedi_perror("comedi_command_test");
exit(1);
}
err = comedi_command_test(dev, &cmd);
if (err < 0) {
comedi_perror("comedi_command_test");
exit(1);
}
if ((err = comedi_command(dev, &cmd)) < 0) {
comedi_perror("comedi_command");
exit(1);
}
dds_output(data,PLLGenerationBufferNPoints);
n = PLLGenerationBufferNPoints * sizeof(sampl_t);
m = write(comedi_fileno(dev), (void *)data, n);
if(m < 0){
perror("write");
exit(1);
}else if(m < n)
{
fprintf(stderr, "failed to preload output buffer with %i bytes, is it too small?\n"
"See the --write-buffer option of comedi_config\n", n);
exit(1);
}
if(!(PLLRefGen_Task = rt_task_init_schmod(nam2num( "PLLReferenceGeneration" ), // Name
2, // Priority
0, // Stack Size
0, //, // max_msg_size
SCHED_FIFO, // Policy
CPUMAP ))) // cpus_allowed
{
printf("ERROR: Cannot initialize pll reference generation task\n");
exit(1);
}
//specify that this is to run on one CPU
rt_set_runnable_on_cpuid(PLLRefGen_Task, 1);
//Convert samp_time, which is in nanoseconds, to tick time
//sampling_interval = nano2count(SAMP_TIME); //Converts a value from
//nanoseconds to internal count units.
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_make_hard_real_time();
PLLUpdateTime = nano2count(PLLGenerationLoopTime);
rt_printk("PLL generation update time is %f12 \n",count2nano((float) PLLUpdateTime));
// Let's make this task periodic..
expected = rt_get_time() + 100*PLLUpdateTime;
rt_task_make_periodic(PLLRefGen_Task, expected, PLLUpdateTime); //period in counts
//rt_task_resume(Sinewaveloop_Task);
PLLGenerationOn = TRUE;
// Concurrent function Loop
//rt_printk("SineWaveAmplitude is is %f \n",SineWaveAmplitude);
//rt_printk("SineWaveFrequency is %f \n",SineWaveFrequency);
//rt_printk("sine_loop_running is %d \n",sine_loop_running);
//rt_printk("SAMP_TIME is %d \n",SAMP_TIME);
start_time = (float)rt_get_time_ns()/1E9; //in seconds
old_time = start_time;
rt_printk("PLLReferenceGenerationChannel board_it is %p \n",PLLReferenceGenerationChannel.board_id);
rt_printk("PLLReferenceGenerationChannel devicename is %p \n",*(PLLReferenceGenerationChannel.devicename));
rt_printk("PLLReferenceGenerationChannel boardname is %p \n",*(PLLReferenceGenerationChannel.boardname));
rt_printk("PLLReferenceGenerationChannel subdevice is %d \n",PLLReferenceGenerationChannel.subdevice);
rt_printk("PLLReferenceGenerationChannel channel is %d \n",PLLReferenceGenerationChannel.channel);
OutputValue = 1;
PLLGenerationBufferSize = comedi_get_buffer_size(dev, PLLReferenceGenerationChannel.subdevice);
//sine_loop_running = 0; //set this to 0 for testing
while(PLLGenerationOn)
{
i++; // Count Loops.
current_time = (float)rt_get_time_ns()/1E9;
//rt_printk("LOOP %d,-- Period time: %f12 %f12\n",i, current_time - old_time,count2nano((float)sampling_interval)/1E9);
OutputValue = SineWaveAmplitude*sin(2*PI*SineWaveFrequency*(current_time-start_time));
//OutputValue = -1*OutputValue;
//rt_printk("OutputValue is %f12 \n",OutputValue);
data_to_card = (lsampl_t) nearbyint(((OutputValue - MinOutputVoltage)/OutputRange)*MaxOutputBits);
//m=rt_comedi_command_data_write(AnalogOutputChannel.board_id, AnalogOutputChannel.subdevice, NCHAN, data_to_card);
comedi_lock(dev, AnalogOutputChannel.subdevice);
m=comedi_data_write(dev, AnalogOutputChannel.subdevice, AnalogOutputChannel.channel, AO_RANGE, AREF_DIFF, data_to_card);
comedi_unlock(dev, AnalogOutputChannel.subdevice);
// m=comedi_data_write(AnalogOutputChannel.board_id, AnalogOutputChannel.subdevice,
// AnalogOutputChannel.channel, AO_RANGE, AREF_GROUND, data_to_card);
//rt_printk("Data_to_card is %d; result from rt_comedi_command_data_write is %d \n",data_to_card, m);
//rt_printk("LOOP %d,-- AO Out time: %f12 \n",i, (float)rt_get_time_ns()/1E9 - current_time);
//rt_printk("Data_to_card is %d \n",data_to_card);
//old_time = current_time;
/* if (i== 100000)
{
sine_loop_running = 0;
//printf("LOOP -- run: %d %d\n ",keep_on_running,&keep_on_running);
//printf("RTAI LOOP -- run: %d \n ",i);
break;
}
*/
rt_task_wait_period(); // And waits until the end of the period.
}
rt_make_soft_real_time();
comedi_close(dev);
rt_task_delete(Sinewaveloop_Task); //Self termination at end.
pthread_exit(NULL);
return 0;
}
static void mdlStart(SimStruct *S)
{
#ifndef MATLAB_MEX_FILE
void *dev;
int subdev;
int index = (int)COMEDI_DEVICE - 1;
unsigned int channel = (unsigned int)COMEDI_CHANNEL;
unsigned int range = (unsigned int)COMEDI_RANGE;
int n_channels;
char *devname[4] = {"/dev/comedi0","/dev/comedi1","/dev/comedi2","/dev/comedi3"};
char board[50];
static char_T errMsg[256];
comedi_krange krange;
double range_min, range_max;
if (!ComediDev[index]) {
dev = comedi_open(devname[index]);
if (!dev) {
sprintf(errMsg, "Comedi open failed\n");
ssSetErrorStatus(S, errMsg);
printf("%s", errMsg);
return;
}
rt_comedi_get_board_name(dev, board);
printf("COMEDI %s (%s) opened.\n\n", devname[index], board);
ComediDev[index] = dev;
} else {
dev = ComediDev[index];
}
if ((subdev = comedi_find_subdevice_by_type(dev, COMEDI_SUBD_AI, 0)) < 0) {
sprintf(errMsg, "Comedi find_subdevice failed (No analog input)\n");
ssSetErrorStatus(S, errMsg);
printf("%s", errMsg);
comedi_close(dev);
return;
}
if (!ComediDev_AIInUse[index] && comedi_lock(dev, subdev) < 0) {
sprintf(errMsg, "Comedi lock failed for subdevice %d\n", subdev);
ssSetErrorStatus(S, errMsg);
printf("%s", errMsg);
comedi_close(dev);
return;
}
if ((n_channels = comedi_get_n_channels(dev, subdev)) < 0) {
sprintf(errMsg, "Comedi get_n_channels failed for subdevice %d\n", subdev);
ssSetErrorStatus(S, errMsg);
printf("%s", errMsg);
comedi_unlock(dev, subdev);
comedi_close(dev);
return;
}
if ((comedi_get_krange(dev, subdev, channel, range, &krange)) < 0) {
sprintf(errMsg, "Comedi get range failed for subdevice %d\n", subdev);
ssSetErrorStatus(S, errMsg);
printf("%s", errMsg);
comedi_unlock(dev, subdev);
comedi_close(dev);
return;
}
ComediDev_InUse[index]++;
ComediDev_AIInUse[index]++;
range_min = (double)(krange.min)*1.e-6;
range_max = (double)(krange.max)*1.e-6;
printf("AI Channel %d - Range : %1.2f [V] - %1.2f [V]\n", channel, range_min, range_max);
ssGetPWork(S)[0] = (void *)dev;
ssGetIWork(S)[0] = subdev;
ssGetRWork(S)[0] = range_min;
ssGetRWork(S)[1] = range_max;
#endif
}
static void init(scicos_block *block)
{
struct ADCOMDev * comdev = (struct ADCOMDev *) malloc(sizeof(struct ADCOMDev));
*block->work = (void *)comdev;
char devName[15];
char board[50];
comedi_krange krange;
comdev->channel = block->ipar[0];
comdev->range = block->ipar[1];
comdev->aref = block->ipar[2];
comdev->index = block->ipar[3];
sprintf(devName,"/dev/comedi%d", comdev->index);
if (!ComediDev[comdev->index]) {
comdev->dev = comedi_open(devName);
if (!(comdev->dev)) {
fprintf(stderr, "COMEDI %s open failed\n", devName);
exit_on_error();
return;
}
rt_comedi_get_board_name(comdev->dev, board);
printf("COMEDI %s (%s) opened.\n\n", devName, board);
ComediDev[comdev->index] = comdev->dev;
} else
comdev->dev = ComediDev[comdev->index];
if ((comdev->subdev = comedi_find_subdevice_by_type(comdev->dev, COMEDI_SUBD_AI, 0)) < 0) {
fprintf(stderr, "Comedi find_subdevice failed (No analog input)\n");
comedi_close(comdev->dev);
exit_on_error();
return;
}
if (!ComediDev_AIInUse[comdev->index] && comedi_lock(comdev->dev, comdev->subdev) < 0) {
fprintf(stderr, "Comedi lock failed for subdevice %d\n", comdev->subdev);
comedi_close(comdev->dev);
exit_on_error();
return;
}
if (comdev->channel >= comedi_get_n_channels(comdev->dev, comdev->subdev)) {
fprintf(stderr, "Comedi channel not available for subdevice %d\n", comdev->subdev);
comedi_unlock(comdev->dev, comdev->subdev);
comedi_close(comdev->dev);
exit_on_error();
return;
}
if ((comedi_get_krange(comdev->dev, comdev->subdev, comdev->channel, comdev->range, &krange)) < 0) {
fprintf(stderr, "Comedi get_range failed for subdevice %d\n", comdev->subdev);
comedi_unlock(comdev->dev, comdev->subdev);
comedi_close(comdev->dev);
exit_on_error();
return;
}
#ifdef SOFTCALIB
int flags;
if ((flags = comedi_get_subdevice_flags(comdev->dev, comdev->subdev)) < 0) {
fprintf(stderr, "Comedi get_subdevice_flags failed for subdevice %d\n", comdev->subdev);
} else {
if (flags & SDF_SOFT_CALIBRATED) {/* board uses software calibration */
if ((comdev->use_softcal = get_softcal_coef(devName, comdev->subdev,
comdev->channel, comdev->range, 0, comdev->coefficients)) == 0)
fprintf(stderr, "No software calibration found for AI Channel %d\n",comdev->channel);
}
}
#endif
ComediDev_InUse[comdev->index]++;
ComediDev_AIInUse[comdev->index]++;
comdev->maxdata = comedi_get_maxdata(comdev->dev, comdev->subdev, comdev->channel);
comdev->range_min = (double)(krange.min)*1.e-6;
comdev->range_max = (double)(krange.max)*1.e-6;
printf("AI Channel %d - Range : %1.2f [V] - %1.2f [V]%s\n\n", comdev->channel,
comdev->range_min, comdev->range_max, (comdev->use_softcal)?" Software calibration used":"");
}
