-
Notifications
You must be signed in to change notification settings - Fork 0
/
ODriveSimpleTest copy.py
190 lines (165 loc) · 7.16 KB
/
ODriveSimpleTest copy.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
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
#!/usr/bin/python3
import odrive
from odrive.enums import *
from odrive.utils import start_liveplotter
import time
import serial
from time import sleep
import logging
import odrive.utils
from odrive.utils import *
from odrive.enums import *
odrive_0 = True
odrive_1 = False
axis_0_0 = True
axis_0_1 = False
axis_1_0 = False
axis_1_1 = False
sensorless = False
vel_limit = 50
vel_gain = 0.01
pos_gain = 1
input_filter_bandwidth = 0.1
torque_constant = 0.021763157099485397
odrive_clone_serialnum = "368838553333"
odrive_0_serialnum = "2088399B4D4D"
#odrv0.system_stats
#odrv0.axis0.config
print(odrive.__version__)
# Find a connected ODrive (this will block until you connect one)
print("finding an odrive...")
if sensorless == False:
if not odrive_0 and not odrive_1:
print("no odrive set in parameter!")
exit()
if odrive_0 and not axis_0_0 and not axis_0_1:
print("no axis selected on odrive0")
exit()
if odrive_1 and not axis_1_0 and not axis_1_1:
print("no axis seleceted on odrive1")
exit()
if odrive_0:
print("odrive_0 found")
odrv0 = odrive.find_any(serial_number=odrive_clone_serialnum)
if axis_0_0:
odrv0.axis0.controller.config.vel_limit = vel_limit
odrv0.axis0.controller.config.vel_gain = vel_gain
odrv0.axis0.controller.config.pos_gain = pos_gain
odrv0.axis0.controller.config.input_filter_bandwidth = input_filter_bandwidth
odrv0.axis0.motor.config.pole_pairs = 11
odrv0.axis0.motor.config.torque_constant = torque_constant
odrv0.axis0.motor.config.motor_type = MOTOR_TYPE_HIGH_CURRENT
if axis_0_1:
odrv0.axis1.controller.config.vel_limit = vel_limit
odrv0.axis1.controller.config.vel_gain = vel_gain
odrv0.axis1.controller.config.pos_gain = pos_gain
odrv0.axis1.controller.config.input_filter_bandwidth = input_filter_bandwidth
odrv0.axis1.motor.config.pole_pairs = 11
odrv0.axis1.motor.config.torque_constant = torque_constant
odrv0.axis1.motor.config.motor_type = MOTOR_TYPE_HIGH_CURRENT
if odrive_1:
print("odrive_1 found")
odrv1 = odrive.find_any(serial_number="2068399D4D4D")
if axis_1_0:
odrv1.axis0.controller.config.vel_limit = vel_limit
odrv1.axis0.controller.config.vel_gain = vel_gain
odrv1.axis0.controller.config.pos_gain = pos_gain
odrv1.axis0.controller.config.input_filter_bandwidth = input_filter_bandwidth
odrv1.axis0.motor.config.pole_pairs = 11
odrv1.axis0.motor.config.torque_constant = torque_constant
odrv1.axis0.motor.config.motor_type = MOTOR_TYPE_HIGH_CURRENT
if axis_1_1:
odrv1.axis1.controller.config.vel_limit = vel_limit
odrv1.axis1.controller.config.vel_gain = vel_gain
odrv1.axis1.controller.config.pos_gain = pos_gain
odrv1.axis1.controller.config.input_filter_bandwidth = input_filter_bandwidth
odrv1.axis1.motor.config.pole_pairs = 11
odrv1.axis1.motor.config.torque_constant = torque_constant
odrv1.axis1.motor.config.motor_type = MOTOR_TYPE_HIGH_CURRENT
if sensorless:
print("odrive_clone found")
odrv1 = odrive.find_any(serial_number=odrive_clone_serialnum)
odrv1.axis0.controller.config.vel_limit = vel_limit
odrv1.axis0.controller.config.vel_gain = vel_gain
odrv1.axis0.controller.config.pos_gain = pos_gain
odrv1.axis0.controller.config.input_filter_bandwidth = input_filter_bandwidth
odrv1.axis0.motor.config.pole_pairs = 11
odrv1.axis0.motor.config.torque_constant = torque_constant
odrv1.axis0.motor.config.motor_type = MOTOR_TYPE_HIGH_CURRENT
# odrv0.axis0.controller.config.vel_gain = 0.01
# odrv0.axis0.controller.config.vel_integrator_gain = 0.05
# odrv0.axis0.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
# odrv0.axis0.controller.config.vel_limit = (odrv0.axis0.config.sensorless_ramp.vel + 50) / (2* 3.14159265359 * 11)
# odrv0.axis0.motor.config.current_lim = 2 * odrv0.axis0.config.sensorless_ramp.current
# odrv0.axis0.sensorless_estimator.config.pm_flux_linkage = 5.51328895422 / (11 * 380)
# odrv0.axis0.config.enable_sensorless_mode = True
# Find an ODrive that is connected on the serial port /dev/ttyUSB0
#my_drive = odrive.find_any("serial:/dev/ttyUSB0")
# Calibrate motor and wait for it to finish
print("starting calibration...")
# odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
# odrv0.axis1.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
# odrv1.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
# odrv1.axis1.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
if odrive_0:
if axis_0_0:
odrv0.axis0.requested_state = INPUT_MODE_POS_FILTER
while odrv0.axis0.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
if axis_0_1:
odrv0.axis1.requested_state = INPUT_MODE_POS_FILTER
while odrv0.axis1.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
odrv0.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
if odrive_1:
if axis_1_0:
odrv1.axis0.requested_state = INPUT_MODE_POS_FILTER
while odrv1.axis0.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
odrv1.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
if axis_1_1:
while odrv1.axis1.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
odrv1.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
if sensorless:
odrv1.axis0.requested_state = INPUT_MODE_POS_FILTER
while odrv0.axis0.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
#odrv1.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
# errors_odrv1 = odrive.utils.dump_errors(odrv1, True)
# odrv1.clear_errors()
#start_liveplotter(lambda:[odrv0.axis0.encoder.pos_estimate, odrv0.axis0.controller.pos_setpoint])
#start_liveplotter(lambda:[odrv1.axis0.encoder.pos_estimate, odrv1.axis0.controller.pos_setpoint,odrv0.axis0.encoder.pos_estimate, odrv0.axis0.controller.pos_setpoint])
odrv_dict = {}
if odrive_0 :
odrv_dict.update({0: odrv0.axis0.controller, 1: odrv0.axis1.controller})
if odrive_1 :
odrv_dict.update({2: odrv1.axis0.controller, 3: odrv1.axis1.controller})
# odrv_dict = {0: odrv0.axis0.controller, 1: odrv0.axis1.controller,
# 2: odrv1.axis0.controller, 3: odrv1.axis1.controller}
while True:
value = int(input("enter position: "))
axis = int(input("enter axis: "))
if axis in odrv_dict:
odrv_dict[axis].input_pos = value
else:
print("Invalid axis. Please enter a value between 0 and 3.")
break
# while True:
# value = input("Enter position: ")
# value = int(value)
# axis = input("Enter axis: ")
# axis = int(axis)
# if axis == 0:
# odrv0.axis0.controller.input_pos = value
# elif axis == 1:
# odrv0.axis1.controller.input_pos = value
# elif axis == 2:
# odrv1.axis0.controller.input_pos = value
# elif axis == 3:
# odrv1.axis1.controller.input_pos = value
# else:
# print("Something went wrong")
# break