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