Statistics
| Revision:

## root / trunk / code / projects / traffic_navigation / validTurns.c @ 1884

 1 ```/* ``` ``` * Deterministic turning implemented using table lookup. Using a ``` ``` * table, the function looks up a value from the table using the ``` ``` * barcode value, and then the robot decides to go straight, ``` ``` * left, right, or u-turn depending on the valid turn types ``` ``` * for the intersection it is at. ``` ``` * ``` ``` */ ``` ```#include ``` ```#include "validTurns.h" ``` ```/******************************Random Num Gen Version of validTurns ``` ```int randomNumGen(int max){ ``` ``` int x = range_read_distance(IR2); ``` ``` if (x>0) return range_read_distance(IR2)%max; ``` ``` else return randomNumGen(max); ``` ```} ``` ``` ``` ```int getCrossType(int barcode) ``` ```{ ``` ``` int x = randomNumGen(4); ``` ``` if (x == DOUBLE) { ``` ``` int y = randomNumGen(2); ``` ``` if (y == 0) x = DOUBLE_C; ``` ``` else x = DOUBLE_T; ``` ``` } ``` ``` return x; ``` ```} ``` ``` ``` ```int getCrossPos(int barcode, int max) ``` ```{ ``` ``` return randomNumGen(max); ``` ```} ``` ``` ``` ```int getTurnType(int barcode) ``` ```{ ``` ``` return randomNumGen(4); ``` ```} ``` ```*/ ``` ```int getCrossType(int barcode){ ``` ``` int crosstype = (barcode>>2)&7; ``` ``` return crosstype; ``` ```} ``` ```int getCrossPos(int barcode, int max){ ``` ``` //@TODO: implement some max checking ``` ``` return (barcode)&3; ``` ```} ``` ```int validateTurn(int barcode, int turn_type) ``` ```{ ``` ``` int cross_type; ``` ``` int cross_pos; ``` ``` cross_type = getCrossType(barcode); ``` ``` switch (cross_type) ``` ``` { ``` ``` case INTERSECTION_DOUBLE_C: ``` ``` { ``` ``` cross_pos = getCrossPos(barcode, 4); ``` ``` if (0<=cross_pos && cross_pos<=3) ``` ``` return turn_type; ``` ``` break; ``` ``` } ``` ``` case DOUBLE: //Implements DOUBLE as DOUBLE_T ``` ``` //@TODO: we really shouldn't use DOUBLE, it's part of another system. but it equals 0, so error checking. ``` ``` case INTERSECTION_DOUBLE_T: ``` ``` { ``` ``` cross_pos = getCrossPos(barcode, 3); ``` ``` switch (cross_pos) ``` ``` { ``` ``` case TLEFT: ``` ``` { ``` ``` if (turn_type == ILEFT) turn_type = ISTRAIGHT; ``` ``` return turn_type; ``` ``` break; ``` ``` } ``` ``` case TRIGHT: ``` ``` { ``` ``` if (turn_type == IRIGHT) turn_type = ISTRAIGHT; ``` ``` return turn_type; ``` ``` break; ``` ``` } ``` ``` case TMIDDLE: ``` ``` { ``` ``` if (turn_type == ISTRAIGHT) turn_type = IUTURN; ``` ``` return turn_type; ``` ``` break; ``` ``` } ``` ``` default: ``` ``` return -1; ``` ``` } ``` ``` break; ``` ``` } ``` ``` case INTERSECTION_SINGLE: ``` ``` { ``` ``` cross_pos = getCrossPos(barcode, 2); ``` ``` switch (cross_pos) ``` ``` { ``` ``` case SACROSS: ``` ``` { ``` ``` if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT; ``` ``` return turn_type; ``` ``` break; ``` ``` } ``` ``` case SUP: ``` ``` { ``` ``` if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT; ``` ``` return turn_type; ``` ``` break; ``` ``` } ``` ``` default: ``` ``` return -1; ``` ``` } ``` ``` break; ``` ``` } ``` ``` case INTERSECTION_ON_RAMP: ``` ``` { ``` ``` cross_pos = getCrossPos(barcode, 3); ``` ``` switch (cross_pos) ``` ``` { ``` ``` case R_LEFT: ``` ``` { ``` ``` if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT; ``` ``` return turn_type; ``` ``` break; ``` ``` } ``` ``` case R_RIGHT: ``` ``` { ``` ``` if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT; ``` ``` return turn_type; ``` ``` break; ``` ``` } ``` ``` default: ``` ``` return -1; ``` ``` } ``` ``` break; ``` ``` } ``` ``` case INTERSECTION_OFF_RAMP: ``` ``` { ``` ``` cross_pos = getCrossPos(barcode, 3); ``` ``` switch (cross_pos) ``` ``` { ``` ``` case R_LEFT: ``` ``` { ``` ``` int turn_type = ISTRAIGHT; ``` ``` return turn_type; ``` ``` break; ``` ``` } ``` ``` case R_RIGHT: ``` ``` { ``` ``` int turn_type = ISTRAIGHT; ``` ``` return turn_type; ``` ``` break; ``` ``` } ``` ``` case R_RAMP: ``` ``` { ``` ``` if (turn_type == ISTRAIGHT || turn_type == IUTURN) turn_type = ILEFT; ``` ``` return turn_type; ``` ``` break; ``` ``` } ``` ``` default: ``` ``` return -1; ``` ``` } ``` ``` break; ``` ``` } ``` ``` default: ``` ``` return -1; ``` ```} ``` ```} ```