void motorControl::controlLoop(void)
{
int32 error=0;
float cotexDrive = 0.0;
bool keepReading=TRUE;
bool32 isLate = {0};
double tick=0.0,tock=0.0;
float64 motorCommand[3]={0.0,0.0,0.0},errorForce[2]= {0.0,0.0},integral[2]={0.0,0.0},EMG={0.0};
char errBuff[2048]={'\0'};
FILE *dataFile;
time_t t = time(NULL);
tm* timePtr = localtime(&t);
char fileName[200];
char dataSample[600]="";
char dataTemp[100]="";
sprintf_s(
fileName,
"C:\\data\\realTimeData%4d_%02d_%02d_%02d_%02d_%02d.txt",
timePtr->tm_year+1900,
timePtr->tm_mon+1,
timePtr->tm_mday,
timePtr->tm_hour,
timePtr->tm_min,
timePtr->tm_sec
);
dataFile = fopen(fileName,"w");
//fprintf(dataFile,"Time, Load Cell1, Load Cell2, Motor Command1, Motor Command2, Length 1, Length2, Velocity1, Velocity2, EMG1, EMG2, is sample missed\n");
//fprintf(dataFile,"Time, Load Cell1, Load Cell2, Length 1, Length2, Velocity1, Velocity2, EMG1, EMG2, GammaStat, GammaDyn, is sample missed\n");
//fprintf(dataFile,"Time, Load Cell1, Load Cell2, Length 1, Length2, motorRef1, motorRef2, spindleIa1, spindleIa2, spindleII1, spindleII2, EMG1, EMG2, GammaStat, GammaDyn, is sample missed\n");
//fprintf(dataFile,"Time, Length 1, Length2, EMG1, EMG2, GammaStat, GammaDyn, is sample missed\n");
fprintf(dataFile,header);
DAQmxErrChk (DAQmxStartTask(loadCelltaskHandle));
DAQmxErrChk (DAQmxStartTask(motorTaskHandle));
DAQmxErrChk (DAQmxStartTask(encodertaskHandle[0]));
DAQmxErrChk (DAQmxStartTask(encodertaskHandle[1]));
timeData.resetTimer();
tick = timeData.getCurrentTime();
float64 goffsetLoadCell[2]={0};
int expProtocol = 0;
int expProtocoAdvance = 0;
while(live)
{
WaitForSingleObject(hIOMutex, INFINITE);
//desire Forced, muscle Length, muscle Velocity PIPES should be read here
DAQmxErrChk(DAQmxWaitForNextSampleClock(loadCelltaskHandle,10, &isLate));
DAQmxErrChk (DAQmxReadAnalogF64(loadCelltaskHandle,-1,10.0,DAQmx_Val_GroupByScanNumber,loadCellData,2,NULL,NULL));
DAQmxErrChk (DAQmxWriteAnalogF64(motorTaskHandle,1,FALSE,10,DAQmx_Val_GroupByChannel,motorCommand,NULL,NULL));
DAQmxErrChk (DAQmxReadCounterF64(encodertaskHandle[0],1,10.0,encoderData1,1,NULL,0));
DAQmxErrChk (DAQmxReadCounterF64(encodertaskHandle[1],1,10.0,encoderData2,1,NULL,0));
if (dataAcquisitionFlag[1]){
EMG = muscleEMG[0];
if (EMG > 6)
EMG = 6;
if (EMG < -6)
EMG = -6;
}
else
EMG = 0;
tock = timeData.getCurrentTime();
if (resetMuscleLength)
{
muscleLengthOffset[0] = 2 * PI * shaftRadius * encoderData1[0] / 365;
muscleLengthOffset[1] = 2 * PI * shaftRadius * encoderData2[0] / 365;
resetMuscleLength = FALSE;
}
muscleLength[0] = ((2 * PI * shaftRadius * encoderData1[0] / 365) - muscleLengthOffset[0]);
//muscleLength[0] = 0.95 + (muscleLength[0] + 0.0059)*24.7178;
//muscleLength[0] = 0.95 + (muscleLength[0] + 0.0059)*40;
muscleLength[0] = encoderBias[0] + muscleLength[0] *encoderGain[0];
muscleLength[1] = ((2 * PI * shaftRadius * encoderData2[0] / 365) - muscleLengthOffset[1]);
//muscleLength[1] = 1 + (muscleLength[1] - 0.0058)*30 + 0.5;
//muscleLength[1] = 0.95 + (muscleLength[1] - 0.0058)*24.4399;
muscleLength[1] = encoderBias[1] + muscleLength[1] *encoderGain[1];
muscleVel[0] = (muscleLength[0] - muscleLengthPreviousTick[0]) / (tock - tick);
muscleVel[1] = (muscleLength[1] - muscleLengthPreviousTick[1]) / (tock - tick);
muscleLengthPreviousTick[0] = muscleLength[0];
muscleLengthPreviousTick[1] = muscleLength[1];
loadCellData[0] = (loadCellData[0] * loadCellScale1) - loadCellOffset1;
loadCellData[1] = (loadCellData[1] * loadCellScale2) - loadCellOffset2;
errorForce[0] = motorRef[0] - loadCellData[0];
errorForce[1] = motorRef[1] - loadCellData[1];
integral[0] = integral[0] + errorForce[0] * (tock - tick);
integral[1] = integral[1] + errorForce[1] * (tock - tick);
motorCommand[0] = integral[0] * I;
motorCommand[1] = integral[1] * I;
motorCommand[2] = EMG;
if (motorCommand[0] > motorMaxVoltage)
motorCommand[0] = motorMaxVoltage;
if (motorCommand[0] < motorMinVoltage)
motorCommand[0] = motorMinVoltage;
if (motorCommand[1] > motorMaxVoltage)
motorCommand[1] = motorMaxVoltage;
if (motorCommand[1] < motorMinVoltage)
motorCommand[1] = motorMinVoltage;
printf("F1: %+6.2f; F2: %+6.2f;L1: %+6.2f; L2: %+6.2f;, Dyn: %d, Sta: %d, \r",loadCellData[0],loadCellData[1],muscleLength[0],muscleLength[1],gammaDynamic1, gammaStatic1);
ReleaseMutex( hIOMutex);
//fprintf(dataFile,"%.3f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%d\n",tock,loadCellData[0],loadCellData[1],motorRef[0],motorRef[1], muscleLength[0], muscleLength[1], muscleVel[0],muscleVel[1], muscleEMG[0], muscleEMG[1], isLate);
//fprintf(dataFile,"%.3f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%d,%d,%d\n",tock,loadCellData[0],loadCellData[1], muscleLength[0], muscleLength[1], muscleVel[0],muscleVel[1], muscleEMG[0], muscleEMG[1], gammaStatic, gammaDynamic, isLate);
//fprintf(dataFile,"%.3f,%.6f,%.6f,%.6f,%.6f,%d,%d,%d\n",tock, muscleLength[0], muscleLength[1], muscleEMG[0], muscleEMG[1], gammaStatic, gammaDynamic, isLate);
sprintf(dataSample,"%.3f,%d,%.6f,%.6f,%.6f,%.6f",tock,expProtocol,muscleLength[0], muscleLength[1], loadCellData[0],loadCellData[1]);
if (dataAcquisitionFlag[0]){
sprintf(dataTemp,",%.6f,%.6f",motorRef[0],motorRef[1]);
strcat (dataSample, dataTemp);
}
if (dataAcquisitionFlag[1]){
sprintf(dataTemp,",%.6f,%.6f",muscleEMG[0], muscleEMG[1]);
strcat (dataSample, dataTemp);
}
if (dataAcquisitionFlag[2]){
sprintf(dataTemp,",%.6f,%.6f",spindleIa[0], spindleIa[1]);
strcat (dataSample, dataTemp);
}
if (dataAcquisitionFlag[3]){
sprintf(dataTemp,",%.6f,%.6f",spindleII[0], spindleII[1]);
strcat (dataSample, dataTemp);
}
if (dataAcquisitionFlag[4]){
sprintf(dataTemp,",%d,%d",muscleSpikeCount[0], muscleSpikeCount[1]);
strcat (dataSample, dataTemp);
}
if (dataAcquisitionFlag[5]){
sprintf(dataTemp,",%u,%u",raster_MN_1[0], raster_MN_1[1]);
strcat (dataSample, dataTemp);
}
if (dataAcquisitionFlag[6]){
sprintf(dataTemp,",%u,%u",raster_MN_2[0], raster_MN_2[1]);
strcat (dataSample, dataTemp);
}
if (dataAcquisitionFlag[7]){
sprintf(dataTemp,",%u,%u",raster_MN_3[0], raster_MN_3[1]);
strcat (dataSample, dataTemp);
}
if (dataAcquisitionFlag[8]){
sprintf(dataTemp,",%u,%u",raster_MN_4[0], raster_MN_4[1]);
strcat (dataSample, dataTemp);
}
if (dataAcquisitionFlag[9]){
sprintf(dataTemp,",%u,%u",raster_MN_5[0], raster_MN_5[1]);
strcat (dataSample, dataTemp);
}
if (dataAcquisitionFlag[10]){
sprintf(dataTemp,",%u,%u",raster_MN_6[0], raster_MN_6[1]);
strcat (dataSample, dataTemp);
}
if (dataAcquisitionFlag[11]){
cortexDrive[0] = max((cortexVoluntaryAmp -0) * sin (2 * 3.1416 * cortexVoluntaryFreq * tick), 0);
cortexDrive[1] = max((cortexVoluntaryAmp -0) * sin (2 * 3.1416 * cortexVoluntaryFreq * tick + 3.1416), 0);
}
//sprintf(dataTemp,",%d,%d,%d,%d,%.3f,%.3f,%d\n",gammaStatic1, gammaDynamic1, gammaStatic2, gammaDynamic2, cortexDrive[0], cortexDrive[1],newTrial);
sprintf(dataTemp,"\n");
if (trialTrigger == 1){
expProtocoAdvance = 1;
trialTrigger = 0;
}
if (trialTrigger == 2){
expProtocoAdvance = 10;
trialTrigger = 0;
}
if (trialTrigger == 3){
expProtocoAdvance = 11;
trialTrigger = 0;
}
expProtocol = 0;
switch(expProtocoAdvance){
case 1:
expProtocol = -1000;
expProtocoAdvance = 2;
break;
case 2:
expProtocol = gammaDynamic1;
expProtocoAdvance = 3;
break;
case 3:
expProtocol = gammaStatic1;
expProtocoAdvance = 4;
break;
case 4:
expProtocol = cortexDrive[0];
expProtocoAdvance = 5;
break;
case 5:
expProtocol = gammaDynamic2;
expProtocoAdvance = 6;
break;
case 6:
expProtocol = gammaStatic2;
expProtocoAdvance = 7;
break;
case 7:
expProtocol = cortexDrive[1];
expProtocoAdvance = 8;
break;
case 8:
expProtocol = angle;
expProtocoAdvance = 9;
break;
case 9:
expProtocol = velocity;
expProtocoAdvance = 0;
break;
case 10:
expProtocol = -1;
expProtocoAdvance = 0;
break;
case 11:
expProtocol = -2;
expProtocoAdvance = 0;
break;
}
strcat (dataSample, dataTemp);
fprintf(dataFile,dataSample);
tick = timeData.getCurrentTime();
}
isControlling = FALSE;
DAQmxStopTask(motorTaskHandle);
fclose(dataFile);
Error:
if( DAQmxFailed(error) ) {
DAQmxGetExtendedErrorInfo(errBuff,2048);
/*********************************************/
// DAQmx Stop Code
/*********************************************/
DAQmxStopTask(loadCelltaskHandle);
DAQmxClearTask(loadCelltaskHandle);
DAQmxStopTask(encodertaskHandle);
DAQmxClearTask(encodertaskHandle);
DAQmxStopTask(motorTaskHandle);
DAQmxClearTask(motorTaskHandle);
DAQmxStopTask(motorEnableHandle);
DAQmxClearTask(motorEnableHandle);
printf("DAQmx Error: %s\n",errBuff);
printf("Motor control Error\n");
}
}
int32 Tweezers::Run(float64* force, float64* indata, void* lpParam)
{
threadinfo* t = (threadinfo*)lpParam;
// init output data array
float64 out_data[1];
out_data[0] = 0.0;
register int i = 0,j = 0;
// # of samples to be generated per cycle
register int nofSamples = (int)(t->nofFrames/(*t->cycles)) * (*t->delta)*1E-3 * Srate_HTSP;
//cerr<<nofSamples<<" samples\n";
// setup HTSP timing and deactivate onboard memory
DAQmxErrRtn(DAQmxCfgSampClkTiming(AOtaskHandle,"OnboardClock",Srate_HTSP,DAQmx_Val_Rising,DAQmx_Val_HWTimedSinglePoint,nofSamples));
DAQmxSetRealTimeConvLateErrorsToWarnings(AOtaskHandle, TRUE);
// setup start trigger
DAQmxErrRtn(DAQmxCfgDigEdgeStartTrig(AOtaskHandle, "PFI0", DAQmx_Val_Rising));
// setup counter task and start trigger
// maybe change trigger to digital line? need to use ContSamps, otherwise limited frame #
//DAQmxErrRtn(DAQmxCreateCOPulseChanTime(DOtaskHandle,"Dev1/Ctr0","",DAQmx_Val_Seconds,DAQmx_Val_Low,0,1E-3*(*t->delta)/2,1E-3*(*t->delta/2)));
DAQmxErrRtn(DAQmxCfgImplicitTiming(DOtaskHandle, DAQmx_Val_ContSamps, 1));
// setup start trigger
DAQmxErrRtn(DAQmxCfgDigEdgeStartTrig(DOtaskHandle, "PFI0", DAQmx_Val_Rising));
// setup analog in task
DAQmxErrRtn(DAQmxCfgSampClkTiming(AItaskHandle,"Ctr0Out",100,DAQmx_Val_Rising,DAQmx_Val_FiniteSamps,*t->cycles * t->nofFrames));
//DAQmxErrRtn(DAQmxCfgDigEdgeStartTrig(AItaskHandle, "PFI0", DAQmx_Val_Rising));
// start tasks (still waiting for edge trigger from TRtask)
DAQmxErrRtn(DAQmxStartTask(DOtaskHandle));
DAQmxErrRtn(DAQmxStartTask(AOtaskHandle));
DAQmxErrRtn(DAQmxStartTask(AItaskHandle));
// precalulate number of loop runs to speed up loop
int samps = (nofSamples * (*t->cycles) - 1);
// write first sample outside loop, so there is enough time to send the TR start trigger
out_data[0] = current(force[0],t->dist);
DAQmxErrRtn(DAQmxWriteAnalogF64(AOtaskHandle,1,true,-1,DAQmx_Val_GroupByChannel,out_data,&written,NULL));
// GO!
DAQmxErrRtn(DAQmxWriteDigitalLines(TRtaskHandle,1,1,10,DAQmx_Val_GroupByChannel,trigdata[1],&written,NULL));
// 1 kHz sample generation loop
while(!(*t->bead_lost &&
// [email protected]: 05/10/2012 16:16 - dont stop recording if bead is lost in Creep-noint protocol
*t->protocol != "Creep-noint"
// end of change
) && i < samps)
{
j = (++i % nofSamples);
//cerr<<j<<"\r";
out_data[0] = current(force[j],t->dist);
DAQmxErrRtn(DAQmxWaitForNextSampleClock(AOtaskHandle, 1.0, &islate));
DAQmxErrRtn(DAQmxWriteAnalogF64(AOtaskHandle,1,true,-1,DAQmx_Val_GroupByChannel,out_data,&written,NULL));
if(islate)
{
cerr<<i++<<"\n****** late write - out of sync! *******\n";
*t->bead_lost = TRUE;
t->syncerror = TRUE;
}
}
printf("DEBUG: bead_lost(%d) protocol(%s) i(%d) samps(%d)\n",
*t->bead_lost, *t->protocol, i, samps);
// wait for last sample to be generated
DAQmxErrRtn(DAQmxWaitForNextSampleClock(AOtaskHandle, 1.0, &islate));
// make sure enough trigger pulses are generated
Sleep(*t->delta * 5);
// stop tasks
DAQmxErrRtn(DAQmxStopTask(DOtaskHandle));
DAQmxErrRtn(DAQmxStopTask(AOtaskHandle));
// number of frames actually taken
t->FramesTaken = 1 + floor(t->nofFrames * (double)i/(double)(nofSamples * (*t->cycles)));
// read protocol samples
DAQmxReadAnalogF64(AItaskHandle,t->FramesTaken,5.0,DAQmx_Val_GroupByChannel,indata,t->FramesTaken*2,&read,NULL);
DAQmxErrRtn(DAQmxStopTask(AItaskHandle));
// disable start triggers
DAQmxErrRtn(DAQmxDisableStartTrig(DOtaskHandle));
DAQmxErrRtn(DAQmxDisableStartTrig(AOtaskHandle));
// reset trigger line
DAQmxErrRtn(DAQmxWriteDigitalLines(TRtaskHandle,1,1,10,DAQmx_Val_GroupByChannel,trigdata[0],&written,NULL));
}