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 setOutput(int group, float val) { if(bound(0,getPidGroupDataTable(group)->config.tipsScale, .001, .001)){ println_W("PID TPS Sclale close to zero");p_fl_W(getPidGroupDataTable(group)->config.tipsScale); } val *= getPidGroupDataTable(group)->config.tipsScale; val += getPidStop(group); if (val > getPidStop(group) && val < getUpperPidHistoresis(group) ){ val = getUpperPidHistoresis(group); println_E("Upper histerisys"); } if (val < getPidStop(group) && val > getLowerPidHistoresis(group)){ val = getLowerPidHistoresis(group); println_E("Lower histerisys"); } getPidGroupDataTable(group)->OutputSet = val; // setOutputLocal(group, val); }
BYTE setInterpolateXYZ(float x, float y, float z,float ms){ int i=0; if(ms<.01) ms=0; float start = getMs(); intCartesian[0].set=x; intCartesian[1].set=y; intCartesian[2].set=z; updateCurrentPositions(); intCartesian[0].start=xCurrent; intCartesian[1].start=yCurrent; intCartesian[2].start=zCurrent; println_W("\n\nSetting new position x=");p_fl_W(x);print_W(" y=");p_fl_W(y);print_W(" z=");p_fl_W(z);print_W(" Time MS=");p_fl_W(ms); println_W("Current position cx=");p_fl_W(xCurrent); print_W(" cy=");p_fl_W(yCurrent); print_W(" cz=");p_fl_W(zCurrent); println_W("Current angles Alpha=");p_fl_W(getLinkAngle(0));print_W(" Beta=");p_fl_W(getLinkAngle(1));print_W(" Gamma=");p_fl_W(getLinkAngle(2)); for(i=0;i<3;i++){ intCartesian[i].setTime=ms; intCartesian[i].startTime=start; } if(ms==0){ setXYZ( x, y, z,0); }else{ keepCartesianPosition=TRUE; float ms= getMs(); x = interpolate((INTERPOLATE_DATA *)&intCartesian[0],ms); y = interpolate((INTERPOLATE_DATA *)&intCartesian[1],ms); z = interpolate((INTERPOLATE_DATA *)&intCartesian[2],ms); setXYZ( x, y, z,0); } }
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; } }