From 6f12f59acf8af952c2812928f98aa831dbf312db Mon Sep 17 00:00:00 2001 From: "Dmitry V. Sokolov" Date: Wed, 19 Feb 2020 17:00:27 +0100 Subject: Penny moving sequences explained --- README.md | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index acc096d..0779838 100644 --- a/README.md +++ b/README.md @@ -138,8 +138,30 @@ Then in an endless loop I invoke `movement_planner()`, it sets the goal `pos[]` ### Gait sequences Note that all movement planner variables are stored in 3-element arrays, thus the movements (including the speeds) can be independent one from another. Despite that, my current gait implementation uses synchronized movements of all three servos. - - +Let us see how Penny goes forward. To advance, Penny repeats in a loop 4 steps. +All 4 steps are linear movements to following goal positions: +* step 1: `{zero[0]-range[0], zero[1]-range[1], zero[2]+range[2]}` +* step 2: `{zero[0]-range[0], zero[1]-range[1], zero[2]-range[2]}` +* step 3: `{zero[0]+range[0], zero[1]+range[1], zero[2]-range[2]}` +* step 4: `{zero[0]+range[0], zero[1]+range[1], zero[2]+range[2]}` + +We can express that as a 2d array (4 triplets of goal positions): +```c +const int8_t advance_sequence[4][3] = {{-1, -1, 1}, {-1, -1, -1}, { 1, 1, -1}, { 1, 1, 1}}; +``` +This array tells us that the goal position of the servo `i` at the step `step` is `zero[i] + range[i]*advance_sequence[step][i]`. +Then the following code makes Penny to go forward indefinitely: +``` + uint8_t step = steps_per_sequence-1; // at the initialization stage the (previous) movement is considered to be complete, thus the next movement will be planned starting from the step 0 + while (1) { + if (is_movement_finished()) { + step = (step + 1) % 4; // if previous movement is complete, then perform the next step; this variable loops as 0,1,2,3. + plan_next_movement(step, advance_sequence); // execute next movement + } + movement_planner(); // update the servos position according to the planning + _delay_ms(1); + } +``` ### Obstacle detection -- cgit v1.2.3