#------------------------------------------------------------------------------
# TiViPE Textual Robotics Language. 
# Copyright (c) 2016, TiViPE, All rights reserved.
# Url:    www.tivipe.com
# Email:  tino@tivipe.com
# Author: Tino Lourens
# Date:   12 Sep 2016
#------------------------------------------------------------------------------
#------------------------------------------------------------------------------
# Enable motor stiffness; goto stand init position
#------------------------------------------------------------------------------
wait(2000)
&
stiff (1, 500, 0)
&
posture(Crouch)
&
#------------------------------------------------------------------------------
# move moves a single motor 
# movem multiple motors to multiple angles
#------------------------------------------------------------------------------
say(Lets_move_the_shoulders__wrists__and_fingers_at_the_same_time)
&
movem(LWristYaw,    -100, 1000, -50, 1000, -100, 1000,
      RWristYaw,     100, 1000,  50, 1000,  100, 1000,
	  LHand,          57, 1000,   0, 1000,
	  RHand,          57, 1000,   0, 1000,
	  LShoulderPitch, 40, 1000,  80, 1000,
	  RShoulderPitch, 40, 1000,  80, 1000)
&
[
  [say(No) & wait(1000) & say(No)]
  |
  [move(HeadYaw, 10, 250, 1)
   &
   movem(HeadYaw, -10, 500, 10, 500, -10, 500, 10, 500, 0, 250)
  ]
]
&
wait(2000)
&
[
  [say(Yes) & wait(1000) & say(Yes)]
  |
  [move(HeadPitch, 10, 250, 1)
   &
   movem(HeadPitch, -10, 500, 10, 500, -10, 500, 10, 500, 0, 250)
  ]
]
&
[
  [  
     say(How_to_move_forward_without_walking) & 
     wait(2000) & 
	 say (I_like_to_move_it_move_it) & 
	 wait(2000) &
	 say(I_like_to_move_it)
  ]
  |
  [
    do(12)
      movem(HeadPitch, -38.5, 175, 29.5, 175, -38.5, 175, 29.5, 175,-38.5, 175, 29.5, 175) 
    enddo
    &
    movem(HeadPitch, 0.0, 175)
  ]
]
&
wait(2000)
&
say(Cool__I_moved_15_centimeters_forward)
&
#------------------------------------------------------------------------------
# Relax motor stiffness
#------------------------------------------------------------------------------
posture(Crouch)
&
stiff(0, 500, 0)
#------------------------------------------------------------------------------
# End of program
#------------------------------------------------------------------------------