int main(void)
{
double omega = (2.0*M_PI*SIN_FREQ*SAMP_TIME)/1.0E9;
RTIME until;
RT_TASK *task;
lsampl_t data[NCHAN*2];
long k, sinewave, retval = 0;
signal(SIGKILL, endme);
signal(SIGTERM, endme);
start_rt_timer(0);
task = rt_task_init_schmod(nam2num("MYTASK"), 1, 0, 0, SCHED_FIFO, 0xF);
printf("COMEDI CMD TEST BEGINS: SAMPLING FREQ: %d, RUN TIME: %d.\n", SAMP_FREQ, RUN_TIME);
if (init_board()) {;
printf("Board initialization failed.\n");
return 1;
}
do_cmd();
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
until = rt_get_cpu_time_ns() + (long long)RUN_TIME*1000000000;
for (k = 0; k < SAMP_FREQ*RUN_TIME && !end; k++) {
sinewave = (long)(maxdata/4*sin(k*omega));
data[0] = (lsampl_t)( sinewave + maxdata/2);
data[1] = (lsampl_t)(- sinewave + maxdata/2);
while (rt_comedi_command_data_write(dev, subdev, NCHAN, data) != NCHAN) {
rt_sleep(nano2count(SAMP_TIME/2));
}
if (k == TRIGSAMP) {
rt_comedi_trigger(dev, subdev);
}
}
while (until > rt_get_cpu_time_ns()) {
rt_sleep(nano2count(100000));
}
comedi_cancel(dev, subdev);
comedi_close(dev);
comedi_data_write(dev, subdev, 0, 0, AREF_GROUND, 2048);
comedi_data_write(dev, subdev, 1, 0, AREF_GROUND, 2048);
printf("COMEDI TEST ENDS.\n");
if (retval < 0) {
printf("rt_comedi_wait_timed overruns: %d\n", abs(retval));
}
stop_rt_timer();
rt_make_soft_real_time();
rt_task_delete(task);
return 0;
}
void com_data_write(unsigned int index, uint64_t value)
{
lsampl_t data = (lsampl_t)(value % 0x100000000);
comedi_data_write(it,
Comedi_subdev_aq,
index,
0,//unsigned int range,
AREF_GROUND,//unsigned int aref,
data);
}
bool PIDFlowController::writeVolts(double v)
{
lsampl_t samp = aov2s(v);
int ret;
if (daq_ao) {
ret = daq_ao->putSample(params.chan_ao, samp) ? 1 : 0;
} else {
ret = comedi_data_write(dev_ao, params.subdev_ao, params.chan_ao, 0, 0, samp);
}
return ret >= 1;
}
/**
* @brief MainWindow::RunCalibration
*/
void MainWindow::RunCalibration()
{
lsampl_t n=Ini_ImI->maxdata;
lsampl_t * Results=new lsampl_t[n];
double sum_x=0.0, sum_y=0.0, sum_xy=0.0, sum_x2 =0.0;
if (QMessageBox::information(this,
"Ready to calibrate",
QString(
"Please connect the command output channel\nto the membrane current recording input channel\n and press OK"),
QMessageBox::Ok|QMessageBox::Cancel,QMessageBox::Cancel)==QMessageBox::Cancel)
{
return;
}
WindowUpdateTimer->blockSignals(true);
QString buffer="";
double x,y;
TheSlope=1.0;TheIntercept=0.0;
for (lsampl_t i=0;i<n;i++)
{
/*int ret=*/ comedi_data_write(it, Ini_CmdO->subdevice, Ini_CmdO->Channel, Ini_CmdO->rangeN, 0, i);
// ret is commented out, but you might wish to check its value
y=i;
Results[i]=Ini_ImI->read_sample();
x=Results[i];
sum_y+=y;
sum_x+=x;
sum_xy+=x*y;
sum_x2+=x*x;
if (i%100==0)
{
buffer+=QString("%1;%2]").arg(x).arg(y);
}
};
WindowUpdateTimer->blockSignals(false);
double intercept,slope;
intercept=(sum_y*sum_x2-sum_x*sum_xy)/(n*sum_x2-sum_x*sum_x);
slope=(n*sum_xy-sum_x*sum_y)/(n*sum_x2-sum_x*sum_x);
AppSettings.setValue(Key_ADC_slope,slope);
AppSettings.setValue(Key_ADC_intercept,intercept);
if (QMessageBox::information(this,
"Calibration result",
QString("Slope: %1;\nintercept: %2\n").arg(slope).arg(intercept),
QMessageBox::Apply|QMessageBox::Ignore,QMessageBox::Ignore)==QMessageBox::Apply)
{
TheSlope=slope;
TheIntercept=intercept;
};
delete Results;
return;
}
static void data_write( 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 = *sp++;
int c = comedi_data_write( dev, subdev, chan, range, aref, data );
if( c < 1 ) comedi_perror( "!daq " );
}
int DaqDevice::writeData(int subdev,int channel, int range_idx,int aref,double value)
{
lsampl_t raw;
raw = comedi_from_physical(value,&_converter_an_output[channel]);
if (comedi_data_write(_dev, subdev, channel, range_idx, aref,raw) < 0)
{
return COMEDI_ERROR;
} else {
return COMEDI_OK;
}
}
int set_dac_volts(int chan, double voltage) {
lsampl_t data;
int retval;
DAC_ERROR = FALSE;
data = comedi_from_phys(voltage, da_range, maxdata_ao);
retval = comedi_data_write(it, subdev_ao, chan, range_ao, aref_ao, data);
if (retval < 0) {
comedi_perror("comedi_data_write in set_dac_volts");
DAC_ERROR = TRUE;
}
return retval;
}
int put_dio_bit(int chan, int bit_data) {
lsampl_t data = bit_data;
int retval;
DIO_ERROR = FALSE;
retval = comedi_data_write(it, subdev_dio, chan, range_dio, aref_dio, data);
if (retval < 0) {
comedi_perror("comedi_data_read in put_dio_bits");
DIO_ERROR = TRUE;
return -1;
}
return 0;
}
/**
* @brief Directly set the DAC voltage
*
* @param Volts
*/
void MainWindow::setCommandVoltage(double Volts)
{
if (Ini_CmdO==NULL) return;
lsampl_t CurrentOutputValue=comedi_from_phys(Volts, Ini_CmdO->range,Ini_CmdO->maxdata);
int ret=comedi_data_write(it, Ini_CmdO->subdevice, Ini_CmdO->Channel, Ini_CmdO->rangeN, 0, CurrentOutputValue);
if (ret < 0)
{
//Message=(QString("comedi_data_write failed with cmd %1 (%2 V)\n").arg(CurrentOutputValue).arg(Volts));
}
else
{
//Message=(QString("Debug: command set cmd %1 (%2 V)\n").arg(CurrentOutputValue).arg(Volts));
};
}
void ComediAnalogOutputSoftCal::write(double data)
{
double sample = data*m_outputConversionFactor;
#ifdef TRIM_ANALOG_OUTPUT
if (sample <= m_dataRange->min) {
sample = m_dataRange->min + 0.01*(m_dataRange->max-m_dataRange->min);
//Logger(Debug, "[%f] - Trimming lower limit of the DAQ card.\n", GetGlobalTime());
}
if (sample >= m_dataRange->max) {
sample = m_dataRange->max - 0.01*(m_dataRange->max-m_dataRange->min);
//Logger(Debug, "[%f] - Trimming upper limit of the DAQ card.\n", GetGlobalTime());
}
#endif
comedi_data_write(m_device, m_subdevice, m_channels[0], m_range, m_aref,
comedi_from_physical(sample, &m_converter));
}
bool ComediChan::dataWrite(double d)
{
if (d < m_rangeMin || d > m_rangeMax) {
err = "Out of range.";
return false;
}
lsampl_t samp = static_cast<lsampl_t>(((d-m_rangeMin)/(m_rangeMax-m_rangeMin)) * m_maxdata);
if (daq) {
return daq->writeSample(m_chan, samp);
} else if ( comedi_data_write(m_dev, m_subdev, m_chan, m_range, 0, samp) < 1 ) {
err = "Write error.";
return false;
}
return true;
}
int DaqDevice::setupCounter(int subdev,int channel,int initial_count)
{
int retval;
lsampl_t counter_mode;
retval = comedi_reset(_dev, subdev);
if(retval < 0) return COMEDI_ERROR;
retval = comedi_set_gate_source(_dev, subdev, 0, 0, NI_GPCT_GATE_PIN_GATE_SELECT(0) /* NI_GPCT_GATE_PIN_i_GATE_SELECT *//*| CR_EDGE*/);
if(retval < 0) return COMEDI_ERROR;
retval = comedi_set_gate_source(_dev, subdev, 0, 1, NI_GPCT_DISABLED_GATE_SELECT | CR_EDGE);
if(retval < 0)
{
fprintf(stderr, "Failed to set second gate source. This is expected for older boards (e-series, etc.)\n"
"that don't have a second gate.\n");
}
counter_mode = NI_GPCT_COUNTING_MODE_NORMAL_BITS;
// output pulse on terminal count (doesn't really matter for this application)
counter_mode |= NI_GPCT_OUTPUT_TC_PULSE_BITS;
/* Don't alternate the reload source between the load a and load b registers.
Doesn't really matter here, since we aren't going to be reloading the counter.
*/
counter_mode |= NI_GPCT_RELOAD_SOURCE_FIXED_BITS;
// count up
counter_mode |= NI_GPCT_COUNTING_DIRECTION_UP_BITS;
// don't stop on terminal count
counter_mode |= NI_GPCT_STOP_ON_GATE_BITS;
// don't disarm on terminal count or gate signal
counter_mode |= NI_GPCT_NO_HARDWARE_DISARM_BITS;
retval = comedi_set_counter_mode(_dev, subdev, channel, counter_mode);
if(retval < 0) return COMEDI_ERROR;
/* set initial counter value by writing to channel 0. The "load a" and "load b" registers can be
set by writing to channels 1 and 2 respectively. */
retval = comedi_data_write(_dev, subdev, channel, 0, 0, initial_count);
retval = comedi_arm(_dev, subdev, NI_GPCT_ARM_IMMEDIATE);
if(retval < 0) return COMEDI_ERROR;
return COMEDI_OK;
}
int main(int argc, char *argv[])
{
int ret;
comedi_insn insn;
lsampl_t d[5];
comedi_t *device;
int freq;
struct parsed_options options;
init_parsed_options(&options);
options.freq = -1;
// we hijack this option to switch it on or off
options.n_scan = -1;
options.value = -1;
parse_options(&options, argc, argv);
if ((options.value==-1)&&(options.n_scan==-1)&&(options.freq==-1)) {
fprintf(stderr,
"Usage: %s OPTIONS duty_cycle\n"
"options: \n"
" -N 0 switches PWM off\n"
" -N 1 switches PWM on\n"
" -N 2 enquires the max value for the duty cycle\n"
" -F FREQ sets the PWM frequency\n",
argv[0]);
}
device = comedi_open(options.filename);
if(!device){
comedi_perror(options.filename);
exit(-1);
}
options.subdevice = comedi_find_subdevice_by_type(device,COMEDI_SUBD_PWM,0);
if (options.verbose)
printf("PWM subdevice autodetection gave subdevice number %d\n",
options.subdevice);
if(options.n_scan==2) {
printf("%d\n",comedi_get_maxdata(device,options.subdevice,0));
comedi_close(device);
exit(0);
}
insn.insn=INSN_CONFIG;
insn.data=d;
insn.subdev=options.subdevice;
insn.chanspec=CR_PACK(0,0,0);
if(options.n_scan==1) {
d[0] = INSN_CONFIG_ARM;
d[1] = 0;
insn.n=2;
ret=comedi_do_insn(device,&insn);
if(ret < 0){
fprintf(stderr,"Could not switch on:%d\n",ret);
comedi_perror(options.filename);
exit(-1);
}
}
if(options.n_scan==0) {
d[0] = INSN_CONFIG_DISARM;
d[1] = 0;
insn.n=1;
ret=comedi_do_insn(device,&insn);
if(ret < 0){
fprintf(stderr,"Could not switch off:%d\n",ret);
comedi_perror(options.filename);
exit(-1);
}
}
if(options.freq>0) {
freq = options.freq;
d[0] = INSN_CONFIG_PWM_SET_PERIOD;
d[1] = 1E9/freq;
insn.n=2;
ret=comedi_do_insn(device,&insn);
if(ret < 0){
fprintf(stderr,"Could set frequ:%d\n",ret);
comedi_perror(options.filename);
exit(-1);
}
}
d[0] = INSN_CONFIG_GET_PWM_STATUS;
insn.n=2;
ret=comedi_do_insn(device,&insn);
if(ret < 0){
fprintf(stderr,"Could not get status:%d insn=%d\n",
ret,
d[0]);
comedi_perror(options.filename);
exit(-1);
}
if (options.verbose) {
if (d[1])
fprintf(stderr,
"PWM is on.\n");
else
fprintf(stderr,
"PWM is off.\n");
}
d[0] = INSN_CONFIG_PWM_GET_PERIOD;
insn.n=2;
ret=comedi_do_insn(device,&insn);
if(ret < 0){
fprintf(stderr,"Could get frequ:%d\n",ret);
comedi_perror(options.filename);
exit(-1);
}
freq = 1E9 / d[1];
if (options.verbose)
fprintf(stderr,"PWM frequency is %d\n", freq);
if (options.value>=0)
if(comedi_data_write(device,
options.subdevice,
options.channel,
0,
0,
options.value)<0)
{
fprintf(stderr,"error setting the pwm duty cycle on ");
comedi_perror(options.filename);
exit(1);
}
return 0;
}
/* -------------------------------------------------------------------- */
static long dsetWrite_devAoSyncComedi(aoRecord *pao) {
CONTEXT *p_myContext;
COMEDIDEV_AO *p_myAnalogOutput;
double myMinimumVoltage;
double myMaximumVoltage;
char p_myWarmstartFilename[BUFLEN];
FILE *p_myFileHandle;
dsetLog(3,__FILE__ "[%d] -> %s(%s)\n", __LINE__, __func__,pao->name);
pao->pact=TRUE;
p_myContext=(CONTEXT *)pao->dpvt;
p_myAnalogOutput=p_myContext->p_analogOutput;
myMaximumVoltage=p_myContext->maximumVoltage;
myMinimumVoltage=p_myContext->minimumVoltage;
if (pao->val>pao->drvh) pao->val=pao->drvh;
if (pao->val<pao->drvl) pao->val=pao->drvl;
umask(0000);
sprintf(p_myWarmstartFilename, WARMSTART_FILE_PREFIX "/%s", pao->name);
p_myFileHandle=fopen(p_myWarmstartFilename,"w");
if (p_myFileHandle==NULL) {
dsetLog(1, __FILE__ "[%d] Error: %s\n", __LINE__, pao->name);
dsetLog(1, __FILE__ "[%d] Error: Couldn't open file >%s<\n",
__LINE__, p_myWarmstartFilename);
sleep(SLEEPTIME_ERROR);
return(ERROR);
}
fprintf(p_myFileHandle,"%lg",pao->val);
fclose(p_myFileHandle);
if (pao->drvh!=myMaximumVoltage || pao->drvl!=myMinimumVoltage) {
p_myAnalogOutput->range=getOptimalOperatingRange(pao);
p_myAnalogOutput->c_range=comedi_get_range(p_myAnalogOutput->device,
p_myAnalogOutput->subdevice,
p_myAnalogOutput->channel,
p_myAnalogOutput->range);
}
p_myAnalogOutput->data=comedi_from_phys(
pao->val,
p_myAnalogOutput->c_range,
p_myAnalogOutput->maxdata);
dsetLog(7, __FILE__ "[%d] Synchronous write of a single value\n", __LINE__);
if( comedi_data_write(
p_myAnalogOutput->device,
p_myAnalogOutput->subdevice,
p_myAnalogOutput->channel,
p_myAnalogOutput->range,
p_myAnalogOutput->aref,
p_myAnalogOutput->data) <= ERROR ) {
comedi_perror(p_myAnalogOutput->p_deviceFilename);
sleep(SLEEPTIME_ERROR);
return(ERROR);
}
dsetLog(2,__FILE__ "[%d] %s -> %lg\n", __LINE__, pao->name, pao->val);
pao->pact=FALSE;
dsetLog(3, __FILE__ "[%d] <- %s\n", __LINE__, __func__ );
/* CONVERT according to calibration settings */
return(CONVERT);
}
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;
}
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 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 main(void)
{
RTIME until;
RT_TASK *task;
comedi_insn insn[NCHAN];
unsigned int read_chan[NICHAN] = { 0 };
unsigned int write_chan[NOCHAN] = { 0 };
#if !SINGLE_INSN
comedi_insnlist ilist = { NCHAN, insn };
#endif
#if SIN_FREQ
lsampl_t sinewave;
double omega = (2.0*M_PI*SIN_FREQ)/1.0E9;
double actualtime;
#endif
lsampl_t *hist;
lsampl_t data[NCHAN];
long i, k, n, retval;
int toggle;
FILE *fp;
double tms[2];
#if SINGLE_INSN
printf("single insn true\n");
#endif
#if SIN_FREQ
printf(" true\n");
#endif
signal(SIGKILL, endme);
signal(SIGTERM, endme);
hist = malloc(SAMP_FREQ*RUN_TIME*NCHAN*sizeof(lsampl_t) + 1000);
memset(hist, 0, SAMP_FREQ*RUN_TIME*NCHAN*sizeof(lsampl_t) + 1000);
start_rt_timer(0);
task = rt_task_init_schmod(nam2num("MYTASK"), 1, 0, 0, SCHED_FIFO, 0xF);
printf("COMEDI INSN%s TEST BEGINS: SAMPLING FREQ: %d, RUN TIME: %d.\n", SINGLE_INSN ? "" : "LIST", SAMP_FREQ, RUN_TIME);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
if (init_board()) {;
printf("Board initialization failed.\n");
return 1;
}
for (i = 0; i < NICHAN; i++) {
BUILD_AREAD_INSN(insn[i], subdevai, data[i], 1, read_chan[i], AI_RANGE, AREF_GROUND);
}
for (i = 0; i < NOCHAN; i++) {
BUILD_AWRITE_INSN(insn[NICHAN + i], subdevao, data[NICHAN + i], 1, write_chan[i], AO_RANGE, AREF_GROUND);
}
printf("done building.\n"); fflush (stdout);
until = rt_get_time();
for (toggle = n = k = 0; k < SAMP_FREQ*RUN_TIME && !end; k++) {
#if SIN_FREQ
actualtime = count2nano(rt_get_time());
if(k<2) tms[k] = actualtime;
sinewave = (int) (maxdatao/8*sin(omega*actualtime));
data[NICHAN] = sinewave+maxdatao/2;
//data[NICHAN + 1] = -sinewave+maxdatao/2;
#else
data[NICHAN] = toggle*maxdatao/2;
data[NICHAN + 1] = (1 - toggle)*maxdatao/2;
toggle = 1 - toggle;
#endif
#if SINGLE_INSN
for (i = 0; i < NCHAN; i++) {
if ((retval = comedi_do_insn(dev, insn + i)) > 0) {
hist[n++] = data[i];
} else {
printf("Comedi insn failed # %ld out of %d instructions, retval %ld.\n", i, NCHAN, retval);
break;
}
}
#else
if ((retval = rt_comedi_do_insnlist(dev, &ilist)) == NCHAN) {
for (i = 0; i < NCHAN; i++) {
hist[n++] = data[i];
}
} else {
printf("Comedi insnlist processed only %lu out of %d instructions.\n", retval, NCHAN);
break;
}
#endif
rt_sleep_until(until += nano2count(SAMP_TIME));
}
comedi_cancel(dev, subdevai);
comedi_cancel(dev, subdevao);
comedi_data_write(dev, subdevao, 0, 0, AREF_GROUND, 2048);
comedi_data_write(dev, subdevao, 1, 0, AREF_GROUND, 2048);
comedi_close(dev);
printf("COMEDI INSN%s ENDS.\n", SINGLE_INSN ? "" : "LIST");
printf("t1: %g\n", tms[0]);
printf("t2: %g\n", tms[1]);
printf("tdiff: %g\n", tms[1]-tms[0]);
fp = fopen("rec.dat", "w");
for (n = k = 0; k < SAMP_FREQ*RUN_TIME; k++) {
fprintf(fp, "# %ld: ", k);
for (i = 0; i < NCHAN; i++) {
fprintf(fp, "%d\t", hist[n++]);
}
fprintf(fp, "\n");
}
fclose(fp);
free(hist);
stop_rt_timer();
rt_make_soft_real_time();
rt_task_delete(task);
return 0;
}
void ComediAnalogOutputHardCal::write(double data)
{
lsampl_t sample = comedi_from_phys(data*m_outputConversionFactor, m_dataRange, m_maxData);
comedi_data_write(m_device, m_subdevice, m_channels[0], m_range, m_aref, sample);
}
// Write the argument to the output of the DAQ board.
void AnalogyAnalogOutputSoftCal::write(double data)
{
comedi_data_write(m_device, m_subdevice, m_channels[0], m_range, m_aref,
comedi_from_physical(data*m_outputConversionFactor, &m_converter));
}
