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