Example 12 Recording Values While Using The Claw

not found

What does this code do?

In this code, the robot moves it’s claw down, while recording where the claw is and how much percent of the code is done.

Function Implementation

 1 #!/usr/bin/python3
 2 import os, sys
 3 sys.path.append("/usr/lib")
 4 from kipr import *
 5
 6 L = 3
 7 R = 0
 8
 9 grey_value = 1700
10 arm = 0
11 claw = 3
12
13 enable_servo(claw)
14 enable_servo(arm)
15
16 def get_current_time():
17     return seconds()
18
19 def read_servo_position(port):
20     position = get_servo_position(port)
21     print("Servo position at port", port, "is:", position)
22     return position
23
24 def set_servo_position_and_print(port, position):
25     set_servo_position(port, position)
26     print("Set servo position at port", port, "to:", position)
27
28 def calculate_position_difference(port, target_position):
29     start_position = get_servo_position(port)
30     position_difference = target_position - start_position
31     print("Start position:", start_position, "Target position:", target_position, "Difference:", position_difference)
32     return start_position, position_difference
33
34 def calculate_position_modifier(start_time, speed):
35     time_elapsed = seconds() - start_time
36     position_modifier = time_elapsed / speed
37     print("Time elapsed:", time_elapsed, "Position modifier:", position_modifier)
38     return position_modifier
39
40 def move_servo_smoothly(port, target_position, speed):
41     start_time = get_current_time()
42     start_position, position_difference = calculate_position_difference(port, target_position)
43
44     while get_current_time() - start_time < speed:
45         position_modifier = calculate_position_modifier(start_time, speed)
46         new_position = int(start_position + (position_difference * position_modifier))
47         set_servo_position_and_print(port, new_position)
48
49     set_servo_position_and_print(port, target_position)
50     msleep(50)  # Add a short delay
51
52 move_servo_smoothly(arm, 2000, 2)  # Move servo smoothly to position 1000 over 2 seconds
53
54 def slow_servo(port, position, speed):
55     start_time = seconds()
56     start_position = get_servo_position(port)
57     position_difference = position - start_position
58     while seconds() - start_time < speed:
59         position_modifier = (seconds() - start_time) / speed
60         print("Percent =", (seconds() - start_time) / speed)
61         set_servo_position(port, int(start_position + (position_difference * position_modifier)))
62     set_servo_position(port, position)
63     msleep(50)
64
65 slow_servo(arm, 1000, 2)  # Execute the complete slow servo function