Revision ba75ee1d devices/DiWheelDrive/userthread.cpp

View differences:

devices/DiWheelDrive/userthread.cpp
306 306 
        break;
307 307 
      // ---------------------------------------
308 308 
      case states::CHECK_POSITIONING:
309 
        if(checkDockingSuccess()/* && checkVoltage() */){
310 
          newState = states::CHARGING;
309 
        if(checkDockingSuccess()){
310 
          newState = states::CHECK_VOLTAGE;
311 311 
        }else{
312 312 
          newState = states::CORRECT_POSITIONING;
313 313 
        }
314 314 
        break;
315 315 
      // ---------------------------------------
316 
      case states::CHECK_VOLTAGE:
317 
        if(!checkPinEnabled()){
318 
          global.robot.requestCharging(1);
319 
        } else {
320 
          if(checkPinVoltage()){
321 
            // Pins are under voltage -> correctly docked
322 
            newState = states::CHARGING;
323 
          }else{
324 
            // No voltage on pins -> falsely docked
325 
            // deactivate pins
326 
            global.robot.requestCharging(0);
327 
            newState = states::CORRECT_POSITIONING;
328 
          }
329 
        }
330 
      // ---------------------------------------
316 331 
      case states::CHARGING:
317 332 
        if (global.motorcontrol.getMotorEnable()){
318 333 
          global.motorcontrol.toggleMotorEnable();

Also available in: Unified diff