Revision ba75ee1d

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();
devices/DiWheelDrive/userthread.hpp
51 51
      CORRECT_POSITIONING,
52 52
      REVERSE,
53 53
      CHECK_POSITIONING,
54
      CHECK_VOLTAGE,
54 55
      IDLE
55 56
    };
56 57

  

Also available in: Unified diff