'****************************************************************
' walking-gaits.bas
' walking forward, reverse, turning left and turning right 
' PicBasic Pro Compiler - microEngineering Labs
'****************************************************************

temp var byte      ' initialize variables
timer var byte
m_servo var byte
l_servo var byte
r_servo var byte

portb.7 = 0        ' set servo channels to start low  
portb.6 = 0
portb.5 = 0 

m_servo = 140      ' value for servos in middle position  
l_servo = 140
r_servo = 140
 
walk_forward:

             for temp = 1 to 7
                 m_servo = 170
                 gosub servo
                 l_servo = 160
                 r_servo = 160
                 gosub servo
                 m_servo = 100
                 gosub servo
                 l_servo = 120
                 r_servo = 120
                 gosub servo
             next temp

turn_left:

             for temp = 1 to 3
                 m_servo = 170
                 gosub servo
                 l_servo = 120
                 r_servo = 160
                 gosub servo
                 m_servo = 100
                 gosub servo
                 l_servo = 160
                 r_servo = 120
                 gosub servo
             next temp
             
walk_reverse:
             for temp = 1 to 7
                 m_servo = 170
                 gosub servo
                 l_servo = 120
                 r_servo = 120
                 gosub servo
                 m_servo = 100
                 gosub servo
                 l_servo = 160
                 r_servo = 160
                 gosub servo
             next temp

turn_right:

             for temp = 1 to 3
                 m_servo = 170
                 gosub servo
                 l_servo = 160
                 r_servo = 120
                 gosub servo
                 m_servo = 100
                 gosub servo
                 l_servo = 120
                 r_servo = 160
                 gosub servo
             next temp
              
goto walk_forward

servo:                           ' subroutine to set servos

       for timer = 1 to 10
       pulsout portb.7,m_servo
       pulsout portb.6,l_servo
       pulsout portb.5,r_servo
       pause 13
       next timer
return

end