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