void InitPID(void){ BYTE i; //WORD loop; for (i=0;i<NUM_PID_GROUPS;i++){ //DyPID values dyPid[i].inputChannel=DYPID_NON_USED; dyPid[i].outputChannel=DYPID_NON_USED; dyPid[i].inputMode=0; dyPid[i].outputMode=0; pidGroups[i].Enabled=FALSE; pidGroups[i].channel = i; vel[i].enabled=FALSE; vel[i].K.P=.1; limits[i].type=NO_LIMIT; LoadPIDvals(&pidGroups[i],&dyPid[i]); if( dyPid[i].inputChannel==DYPID_NON_USED || dyPid[i].outputChannel==DYPID_NON_USED || dyPid[i].outputChannel==dyPid[i].inputChannel) { dyPid[i].inputChannel=DYPID_NON_USED; dyPid[i].outputChannel=DYPID_NON_USED; WritePIDvalues(&pidGroups[i],&dyPid[i]); } force[i].MsTime=0; force[i].setPoint=200; } InitilizePidController( pidGroups, vel, NUM_PID_GROUPS, &getPositionMine, &setOutputMine, &resetPositionMine, //&asyncCallback, &onPidConfigureMine, &checkPIDLimitEventsMine); for (i=0;i<NUM_PID_GROUPS;i++){ printPIDvals(i); if(pidGroups[i].Enabled){ initPIDChans(i); println_I("Resetting PID channel from init"); int value = 0; if(dyPid[i].inputMode == IS_ANALOG_IN) value = 512; pidReset(i,value); } } }
int main(){ //setPrintLevelInfoPrint(); setPrintLevelWarningPrint(); //setPrintLevelNoPrint(); hardwareInit(); RunEveryData loop = {0.0,2000.0}; while(1){ if (_RF5==1){ setPrintLevelErrorPrint(); p_int_E(0);print_E(" Reset Button Pressed from loop"); SetColor(1,1,1); U1CON = 0x0000; DelayMs(100); Reset(); } if(RunEvery(&loop)>0){ // clearPrint(); // printCartesianData(); } if( printCalibrations == false && GetPIDCalibrateionState(linkToHWIndex(0))==CALIBRARTION_DONE&& GetPIDCalibrateionState(linkToHWIndex(1))==CALIBRARTION_DONE&& GetPIDCalibrateionState(linkToHWIndex(2))==CALIBRARTION_DONE ){ printCalibrations = true; int index=0; for(index=0;index<3;index++){ int group = linkToHWIndex(index); println_E("For Axis ");p_int_E(group); print_E(" upper: ");p_int_E(getPidGroupDataTable(group)->config.upperHistoresis); print_E(" lower: ");p_int_E(getPidGroupDataTable(group)->config.lowerHistoresis); print_E(" stop: ");p_int_E(getPidGroupDataTable(group)->config.stop); } startHomingLinks(); //Print_Level l= getPrintLevel(); //setPrintLevelInfoPrint(); printCartesianData(); int i; for(i=0;i<numPidMotors;i++){ printPIDvals(i); } //setPrintLevel(l); } bowlerSystem(); } }
BOOL initFlashLocal(){ if(bytesOfRaw > 0x1000-FLASHSTORE){ println_E("Too much data to store"); SoftReset(); } println_W("Size of Flash data = ");p_int_W(bytesOfRaw); SetFlashData( localData.data ,bytesOfRaw/4); FlashLoad(); int i=0,j=0, index; BOOL rawFlashDetect=FALSE; for(i=0;i<numPidTotal;i++){ index = i*sizeof(AbsPID_Config); for(j=0;j<sizeof(AbsPID_Config)/4;j++){ getPidGroupDataTable()[i].raw[j]=localData.data[index+j]; } if( (getPidGroupDataTable()[i].config.Enabled != 1 && getPidGroupDataTable()[i].config.Enabled != 0) ){ rawFlashDetect = TRUE; println_E("Detected raw flash, setting defaults : ");p_int_E(i); printPIDvals(i); getPidGroupDataTable()[i].config.Enabled = FALSE; getPidGroupDataTable()[i].config.Async=0; getPidGroupDataTable()[i].config.IndexLatchValue=0; getPidGroupDataTable()[i].config.stopOnIndex=0; getPidGroupDataTable()[i].config.useIndexLatch=0; getPidGroupDataTable()[i].config.K.P=.1; getPidGroupDataTable()[i].config.K.I=0; getPidGroupDataTable()[i].config.K.D=0; getPidGroupDataTable()[i].config.V.P=.1; getPidGroupDataTable()[i].config.V.D=0; getPidGroupDataTable()[i].config.Polarity=1; getPidGroupDataTable()[i].config.stop=0; getPidGroupDataTable()[i].config.upperHistoresis=0; getPidGroupDataTable()[i].config.lowerHistoresis=0; } } if(rawFlashDetect ) writeFlashLocal(); return !rawFlashDetect; }
void writeFlashLocal(){ if(bytesOfRaw > 0x1000-FLASHSTORE){ println_E("Too much data to store"); SoftReset(); } println_W("Writing values to Flash"); int i=0,j=0, index; for(i=0;i<numPidTotal;i++){ index = i*sizeof(AbsPID_Config); for(j=0;j<sizeof(AbsPID_Config)/4;j++){ localData.data[index+j]=getPidGroupDataTable()[i].raw[j]; } } FlashSync(); FlashLoad(); for(i=0;i<numPidTotal;i++){ printPIDvals(i); } }
void InitPID(void){ //println_I("Starting PID initialization "); uint8_t i; //uint16_t loop; for (i=0;i<NUM_PID_GROUPS;i++){ //DyPID values dyPid[i].inputChannel=DYPID_NON_USED; dyPid[i].outputChannel=DYPID_NON_USED; dyPid[i].inputMode=0; dyPid[i].outputMode=0; dyPid[i].flagValueSync=false; pidGroups[i].config.tipsScale=1.0; pidGroups[i].config.Enabled = false; pidGroups[i].config.Async = 1; pidGroups[i].config.IndexLatchValue = 0; pidGroups[i].config.stopOnIndex = 0; pidGroups[i].config.useIndexLatch = 0; pidGroups[i].config.K.P = .1; pidGroups[i].config.K.I = 0; pidGroups[i].config.K.D = 0; pidGroups[i].config.V.P = .1; pidGroups[i].config.V.D = 0; pidGroups[i].config.Polarity = 0; pidGroups[i].config.stop = 0; pidGroups[i].config.upperHistoresis = 0; pidGroups[i].config.lowerHistoresis = 0; pidGroups[i].config.offset = 0.0; pidGroups[i].config.calibrationState = CALIBRARTION_DONE; pidGroups[i].interpolate.set=0; pidGroups[i].interpolate.setTime=0; pidGroups[i].interpolate.start=0; pidGroups[i].interpolate.startTime=0; limits[i].type=NO_LIMIT; LoadPIDvals(&pidGroups[i],&dyPid[i],i); if( dyPid[i].inputChannel==DYPID_NON_USED || dyPid[i].outputChannel==DYPID_NON_USED || dyPid[i].outputChannel==dyPid[i].inputChannel|| pidGroups[i].config.Enabled !=true) { dyPid[i].inputChannel=DYPID_NON_USED; dyPid[i].outputChannel=DYPID_NON_USED; pidGroups[i].config.Enabled = false; WritePIDvalues(&pidGroups[i],&dyPid[i],i); } force[i].MsTime=0; force[i].setPoint=200; } InitilizePidController( pidGroups, NUM_PID_GROUPS, &getPositionMine, &setOutputMine, &resetPositionMine, &onPidConfigureMine, &checkPIDLimitEventsMine); for (i=0;i<NUM_PID_GROUPS;i++){ // if(pidGroups[i].config.Enabled){ println_W("PID ");p_int_W(i); printPIDvals(i); initPIDChans(i); int value = getPositionMine(i); pidGroups[i].CurrentState=value; pidReset(i,value); } } }