//*****************************************************************************
//
//! Control robot to go straight forward by following wall
//!
//!
//! \return true if left/right wall is detected
//! false if no left/right wall is detected
//
static bool Forward()
{
LED1_OFF();LED2_ON();LED3_OFF();
//bluetooth_print("%d\r\n",avrSpeed);
if ((!isWallLeft) || (!isWallRight))
{
forwardUpdateByWall();
return true;
}
forwardUpdate();
if (avrSpeed<AVG_SPEED_FWD_FAST-20)
avrSpeed+=20;
else if (avrSpeed>AVG_SPEED_FWD_FAST)
avrSpeed=AVG_SPEED_FWD_FAST;
if (isWallRight)
{
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_RIGHT);
}
else if (isWallLeft)
{
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_LEFT);
}
else
{
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
return false;
}
void dspGLTransactions::sFillList()
{
if (!_metrics->boolean("ManualForwardUpdate") &&
_showRunningTotal->isChecked() &&
_showRunningTotal->isVisible())
{
if (!forwardUpdate())
return;
}
ParameterList params;
if (! setParams(params))
return;
if (_showRunningTotal->isChecked() &&
_showRunningTotal->isVisible())
{
list()->showColumn("running");
qDebug("begbal %f", params.value("beginningBalance").toDouble());
_beginningBalance->setDouble(params.value("beginningBalance").toDouble());
}
else
list()->hideColumn("running");
display::sFillList();
}
void dspTrialBalances::sPrint()
{
if (!_metrics->boolean("ManualForwardUpdate"))
{
if (!forwardUpdate())
return;
}
display::sPrint();
}
void dspGLTransactions::sPrint()
{
if (!_metrics->boolean("ManualForwardUpdate") &&
_showRunningTotal->isChecked())
{
if (!forwardUpdate())
return;
}
display::sPrint();
}
void dspTrialBalances::sFillList()
{
list()->clear();
if (!_metrics->boolean("ManualForwardUpdate"))
{
if (!forwardUpdate())
return;
}
display::sFillList();
}
void dspFinancialReport::sFillList()
{
if (!sCheck())
return;
if (!_metrics->boolean("ManualForwardUpdate"))
{
if (!forwardUpdate())
return;
}
if (_trend->isChecked())
sFillListTrend();
else
sFillListStatement();
}
void dspTrialBalances::sPrint()
{
if (!_metrics->boolean("ManualForwardUpdate"))
{
if (!forwardUpdate())
return;
}
ParameterList params;
setParams(params);
orReport report("TrialBalances", params);
if (report.isValid())
report.print();
else
report.reportError(this);
}
void dspGLTransactions::sPrint()
{
if (!_metrics->boolean("ManualForwardUpdate") &&
_selectedAccount->isChecked() &&
_showRunningTotal->isChecked())
{
if (!forwardUpdate())
return;
}
ParameterList params;
if (! setParams(params))
return;
orReport report("GLTransactions", params);
if (report.isValid())
report.print();
else
report.reportError(this);
}
void pid_Wallfollow_process(void)
{
if (ControlFlag)
{
static int i;
pid_Runtimeout(&pid_process_callback, ui32_msLoop);
ControlFlag = false;
leftError=(float)IR_get_calib_value(IR_CALIB_BASE_LEFT) - (float)IR_GetIrDetectorValue(1);
rightError=(float)IR_get_calib_value(IR_CALIB_BASE_RIGHT) - (float)IR_GetIrDetectorValue(2);
isWallLeft = IR_GetIrDetectorValue(1)<IR_get_calib_value(IR_CALIB_MAX_LEFT);
isWallRight = IR_GetIrDetectorValue(2)<IR_get_calib_value(IR_CALIB_MAX_RIGHT);
isWallFrontLeft = IR_GetIrDetectorValue(0)<IR_get_calib_value(IR_CALIB_MAX_FRONT_LEFT);
isWallFrontRight = IR_GetIrDetectorValue(3)<IR_get_calib_value(IR_CALIB_MAX_FRONT_RIGHT);
switch(eMove)
{
case FORWARD:
switch (moveStage)
{
case 1:
if (Forward())
moveStage++;
if (isWallFrontLeft| isWallFrontRight)
{
preMove=eMove;
eMove=getMove(isWallLeft,isWallFrontLeft|isWallFrontRight,isWallRight);
}
break;
case 2:
posLeftTmp=qei_getPosLeft();
moveStage++;
i=1;
avrSpeedTmp=avrSpeed;
case 3://slow down
forwardUpdate();
if (!isWallLeft)
{
rqTurnLeft=true;
}
if (!isWallRight)
{
rqTurnRight=true;
}
if ((abs(qei_getPosLeft()-posLeftTmp)<5000)
&& (!isWallFrontLeft) && (!isWallFrontRight))
{
if ((abs(qei_getPosLeft()-posLeftTmp)>i*500) && (avrSpeed>AVG_SPEED_FWD-40))
{
avrSpeed -= 10;
i++;
}
if (isWallLeft|isWallRight)
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_AUTO);
else
{
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_FORWARD_MOVE
speed_Enable_Hbridge(false);
#endif
preMove=eMove;
eMove=getMove(!rqTurnLeft,isWallFrontLeft|isWallFrontRight,!rqTurnRight);
if (eMove==FORWARD)
avrSpeed=AVG_SPEED_FWD;
rqTurnLeft=false;
rqTurnRight=false;
moveStage=1;
}
break;
}
break;
case TURN_LEFT:
switch (moveStage)
{
case 1:
if (preMove!=FORWARD)//after turning left or right
//test
// ____
// | |
// | | |
fwdPulse=6000;
else if ((preMove==FORWARD) && (avrSpeed<AVG_SPEED_FWD_FAST))
//after turning back
//test
// ___
// |__ |
// |__|
fwdPulse=5500;
else//after moving forward
//test
// ___
// ___ |
// | |
// |_|
fwdPulse=4500;
moveStage++;
case 2:
if (TurnLeft(fwdPulse,60,240,7800,1700+CELL_ENC))
{
moveStage++;
}
break;
case 3:
posLeftTmp=qei_getPosLeft();
moveStage++;
case 4:
//go straight a little bit to check wall
forwardUpdate();
if (abs(qei_getPosLeft()-posLeftTmp)<2000)
{
if (abs(qei_getPosLeft()-posLeftTmp)>1500)
{
if (!isWallRight)
rqTurnRight=1;
if (!isWallLeft)
rqTurnLeft=1;
}
avrSpeed=AVG_SPEED_FWD_SLOW;
if (isWallLeft|isWallRight)
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_AUTO);
else
{
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_RIGHT, avrSpeed);//left motor is faster
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_TURNLEFT_MOVE3
speed_Enable_Hbridge(false);
#endif
//time to check front wall
preMove=eMove;
eMove=getMove(!rqTurnLeft,isWallFrontLeft|isWallFrontRight,!rqTurnRight);
rqTurnLeft=false;
rqTurnRight=false;
moveStage=1;
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
}
}
break;
case TURN_RIGHT:
switch (moveStage)
{
case 1:
if (preMove!=FORWARD)//after turning left or right
//test
// ____
// | |
// | | |
fwdPulse=6000;
else if ((preMove==FORWARD) && (avrSpeed<AVG_SPEED_FWD_FAST))
//after turning back
//test
// ____
// | ____
// |__|
fwdPulse=5500;
else//after moving forward
//test
// _____
// | _____
// | |
// |_|
fwdPulse=4500;
moveStage++;
case 2:
if (TurnRight(fwdPulse,200,40,8000,1700+CELL_ENC))
{
moveStage++;
}
break;
case 3:
posLeftTmp=qei_getPosLeft();
moveStage++;
case 4:
forwardUpdate();
if (abs(qei_getPosLeft()-posLeftTmp)<1000)
{
if (abs(qei_getPosLeft()-posLeftTmp)>500)
{
if (!isWallRight)
rqTurnRight=1;
if (!isWallLeft)
rqTurnLeft=1;
}
avrSpeed=AVG_SPEED_FWD_SLOW;
if (isWallLeft|isWallRight)
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_AUTO);
else
{
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_TURNRIGHT_MOVE3
speed_Enable_Hbridge(false);
#endif
preMove=eMove;
eMove=getMove(!rqTurnLeft,isWallFrontLeft|isWallFrontRight,!rqTurnRight);
rqTurnLeft=false;
rqTurnRight=false;
moveStage=1;
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
}
}
break;
case TURN_BACK:
switch (moveStage)
{
case 1:
if (preMove==FORWARD)
fwdPulse=8000;
else
fwdPulse=9000;
moveStage++;
case 2:
if (TurnBack(fwdPulse,-140,60,8000,13000))
{
//rotate more if we still detect front wall: do it yourself ;D
moveStage++;
}
break;
case 3:
if (move(-9000,-9000,AVG_SPEED_BWD,AVG_SPEED_BWD))
{
#ifdef TEST_TURNBACK_BACKWARD
speed_Enable_Hbridge(false);
#endif
forwardUpdate();
avrSpeed = AVG_SPEED_FWD_SLOW;
preMove=eMove;
eMove=FORWARD;
moveStage = 1;
}
}
break;
}
}
}
//*****************************************************************************
//
//! Control two motor to make robot turn back 180 degree.
//!
//! \param fwdPulse is the distance robot will go straight before turn right
//!, the robot will stand between the next cell of maze.
//! \param avrSpeedLeft is the speed of left motor.
//! \param avrSpeedRight is the speed of left motor.
//! \param NumPulse is the total pulse of two encoder after turn
//! \param resetEnc is the reset value for encoder after turning back
//! \return true if finish
//! false if not
//
static bool TurnBack(int fwdPulse, int avrSpeedLeft,int avrSpeedRight,int turnPulse,
int resetEnc)
{
LED1_ON();LED2_ON();LED3_ON();
switch (CtrlStep)
{
case 1:
{
posLeftTmp = qei_getPosLeft();
avrSpeedTmp = avrSpeed;
CtrlStep++;
}
case 2://go forward a litte bit
{
if (abs(qei_getPosLeft()-posLeftTmp)<fwdPulse)
{
avrSpeed = ((abs(fwdPulse + posLeftTmp - qei_getPosLeft()) / (fwdPulse / avrSpeedTmp)) / 2)
+ (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
if (isWallRight)
pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_RIGHT);
else if (isWallLeft)
pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_LEFT);
else
{
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
forwardUpdate();
CtrlStep++;
avrSpeed = avrSpeedTmp;
}
break;
}
case 3:
posLeftTmp=qei_getPosLeft();
posRightTmp=qei_getPosRight();
CtrlStep++;
case 4://turing 90 degree
{
#ifdef TEST_TURNBACK_FWD
speed_Enable_Hbridge(false);
#endif
if ((abs(qei_getPosLeft()-posLeftTmp)+abs(qei_getPosRight()-posRightTmp))<turnPulse)
{
speed_set(MOTOR_RIGHT, avrSpeedRight);
speed_set(MOTOR_LEFT, avrSpeedLeft);
}
else
{
currentDir=(currentDir+3)%4;
CtrlStep++;
}
break;
}
case 5:
posLeftTmp=qei_getPosLeft();
posRightTmp=qei_getPosRight();
CtrlStep++;
case 6://turning another 90 degree
{
#ifdef TEST_TURNBACK_TURN1
speed_Enable_Hbridge(false);
#endif
if ((abs(qei_getPosLeft()-posLeftTmp)+abs(qei_getPosRight()-posRightTmp))<turnPulse)
{
speed_set(MOTOR_RIGHT, -avrSpeedLeft);
speed_set(MOTOR_LEFT, -avrSpeedRight);
}
else
{
#ifdef TEST_TURNBACK_TURN2
speed_Enable_Hbridge(false);
#endif
currentDir=(currentDir+3)%4;
clearPosition();
qei_setPosLeft(resetEnc);
qei_setPosRight(resetEnc);
forwardUpdate();
CtrlStep=1;
return true;
}
break;
}
}
return false;
}
//*****************************************************************************
//
//! Control two motor to make robot turn left 90 degree
//!
//! \param fwdPulse is the distance robot will go straight before turn right
//!, the robot will stand between the next cell of maze.
//! \param avrSpeedLeft is the speed of left motor.
//! \param avrSpeedRight is the speed of right motor.
//! \param turnPulse is the total pulse of two encoder after turn
//! \param resetEnc is reset value for encoder after turning 90 degree, ignore this if you don't want to estimate position
//! \return true if finish
//! false if not
//
//*****************************************************************************
static bool TurnLeft(int fwdPulse,int avrSpeedLeft,int avrSpeedRight,int turnPulse,
int resetEnc)
{
static int vt,vp;
LED1_ON();LED2_OFF();LED3_OFF();
// bluetooth_print("LS %d\r\n",CtrlStep);
switch (CtrlStep)
{
case 1:
posLeftTmp=qei_getPosLeft();
CtrlStep++;
avrSpeedTmp=avrSpeed;
case 2://go straight
if ((abs(qei_getPosLeft()-posLeftTmp)<fwdPulse) ||
(isWallFrontLeft && isWallFrontRight &&
(IR_GetIrDetectorValue(3)>IR_get_calib_value(IR_CALIB_BASE_FRONT_RIGHT))&&
(IR_GetIrDetectorValue(0)>IR_get_calib_value(IR_CALIB_BASE_FRONT_LEFT))))
{
if (qei_getPosLeft()<fwdPulse+posLeftTmp)
avrSpeed = ((abs(fwdPulse + posLeftTmp - qei_getPosLeft()) / (fwdPulse / avrSpeedTmp)) / 2)
+ (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
else
avrSpeed = (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
if (isWallRight)
pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_RIGHT);
else
{
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_TURNLEFT_MOVE1
speed_Enable_Hbridge(false);
#endif
pid_reset(&pid_wall_right);
pid_reset(&pid_wall_left);
forwardUpdate();
CtrlStep++;
avrSpeed=avrSpeedTmp;
}
break;
case 3:
posLeftTmp=qei_getPosLeft();
posRightTmp=qei_getPosRight();
CtrlStep++;
vp=1;
vt=1;
case 4://turn 90 degree
if (abs(qei_getPosLeft()-posLeftTmp) + abs(qei_getPosRight()-posRightTmp) < turnPulse)
{
speed_set(MOTOR_RIGHT, avrSpeedRight);
speed_set(MOTOR_LEFT, -avrSpeedLeft);
if((abs(qei_getPosRight()-posRightTmp)>(turnPulse*0.8*vp/8)) && (vp<9))
{
if (avrSpeedRight>=24)
avrSpeedRight-=24;
vp++;
}
if((abs(qei_getPosLeft()-posLeftTmp)>(turnPulse*0.2*vt/8)) && (vt<9))
{
if (avrSpeedLeft>=4)
avrSpeedLeft-=4;
vt++;
}
}
else
{
#ifdef TEST_TURNLEFT_TURN
speed_Enable_Hbridge(false);
#endif
currentDir=(currentDir+3)%4;
clearPosition();
qei_setPosLeft(resetEnc);
qei_setPosRight(resetEnc);
forwardUpdate();
CtrlStep=1;
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_LEFT, avrSpeed);
speed_set(MOTOR_RIGHT, avrSpeed);
return true;
}
break;
}
return false;
}
void dspTrialBalances::sFillList()
{
_trialbal->clear();
if (!_metrics->boolean("ManualForwardUpdate"))
{
if (!forwardUpdate())
return;
}
QString sql( "SELECT accnt_id, period_id, accnt_descrip, trialbal_dirty,"
" period_start, period_end,"
" formatGLAccount(accnt_id) AS account,"
" (trialbal_debits) AS debits,"
" (trialbal_credits) AS credits,"
" trialbal_beginning AS beginning,"
" trialbal_ending AS ending,"
" (trialbal_debits - trialbal_credits) AS diff,"
" CASE WHEN ((trialbal_beginning*-1.0)<0.0) THEN 'CR' END AS beginningsense,"
" CASE WHEN ((trialbal_ending*-1.0)<0.0) THEN 'CR' END AS endingsense,"
" CASE WHEN ((trialbal_debits - trialbal_credits)<0.0) THEN 'CR' END AS diffsense,"
" 'curr' AS beginning_xtnumericrole,"
" 'curr' AS debits_xtnumericrole,"
" 'curr' AS credits_xtnumericrole,"
" 'curr' AS ending_xtnumericrole,"
" 'curr' AS diff_xtnumericrole,"
" 0 AS beginning_xttotalrole,"
" 0 AS debits_xttotalrole,"
" 0 AS credits_xttotalrole,"
" 0 AS ending_xttotalrole,"
" 0 AS diff_xttotalrole,"
" CASE WHEN (trialbal_beginning < 0.0) THEN ABS(trialbal_beginning) END AS beginning_qtdisplayrole,"
" CASE WHEN (trialbal_ending < 0.0) THEN ABS(trialbal_ending) END AS ending_qtdisplayrole,"
" CASE WHEN ((trialbal_debits - trialbal_credits) < 0.0) THEN ABS(trialbal_debits - trialbal_credits) END AS diff_qtdisplayrole,"
" CASE WHEN (trialbal_dirty) THEN 'warning' END AS ending_qtforegroundrole "
"FROM trialbal, accnt, period "
"WHERE ( (trialbal_accnt_id=accnt_id)"
" AND (trialbal_period_id=period_id)"
"<? if exists(\"accnt_id\") ?>"
" AND (trialbal_accnt_id=<? value(\"accnt_id\") ?>)"
"<? endif ?>"
"<? if exists(\"period_id\") ?>"
" AND (period_id=<? value(\"period_id\") ?>)"
"<? endif ?>"
"<? if not exists(\"showZero\") ?>"
" AND (abs(trialbal_beginning)+abs(trialbal_ending)+abs(trialbal_debits)+abs(trialbal_credits) > 0) "
"<? endif ?>"
") "
"ORDER BY period_start, formatGLAccount(accnt_id);" );
ParameterList params;
setParams(params);
MetaSQLQuery mql(sql);
q = mql.toQuery(params);
if (q.first())
{
_trialbal->populate(q, true);
}
else if (q.lastError().type() != QSqlError::NoError)
{
systemError(this, q.lastError().databaseText(), __FILE__, __LINE__);
return;
}
}
void dspGLTransactions::sFillList()
{
if (!_metrics->boolean("ManualForwardUpdate") &&
_selectedAccount->isChecked() &&
_showRunningTotal->isChecked())
{
if (!forwardUpdate())
return;
}
MetaSQLQuery mql("SELECT gltrans.*,"
" CASE WHEN(gltrans_docnumber='Misc.' AND"
" invhist_docnumber IS NOT NULL) THEN"
" (gltrans_docnumber || ' - ' || invhist_docnumber)"
" ELSE gltrans_docnumber"
" END AS docnumber,"
" firstLine(gltrans_notes) AS notes,"
" (formatGLAccount(accnt_id) || ' - ' || accnt_descrip) AS account,"
" CASE WHEN (gltrans_amount < 0) THEN ABS(gltrans_amount)"
" ELSE NULL"
" END AS debit,"
" CASE WHEN (gltrans_amount > 0) THEN gltrans_amount"
" ELSE NULL"
" END AS credit,"
" CASE WHEN accnt_type IN ('A','E') THEN "
" gltrans_amount * -1 "
" ELSE gltrans_amount END AS running,"
" 'curr' AS debit_xtnumericrole,"
" 'curr' AS credit_xtnumericrole,"
" 'curr' AS running_xtnumericrole,"
" 0 AS running_xtrunningrole,"
" <? value(\"beginningBalance\") ?> AS running_xtrunninginit "
"FROM gltrans JOIN accnt ON (gltrans_accnt_id=accnt_id) "
" LEFT OUTER JOIN invhist ON (gltrans_misc_id=invhist_id"
" AND gltrans_docnumber='Misc.') "
"WHERE ((gltrans_date BETWEEN <? value(\"startDate\") ?>"
" AND <? value(\"endDate\") ?>)"
"<? if exists(\"accnt_id\") ?>"
" AND (gltrans_accnt_id=<? value(\"accnt_id\") ?>)"
"<? endif ?>"
"<? if exists(\"source\") ?>"
" AND (gltrans_source=<? value(\"source\") ?>)"
"<? endif ?>"
") "
"ORDER BY gltrans_created"
"<? if not exists(\"beginningBalance\") ?> DESC <? endif ?>,"
" gltrans_sequence, gltrans_amount;");
ParameterList params;
if (! setParams(params))
return;
_beginningBalanceLit->setVisible(_selectedAccount->isChecked() && _showRunningTotal->isChecked());
_beginningBalance->setVisible(_selectedAccount->isChecked() && _showRunningTotal->isChecked());
if (_selectedAccount->isChecked() && _showRunningTotal->isChecked())
{
_gltrans->showColumn("running");
qDebug("begbal %f", params.value("beginningBalance").toDouble());
_beginningBalance->setDouble(params.value("beginningBalance").toDouble());
}
else
_gltrans->hideColumn("running");
q = mql.toQuery(params);
_gltrans->populate(q);
if (q.lastError().type() != QSqlError::NoError)
{
systemError(this, q.lastError().databaseText(), __FILE__, __LINE__);
return;
}
}
