Example 11 Recording Values

not found

What does this code do?

This code, the robot records where the servo and motor positions are. Then it calculates the difference between the arm and the servo. Finally it outputs all the recorded values.

Functions Used

variable = (any word)

enable_servo( servo port)

print(inputted message)

return(variable)

read_servo_position( servo port)

set_servo_position_and_print(servo port, position from 1 to 500)

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 read_servo_position(port):
17     position = get_servo_position(port)
18     print("Servo position at port", port, "is:", position)
19     return position
20
21 read_servo_position(arm)  # Read the servo position at port 1
22
23 def set_servo_position_and_print(port, position):
24     set_servo_position(port, position)
25     print("Set servo position at port", port, "to:", position)
26
27 set_servo_position_and_print(arm, 500)  # Set the servo position at port 1 to 500
28
29 def calculate_position_difference(port, target_position):
30     start_position = get_servo_position(port)
31     position_difference = target_position - start_position
32     print("Start position:", start_position, "Target position:", target_position, "Difference:", position_difference)
33     return start_position, position_difference
34
35 calculate_position_difference(arm, 1000)  # Calculate difference to move servo from current position to 1000
36
37 def calculate_position_modifier(start_time, speed):
38     time_elapsed = seconds() - start_time
39     position_modifier = time_elapsed / speed
40     print("Time elapsed:", time_elapsed, "Position modifier:", position_modifier)
41     return position_modifier
42
43 calculate_position_modifier(seconds(), 2)  # Calculate position modifier over 2 seconds
44
45 def move_servo_smoothly(port, target_position, speed):
46     start_time = seconds()
47     start_position, position_difference = calculate_position_difference(port, target_position)
48
49     while seconds() - start_time < speed:
50         position_modifier = calculate_position_modifier(start_time, speed)
51         new_position = int(start_position + (position_difference * position_modifier))
52         set_servo_position_and_print(port, new_position)
53
54     set_servo_position_and_print(port, target_position)
55     msleep(50)