walls ask_cell_state ()
{
walls cell_state;
memset(&cell_state, NO_KNOWN, sizeof(walls));
int joystick = expanderJoyFiltered();
while (joystick != JOY_UP)
{
joystick = expanderJoyFiltered();
switch (joystick) {
case JOY_DOWN:
if (cell_state.front == WALL_PRESENCE)
{
cell_state.front = NO_WALL;
}
else
{
cell_state.front = WALL_PRESENCE;
}
break;
case JOY_RIGHT:
if (cell_state.left == WALL_PRESENCE)
{
cell_state.left = NO_WALL;
}
else
{
cell_state.left = WALL_PRESENCE;
}
break;
case JOY_LEFT:
if (cell_state.right == WALL_PRESENCE)
{
cell_state.right = NO_WALL;
}
else
{
cell_state.right = WALL_PRESENCE;
}
break;
default:
break;
}
print_cell_state(cell_state);
ssd1306Refresh();
}
return cell_state;
}
void mulimeterTest(void)
{
while (expanderJoyFiltered() != JOY_LEFT)
{
ssd1306ClearScreen(MAIN_AREA);
ssd1306PrintIntAtLine(0, 0, "Temp. Gyro = ", (int) (multimeterGyroTemp() * 10.00), &Font_5x8);
ssd1306PrintIntAtLine(0, 1, "Temp. STM32 = ", (int) (multimeterSTM32Temp() * 10.00), &Font_5x8);
ssd1306PrintIntAtLine(0, 3, "vbat (mV) = ", (int) multimeterGetBatVoltage(), &Font_5x8);
ssd1306Refresh();
}
}
void bluetoothTest(void)
{
int i = 0;
while (expanderJoyFiltered() != JOY_LEFT)
{
bluetoothPrintf("hello ZHONX_III, nb send = %d\r\n", i);
ssd1306ClearScreen(MAIN_AREA);
ssd1306DrawStringAtLine(0, 0, "send hello ZHONX III", &Font_5x8);
ssd1306PrintIntAtLine(0, 1, "nb send = ", i, &Font_5x8);
ssd1306Refresh();
i++;
}
antiBounceJoystick();
}
int test_maze_flash ()
{
int rv = MAZE_SOLVER_E_SUCCESS;
labyrinthe maze, maze_to_discovert = {{{{WALL_PRESENCE,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT}},{{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,WALL_PRESENCE,INFINITY_WEIGHT},{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,WALL_PRESENCE,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,NO_WALL,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,NO_WALL,NO_WALL,INFINITY_WEIGHT}},{{WALL_PRESENCE,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,NO_WALL,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT},{NO_WALL,WALL_PRESENCE,WALL_PRESENCE,NO_WALL,INFINITY_WEIGHT}}}};
coordinate end_coordinate = {0,0};
positionRobot start_position = {0,FALSE,{8,8}};
rv=saveMaze(&maze_to_discovert, &start_position, &end_coordinate);
if (rv != FLASH_DRIVER_E_SUCCESS)
{
}
loadMaze(&maze, &start_position, &end_coordinate);
printMaze(maze,end_coordinate);
printLength(maze, end_coordinate.x, end_coordinate.y);
while (expanderJoyFiltered() != JOY_LEFT);
return rv;
}
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();
}
int waitValidation(unsigned long timeout)
{
timeout += HAL_GetTick();
do
{
switch (expanderJoyFiltered())
{
case JOY_RIGHT:
return JOY_RIGHT;
break;
case JOY_LEFT:
return JOY_LEFT;
break;
}
}
while (timeout > HAL_GetTick());
return -1;
}
void lineSensorsTest(void)
{
lineSensorsInit();
lineSensorsStart();
while(expanderJoyFiltered()!=JOY_LEFT)
{
ssd1306ClearScreen();
ssd1306PrintInt(10, 5, "LEFT_EXT = ", (uint16_t) lineSensors.left_ext.adc_value, &Font_5x8);
ssd1306PrintInt(10, 15, "LEFT = ", (uint16_t) lineSensors.left.adc_value, &Font_5x8);
ssd1306PrintInt(10, 25, "FRONT = ", (uint16_t) lineSensors.front.adc_value, &Font_5x8);
ssd1306PrintInt(10, 35, "RIGHT = ", (uint16_t) lineSensors.right.adc_value, &Font_5x8);
ssd1306PrintInt(10, 45, "RIGHT_EXT = ", (uint16_t) lineSensors.right_ext.adc_value, &Font_5x8);
ssd1306Refresh();
}
lineSensorsStop();
}
void graphMotorSettings (float *acceleration, float *maxSpeed, float *deceleration)
{
int number_value=0;
float* values[3]={acceleration,maxSpeed,deceleration};
while(true)
{
printGraphMotor ( *acceleration, *maxSpeed, *deceleration);
switch (expanderJoyFiltered())
{
case JOY_LEFT:
if (number_value <= 0)
{
return;
}
else
{
number_value--;
}
break;
case JOY_DOWN:
*(values[number_value])-=0.1;
break;
case JOY_UP:
*(values[number_value])+=0.1;
break;
case JOY_RIGHT:
if (number_value >= 2)
{
return;
}
else
{
number_value++;
}
break;
default:
break;
}
}
}
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();
}
void mainControlDisplayTest(void)
{
while(expanderJoyFiltered()!=JOY_LEFT)
{
ssd1306ClearScreen();
ssd1306PrintInt(10, 5, "speed dist = ",(int) (speed_control.current_distance * 100), &Font_5x8);
ssd1306PrintInt(10, 15, "follow err = ",(int) (wall_follow_control.follow_error), &Font_5x8);
ssd1306PrintInt(10, 25, "right_dist = ",(int) (position_control.end_control * 100), &Font_5x8);
ssd1306PrintInt(10, 35, "gyro = ",(int16_t) GyroGetAngle(), &Font_5x8);
ssd1306PrintInt(10, 45, "left PWM = ",(int16_t) transfert_function.left_motor_pwm, &Font_5x8);
ssd1306PrintInt(10, 55, "right PWM = ",(int16_t) transfert_function.right_motor_pwm, &Font_5x8);
// bluetoothPrintf("pwm right :%d \t %d \n",(int)transfert_function.right_motor_pwm, (int)(follow_control.follow_error*100));
// bluetoothPrintInt("error", follow_control.follow_error);
// transfert_function.right_motor_pwm = (speed_control.speed_command - (position_control.position_command + follow_control.follow_command)) * transfert_function.pwm_ratio;
// transfert_function.left_motor_pwm = (speed_control.speed_command + (position_control.position_command + follow_control.follow_command)) * transfert_function.pwm_ratio;
ssd1306Refresh();
}
pid_loop.start_state = FALSE;
telemetersStop();
motorsSleepDriver(ON);
}
int modifyLongParam( char *param_name,long *param)
{
int step=1;
char str[40];
long param_copy = *param;
char collone=0;
ssd1306ClearScreen();
// Write the parameter name
ssd1306DrawString(0, 0,param_name, &Font_5x8);
ssd1306DrawLine(0, 9, 128, 9);
sprintf(str, "%10i", (int)param_copy);
ssd1306DrawString(0, 28, str, &Font_8x8);
ssd1306DrawString(0, 50, "PRESS 'RIGHT' TO VALIDATE", &Font_3x6);
ssd1306DrawString(0, 57, " 'LEFT' TO RETURN.", &Font_3x6);
ssd1306DrawString((10-collone)*8,20,"^",&Font_8x8);
ssd1306DrawString((10-collone)*8,36,"v",&Font_8x8);
ssd1306Refresh();
while (1)
{
// Exit Button
int joystick=expanderJoyFiltered();
switch (joystick)
{
case JOY_LEFT :
if (collone==10)
return SUCCESS;
else
{
collone++;
ssd1306ClearRect(0,20,128,8);
ssd1306ClearRect(0,36,128,8);
ssd1306DrawString((9-collone)*9,20,"^",&Font_8x8);
ssd1306DrawString((9-collone)*9,36,"v",&Font_8x8);
ssd1306Refresh();
}
break;
case JOY_UP:
//param_copy +=1;
param_copy += (step*pow(10,collone));
ssd1306ClearRect(0, 28, 164, 8);
sprintf(str, "%10i", (int)param_copy);
ssd1306DrawString(0, 28, str, &Font_8x8);
ssd1306Refresh();
break;
case JOY_DOWN :
param_copy -= (step*pow(10,collone));
//param_copy -= 1;
ssd1306ClearRect(0, 28, 164, 8);
sprintf(str, "%10i", (int)param_copy);
ssd1306DrawString(0, 28, str, &Font_8x8);
ssd1306Refresh();
break;
case JOY_RIGHT :
if(collone==0)
{
*param = param_copy;
ssd1306Refresh();
return SUCCESS;
}
else
{
collone--;
ssd1306ClearRect(0,20,128,8);
ssd1306ClearRect(0,36,128,8);
ssd1306DrawString((9-collone)*9,20,"^",&Font_8x8);
ssd1306DrawString((9-collone)*9,36,"v",&Font_8x8);
ssd1306Refresh();
}
break;
default:
break;
}
}
return SUCCESS;
}
int modifyBoolParam( char *param_name, unsigned char *param)
{
char str[4];
bool param_copy = (bool)*param;
ssd1306ClearScreen();
// Write the parameter name
ssd1306DrawString(0, 0,param_name, &Font_5x8);
ssd1306DrawLine(0, 9, 128, 9);
if (param_copy == true)
{
sprintf(str, "YES");
}
else
{
sprintf(str, "NO");
}
ssd1306DrawString(0, 28, str, &Font_8x8);
ssd1306DrawString(0, 50, "PRESS 'RIGHT' TO VALIDATE", &Font_3x6);
ssd1306DrawString(0, 57, " 'LEFT' TO RETURN.", &Font_3x6);
ssd1306Refresh();
while (1)
{
int joystick=expanderJoyFiltered();
switch (joystick)
{
case JOY_LEFT :
return SUCCESS;
break;
case JOY_DOWN:
case JOY_UP :
if (param_copy == true)
{
param_copy = false;
sprintf(str, "NO");
}
else
{
param_copy = true;
sprintf(str, "YES");
}
ssd1306ClearRect(0, 28, 164, 8);
ssd1306DrawString(0, 28, str, &Font_8x8);
ssd1306Refresh();
break;
case JOY_RIGHT:
*param = param_copy;
ssd1306ClearScreen();
ssd1306Refresh();
return SUCCESS;
break;
}
}
return SUCCESS;
}
int menu(const menuItem Menu)
{
signed char line_screen = 1;
signed char line_menu = 0;
displayMenu(Menu, line_menu);
ssd1306InvertArea(0, MARGIN, HIGHLIGHT_LENGHT, HIGHLIGHT_HEIGHT);
ssd1306Refresh();
while (true)
{
int joystick = expanderJoyFiltered();
// Exit Button JOYSTICK_LEFT
switch (joystick)
{
case JOY_LEFT:
return SUCCESS;
break;
// Joystick down
case JOY_DOWN:
//beeper
if (Menu.line[line_menu + 1].name != null)
{
line_menu++;
line_screen++;
if (line_screen > MAX_LINE_SCREEN)
{
line_screen--;
displayMenu(Menu, line_menu - (line_screen - 1));
ssd1306InvertArea(0, line_screen * MARGIN,
HIGHLIGHT_LENGHT, HIGHLIGHT_HEIGHT);
ssd1306Refresh();
}
else
{
menuHighlightedMove((line_screen - 1) * ROW_HEIGHT + 1,
(line_screen) * ROW_HEIGHT);
}
}
break;
case JOY_UP:
//beeper
if (line_screen == 1)
{
if (line_menu > 0)
{
line_menu--;
displayMenu(Menu, line_menu);
ssd1306InvertArea(0, MARGIN, HIGHLIGHT_LENGHT,
HIGHLIGHT_HEIGHT);
ssd1306Refresh();
}
}
else
{
line_menu--;
line_screen--;
menuHighlightedMove((line_screen + 1) * ROW_HEIGHT - 1,
(line_screen) * ROW_HEIGHT);
}
break;
case JOY_RIGHT: // Validate button joystick right
//hal_beeper_beep(app_context.beeper, 4000, 10);
switch (Menu.line[line_menu].type)
{
case 'b':
modifyBoolParam(Menu.line[line_menu].name,
(unsigned char*) Menu.line[line_menu].param);
break;
case 'i':
modifyLongParam(Menu.line[line_menu].name,
(long*) (int*) Menu.line[line_menu].param);
break;
case 'l':
modifyLongParam(Menu.line[line_menu].name,
(long*) Menu.line[line_menu].param);
break;
case 'm':
menu(*(const menuItem*) Menu.line[line_menu].param);
break;
case 'f':
if (Menu.line[line_menu].param != null)
{
ssd1306ClearScreen();
ssd1306Refresh();
Menu.line[line_menu].param();
}
break;
case 'g':
graphMotorSettings(
(float*) Menu.line[line_menu - 3].param,
(float*) Menu.line[line_menu - 2].param,
(float*) Menu.line[line_menu - 1].param);
break;
default:
break;
}
displayMenu(Menu, line_menu - (line_screen - 1));
ssd1306InvertArea(0, MARGIN * line_screen, HIGHLIGHT_LENGHT,
HIGHLIGHT_HEIGHT);
ssd1306Refresh();
break;
default:
break;
}
cmdline_parse();
}
return -1;
}
