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); } }
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; }
BOOL bcsIoAsyncEventCallback(BowlerPacket *Packet,BOOL (*pidAsyncCallbackPtr)(BowlerPacket *)){ int i; BOOL update=FALSE; println_W("Async ");print_W(ioNSName); for(i=0;i<GetNumberOfIOChannels();i++){ //println_W("Checking ");p_int_W(i); if(pushAsyncReady(i)){ update=TRUE; } } if(update){ println_I(__FILE__);println_I("Async: "); populateGACV(Packet); Packet->use.head.Method=BOWLER_ASYN; FixPacket(Packet); printBowlerPacketDEBUG(Packet,INFO_PRINT); if(pidAsyncCallbackPtr!=NULL){ pidAsyncCallbackPtr(Packet); } } println_W("Done ");print_W(ioNSName); return FALSE; }
BOOL bcsSafeAsyncEventCallback(BowlerPacket *Packet,BOOL (*pidAsyncCallbackPtr)(BowlerPacket *Packet)){ println_W("Async ");print_W(safeNSName); return FALSE; }
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; } }