Revision e5606def devices/DiWheelDrive/userthread.cpp
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