void mainControlTest(void) { mainControlInit(); telemetersStart(); HAL_Delay(500); wall_follow_control.follow_type = NOFOLLOW; rotate180WithCal(CCW, 400, 0); telemetersStop(); mainControlDisplayTest(); }
void followWallTest() { mainControlInit(); telemetersStart(); motorsSleepDriver(OFF); HAL_Delay(500); // control_params.wall_follow_state = TRUE; wall_follow_control.follow_type = FOLLOW_WALL;//NOFOLLOW // moveUTurn(300, 300, 0); //while(1); move(0, 0, 0, 0); moveStartCell(500, 500); moveCell(5, 500, 300); moveRotateCW90(300, 300); moveCell(3, 500, 300); moveRotateCW90(300, 300); moveCell(2, 500, 300); moveRotateCW90(300, 300); moveCell(2, 500, 300); moveRotateCCW90(300, 300); moveCell(1, 500, 300); moveRotateCW90(300, 0); // moveRotateCW90(50, 10); // moveRotateCCW90(50, 10); // moveRotateCCW90(50, 10); while(1); moveRotateCW90(600, 200); moveRotateCW90(600, 200); moveCell(1, 600, 200); moveRotateCCW90(600, 200); moveRotateCCW90(600, 200); HAL_Delay(1000); moveCell(4, 1000, 200); HAL_Delay(1000); moveRotateCW90(600, 200); moveRotateCCW90(600, 200); moveRotateCW90(600, 200); moveRotateCCW90(600, 200); moveRotateCW90(600, 200); moveRotateCCW90(600, 200); moveRotateCW90(600, 200); moveCell(1, 600, 200); telemetersStop(); mainControlDisplayTest(); mainControlDisplayTest(); }
void testPostSensors() { telemetersInit(); telemetersStart(); while (expanderJoyFiltered() != JOY_LEFT) { ssd1306ClearScreen(MAIN_AREA); if (fabs(getTelemeterSpeed(TELEMETER_DL)) > 500) { ssd1306FillRect(0, 58, 5, 5); } // else // { // ssd1306DrawRect(0,49,5,5); // } // ssd1306FillRect(0,0,5,5); // ssd1306DrawRect(0,0,5,5); if (fabs(getTelemeterSpeed(TELEMETER_DR)) > 500) { ssd1306FillRect(49, 58, 5, 5); } // else // { // ssd1306DrawRect(49,49,5,5); // } // ssd1306FillRect(49,49,5,5); // // ssd1306DrawRect(49,49,5,5); // // ssd1306FillRect(49,0,5,5); // // ssd1306DrawRect(49,0,5,5); ssd1306PrintIntAtLine(55, 1, "FL", (int32_t) getTelemeterSpeed(TELEMETER_FL), &Font_5x8); ssd1306PrintIntAtLine(55, 2, "FR", (int32_t) getTelemeterSpeed(TELEMETER_DL), &Font_5x8); ssd1306PrintIntAtLine(55, 3, "DL", (int32_t) getTelemeterSpeed(TELEMETER_DR), &Font_5x8); ssd1306PrintIntAtLine(55, 4, "DR", (int32_t) getTelemeterSpeed(TELEMETER_FR), &Font_5x8); ssd1306Refresh(); } telemetersStop(); }
void testWallsSensors() { telemetersInit(); telemetersStart(); while (expanderJoyFiltered() != JOY_LEFT) { ssd1306ClearScreen(MAIN_AREA); if (getWallPresence(FRONT_WALL) == TRUE) { ssd1306FillRect(0, 59, 54, 5); } else { ssd1306DrawRect(0, 59, 54, 5); } if (getWallPresence(LEFT_WALL) == TRUE) { ssd1306FillRect(0, 10, 5, 54); } else { ssd1306DrawRect(0, 10, 5, 54); } if (getWallPresence(RIGHT_WALL) == TRUE) { ssd1306FillRect(49, 10, 5, 54); } else { ssd1306DrawRect(49, 10, 5, 54); } ssd1306PrintIntAtLine(60, 1, "FL ", (uint32_t) (getTelemeterDist(TELEMETER_FL) * 10.00), &Font_5x8); ssd1306PrintIntAtLine(60, 2, "DL ", (uint32_t) (getTelemeterDist(TELEMETER_DL) * 10.00), &Font_5x8); ssd1306PrintIntAtLine(60, 3, "DR ", (uint32_t) (getTelemeterDist(TELEMETER_DR) * 10.00), &Font_5x8); ssd1306PrintIntAtLine(60, 4, "FR ", (uint32_t) (getTelemeterDist(TELEMETER_FR) * 10.00), &Font_5x8); ssd1306Refresh(); } telemetersStop(); }
int test_move_zhonx () { labyrinthe maze; positionRobot zhonx_position; int max_speed_rotation = SAFE_SPEED_ROTATION; int max_speed_translation = SAFE_SPEED_TRANSLATION; int min_speed_translation = SAFE_SPEED_TRANSLATION; zhonx_position.coordinate_robot.x = 8; zhonx_position.coordinate_robot.y = 8; zhonx_position.midOfCell = TRUE; zhonx_position.orientation = NORTH; coordinate way[]={{8,7},{8,6},{8,5},{8,4},{8,3},{8,2},{8,1},{8,0},{9,0},{10,0},{11,0},{10,0},{9,0},{8,0},{8,1},{8,2},{8,3},{8,4},{8,5},{8,6},{8,7},{8,8},{END_OF_LIST,END_OF_LIST}}; motorsInit(); HAL_Delay(2000); telemetersStart(); mainControlSetFollowType(WALL_FOLLOW); positionControlSetPositionType(GYRO); moveRealZhonxArc(&maze, &zhonx_position, way, max_speed_rotation, max_speed_translation, min_speed_translation); HAL_Delay(500); motorsDriverSleep(ON); telemetersStop(); return MAZE_SOLVER_E_SUCCESS; }