// check motor current
void Robot::checkCurrent(){
if (millis() < nextTimeCheckCurrent) return;
nextTimeCheckCurrent = millis() + 5000;
//bb add test MotorCurrent in manual mode and stop immediatly If >Powermax
/** if (stateCurr == STATE_MANUAL)
{
if (motorLeftSense >= motorPowerMax)
{
motorLeftSenseCounter++;
setSensorTriggered(SEN_MOTOR_LEFT);
setMotorPWM( 0, 0, false );
addErrorCounter(ERR_MOTOR_LEFT);
setNextState(STATE_ERROR, 0);
Console.println("Error: Motor Left current");
}
if (motorRightSense >= motorPowerMax)
{
motorRightSenseCounter++;
setSensorTriggered(SEN_MOTOR_RIGHT);
setMotorPWM( 0, 0, false );
addErrorCounter(ERR_MOTOR_RIGHT);
setNextState(STATE_ERROR, 0);
Console.println("Error: Motor Right current");
}
}
**/
if (motorMowSense >= motorMowPowerMax){
motorMowSenseCounter++;
setSensorTriggered(SEN_MOTOR_MOW);
}
else{
errorCounterMax[ERR_MOW_SENSE] = 0;
motorMowSenseCounter = 0;
if ( (lastTimeMotorMowStuck != 0) && (millis() >= lastTimeMotorMowStuck + 30000) ) { // wait 30 seconds before switching on again
errorCounter[ERR_MOW_SENSE] = 0;
motorMowEnable = true;
lastTimeMotorMowStuck = 0;
}
}
if (motorMowSenseCounter >= 30){ //ignore motorMowPower for 3 seconds
motorMowEnable = false;
Console.println("Error: Motor mow current");
addErrorCounter(ERR_MOW_SENSE);
lastTimeMotorMowStuck = millis();
// if (rollDir == RIGHT) reverseOrBidir(LEFT); // toggle roll dir
//else reverseOrBidir(RIGHT);
}
/** if (motorLeftSense >=motorPowerMax){
// left wheel motor overpowered
if ( ((stateCurr == STATE_PERI_FIND) || (stateCurr == STATE_PERI_TRACK)) / (stateCurr == STATE_FORWARD) || //
&& (millis() > stateStartTime + motorPowerIgnoreTime)){
//beep(1);
// motorLeftSenseCounter++;
// setSensorTriggered(SEN_MOTOR_LEFT);
setMotorPWM( 0, 0, false );
// reverseOrBidir(RIGHT);
} else if ((stateCurr == STATE_REVERSE) && (millis() > stateStartTime + motorPowerIgnoreTime)){
motorLeftSenseCounter++;
setSensorTriggered(SEN_MOTOR_LEFT);
setMotorPWM( 0, 0, false );
// reverseOrBidir(RIGHT);
setNextState(STATE_ROLL,RIGHT);
} else if ((stateCurr == STATE_ROLL) && (millis() > stateStartTime + motorPowerIgnoreTime)){
motorLeftSenseCounter++;
setSensorTriggered(SEN_MOTOR_LEFT);
setMotorPWM( 0, 0, false );
setNextState(STATE_FORWARD, 0);
}
}**/
/** else if (motorRightSense >= motorPowerMax){
// right wheel motor overpowered
if ( ((stateCurr == STATE_PERI_FIND)) && (millis() > stateStartTime + motorPowerIgnoreTime)){ //(stateCurr == STATE_FORWARD) || //
//beep(1);
// motorRightSenseCounter++;
// setSensorTriggered(SEN_MOTOR_RIGHT);
setMotorPWM( 0, 0, false );
// reverseOrBidir(RIGHT);
} else if ((stateCurr == STATE_REVERSE) && (millis() > stateStartTime + motorPowerIgnoreTime)){
motorRightSenseCounter++;
setSensorTriggered(SEN_MOTOR_RIGHT);
setMotorPWM( 0, 0, false );
setNextState(STATE_ROLL,LEFT);
} else if ((stateCurr == STATE_ROLL) && (millis() > stateStartTime + motorPowerIgnoreTime)){
motorRightSenseCounter++;
setSensorTriggered(SEN_MOTOR_RIGHT);
setMotorPWM( 0, 0, false );
setNextState(STATE_FORWARD, 0);
}
}**/
}