From 4f44ffe177d306b3a42850474a34caafbe065f89 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jakub=20Van=C4=9Bk?= <linuxtardis@gmail.com>
Date: Fri, 30 Jun 2023 08:02:23 +0200
Subject: [PATCH] motor model experiments (current estimation)
---
programs/examples/ev3/motor_ident.py | 45 +++++++++++++++++++++++
programs/hbridge_test.py | 54 +++++++++++++++++++++++-----
2 files changed, 91 insertions(+), 8 deletions(-)
create mode 100644 programs/examples/ev3/motor_ident.py
diff --git a/programs/examples/ev3/motor_ident.py b/programs/examples/ev3/motor_ident.py
new file mode 100644
index 0000000..8ae6b36
--- /dev/null
+++ b/programs/examples/ev3/motor_ident.py
@@ -0,0 +1,45 @@
+import machine
+import uarray
+import utime
+
+import lib.hw_defs.pins
+from lib.ev3 import Motor
+from lib.robot_consts import Port
+
+
+def main():
+ global robot
+ robot.battery.deinit()
+ adc = machine.ADC(machine.Pin(lib.hw_defs.pins.VBATT_ADC_PIN))
+ m = Motor(Port.M1)
+ samples = 2048
+ time = uarray.array('L', [0] * samples)
+ pos = uarray.array('l', [0] * samples)
+ power = uarray.array('b', [0]*samples)
+ vbatt = uarray.array('H', [0]*samples)
+
+ tstart = utime.ticks_us()
+ tnext = utime.ticks_add(tstart, 1000)
+ idx = 0
+ while idx < samples:
+ if idx == 48:
+ m.dc(100)
+
+ time[idx] = utime.ticks_diff(utime.ticks_us(), tstart)
+ pos[idx] = m.angle()
+ power[idx] = m.power()
+ vbatt[idx] = adc.read_u16()
+ idx += 1
+
+ tremain = utime.ticks_diff(tnext, utime.ticks_us())
+ tnext = utime.ticks_add(tnext, 1000)
+ if tremain > 0:
+ utime.sleep_us(tremain)
+
+ print("================")
+ print("time,pos,power,vbatt")
+ for i in range(0, idx):
+ print(f"{time[i]},{pos[i]},{power[i]},{vbatt[i]}")
+ print("================")
+
+main()
diff --git a/programs/hbridge_test.py b/programs/hbridge_test.py
index b011534..e027a5e 100644
--- a/programs/hbridge_test.py
+++ b/programs/hbridge_test.py
@@ -1,10 +1,36 @@
-import utime
+import math
+import machine
+
+from lib.cube.sh1106 import SH1106_I2C
from lib.ev3 import Motor
from lib.hw_defs.ports import *
-from lib.cube.sh1106 import SH1106_I2C
-from lib.robot_consts import Button
+from lib.robot_consts import Button
+
+K_MOTOR = 0.459 * math.pi / 180
+# R_POWERED = 6.148
+# R_SHORTED = 7.0
+
+# R_MOTOR = 5.2 # direct measurement (ohmmeter across the motor)
+# R_SHORTED = 6.0 # direct measurement (ohmmeter in the shorted loop)
+I_FULL = 1.21 # measured in-loop
+U_BATT_FULL = 7.28 # value on display
+U_VM_FULL = 6.20 # measured across the motor pins
+
+I_SHORTED = 0.35 # measured in the loop
+U_SHORTED = 0.212 # measured across the motor when shorted
+W_SHORTED = 280 # value on display; very rough estimate
+
+R_HBRIDGE_SHORTED = U_SHORTED / I_SHORTED
+R_MOTOR = U_VM_FULL / I_FULL
+R_HBRIDGE_POWERED = (U_BATT_FULL - U_VM_FULL) / I_FULL
+R_POWERED = R_HBRIDGE_POWERED + R_MOTOR
+R_SHORTED = R_HBRIDGE_SHORTED + R_MOTOR
+
+# W_FREERUN = 840
+# U_FREERUN = 6.65
+# I_FREERUN = 0
class Hbridge:
def __init__(self, port: int):
@@ -30,7 +56,7 @@ class Hbridge:
if self.is_enabled:
self.motor.dc(self.power_pct)
else:
- self.motor.brake()
+ self.motor.coast()
def get_position(self) -> int:
return self.motor.angle()
@@ -41,6 +67,8 @@ class Hbridge:
def main():
global robot
+ robot.battery.deinit()
+ vbatt_adc = machine.ADC(machine.Pin(VBATT_ADC_PIN))
display: SH1106_I2C = robot.display
ports = [
Hbridge(i)
@@ -68,8 +96,18 @@ def main():
current_port_idx = (current_port_idx + 1) % 4
current_port = ports[current_port_idx]
- vbatt = robot.battery.voltage()
+ pmot = current_port.motor.power()
+ xmot = current_port.get_position()
+ wmot = current_port.get_speed()
+
+ vbatt = vbatt_adc.read_u16() / 65535 * 3.2 * 4
vbatt_str = f"{vbatt:3.2f}V"
+ if current_port.is_enabled:
+ p = pmot / 100
+ vemf = K_MOTOR * wmot
+ imot = (vbatt - vemf) / R_POWERED * p - vemf / R_SHORTED * (1 - p)
+ else:
+ imot = 0
display.fill(0)
display.text(f"Motor test", 0, 0)
@@ -80,10 +118,10 @@ def main():
display.text(f"Port {current_port_idx + 1}:", 0, 0 + TOP_SPACER)
display.text(f" hbridge {'ON' if current_port.is_enabled else 'OFF'}", 0, 8 + TOP_SPACER)
display.text(f" pwm: {current_port.power_pct:+5d} %", 0, 16 + TOP_SPACER)
- display.text(f" pos: {current_port.get_position():+5d} d", 0, 24 + TOP_SPACER)
- display.text(f" spd: {current_port.get_speed():+5d} d/s", 0, 32 + TOP_SPACER)
+ display.text(f" pos: {xmot:+5d} d", 0, 24 + TOP_SPACER)
+ display.text(f" spd: {wmot:+5d} d/s", 0, 32 + TOP_SPACER)
+ display.text(f" cur: {imot:+5.2f} A", 0, 40 + TOP_SPACER)
display.show()
- utime.sleep_ms(100)
main()
--
GitLab