bool doTask(int task) { switch (task) { case GO_FORWARD: return goForward(false); case GO_BACKWARD: return goBackward(); case CALC_NEXT_LINE: return calcNextLine(); case GO_FORWARD_AND_COUNT: return goForwardAndCount(); case TURN_LEFT: return turn90(LEFT); case TURN_RIGHT: return turn90(RIGHT); case TURN_90: // turn this based on the side return true; case TURN_180: return turn180(); case TO_VERTICAL: return craneToVer(); case TO_HORIZONTAL: return craneToHor(); case SLIDE_OUT: return slideOut(); case SLIDE_IN: return slideIn(); case OPEN_GRIPPER: return openGripper(); case CLOSE_GRIPPER: return closeGripper(); case SWITCH_SIDE: return switchSide(); default: return true; } }
void MappingControllerThread(void) { int16_t backLength; // the distance to the center of a crossing int16_t w_right, w_left, w_front, w_rear; // the widths of a crossing // needed for collecting us sensor data for the information about edges int8_t us_pos = 0; uint8_t wait_period = 1; int8_t k = 1; map_node_info node_info; // info about the crossing that shall be saved in the map for (;;) { // Seg_Hex(0x8); os_wait(250); // description of the algorithm for collecting this data: // for every edge each 10 ( = NUMBER_US_VALUES) values of us left and right sensor data // shall be saved. Therefore the array us_values is filled alternately with // left (even positions) and right (odd positions) us sensor data. If the array is full, // every second value will be deleted and afterwards only every second measured // us sensor data is saved in the array to keep the distance between two values constant. // and so on. // --> The array will always be at least half-full and contain us sensor values with // equal distances between them. if (k == wait_period && getWidthFront()==0) { // collect us values for edge information if (us_pos < 2 * NUMBER_US_VALUES) { us_values[us_pos] = Us_Data.Left_Distance; // even value: left us_values[us_pos + 1] = Us_Data.Right_Distance; // odd value: right us_pos += 2; } else { // take every second value and increment the wait period // the array is half-full afterwards uint8_t n; for (n = 0; n < NUMBER_US_VALUES; n += 2) { us_values[n] = us_values[2 * n]; us_values[n + 1] = us_values[2 * n + 1]; } us_pos /= 2; wait_period = wait_period<<=1; // wait_period * 2 } k = 1; // reset k } else k++; // --------------------- // START of crossing if (getCrossingStarted() == 1) { uint8_t edge_length; // calculate length of last edge edge_length = calculateDrivenLen(os_getTime() - getTimeEdgeOld()); // add edge to map and increase the currentNodeNumber addEdge(currentNodeNumber, ++currentNodeNumber, edge_length); // reset value resetCrossingStarted(); } // --------------------- // END of crossing if (getCrossingDetected() == 0) continue; #ifdef MAPPINGCONTROLLER_DEBUG printf("Crossing detected, MappingController is handling it...\r\n"); #endif stopCrossingAnalyzer(); stopWallFollow(); // get the widths and add this information to the node info w_left = getWidthLeft(); w_right = getWidthRight(); w_front = getWidthFront(); w_rear = getWidthRear(); node_info.w0 = w_front; node_info.w1 = w_left; node_info.w2 = w_rear; node_info.w3 = w_right; // Filter open doors etc. // the width of a corridor must be bigger than 40cm if (w_front < 40) w_front = 0; if (w_rear < 40) w_rear = 0; if (w_left < 40) w_left = 0; if (w_right < 40) w_right = 0; // Drive backwards to center of crossing if (w_left != 0 && w_right != 0) backLength = (w_right + w_left) / 4; else backLength = (w_right + w_left) / 2; if (backLength == 0) // dead end backLength = 20; if (w_rear == 0) backLength -= 20; backLength *= -1; // backwards driving #ifdef MAPPINGCONTROLLER_DEBUG printf("Driving %dcm\r\n", backLength); #endif driveLen(backLength); // Handle different crossing types // width_front = direction the car came from // width_rear = direction the car was heading to // side_end: 1: coming up, 2: left, 3: straight, 4: right // add information about the edge to the map and resume driving if (w_left == 0 && w_right == 0 && w_front != 0 && w_rear == 0) { noteCrossing(DeadEnd); addEdgeInfo(last_edge_side, 1, NUMBER_US_VALUES, us_values); last_edge_side = 1; node_info.crossing_type = DeadEnd; turn180(RIGHT, BACKWARDS); resumeDriving(0); } else if (w_left != 0 && w_right == 0 && w_front != 0 && w_rear != 0) { noteCrossing(LeftStraight); addEdgeInfo(last_edge_side, 3, NUMBER_US_VALUES, us_values); last_edge_side = 3; node_info.crossing_type = LeftStraight; // Behavior: drive straight ahead! resumeDriving(w_left / 2 + 50); } else if (w_left == 0 && w_right != 0 && w_front != 0 && w_rear != 0) { noteCrossing(RightStraight); addEdgeInfo(last_edge_side, 4, NUMBER_US_VALUES, us_values); last_edge_side = 4; node_info.crossing_type = RightStraight; turn90(RIGHT, BACKWARDS); resumeDriving(w_rear / 2 + 50); } else if (w_left == 0 && w_right != 0 && w_front != 0 && w_rear == 0) { noteCrossing(RightOnly); addEdgeInfo(last_edge_side, 4, NUMBER_US_VALUES, us_values); last_edge_side = 4; node_info.crossing_type = RightOnly; turn90(RIGHT, BACKWARDS); resumeDriving(w_rear / 2 + 50); } else if (w_left != 0 && w_right == 0 && w_front != 0 && w_rear == 0) { noteCrossing(LeftOnly); addEdgeInfo(last_edge_side, 2, NUMBER_US_VALUES, us_values); last_edge_side = 2; node_info.crossing_type = LeftOnly; turn90(LEFT, BACKWARDS); resumeDriving(w_rear / 2 + 50); } else if (w_left != 0 && w_right != 0 && w_front != 0 && w_rear == 0) { noteCrossing(LeftRight); addEdgeInfo(last_edge_side, 2, NUMBER_US_VALUES, us_values); last_edge_side = 2; node_info.crossing_type = LeftRight; turn90(LEFT, BACKWARDS); resumeDriving(w_rear / 2 + 50); } else if (w_left != 0 && w_right != 0 && w_front != 0 && w_rear != 0) { noteCrossing(All); addEdgeInfo(last_edge_side, 3, NUMBER_US_VALUES, us_values); last_edge_side = 3; node_info.crossing_type = All; // Behavior: drive straight ahead! resumeDriving(w_left / 2 + 50); } else { // dunno resumeDriving(0); } // add information about the current node addNodeInfo(currentNodeNumber, node_info); // reset values for (k = 0; k < 2 * NUMBER_US_VALUES; k++) { us_values[k] = 0; } } }