| Pseudocode of Autonomy Algorithm in sTetro: |
| // Initialization Start |
| While (staircase Not detected) // using staircase recognition algorithm. |
| TURN in-place; // rotate CCW |
| endWhile |
| STOP Turning; |
| Initialization ends |
| //Step#1; |
| If (ToF0_range< 1500mm) // continue staircase climbing until ToF0 > 1.5 m. |
| // Forward motion. |
| While (ToF0_range > 5mm) // move fwd until ToF0 measure < 5 mm. |
| MOVE FWD; |
| endWhile |
| STOP Moving fwd; |
| LIFT Block-1 to ~18 cm; // determined by mechanical lock. |
| While (SW1_OFF or SW2_OFF) // move fwd until SW1 or SW2 is ON. |
| MOVE FWD; |
| endWhile |
| STOP Moving fwd; |
| While (ToF1_Range_Down < 1) // drop Block_1 until ToF1 senses < 1cm range. |
| DROP Block-1; |
| endWhile |
| STOP Dropping Block-1; |
| TURN Herkulex (H-) wheels to left 90° CCW; |
| // Leftwards motion. |
| While (BM_1 ~ 3 Not Touch) // move leftwards until BM_1 ~ 3 touches left wall. |
| START Moving leftwards; |
| If (Heading >3°) // misalignment detection & correction. |
| MOVE H-wheels CW to achieve robot alignment; |
| elseif (Heading > -3°) |
| MOVE H-wheels CCW to achieve robot alignment; |
| endif |
| endWhile |
| STOP moving leftwards; |
| // Rightwards motion. |
| While (BM 4 ~ 6 Not Touch) // move rightwards until BM_4~6 touches right wall. |
| START Moving rightwards; |
| If (Heading > 3°) // misalignment detection & correction. |
| MOVE H-wheels CCW to achieve robot alignment; |
| elseif (Heading > -3°) |
| MOVE H-wheels CW to achieve robot alignment; |
| endif |
| endWhile |
| STOP moving rightwards; |
| MOVE leftwards for 2 ~ 3 sec. // this movement is to keep robot little away from right wall |
| // of stairs, to prevent rubbing with right side-wall, before stepping to next staircase. |
| Turn H-wheels 90° CW. |
| LIFT Block-2 upwards (~18cm) // determined by mechanical lock. |
| While (ToF3 >0.5cm) // move fwd until ToF3 measures< 0.5cm. |
| MOVE FWD; |
| endWhile |
| STOP Moving fwd; |
| While (ToF2_Range_Down < 0.5) // drop Block_2 until ToF2 senses < 0.5cm range. |
| DROP Block-2; |
| endWhile |
| STOP Dropping Block-2; |
| LIFT Block-3 upwards (~18cm); // determined by mechanical lock. |
| Go to Step#1; |
| endIf |
| STOP sTetro; // finished climbing up the staircase. |
