float pid(float value, float targetValue){
float error, lastOut = 0.0f, lastError = 0.0f, lastError2 = 0.0f,output,tmp1,tmp2,tmp3;
error = targetValue - value;
error = correctError(error);
if(error == 0.0){
return error;
}
tmp1 = error - lastError;
tmp1 *= KP;
tmp2 = (error + lastError) / 2.0f;
tmp2 *= KI;
tmp3 = error - (2.0f * lastError) + lastError2;
tmp3 *= KD;
output = lastOut + tmp1 + tmp2 + tmp3;
lastOut = output;
lastError2 = lastError;
lastError = error;
return output;
}
float pi(float value, float targetValue){
float error, lastOut = 0.0f, lastError = 0.0f,output,tmp1,tmp2;
error = targetValue - value;
error = correctError(error);
if(error == 0.0){
return error;
}
tmp1 = error - lastError;
tmp1 *= KP;
tmp2 = (error + lastError) / 2.0f;
tmp2 *= KI;
output = lastOut + tmp1 + tmp2;
output = fmaxf(output,MAXVAL);
output = fminf(output,MINVAL);
lastOut = output;
lastError = error;
return error;
}
float p(float value, float targetValue){
float error;
error = targetValue - value; //error is target - actual
ROS_DEBUG("Error: %.3f Target: %.3f Actual %.3f",error, targetValue, value);
error = correctError(error);
if(error == 0.0){
return error;
}
error *= KP; //multiply by constant
return error;
}
float pd(float value, float targetValue){
float error,derivative,lastError = 0.0f;
error = targetValue - value;
error = correctError(error);
if(error == 0.0){
return error;
}
derivative = lastError - error;
lastError = error;
error *= KP;
derivative *= KD;
error += derivative;
return error;
}
void PlSqlParser::correctError(int *token, ParsingTableRow *row, ParsingTableAction **actionOnCurrentToken)
{
//qDebug("--------started error recovery--------------");
bool reservedWord = parsingTable->isReservedWord(scanner->getTokenLexeme());
if(*token!=PLS_SEMI){
*actionOnCurrentToken=row->actions->value(PLS_NOT_SEMI, 0);
if(!reservedWord){
replaceKeywordWithIdentifier(*token, row, actionOnCurrentToken);
}
}
if(*actionOnCurrentToken == 0 && !this->strict){ //try to recognize major constructs
//read input until we encounter one of (first of) PLS_SEMI, END opt_identifier PLS_SEMI
//while reading add all read tokens to token stack
QList<TokenInfo*> reduceTokens;
bool hasNonUsedReduceTokens = false;
do{
if(!reservedWord || countOnLastPosition>maxStayCountOnSamePosition){
reduceTokens.prepend(createTokenInfo(*token));
hasNonUsedReduceTokens = true;
}
if(*token == PLS_SEMI || reservedWord){
int reducedConstruct = 0;
QList<int> expectedTerminals = getExpectedTokens(row).first;
//bool waitingForSemicolon = (expectedTerminals.size()==1 && expectedTerminals[0]==PLS_SEMI);
bool waitingForSemicolon = expectedTerminals.contains(PLS_SEMI);
if(reservedWord && countOnLastPosition<=maxStayCountOnSamePosition &&
!waitingForSemicolon){ //if current token is reserved word, try to reduce expression first
int constuctsToCheck[] = {R_EXPRESSION, R_DECLARATION, R_STATEMENT};
reducedConstruct = reduceMajorConstruct(reduceTokens, constuctsToCheck, 3);
}else{ //on semicolon try to reduce declaration or statement
int constuctsToCheck[] = {R_DECLARATION, R_STATEMENT};
reducedConstruct = reduceMajorConstruct(reduceTokens, constuctsToCheck, 2);
}
if(reducedConstruct){
//read next token for parser to operate
if(!reservedWord || countOnLastPosition>maxStayCountOnSamePosition){
*token = scanner->getNextToken();
if(*token == PLS_SEMI){
correctError(token, row, actionOnCurrentToken);
}
}
ParsingTableRow *newRow=parsingTable->rows.at(stack.top()); //reduceMajorConstruct pushed new gotoState into stack
*actionOnCurrentToken=(*newRow->actions).value(*token, 0);
replaceKeywordWithIdentifier(*token, newRow, actionOnCurrentToken);
}else{
qDeleteAll(reduceTokens);
}
hasNonUsedReduceTokens = false;
break;
}
*token = scanner->getNextToken();
reservedWord = parsingTable->isReservedWord(scanner->getTokenLexeme());
}while(*token != PLS_E_O_F);
if(hasNonUsedReduceTokens){
qDeleteAll(reduceTokens);
}
}
if(scanner->getTokenStartPos() == lastErrorPosition){
++countOnLastPosition;
}else if(countOnLastPosition > 0){
countOnLastPosition = 0;
}
lastErrorPosition = scanner->getTokenStartPos();
//qDebug("--------completed error recovery--------------");
}
