-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathi2c_current_sense_calibrate_example.py
executable file
·107 lines (84 loc) · 3.39 KB
/
i2c_current_sense_calibrate_example.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
#!/usr/bin/env python3
# This example for the Motoron M2H controllers shows how to automatically
# measure the current sense offsets at startup and load them into the Motoron so
# that the processed current measurements are more accurate.
#
# It also uses those current sense offsets to help set current limits.
import time
import motoron
mc = motoron.MotoronI2C()
# ADC reference voltage
reference_mv = 3300
# Specifies what type of Motoron you are using, which is needed for converting
# current sense readings to milliamps.
type = motoron.CurrentSenseType.MOTORON_18V18
# Minimum allowed VIN voltage. This example does not take current calibration
# readings while VIN is below this configurable level.
min_vin_voltage_mv = 6500
units = motoron.current_sense_units_milliamps(type, reference_mv)
def calibrate_current():
mc.set_speed(1, 0)
mc.set_speed(2, 0)
desired_sample_count = 32
sample_count = 0
totals = [ 0, 0 ]
last_time_conditions_not_met = time.monotonic()
while True:
status_mask = ((1 << motoron.STATUS_FLAG_MOTOR_FAULTING) |
(1 << motoron.STATUS_FLAG_NO_POWER) |
(1 << motoron.STATUS_FLAG_MOTOR_OUTPUT_ENABLED) |
(1 << motoron.STATUS_FLAG_MOTOR_DRIVING))
status_desired = 1 << motoron.STATUS_FLAG_MOTOR_OUTPUT_ENABLED
if ((mc.get_status_flags() & status_mask) != status_desired or
(mc.get_vin_voltage_mv(reference_mv) < min_vin_voltage_mv)):
last_time_conditions_not_met = time.monotonic()
sample_count = 0
totals = [0, 0]
if (time.monotonic() - last_time_conditions_not_met) > 0.02:
totals[0] += mc.get_current_sense_raw(1)
totals[1] += mc.get_current_sense_raw(2)
sample_count += 1
if sample_count == desired_sample_count: break
mc.set_current_sense_offset(1, round(totals[0] / desired_sample_count))
mc.set_current_sense_offset(2, round(totals[1] / desired_sample_count))
mc.set_current_sense_minimum_divisor(1, 100)
mc.set_current_sense_minimum_divisor(2, 100)
print(f"Current sense offsets: {mc.get_current_sense_offset(1)} {mc.get_current_sense_offset(2)}")
mc.reinitialize()
mc.clear_reset_flag()
# By default, the Motoron is configured to stop the motors if
# it does not get a motor control command for 1500 ms. You
# can uncomment a line below to adjust this time or disable
# the timeout feature.
# mc.set_command_timeout_milliseconds(1000)
# mc.disable_command_timeout()
# Configure motor 1
mc.set_max_acceleration(1, 200)
mc.set_max_deceleration(1, 200)
# Configure motor 2
mc.set_max_acceleration(2, 200)
mc.set_max_deceleration(2, 200)
calibrate_current()
# Set current limits using the offsets we just measured.
# The second argument to calculate_current_limit is a current limit
# in milliamps.
mc.set_current_limit(1, motoron.calculate_current_limit(10000,
type, reference_mv, mc.get_current_sense_offset(1)))
mc.set_current_limit(2, motoron.calculate_current_limit(10000,
type, reference_mv, mc.get_current_sense_offset(2)))
try:
while True:
mc.set_speed(1, 200)
time.sleep(1)
processed = mc.get_current_sense_processed(1)
print(f"Motor 1 current: {processed} = {round(processed * units)} mA")
mc.set_speed(1, 0)
time.sleep(1)
mc.set_speed(2, 200)
time.sleep(1)
processed = mc.get_current_sense_processed(2)
print(f"Motor 2 current: {processed} = {round(processed * units)} mA")
mc.set_speed(2, 0)
time.sleep(1)
except KeyboardInterrupt:
pass