Revision e5606def devices/DiWheelDrive/userthread.cpp

View differences:

devices/DiWheelDrive/userthread.cpp
723 723
      break;
724 724
      // ---------------------------------------
725 725
    case states::RELEASE:
726

  
726 727
      if (global.forwardSpeed != DETECTION_SPEED){
727 728
          global.rpmForward[0] = DETECTION_SPEED;
728 729
        }
......
781 782
      break;
782 783
      // ---------------------------------------
783 784
    case states::TEST_MAP_STATE:{
784
      /* Test suit for amiro map */
785
      // Test suit for amiro map
785 786

  
786 787

  
787 788
      setAttributes(global.testmap, 0, 1, 2, 1);
......
790 791
      setAttributes(global.testmap, 3, 0, 0, 0xff);
791 792
      // AmiroMap map = AmiroMap(&global);
792 793

  
793
    //   // --------------------------------------------------
794
      // --------------------------------------------------
794 795

  
795 796
      global.tcase = 0;
796 797
      map.initialize();
......
839 840
      break;
840 841
    }
841 842

  
843
    // In case a new state is set:
844
    // 1. Record the state transition
842 845
    if (currentState != newState){
846

  
847
      global.stateTransitionCounter++;
848
      // Clear all state transitions to prevent overflow
849
      if (global.stateTransitionCounter >= 255) {
850
        global.stateTransitionCounter = 0;
851
        for (int i = 0; i < 24; i++) {
852
          global.stateTracker[i] = 0;
853
        }
854
}
855
      // Transmit the new state over can
843 856
      chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
844 857
      global.robot.transmitState(newState);
845
    //   if (newState == states::IDLE)
846
    //     {global.stateTracker[states::IDLE] += 1;}
847
    //   else if (newState == states::FOLLOW_LINE)
848
    //     {global.stateTracker[states::FOLLOW_LINE] += 1;}
849
    //   else if (newState == states::DETECT_STATION)
850
    //     {global.stateTracker[states::DETECT_STATION] += 1;}
851
    //   else if (newState == states::REVERSE)
852
    //     {global.stateTracker[states::REVERSE] += 1;}
853
    //   else if (newState == states::PUSH_BACK)
854
    //     {global.stateTracker[states::PUSH_BACK] += 1;}
855
    //   else if (newState == states::CHECK_POSITIONING)
856
    //     {global.stateTracker[states::CHECK_POSITIONING] += 1;}
857
    //   else if (newState == states::CHECK_VOLTAGE)
858
    //     {global.stateTracker[states::CHECK_VOLTAGE] += 1;}
859
    //   else if (newState == states::CHARGING)
860
    //     {global.stateTracker[states::CHARGING] += 1;}
861
    //   else if (newState == states::RELEASE)
862
    //     {global.stateTracker[states::RELEASE] += 1;}
863
    //   else if (newState == states::RELEASE_TO_CORRECT)
864
    //     {global.stateTracker[states::RELEASE_TO_CORRECT] += 1;}
865
    //   else if (newState == states::CORRECT_POSITIONING)
866
    //     {global.stateTracker[states::CORRECT_POSITIONING] += 1;}
867
    //   else if (newState == states::TURN)
868
    //     {global.stateTracker[states::TURN] += 1;}
869
    //   else if (newState == states::INACTIVE)
870
    //     {global.stateTracker[states::INACTIVE] += 1;}
871
    //   else if (newState == states::CALIBRATION)
872
    //     {global.stateTracker[states::CALIBRATION] += 1;}
873
    //   else if (newState == states::CALIBRATION_CHECK)
874
    //     {global.stateTracker[states::CALIBRATION_CHECK] += 1;}
875
    //   else if (newState == states::DEVIATION_CORRECTION)
876
    //     {global.stateTracker[states::DEVIATION_CORRECTION] += 1;}
877
    //   else if (newState == states::DOCKING_ERROR)
878
    //     {global.stateTracker[16+(-states::DOCKING_ERROR)] += 1;}
879
    //   else if (newState == states::REVERSE_TIMEOUT_ERROR)
880
    //     {global.stateTracker[16+(-states::REVERSE_TIMEOUT_ERROR)] += 1;}
881
    //   else if (newState == states::CALIBRATION_ERROR)
882
    //     {global.stateTracker[16+(-states::CALIBRATION_ERROR)] += 1;}
883
    //   else if (newState == states::WHITE_DETECTION_ERROR)
884
    //     {global.stateTracker[16+(-states::WHITE_DETECTION_ERROR)] += 1;}
885
    //   else if (newState == states::PROXY_DETECTION_ERROR)
886
    //     {global.stateTracker[16+(-states::PROXY_DETECTION_ERROR)] += 1;}
887
    //   else if (newState == states::NO_CHARGING_POWER_ERROR)
888
    //     {global.stateTracker[16+(-states::NO_CHARGING_POWER_ERROR)] += 1;}
889
    //   else if (newState == states::UNKNOWN_STATE_ERROR)
890
    //     {global.stateTracker[16+(-states::UNKNOWN_STATE_ERROR)] += 1;}
858

  
859
      // Increase state count for specific state
860
      // TODO: Improve with dictionary or other than switch case
861
      if (newState == states::IDLE)
862
        {global.stateTracker[states::IDLE] += 1;}
863
      else if (newState == states::FOLLOW_LINE)
864
        {global.stateTracker[states::FOLLOW_LINE] += 1;}
865
      else if (newState == states::DETECT_STATION)
866
        {global.stateTracker[states::DETECT_STATION] += 1;}
867
      else if (newState == states::REVERSE)
868
        {global.stateTracker[states::REVERSE] += 1;}
869
      else if (newState == states::PUSH_BACK)
870
        {global.stateTracker[states::PUSH_BACK] += 1;}
871
      else if (newState == states::CHECK_POSITIONING)
872
        {global.stateTracker[states::CHECK_POSITIONING] += 1;}
873
      else if (newState == states::CHECK_VOLTAGE)
874
        {global.stateTracker[states::CHECK_VOLTAGE] += 1;}
875
      else if (newState == states::CHARGING)
876
        {global.stateTracker[states::CHARGING] += 1;}
877
      else if (newState == states::RELEASE)
878
        {global.stateTracker[states::RELEASE] += 1;}
879
      else if (newState == states::RELEASE_TO_CORRECT)
880
        {global.stateTracker[states::RELEASE_TO_CORRECT] += 1;}
881
      else if (newState == states::CORRECT_POSITIONING)
882
        {global.stateTracker[states::CORRECT_POSITIONING] += 1;}
883
      else if (newState == states::TURN)
884
        {global.stateTracker[states::TURN] += 1;}
885
      else if (newState == states::INACTIVE)
886
        {global.stateTracker[states::INACTIVE] += 1;}
887
      else if (newState == states::CALIBRATION)
888
        {global.stateTracker[states::CALIBRATION] += 1;}
889
      else if (newState == states::CALIBRATION_CHECK)
890
        {global.stateTracker[states::CALIBRATION_CHECK] += 1;}
891
      else if (newState == states::DEVIATION_CORRECTION)
892
        {global.stateTracker[states::DEVIATION_CORRECTION] += 1;}
893
      else if (newState == states::DOCKING_ERROR)
894
        {global.stateTracker[16+(-states::DOCKING_ERROR)] += 1;}
895
      else if (newState == states::REVERSE_TIMEOUT_ERROR)
896
        {global.stateTracker[16+(-states::REVERSE_TIMEOUT_ERROR)] += 1;}
897
      else if (newState == states::CALIBRATION_ERROR)
898
        {global.stateTracker[16+(-states::CALIBRATION_ERROR)] += 1;}
899
      else if (newState == states::WHITE_DETECTION_ERROR)
900
        {global.stateTracker[16+(-states::WHITE_DETECTION_ERROR)] += 1;}
901
      else if (newState == states::PROXY_DETECTION_ERROR)
902
        {global.stateTracker[16+(-states::PROXY_DETECTION_ERROR)] += 1;}
903
      else if (newState == states::NO_CHARGING_POWER_ERROR)
904
        {global.stateTracker[16+(-states::NO_CHARGING_POWER_ERROR)] += 1;}
905
      else if (newState == states::UNKNOWN_STATE_ERROR)
906
        {global.stateTracker[16+(-states::UNKNOWN_STATE_ERROR)] += 1;}
891 907
    }
908

  
909
    // Keep track of last state and set the new state for next iteration
892 910
    prevState = currentState;
893 911
    currentState = newState;
894 912

  

Also available in: Unified diff