Revision e404e6c0
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 914 | 914 |
// Print out the error info collected. Clear buffer after calling |
| 915 | 915 |
chprintf(chp, "Error Info\n"); |
| 916 | 916 |
chprintf(chp, "\n"); |
| 917 |
int IDLE = 0; |
|
| 918 |
int FOLLOW_LINE = 1; |
|
| 919 |
int DETECT_STATION = 2; |
|
| 920 |
int REVERSE = 3; |
|
| 921 |
int PUSH_BACK = 4; |
|
| 922 |
int CHECK_POSITIONING = 5; |
|
| 923 |
int CHECK_VOLTAGE = 6; |
|
| 924 |
int CHARGING = 7; |
|
| 925 |
int RELEASE = 8; |
|
| 926 |
int RELEASE_TO_CORRECT = 9; |
|
| 927 |
int CORRECT_POSITIONING = 10; |
|
| 928 |
int TURN = 12; |
|
| 929 |
int INACTIVE = 13; |
|
| 930 |
int CALIBRATION = 14; |
|
| 931 |
int CALIBRATION_CHECK = 15; |
|
| 932 |
int DEVIATION_CORRECTION = 16; |
|
| 933 |
int DOCKING_ERROR = 16+1; |
|
| 934 |
int REVERSE_TIMEOUT_ERROR = 16+2; |
|
| 935 |
int CALIBRATION_ERROR = 16+3; |
|
| 936 |
int WHITE_DETECTION_ERROR = 16+4; |
|
| 937 |
int PROXY_DETECTION_ERROR = 16+5; |
|
| 938 |
int NO_CHARGING_POWER_ERROR = 16+6; |
|
| 939 |
int UNKNOWN_STATE_ERROR = 16+7; |
|
| 940 |
|
|
| 941 |
chprintf(chp, "IDLE: %d\n", global.stateTracker[IDLE]); |
|
| 942 |
chprintf(chp, "FOLLOW_LINE: %d\n", global.stateTracker[FOLLOW_LINE]); |
|
| 943 |
chprintf(chp, "DETECT_STATION: %d\n", global.stateTracker[DETECT_STATION]); |
|
| 944 |
chprintf(chp, "REVERSE: %d\n", global.stateTracker[REVERSE]); |
|
| 945 |
chprintf(chp, "PUSH_BACK: %d\n", global.stateTracker[PUSH_BACK]); |
|
| 946 |
chprintf(chp, "CHECK_POSITIONING: %d\n", global.stateTracker[CHECK_POSITIONING]); |
|
| 947 |
chprintf(chp, "CHECK_VOLTAGE: %d\n", global.stateTracker[CHECK_VOLTAGE]); |
|
| 948 |
chprintf(chp, "CHARGING: %d\n", global.stateTracker[CHARGING]); |
|
| 949 |
chprintf(chp, "RELEASE: %d\n", global.stateTracker[RELEASE]); |
|
| 950 |
chprintf(chp, "RELEASE_TO_CORRECT: %d\n", global.stateTracker[RELEASE_TO_CORRECT]); |
|
| 951 |
chprintf(chp, "CORRECT_POSITIONING: %d\n", global.stateTracker[CORRECT_POSITIONING]); |
|
| 952 |
chprintf(chp, "TURN: %d\n", global.stateTracker[TURN]); |
|
| 953 |
chprintf(chp, "INACTIVE: %d\n", global.stateTracker[INACTIVE]); |
|
| 954 |
chprintf(chp, "CALIBRATION: %d\n", global.stateTracker[CALIBRATION]); |
|
| 955 |
chprintf(chp, "CALIBRATION_CHECK: %d\n", global.stateTracker[CALIBRATION_CHECK]); |
|
| 956 |
chprintf(chp, "DEVIATION_CORRECTION: %d\n", global.stateTracker[DEVIATION_CORRECTION]); |
|
| 957 |
chprintf(chp, "DOCKING_ERROR: %d\n", global.stateTracker[DOCKING_ERROR]); |
|
| 958 |
chprintf(chp, "REVERSE_TIMEOUT_ERROR: %d\n", global.stateTracker[REVERSE_TIMEOUT_ERROR]); |
|
| 959 |
chprintf(chp, "CALIBRATION_ERROR: %d\n", global.stateTracker[CALIBRATION_ERROR]); |
|
| 960 |
chprintf(chp, "WHITE_DETECTION_ERROR: %d\n", global.stateTracker[WHITE_DETECTION_ERROR]); |
|
| 961 |
chprintf(chp, "PROXY_DETECTION_ERROR: %d\n", global.stateTracker[PROXY_DETECTION_ERROR]); |
|
| 962 |
chprintf(chp, "NO_CHARGING_POWER_ERROR: %d\n", global.stateTracker[NO_CHARGING_POWER_ERROR]); |
|
| 963 |
chprintf(chp, "UNKNOWN_STATE_ERROR: %d\n", global.stateTracker[UNKNOWN_STATE_ERROR]); |
|
| 964 |
|
|
| 965 |
for (int i=0; i<24;i++){
|
|
| 966 |
global.stateTracker[i] = 0; |
|
| 967 |
} |
|
| 917 |
// uint8_t IDLE = 0; |
|
| 918 |
// uint8_t FOLLOW_LINE = 1; |
|
| 919 |
// uint8_t DETECT_STATION = 2; |
|
| 920 |
// uint8_t REVERSE = 3; |
|
| 921 |
// uint8_t PUSH_BACK = 4; |
|
| 922 |
// uint8_t CHECK_POSITIONING = 5; |
|
| 923 |
// uint8_t CHECK_VOLTAGE = 6; |
|
| 924 |
// uint8_t CHARGING = 7; |
|
| 925 |
// uint8_t RELEASE = 8; |
|
| 926 |
// uint8_t RELEASE_TO_CORRECT = 9; |
|
| 927 |
// uint8_t CORRECT_POSITIONING = 10; |
|
| 928 |
// uint8_t TURN = 12; |
|
| 929 |
// uint8_t INACTIVE = 13; |
|
| 930 |
// uint8_t CALIBRATION = 14; |
|
| 931 |
// uint8_t CALIBRATION_CHECK = 15; |
|
| 932 |
// uint8_t DEVIATION_CORRECTION = 16; |
|
| 933 |
// uint8_t DOCKING_ERROR = 16+1; |
|
| 934 |
// uint8_t REVERSE_TIMEOUT_ERROR = 16+2; |
|
| 935 |
// uint8_t CALIBRATION_ERROR = 16+3; |
|
| 936 |
// uint8_t WHITE_DETECTION_ERROR = 16+4; |
|
| 937 |
// uint8_t PROXY_DETECTION_ERROR = 16+5; |
|
| 938 |
// uint8_t NO_CHARGING_POWER_ERROR = 16+6; |
|
| 939 |
// uint8_t UNKNOWN_STATE_ERROR = 16+7; |
|
| 940 |
|
|
| 941 |
// chprintf(chp, "IDLE: %d\n", global.stateTracker[IDLE]); |
|
| 942 |
// chprintf(chp, "FOLLOW_LINE: %d\n", global.stateTracker[FOLLOW_LINE]); |
|
| 943 |
// chprintf(chp, "DETECT_STATION: %d\n", global.stateTracker[DETECT_STATION]); |
|
| 944 |
// chprintf(chp, "REVERSE: %d\n", global.stateTracker[REVERSE]); |
|
| 945 |
// chprintf(chp, "PUSH_BACK: %d\n", global.stateTracker[PUSH_BACK]); |
|
| 946 |
// chprintf(chp, "CHECK_POSITIONING: %d\n", global.stateTracker[CHECK_POSITIONING]); |
|
| 947 |
// chprintf(chp, "CHECK_VOLTAGE: %d\n", global.stateTracker[CHECK_VOLTAGE]); |
|
| 948 |
// chprintf(chp, "CHARGING: %d\n", global.stateTracker[CHARGING]); |
|
| 949 |
// chprintf(chp, "RELEASE: %d\n", global.stateTracker[RELEASE]); |
|
| 950 |
// chprintf(chp, "RELEASE_TO_CORRECT: %d\n", global.stateTracker[RELEASE_TO_CORRECT]); |
|
| 951 |
// chprintf(chp, "CORRECT_POSITIONING: %d\n", global.stateTracker[CORRECT_POSITIONING]); |
|
| 952 |
// chprintf(chp, "TURN: %d\n", global.stateTracker[TURN]); |
|
| 953 |
// chprintf(chp, "INACTIVE: %d\n", global.stateTracker[INACTIVE]); |
|
| 954 |
// chprintf(chp, "CALIBRATION: %d\n", global.stateTracker[CALIBRATION]); |
|
| 955 |
// chprintf(chp, "CALIBRATION_CHECK: %d\n", global.stateTracker[CALIBRATION_CHECK]); |
|
| 956 |
// chprintf(chp, "DEVIATION_CORRECTION: %d\n", global.stateTracker[DEVIATION_CORRECTION]); |
|
| 957 |
// chprintf(chp, "DOCKING_ERROR: %d\n", global.stateTracker[DOCKING_ERROR]); |
|
| 958 |
// chprintf(chp, "REVERSE_TIMEOUT_ERROR: %d\n", global.stateTracker[REVERSE_TIMEOUT_ERROR]); |
|
| 959 |
// chprintf(chp, "CALIBRATION_ERROR: %d\n", global.stateTracker[CALIBRATION_ERROR]); |
|
| 960 |
// chprintf(chp, "WHITE_DETECTION_ERROR: %d\n", global.stateTracker[WHITE_DETECTION_ERROR]); |
|
| 961 |
// chprintf(chp, "PROXY_DETECTION_ERROR: %d\n", global.stateTracker[PROXY_DETECTION_ERROR]); |
|
| 962 |
// chprintf(chp, "NO_CHARGING_POWER_ERROR: %d\n", global.stateTracker[NO_CHARGING_POWER_ERROR]); |
|
| 963 |
// chprintf(chp, "UNKNOWN_STATE_ERROR: %d\n", global.stateTracker[UNKNOWN_STATE_ERROR]); |
|
| 964 |
|
|
| 965 |
// for (int i=0; i<24;i++){
|
|
| 966 |
// global.stateTracker[i] = 0; |
|
| 967 |
|
|
| 968 |
// } |
|
| 968 | 969 |
} |
| 969 | 970 |
|
| 970 |
inline void setAttributes(uint8_t (&map)[MAX_NODES][NODE_ATTRIBUTES], uint8_t id, |
|
| 971 |
uint8_t l, uint8_t r, uint8_t att) {
|
|
| 972 |
map[id][0] = l; |
|
| 973 |
map[id][1] = r; |
|
| 974 |
map[id][2] = att; |
|
| 975 |
} |
|
| 976 |
|
|
| 977 |
#define TEST_CASES 10 |
|
| 978 |
void shellRequestTestSuite(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 979 |
uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES] = {0};
|
|
| 980 |
bool testres[TEST_CASES]; |
|
| 981 |
setAttributes(testmap, 0, 1, 2, 1); |
|
| 982 |
setAttributes(testmap, 1, 2, 2, 0); |
|
| 983 |
setAttributes(testmap, 2, 1, 0, 0); |
|
| 984 |
setAttributes(testmap, 3, 0, 0, 0xff); |
|
| 985 |
AmiroMap map = AmiroMap(&global); |
|
| 986 |
|
|
| 987 |
// -------------------------------------------------- |
|
| 988 |
|
|
| 989 |
int tcase = 0; |
|
| 990 |
map.initialize(testmap); |
|
| 991 |
testres[tcase] = map.get_valid(); |
|
| 992 |
uint8_t ini = map.initialize(testmap); |
|
| 993 |
chprintf(chp, "Initialized returned: %d \n", ini); |
|
| 994 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 995 |
|
|
| 996 |
tcase++; // 1 |
|
| 997 |
setAttributes(testmap, 0, 1, 2, 0xff); |
|
| 998 |
ini = map.initialize(testmap); |
|
| 999 |
chprintf(chp, "Initialized returned: %d \n", ini); |
|
| 1000 |
testres[tcase] = !map.get_valid(); |
|
| 1001 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1002 |
|
|
| 1003 |
tcase++; // 2 |
|
| 1004 |
setAttributes(testmap, 0, 1, 2, 0); |
|
| 1005 |
setAttributes(testmap, 2, 1, 0, 1); |
|
| 1006 |
map.initialize(testmap); |
|
| 1007 |
testres[tcase] = map.get_current() == 2; |
|
| 1008 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1009 |
|
|
| 1010 |
tcase++; // 3 |
|
| 1011 |
setAttributes(testmap, 3, 0, 0, 0); |
|
| 1012 |
setAttributes(testmap, 4, 0, 0, 0xff); |
|
| 1013 |
map.initialize(testmap); |
|
| 1014 |
testres[tcase] = !map.get_valid(); |
|
| 1015 |
|
|
| 1016 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1017 |
// -------------------------------------------------- |
|
| 1018 |
|
|
| 1019 |
int failed = 0; |
|
| 1020 |
int passed = 0; |
|
| 1021 |
for (int i=0; i<=tcase; i++) {
|
|
| 1022 |
if (testres[i]){
|
|
| 1023 |
passed++; |
|
| 1024 |
chprintf(chp, "Test %d Passed!\n", i); |
|
| 1025 |
}else{
|
|
| 1026 |
failed++; |
|
| 1027 |
chprintf(chp, "Test %d Failed\n", i); |
|
| 1028 |
} |
|
| 1029 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1030 |
} |
|
| 1031 |
chprintf(chp, "Total: %d, Passed: %d, Failed: %d\n", tcase + 1, passed, failed); |
|
| 1032 |
} |
|
| 1033 | 971 |
|
| 1034 | 972 |
void shellRequestMapTest(BaseSequentialStream *chp, int argc, char *argv[]) {
|
| 1035 | 973 |
|
| 1036 | 974 |
// TODO: |
| 1037 | 975 |
// For now see everything fixed. Create array with pointer to all node structs. Determine |
| 1038 | 976 |
|
| 1039 |
// chprintf(chp, " +-------------------+\n"); |
|
| 1040 |
// chprintf(chp, " | |\n"); |
|
| 1041 |
// chprintf(chp, " | |\n"); |
|
| 1042 |
// chprintf(chp, "+-v--+ |\n"); |
|
| 1043 |
// chprintf(chp, "| 0 | +-++\n"); |
|
| 1044 |
// chprintf(chp, "+-+--+ +--------+2 | <-------+\n"); |
|
| 1045 |
// chprintf(chp, " | | +--+ |\n"); |
|
| 1046 |
// chprintf(chp, " | | ^ |\n"); |
|
| 1047 |
// chprintf(chp, " | | | |\n"); |
|
| 1048 |
// chprintf(chp, " | v | |\n"); |
|
| 1049 |
// chprintf(chp, " | +-+-+------+ |\n"); |
|
| 1050 |
// chprintf(chp, " +------> 1 | |\n"); |
|
| 1051 |
// chprintf(chp, " +---+-------------------+\n"); |
|
| 1052 |
|
|
| 1053 |
uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES] = {0};
|
|
| 1054 |
setAttributes(testmap, 0, 1, 2, 1); |
|
| 1055 |
setAttributes(testmap, 1, 2, 2, 0); |
|
| 1056 |
setAttributes(testmap, 2, 1, 0, 0); |
|
| 1057 |
setAttributes(testmap, 3, 0, 0, 0xff); |
|
| 1058 |
AmiroMap map = AmiroMap(&global); |
|
| 1059 |
map.initialize(testmap); |
|
| 1060 |
} |
|
| 977 |
chprintf(chp, " +-------------------+\n"); |
|
| 978 |
chprintf(chp, " | |\n"); |
|
| 979 |
chprintf(chp, " | |\n"); |
|
| 980 |
chprintf(chp, "+-v--+ |\n"); |
|
| 981 |
chprintf(chp, "| 0 | +-++\n"); |
|
| 982 |
chprintf(chp, "+-+--+ +--------+2 | <-------+\n"); |
|
| 983 |
chprintf(chp, " | | +--+ |\n"); |
|
| 984 |
chprintf(chp, " | | ^ |\n"); |
|
| 985 |
chprintf(chp, " | | | |\n"); |
|
| 986 |
chprintf(chp, " | v | |\n"); |
|
| 987 |
chprintf(chp, " | +-+-+------+ |\n"); |
|
| 988 |
chprintf(chp, " +------> 1 | |\n"); |
|
| 989 |
chprintf(chp, " +---+-------------------+\n"); |
|
| 990 |
// BaseThread::sleep(MS2ST(250)); |
|
| 991 |
global.lfStrategy = msg_content::MSG_TEST_MAP_STATE; |
|
| 992 |
global.msgReceived = true; |
|
| 993 |
|
|
| 994 |
} |
|
| 1061 | 995 |
|
| 1062 | 996 |
static const ShellCommand commands[] = {
|
| 1063 | 997 |
{"shutdown", shellRequestShutdown},
|
| ... | ... | |
| 1093 | 1027 |
{"printLocation", shellRequestPrintCoordinate},
|
| 1094 | 1028 |
{"checkPowerPins", shellRequestCheckPower},
|
| 1095 | 1029 |
{"stateInfos", shellRequestErrorInfo},
|
| 1096 |
{"testMap", shellRequestMapTest},
|
|
| 1097 |
{"test", shellRequestTestSuite },
|
|
| 1030 |
{"test", shellRequestMapTest},
|
|
| 1098 | 1031 |
{NULL, NULL}};
|
| 1099 | 1032 |
|
| 1100 | 1033 |
static const ShellConfig shell_cfg1 = {
|
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 1 |
// #include "userthread.hpp" |
|
| 2 | 1 |
#include "global.hpp" |
| 3 | 2 |
#include <cmath> |
| 4 | 3 |
#include "linefollow.hpp" |
| 5 | 4 |
#include "userthread.hpp" |
| 6 |
// #include <cmath>
|
|
| 7 |
// #include "global.hpp" |
|
| 5 |
#include "amiro_map.hpp"
|
|
| 6 |
|
|
| 8 | 7 |
using namespace amiro; |
| 9 | 8 |
|
| 10 | 9 |
extern Global global; |
| ... | ... | |
| 248 | 247 |
return devCor.currentDeviation; |
| 249 | 248 |
} |
| 250 | 249 |
|
| 250 |
void setAttributes(uint8_t (&map)[MAX_NODES][NODE_ATTRIBUTES], |
|
| 251 |
uint8_t id, uint8_t l, uint8_t r, uint8_t att) {
|
|
| 252 |
map[id][0] = l; |
|
| 253 |
map[id][1] = r; |
|
| 254 |
map[id][2] = att; |
|
| 255 |
} |
|
| 251 | 256 |
|
| 252 | 257 |
UserThread::UserThread() : |
| 253 | 258 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
| ... | ... | |
| 272 | 277 |
running = false; |
| 273 | 278 |
LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT; |
| 274 | 279 |
LineFollow lf(&global); |
| 280 |
AmiroMap map(&global); |
|
| 275 | 281 |
/* |
| 276 | 282 |
* LOOP |
| 277 | 283 |
*/ |
| ... | ... | |
| 298 | 304 |
global.msgReceived = false; |
| 299 | 305 |
// running = true; |
| 300 | 306 |
switch(global.lfStrategy){
|
| 301 |
case msg_content::MSG_START:
|
|
| 302 |
newState = states::CALIBRATION_CHECK;
|
|
| 307 |
case msg_content::MSG_START: |
|
| 308 |
newState = states::CALIBRATION_CHECK; |
|
| 303 | 309 |
break; |
| 304 |
case msg_content::MSG_STOP:
|
|
| 305 |
newState = states::IDLE;
|
|
| 310 |
case msg_content::MSG_STOP: |
|
| 311 |
newState = states::IDLE; |
|
| 306 | 312 |
break; |
| 307 |
case msg_content::MSG_EDGE_RIGHT:
|
|
| 308 |
// newState = states::FOLLOW_LINE;
|
|
| 309 |
lStrategy = LineFollowStrategy::EDGE_RIGHT;
|
|
| 313 |
case msg_content::MSG_EDGE_RIGHT: |
|
| 314 |
// newState = states::FOLLOW_LINE; |
|
| 315 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
|
| 310 | 316 |
break; |
| 311 |
case msg_content::MSG_EDGE_LEFT:
|
|
| 312 |
// newState = states::FOLLOW_LINE;
|
|
| 313 |
lStrategy = LineFollowStrategy::EDGE_LEFT;
|
|
| 317 |
case msg_content::MSG_EDGE_LEFT: |
|
| 318 |
// newState = states::FOLLOW_LINE; |
|
| 319 |
lStrategy = LineFollowStrategy::EDGE_LEFT; |
|
| 314 | 320 |
break; |
| 315 |
case msg_content::MSG_FUZZY:
|
|
| 316 |
// newState = states::FOLLOW_LINE;
|
|
| 317 |
lStrategy = LineFollowStrategy::FUZZY;
|
|
| 321 |
case msg_content::MSG_FUZZY: |
|
| 322 |
// newState = states::FOLLOW_LINE; |
|
| 323 |
lStrategy = LineFollowStrategy::FUZZY; |
|
| 318 | 324 |
break; |
| 319 |
case msg_content::MSG_DOCK:
|
|
| 320 |
newState = states::DETECT_STATION;
|
|
| 325 |
case msg_content::MSG_DOCK: |
|
| 326 |
newState = states::DETECT_STATION; |
|
| 321 | 327 |
break; |
| 322 |
case msg_content::MSG_UNDOCK: |
|
| 323 |
newState = states::RELEASE; |
|
| 328 |
case msg_content::MSG_UNDOCK: |
|
| 329 |
newState = states::RELEASE; |
|
| 330 |
break; |
|
| 331 |
case msg_content::MSG_CHARGE: |
|
| 332 |
newState = states::CHARGING; |
|
| 324 | 333 |
break; |
| 325 |
case msg_content::MSG_CHARGE:
|
|
| 326 |
newState = states::CHARGING;
|
|
| 334 |
case msg_content::MSG_RESET_ODOMETRY:
|
|
| 335 |
global.odometry.resetPosition();
|
|
| 327 | 336 |
break; |
| 328 |
case msg_content::MSG_RESET_ODOMETRY: |
|
| 329 |
global.odometry.resetPosition(); |
|
| 337 |
case msg_content::MSG_CALIBRATE_BLACK: |
|
| 338 |
proxCalib.calibrateBlack = true; |
|
| 339 |
// global.odometry.resetPosition(); |
|
| 340 |
newState = states::CALIBRATION; |
|
| 330 | 341 |
break; |
| 331 |
case msg_content::MSG_CALIBRATE_BLACK: |
|
| 332 |
proxCalib.calibrateBlack = true; |
|
| 333 |
// global.odometry.resetPosition(); |
|
| 334 |
newState = states::CALIBRATION; |
|
| 342 |
case msg_content::MSG_CALIBRATE_WHITE: |
|
| 343 |
proxCalib.calibrateBlack = false; |
|
| 344 |
newState = states::CALIBRATION; |
|
| 335 | 345 |
break; |
| 336 |
case msg_content::MSG_CALIBRATE_WHITE: |
|
| 337 |
proxCalib.calibrateBlack = false; |
|
| 338 |
newState = states::CALIBRATION; |
|
| 346 |
case msg_content::MSG_TEST_MAP_STATE: |
|
| 347 |
newState = states::TEST_MAP_STATE; |
|
| 339 | 348 |
break; |
| 340 |
default: |
|
| 341 |
newState = states::IDLE; |
|
| 349 |
|
|
| 350 |
default: |
|
| 351 |
newState = states::IDLE; |
|
| 342 | 352 |
break; |
| 343 | 353 |
} |
| 344 | 354 |
} |
| ... | ... | |
| 490 | 500 |
break; |
| 491 | 501 |
} |
| 492 | 502 |
// --------------------------------------- |
| 493 |
case states::DETECT_STATION:
|
|
| 503 |
case states::DETECT_STATION: |
|
| 494 | 504 |
|
| 495 | 505 |
if (global.forwardSpeed != DETECTION_SPEED){
|
| 496 | 506 |
global.forwardSpeed = DETECTION_SPEED; |
| ... | ... | |
| 516 | 526 |
} |
| 517 | 527 |
break; |
| 518 | 528 |
// --------------------------------------- |
| 519 |
case states::CORRECT_POSITIONING:
|
|
| 529 |
case states::CORRECT_POSITIONING: |
|
| 520 | 530 |
if (global.forwardSpeed != CHARGING_SPEED){
|
| 521 | 531 |
global.forwardSpeed = CHARGING_SPEED; |
| 522 | 532 |
} |
| ... | ... | |
| 535 | 545 |
} |
| 536 | 546 |
break; |
| 537 | 547 |
// --------------------------------------- |
| 538 |
case states::REVERSE:
|
|
| 548 |
case states::REVERSE: |
|
| 539 | 549 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
| 540 | 550 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
| 541 | 551 |
} |
| ... | ... | |
| 584 | 594 |
// setRpmSpeed(rpmSpeed); |
| 585 | 595 |
break; |
| 586 | 596 |
// --------------------------------------- |
| 587 |
case states::DEVIATION_CORRECTION:
|
|
| 597 |
case states::DEVIATION_CORRECTION: |
|
| 588 | 598 |
// if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
| 589 | 599 |
// lf.setStrategy(LineFollowStrategy::REVERSE); |
| 590 | 600 |
// } |
| ... | ... | |
| 628 | 638 |
// } |
| 629 | 639 |
break; |
| 630 | 640 |
// --------------------------------------- |
| 631 |
case states::PUSH_BACK:
|
|
| 641 |
case states::PUSH_BACK: |
|
| 632 | 642 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
| 633 | 643 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
| 634 | 644 |
} |
| ... | ... | |
| 642 | 652 |
} |
| 643 | 653 |
break; |
| 644 | 654 |
// --------------------------------------- |
| 645 |
case states::CHECK_POSITIONING:
|
|
| 655 |
case states::CHECK_POSITIONING: |
|
| 646 | 656 |
setRpmSpeed(stop); |
| 647 | 657 |
checkForMotion(); |
| 648 | 658 |
if(checkDockingSuccess()){
|
| ... | ... | |
| 656 | 666 |
} |
| 657 | 667 |
break; |
| 658 | 668 |
// --------------------------------------- |
| 659 |
case states::CHECK_VOLTAGE:
|
|
| 669 |
case states::CHECK_VOLTAGE: |
|
| 660 | 670 |
if(!checkPinEnabled()){
|
| 661 | 671 |
global.robot.requestCharging(1); |
| 662 | 672 |
} else {
|
| ... | ... | |
| 684 | 694 |
} |
| 685 | 695 |
break; |
| 686 | 696 |
// --------------------------------------- |
| 687 |
case states::RELEASE_TO_CORRECT:
|
|
| 697 |
case states::RELEASE_TO_CORRECT: |
|
| 688 | 698 |
|
| 689 | 699 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION); |
| 690 | 700 |
checkForMotion(); |
| ... | ... | |
| 700 | 710 |
newState = states::CORRECT_POSITIONING; |
| 701 | 711 |
break; |
| 702 | 712 |
// --------------------------------------- |
| 703 |
case states::CHARGING:
|
|
| 713 |
case states::CHARGING: |
|
| 704 | 714 |
global.motorcontrol.setMotorEnable(false); |
| 705 | 715 |
utCount.errorCount = 0; |
| 706 | 716 |
// Formulate Request to enable charging |
| ... | ... | |
| 712 | 722 |
} |
| 713 | 723 |
break; |
| 714 | 724 |
// --------------------------------------- |
| 715 |
case states::RELEASE:
|
|
| 725 |
case states::RELEASE: |
|
| 716 | 726 |
if (global.forwardSpeed != DETECTION_SPEED){
|
| 717 | 727 |
global.rpmForward[0] = DETECTION_SPEED; |
| 718 | 728 |
} |
| ... | ... | |
| 742 | 752 |
// lightAllLeds(Color::BLACK); |
| 743 | 753 |
break; |
| 744 | 754 |
// --------------------------------------- |
| 745 |
case states::DOCKING_ERROR:
|
|
| 746 |
newState = states::RELEASE;
|
|
| 755 |
case states::DOCKING_ERROR: |
|
| 756 |
newState = states::RELEASE; |
|
| 747 | 757 |
break; |
| 748 | 758 |
// --------------------------------------- |
| 749 |
case states::REVERSE_TIMEOUT_ERROR:
|
|
| 750 |
newState = states::IDLE;
|
|
| 759 |
case states::REVERSE_TIMEOUT_ERROR: |
|
| 760 |
newState = states::IDLE; |
|
| 751 | 761 |
break; |
| 752 | 762 |
// --------------------------------------- |
| 753 |
case states::CALIBRATION_ERROR:
|
|
| 754 |
newState = states::IDLE;
|
|
| 763 |
case states::CALIBRATION_ERROR: |
|
| 764 |
newState = states::IDLE; |
|
| 755 | 765 |
break; |
| 756 | 766 |
// --------------------------------------- |
| 757 |
case states::WHITE_DETECTION_ERROR:
|
|
| 758 |
newState = states::IDLE;
|
|
| 767 |
case states::WHITE_DETECTION_ERROR: |
|
| 768 |
newState = states::IDLE; |
|
| 759 | 769 |
break; |
| 760 | 770 |
// --------------------------------------- |
| 761 |
case states::PROXY_DETECTION_ERROR:
|
|
| 762 |
newState = states::IDLE;
|
|
| 771 |
case states::PROXY_DETECTION_ERROR: |
|
| 772 |
newState = states::IDLE; |
|
| 763 | 773 |
break; |
| 764 | 774 |
// --------------------------------------- |
| 765 |
case states::NO_CHARGING_POWER_ERROR:
|
|
| 766 |
newState = states::IDLE;
|
|
| 775 |
case states::NO_CHARGING_POWER_ERROR: |
|
| 776 |
newState = states::IDLE; |
|
| 767 | 777 |
break; |
| 768 | 778 |
// --------------------------------------- |
| 769 |
case states::UNKNOWN_STATE_ERROR:
|
|
| 779 |
case states::UNKNOWN_STATE_ERROR: |
|
| 770 | 780 |
newState = states::IDLE; |
| 771 | 781 |
break; |
| 772 | 782 |
// --------------------------------------- |
| 773 |
default: |
|
| 774 |
newState = states::UNKNOWN_STATE_ERROR; |
|
| 775 |
break; |
|
| 776 |
} |
|
| 777 |
if (currentState != newState){
|
|
| 778 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState); |
|
| 779 |
global.robot.transmitState(newState); |
|
| 780 |
if (newState == states::IDLE) |
|
| 781 |
{global.stateTracker[states::IDLE] += 1;}
|
|
| 782 |
else if (newState == states::FOLLOW_LINE) |
|
| 783 |
{global.stateTracker[states::FOLLOW_LINE] += 1;}
|
|
| 784 |
else if (newState == states::DETECT_STATION) |
|
| 785 |
{global.stateTracker[states::DETECT_STATION] += 1;}
|
|
| 786 |
else if (newState == states::REVERSE) |
|
| 787 |
{global.stateTracker[states::REVERSE] += 1;}
|
|
| 788 |
else if (newState == states::PUSH_BACK) |
|
| 789 |
{global.stateTracker[states::PUSH_BACK] += 1;}
|
|
| 790 |
else if (newState == states::CHECK_POSITIONING) |
|
| 791 |
{global.stateTracker[states::CHECK_POSITIONING] += 1;}
|
|
| 792 |
else if (newState == states::CHECK_VOLTAGE) |
|
| 793 |
{global.stateTracker[states::CHECK_VOLTAGE] += 1;}
|
|
| 794 |
else if (newState == states::CHARGING) |
|
| 795 |
{global.stateTracker[states::CHARGING] += 1;}
|
|
| 796 |
else if (newState == states::RELEASE) |
|
| 797 |
{global.stateTracker[states::RELEASE] += 1;}
|
|
| 798 |
else if (newState == states::RELEASE_TO_CORRECT) |
|
| 799 |
{global.stateTracker[states::RELEASE_TO_CORRECT] += 1;}
|
|
| 800 |
else if (newState == states::CORRECT_POSITIONING) |
|
| 801 |
{global.stateTracker[states::CORRECT_POSITIONING] += 1;}
|
|
| 802 |
else if (newState == states::TURN) |
|
| 803 |
{global.stateTracker[states::TURN] += 1;}
|
|
| 804 |
else if (newState == states::INACTIVE) |
|
| 805 |
{global.stateTracker[states::INACTIVE] += 1;}
|
|
| 806 |
else if (newState == states::CALIBRATION) |
|
| 807 |
{global.stateTracker[states::CALIBRATION] += 1;}
|
|
| 808 |
else if (newState == states::CALIBRATION_CHECK) |
|
| 809 |
{global.stateTracker[states::CALIBRATION_CHECK] += 1;}
|
|
| 810 |
else if (newState == states::DEVIATION_CORRECTION) |
|
| 811 |
{global.stateTracker[states::DEVIATION_CORRECTION] += 1;}
|
|
| 812 |
|
|
| 813 |
else if (newState == states::DOCKING_ERROR){global.stateTracker[16+(-states::DOCKING_ERROR)] += 1;}
|
|
| 814 |
else if (newState == states::REVERSE_TIMEOUT_ERROR){global.stateTracker[16+(-states::REVERSE_TIMEOUT_ERROR)] += 1;}
|
|
| 815 |
else if (newState == states::CALIBRATION_ERROR){global.stateTracker[16+(-states::CALIBRATION_ERROR)] += 1;}
|
|
| 816 |
else if (newState == states::WHITE_DETECTION_ERROR){global.stateTracker[16+(-states::WHITE_DETECTION_ERROR)] += 1;}
|
|
| 817 |
else if (newState == states::PROXY_DETECTION_ERROR){global.stateTracker[16+(-states::PROXY_DETECTION_ERROR)] += 1;}
|
|
| 818 |
else if (newState == states::NO_CHARGING_POWER_ERROR){global.stateTracker[16+(-states::NO_CHARGING_POWER_ERROR)] += 1;}
|
|
| 819 |
else if (newState == states::UNKNOWN_STATE_ERROR){global.stateTracker[16+(-states::UNKNOWN_STATE_ERROR)] += 1;}
|
|
| 783 |
case states::TEST_MAP_STATE:{
|
|
| 784 |
/* Test suit for amiro map */ |
|
| 785 |
|
|
| 786 |
|
|
| 787 |
setAttributes(global.testmap, 0, 1, 2, 1); |
|
| 788 |
setAttributes(global.testmap, 1, 2, 2, 0); |
|
| 789 |
setAttributes(global.testmap, 2, 1, 0, 0); |
|
| 790 |
setAttributes(global.testmap, 3, 0, 0, 0xff); |
|
| 791 |
// AmiroMap map = AmiroMap(&global); |
|
| 792 |
|
|
| 793 |
// // -------------------------------------------------- |
|
| 794 |
|
|
| 795 |
global.tcase = 0; |
|
| 796 |
map.initialize(); |
|
| 797 |
global.testres[global.tcase] = map.get_state()->valid; |
|
| 798 |
|
|
| 799 |
global.tcase++; // 1 |
|
| 800 |
setAttributes(global.testmap, 0, 1, 2, 0xff); |
|
| 801 |
map.initialize(); |
|
| 802 |
global.testres[global.tcase] = !map.get_state()->valid; |
|
| 803 |
|
|
| 804 |
global.tcase++; // 2 |
|
| 805 |
setAttributes(global.testmap, 0, 1, 2, 0); |
|
| 806 |
setAttributes(global.testmap, 2, 1, 0, 1); |
|
| 807 |
map.initialize(); |
|
| 808 |
global.testres[global.tcase] = map.get_state()->current == 2; |
|
| 809 |
|
|
| 810 |
global.tcase++; // 3 |
|
| 811 |
setAttributes(global.testmap, 3, 0, 0, 0); |
|
| 812 |
setAttributes(global.testmap, 4, 0, 0, 0xff); |
|
| 813 |
map.initialize(); |
|
| 814 |
global.testres[global.tcase] = !map.get_state()->valid; |
|
| 815 |
|
|
| 816 |
int failed = 0; |
|
| 817 |
int passed = 0; |
|
| 818 |
for (int i = 0; i <= global.tcase; i++) {
|
|
| 819 |
if (global.testres[i]) {
|
|
| 820 |
passed++; |
|
| 821 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
|
| 822 |
"Test %d Passed!\n", i); |
|
| 823 |
} else {
|
|
| 824 |
failed++; |
|
| 825 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
|
| 826 |
"Test %d Failed\n", i); |
|
| 827 |
} |
|
| 820 | 828 |
} |
| 821 |
prevState = currentState; |
|
| 822 |
currentState = newState; |
|
| 823 |
// if (utCount.stateCount > CAN_TRANSMIT_STATE_THRESH){
|
|
| 824 |
// utCount.stateCount = 0; |
|
| 825 |
// // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState); |
|
| 826 |
// global.robot.transmitState(currentState); |
|
| 827 |
// // global.robot.setOdometry(global.odometry.getPosition()); |
|
| 828 |
|
|
| 829 |
// }else{
|
|
| 830 |
// utCount.stateCount++; |
|
| 831 |
// } |
|
| 829 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
|
| 830 |
"Total: %d, Passed: %d, Failed: %d\n", global.tcase + 1, passed, |
|
| 831 |
failed); |
|
| 832 |
|
|
| 833 |
newState = states::IDLE; |
|
| 834 |
break; |
|
| 835 |
} |
|
| 836 |
// -------------------------------------------------- |
|
| 837 |
default: |
|
| 838 |
newState = states::UNKNOWN_STATE_ERROR; |
|
| 839 |
break; |
|
| 840 |
} |
|
| 841 |
|
|
| 842 |
if (currentState != newState){
|
|
| 843 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState); |
|
| 844 |
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;}
|
|
| 891 |
} |
|
| 892 |
prevState = currentState; |
|
| 893 |
currentState = newState; |
|
| 894 |
|
|
| 832 | 895 |
this->sleep(CAN::UPDATE_PERIOD); |
| 833 | 896 |
} |
| 834 | 897 |
|
| devices/DiWheelDrive/userthread.hpp | ||
|---|---|---|
| 70 | 70 |
|
| 71 | 71 |
// States of user thread state machine |
| 72 | 72 |
enum states : int8_t{
|
| 73 |
IDLE = 0, |
|
| 74 |
FOLLOW_LINE = 1, |
|
| 75 |
DETECT_STATION = 2, |
|
| 76 |
REVERSE = 3, |
|
| 77 |
PUSH_BACK = 4, |
|
| 78 |
CHECK_POSITIONING = 5, |
|
| 79 |
CHECK_VOLTAGE = 6, |
|
| 80 |
CHARGING = 7, |
|
| 81 |
RELEASE = 8, |
|
| 82 |
RELEASE_TO_CORRECT = 9, |
|
| 83 |
CORRECT_POSITIONING = 10, |
|
| 84 |
TURN = 12, |
|
| 85 |
INACTIVE = 13, |
|
| 86 |
CALIBRATION = 14, |
|
| 87 |
CALIBRATION_CHECK = 15, |
|
| 88 |
DEVIATION_CORRECTION = 16, |
|
| 89 |
DOCKING_ERROR = -1, |
|
| 73 |
IDLE = 0, |
|
| 74 |
FOLLOW_LINE = 1, |
|
| 75 |
DETECT_STATION = 2, |
|
| 76 |
REVERSE = 3, |
|
| 77 |
PUSH_BACK = 4, |
|
| 78 |
CHECK_POSITIONING = 5, |
|
| 79 |
CHECK_VOLTAGE = 6, |
|
| 80 |
CHARGING = 7, |
|
| 81 |
RELEASE = 8, |
|
| 82 |
RELEASE_TO_CORRECT = 9, |
|
| 83 |
CORRECT_POSITIONING = 10, |
|
| 84 |
TURN = 12, |
|
| 85 |
INACTIVE = 13, |
|
| 86 |
CALIBRATION = 14, |
|
| 87 |
CALIBRATION_CHECK = 15, |
|
| 88 |
DEVIATION_CORRECTION = 16, |
|
| 89 |
TEST_MAP_STATE = 17, |
|
| 90 |
DOCKING_ERROR = -1, |
|
| 90 | 91 |
REVERSE_TIMEOUT_ERROR = -2, |
| 91 |
CALIBRATION_ERROR = -3, |
|
| 92 |
CALIBRATION_ERROR = -3,
|
|
| 92 | 93 |
WHITE_DETECTION_ERROR = -4, |
| 93 | 94 |
PROXY_DETECTION_ERROR = -5, |
| 94 |
NO_CHARGING_POWER_ERROR = -6,
|
|
| 95 |
UNKNOWN_STATE_ERROR = -7 |
|
| 95 |
NO_CHARGING_POWER_ERROR = -6, |
|
| 96 |
UNKNOWN_STATE_ERROR = -7
|
|
| 96 | 97 |
}; |
| 97 | 98 |
|
| 98 | 99 |
struct ut_counter{
|
Also available in: Unified diff