예제 #1
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;
}
예제 #2
0
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);
}
예제 #3
0
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;
    }
}