Example 10 Using The Claw
========================

.. image:: ../tutorial/code10.gif
    :alt: not found
    :align: center

What does this code do?
-----------------------
In this code the robot opens its claw and goes forward for 20ms, then arm goes down. Claw closes to capture can.
Finally, the arm rises up.

Functions Used
--------------

cmpc( clear_motor_position_counter )

abs( absolute )

gmpc( get_motor_position_counter )

Function Implementation
***********************

.. code-block:: python
   :linenos:

   #!/usr/bin/python3
   import os, sys
   sys.path.append("/usr/lib")
   from kipr import*

   L = 3
   R = 0
   grey_value = 1700
   arm = 0
   claw = 3

   def drive(distance, speed):
       cmpc(L)
       while abs(gmpc(L)) < distance:
           mav(L, speed)
           mav(R, speed)
       mav(L, 0)
       mav(R, 0)
       msleep(20)

   def turn_right(distance, speed):
       cmpc(L)
       while gmpc(L) < distance:
           mav(L, speed)
           mav(R, -speed)
       mav(L, 0)
       mav(R, 0)
       msleep(20)

   def turn_left(distance, speed):
       cmpc(R)
       while gmpc(R) < distance:
           mav(L, -speed)
           mav(R, speed)
       mav(L, 0)
       mav(R, 0)
       msleep(20)

   def line_Right_follow(distance, speed, port):
       cmpc(L)
       while abs(gmpc(0)) < distance:
           error = analog(L) - grey_value
           speed_modifier = error * 0.07
           mav(L, speed - speed_modifier)
           mav(R, speed + speed_modifier)
       mav(L, 0)
       mav(R, 0)
       msleep(20)

   def stop_line(speed):
       while analog(R) < grey_value or analog(L) < grey_value:
           if analog(L) < grey_value:
               mav(L, speed)
           else:
               mav(R, -speed * 0.8)
           if analog(R) < grey_value:
               mav(R, speed)
           else:
               mav(L, -speed * 0.8)
       mav(L, 0)
       mav(R, 0)
       msleep(50)

   def slow_servo(port, position, speed):
       start_time = seconds()
       start_position = get_servo_position(port)
       position_diffirence = position - start_position
       while seconds() - start_time < speed:
           position_modifier = (seconds() - start_time) / speed
           set_servo_position(port, start_position + (position_diffirence * position_modifier))
       set_servo_position(port, position)
       msleep(50)

   def main():
       enable_servo(claw)
       enable_servo(arm)

       slow_servo(claw, 1500, 2)
       slow_servo(arm, 2000, 2)
       drive(2000, 1200)
       slow_servo(claw, 2000, 2)
       slow_servo(arm, 750, 2)
       drive(2000, -1200)

       disable_servo(claw)
       disable_servo(arm)

   if __name__ == "__main__":
      main()