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; }
BOOL isCartesianInterpolationDone(){ updateCurrentPositions(); float targets[3] = {xCurrent,yCurrent,zCurrent}; int setpointBound = 200; float mmPositionResolution = .3; int i; for(i=0;i<4;i++){ if(i<3){ if(!bound(intCartesian[i].set,targets[i],mmPositionResolution,mmPositionResolution)){ //println_W("Interpolation not done on to: ");p_fl_W(intCartesian[i].set);print_W(" is = ");p_fl_W(targets[i]); return FALSE; } } if( (isPIDArrivedAtSetpoint(linkToHWIndex(i), setpointBound) == FALSE) && (i==3) ){ println_W("LINK not done moving index = ");p_int_W(linkToHWIndex(i)); print_W(" currently is = "); p_fl_W(getPidGroupDataTable()[linkToHWIndex(i)].CurrentState); print_W(" heading towards = "); p_fl_W(getPidGroupDataTable()[linkToHWIndex(i)].SetPoint); return FALSE; } } return TRUE; }
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); } } }
void checkLinkHomingStatus(int group) { if (!(GetPIDCalibrateionState(group) == CALIBRARTION_home_down || GetPIDCalibrateionState(group) == CALIBRARTION_home_up || GetPIDCalibrateionState(group) == CALIBRARTION_home_velocity ) ) { return; //Calibration is not running } float current = GetPIDPosition(group); float currentTime = getMs(); if (RunEvery(&getPidGroupDataTable(group)->timer) > 0) { //println_W("Check Homing "); if (GetPIDCalibrateionState(group) != CALIBRARTION_home_velocity) { float boundVal = getPidGroupDataTable(group)->homing.homingStallBound; if (bound(getPidGroupDataTable(group)->homing.previousValue, current, boundVal, boundVal ) ) { pidReset(group, getPidGroupDataTable(group)->homing.homedValue); //after reset the current value will have changed current = GetPIDPosition(group); getPidGroupDataTable(group)->config.tipsScale = 1; println_W("Homing Velocity for group "); p_int_W(group); print_W(", Resetting position to: "); p_fl_W(getPidGroupDataTable(group)->homing.homedValue); print_W(" current "); p_fl_W(current); float speed = -20.0; if (GetPIDCalibrateionState(group) == CALIBRARTION_home_up) speed *= 1.0; else if (GetPIDCalibrateionState(group) == CALIBRARTION_home_down) speed *= -1.0; else { println_E("Invalid homing type"); return; } Print_Level l = getPrintLevel(); //setPrintLevelInfoPrint(); setOutput(group, speed); setPrintLevel(l); getPidGroupDataTable(group)->timer.MsTime = getMs(); getPidGroupDataTable(group)->timer.setPoint = 2000; SetPIDCalibrateionState(group, CALIBRARTION_home_velocity); getPidGroupDataTable(group)->homing.lastTime = currentTime; } } else { current = GetPIDPosition(group); float posDiff = current - getPidGroupDataTable(group)->homing.previousValue; //ticks float timeDiff = (currentTime - getPidGroupDataTable(group)->homing.lastTime) / 1000.0; // float tps = (posDiff / timeDiff); getPidGroupDataTable(group)->config.tipsScale = 20 / tps; println_E("New scale factor: "); p_fl_E(getPidGroupDataTable(group)->config.tipsScale); print_E(" speed "); p_fl_E(tps); print_E(" on "); p_int_E(group); print_E(" Position difference "); p_fl_E(posDiff); print_E(" time difference "); p_fl_E(timeDiff); OnPidConfigure(group); SetPIDCalibrateionState(group, CALIBRARTION_DONE); } getPidGroupDataTable(group)->homing.previousValue = current; } }