-
Notifications
You must be signed in to change notification settings - Fork 0
/
odrive_controller.py
103 lines (61 loc) · 1.95 KB
/
odrive_controller.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
import odrive
from odrive.enums import *
import numpy as np
import time
### CONNECT HIP MOTOR TO AXIS 0
### AND KNEE MOTOR TO AXIS 1
###### DESIRED PARAMS #######
mode = 0 # 0 Position | 1 Velocity
omega = 10
hip_amp = 24
hip_phase = np.pi
knee_amp = 24
knee_phase = np.pi
odrv0 = odrive.find_any()
try: # Reboot causes loss of connection, use try to supress errors
odrv0.reboot()
except:
pass
print("Rebooted [2/7]")
odrv0 = odrive.find_any() # Reconnect to the Odrive
print("Connected [3/7]")
odrv0.axis0.requested_state = AXIS_STATE_IDLE
odrv0.axis1.requested_state = AXIS_STATE_IDLE
time.sleep(0.5)
odrv0.axis0.controller.input_vel = 0
odrv0.axis1.controller.input_vel = 0
odrv0.axis0.requested_state = AXIS_STATE_SENSORLESS_CONTROL
odrv0.axis1.requested_state = AXIS_STATE_SENSORLESS_CONTROL
time.sleep(1)
dt = 0.01
time_passed = 0
if mode == 0:
while time_passed < (8 * np.pi / omega):
hip_pos = hip_amp * np.sin(omega * time_passed + hip_phase)
knee_pos = knee_amp * np.sin(omega * time_passed + knee_phase)
hip_motor_vel = (1/3.7) * hip_vel
knee_motor_vel = (1/3.7) * knee_vel
time_passed += dt
odrv0.axis0.controller.input_vel = hip_motor_vel
odrv0.axis1.controller.input_vel = knee_motor_vel
print(knee_motor_vel)
time.sleep(dt)
elif mode == 1:
while time_passed < (8 * np.pi / omega):
hip_vel = hip_amp * omega * np.cos(omega * time_passed + hip_phase)
knee_vel = knee_amp * omega * np.cos(omega * time_passed + knee_phase)
hip_motor_vel = (1/3.7) * hip_vel
knee_motor_vel = (1/3.7) * knee_vel
time_passed += dt
odrv0.axis0.controller.input_vel = hip_motor_vel
odrv0.axis1.controller.input_vel = knee_motor_vel
print(knee_motor_vel)
time.sleep(dt)
# period = 0.5
# for i in range(5):
# odrv0.axis0.controller.input_vel = 10
# time.sleep(period)
# odrv0.axis0.controller.input_vel = -10
# time.sleep(period)
odrv0.axis0.requested_state = AXIS_STATE_IDLE
odrv0.axis1.requested_state = AXIS_STATE_IDLE