stall torque tests

opensourceleg/benchmarks/benchmark_manager.py

@@ -0,0 +1,62 @@
+from time import time
+import numpy as np
+
+
+class SimpleTimer:
+
+    def __init__(self):
+        self.start_time = 0
+        self.end_time = 0
+        self._checked_end = False
+
+    def start(self, timer_duration):
+        self.start_time = time()
+        self.end_time = self.start_time + timer_duration
+        self._checked_end = False
+
+    @property
+    def just_done(self) -> bool:
+        if self.is_done and not self._checked_end:
+            self._checked_end = True
+            return True
+        else:
+            return False
+
+    @property
+    def is_done(self) -> bool:
+        return time() >= self.end_time
+
+
+class LimitVelocity:
+
+    def __init__(self, velocity, update_freq):
+        self.velocity = velocity # [_/sec], absolute (pos) maximum change in parameter per second
+        self.update_freq = update_freq # [Hz], expected frequency at which parameter will be updated
+        self.on_target = 1
+        self._step = velocity/update_freq # absolute (pos) step in parameter per cycle
+        self._prev_val = 0.0
+        self.target = 0.0
+
+    def update(self, target):
+        self.target = target
+        if target > self._prev_val and target > self._prev_val + self._step: # parameter increasing
+            value = self._prev_val + self._step
+            self.on_target = 0
+        elif target < self._prev_val and target < self._prev_val - self._step: # parameter decreasing
+            value = self._prev_val - self._step
+            self.on_target = 0
+        else:
+            value = target
+            self.on_target = 1
+        self._prev_val = value
+        return value
+
+    @property
+    def is_stopped(self) -> bool:
+        return self._prev_val==0.0 and self.target==0.0
+
+
+
+
+
+

opensourceleg/benchmarks/current_step.py

@@ -0,0 +1,87 @@
+from opensourceleg.actuators.dephy import DephyActuator
+from opensourceleg.logging import LOGGER
+from opensourceleg.time import SoftRealtimeLoop
+from opensourceleg.actuators.base import CONTROL_MODES
+from flexsea.device import Device
+
+from opensourceleg.sensors.torque_sensor import Futek100Nm
+from opensourceleg.benchmarks.benchmark_manager import SimpleTimer, LimitVelocity
+import numpy as np
+import csv
+
+FREQUENCY = 200
+
+
+
+def current_step():
+    t_cond = 2.0 # [sec], time held on target
+    current_velocity = 500 # [mA/s], ramp current velocity
+    max_current = 4000 # mA
+    num_steps = 21
+
+    # current_steps = np.linspace(0, max_current, num=num_steps)
+    current_steps = [(i / (num_steps - 1)) * max_current for i in range(num_steps)]
+
+    futek = Futek100Nm()
+    futek.calibrate_loadcell()
+
+    on_target = 0
+    curr_command = 0.0
+    stopping = False
+
+    driving_motor = DephyActuator(port="/dev/ttyACM0", gear_ratio=9.0, frequency=FREQUENCY)
+    clock = SoftRealtimeLoop(dt=1/FREQUENCY, report=True)
+
+
+    timer = SimpleTimer()
+    ramp = LimitVelocity(current_velocity, FREQUENCY)
+    csv_header = ['time','boolean_on_target', 'target_current', 'current', 'position', 'torque']
+    f_log = open('torque_reading.csv', 'w')
+
+    # Within loop
+    with driving_motor:
+        driving_motor.set_control_mode(CONTROL_MODES.CURRENT)
+        driving_motor.set_current_gains()
+        driving_motor.set_motor_current(0)
+
+        csv_writer = csv.writer(f_log)
+        csv_writer.writerow(csv_header)
+
+        for t in clock:
+
+            driving_motor.update()
+
+            # print("1")
+
+            if timer.is_done:
+                if timer.just_done:
+                    if current_steps is not None:
+                        curr_command = current_steps.pop(0)
+                        print("Setting q-current to %2f mA."% curr_command)
+                    else:
+                        curr_command = 0.0
+                        stopping = True
+                elif on_target:
+                    timer.start(t_cond)
+            # print("2")
+
+            curr_command_lim = ramp.update(curr_command)
+
+            if stopping and ramp.is_stopped:
+                break
+
+            on_target = ramp.on_target
+
+            # print(curr_command_lim)
+            driving_motor.set_motor_current(curr_command_lim)
+
+            current = driving_motor.motor_current
+            output_position = driving_motor.output_position
+            futek.get_torque()
+
+            print("Desired Current: ", curr_command_lim, "Current: ", current, "Futek Torque: ", futek.torque)
+
+            csv_writer.writerow([t, on_target, curr_command_lim, current, output_position, futek.torque])
+
+if __name__ == "__main__":
+    current_step()

opensourceleg/benchmarks/efficiency.py

@@ -0,0 +1,34 @@
+from opensourceleg.actuators.dephy import DephyActuator
+from opensourceleg.logging import LOGGER
+from opensourceleg.time import SoftRealtimeLoop
+from opensourceleg.actuators.base import CONTROL_MODES
+from flexsea.device import Device
+
+
+
+import numpy as np
+import time
+
+FREQUENCY = 500
+
+if __name__ == "__main__":
+    driving_motor = DephyActuator(port="/dev/ttyACM0", gear_ratio=9.0, frequency=FREQUENCY)
+    clock = SoftRealtimeLoop(dt=1/FREQUENCY, report=True)
+
+    with driving_motor:
+        driving_motor.set_control_mode(CONTROL_MODES.VOLTAGE)
+        driving_motor.set_motor_voltage(3000)
+
+
+        for t in clock:
+            driving_motor.update()
+            driving_motor.set_motor_voltage(2000)
+            LOGGER.info(f"{driving_motor.output_position}")
+
+
+
+
+
+
+
+

opensourceleg/benchmarks/torque_reading.py

@@ -0,0 +1,69 @@
+from opensourceleg.actuators.dephy import DephyActuator
+from opensourceleg.logging import LOGGER
+from opensourceleg.time import SoftRealtimeLoop
+from opensourceleg.actuators.base import CONTROL_MODES
+from flexsea.device import Device
+
+# from skip_testbed import futek
+from opensourceleg.sensors.torque_sensor import Futek100Nm
+
+import numpy as np
+import time
+import csv
+
+FREQUENCY = 200
+
+
+
+
+
+
+
+
+if __name__ == "__main__":
+    driving_motor = DephyActuator(port="/dev/ttyACM0", gear_ratio=9.0, frequency=FREQUENCY)
+    clock = SoftRealtimeLoop(dt=1/FREQUENCY, report=True)
+
+    # futek_reader = futek.Big100NmFutek()
+    futek_reader = Futek100Nm()
+
+    csv_header = ['time', 'current', 'position', 'torque']
+
+    f_log = open('torque_reading.csv', 'w')
+
+
+    with driving_motor:
+        driving_motor.set_control_mode(CONTROL_MODES.CURRENT)
+        driving_motor.set_current_gains()
+        driving_motor.set_motor_current(0)
+
+        csv_writer = csv.writer(f_log)
+        csv_writer.writerow(csv_header)
+
+        for t in clock:
+            driving_motor.update()
+            # print(t)
+            if t < 2:
+                # print("t < 200")
+                driving_motor.set_motor_current(0)
+            elif t < 4:
+                # print("t < 400")
+                driving_motor.set_motor_current(1000)
+            else:
+                # print("t > 400")
+                driving_motor.set_motor_current(0)
+            current = driving_motor.motor_current
+            output_position = driving_motor.output_position
+            futek_reader.get_torque()
+            print("Current: ", current, "Futek Torque: ", futek_reader.torque)
+
+
+            csv_writer.writerow([t, current, output_position, futek_reader.torque])
+
+
+
+
+
+
+
+

opensourceleg/sensors/torque_sensor.py

@@ -0,0 +1,101 @@
+import busio
+import board
+import adafruit_ads1x15.ads1115 as ADS
+from adafruit_ads1x15.analog_in import AnalogIn
+import csv
+import traceback
+import sys
+import numpy as np
+from time import time, sleep
+
+from opensourceleg.sensors.base import SensorBase
+
+class AdcManager(object):
+    def __init__(self, csv_file_name=None):
+        self.i2c = busio.I2C(board.SCL, board.SDA)
+        self.ads = ADS.ADS1115(self.i2c, data_rate=860)
+        self.chan = AnalogIn(self.ads, ADS.P0)
+        self.save_csv = not (csv_file_name is None)
+        self.csv_file_name = csv_file_name
+        self.csv_file = None
+        self.csv_writer = None
+        self.volts = -42.0 # error code
+
+    def __enter__(self):
+        self.start()
+        if self.save_csv:
+            with open(self.csv_file_name,'w') as fd:
+                writer = csv.writer(fd)
+                writer.writerow(["pi_time", "voltage", "test_duration"])
+            self.csv_file = open(self.csv_file_name,'a').__enter__()
+            self.csv_writer = csv.writer(self.csv_file)
+        return self
+
+    def __exit__(self, etype, value, tb):
+        """ Closes the file properly """
+        self.stop()
+        if self.save_csv:
+            self.csv_file.__exit__(etype, value, tb)
+        if not (etype is None):
+            traceback.print_exception(etype, value, tb)
+
+
+    def update_adc(self):
+        t0=time()
+        self.volts = self.chan.voltage
+        dur = time()-t0
+        if self.save_csv:
+            self.csv_writer.writerow([time(),self.volts, dur])
+        return self.volts
+
+class Futek100Nm(AdcManager, SensorBase):
+    def __init__(self, **kwargs):
+        super().__init__(**kwargs)
+        self.torque = -42.0
+        self.bias = 0
+
+    def start(self):
+        self._is_streaming = True
+
+    def stop(self):
+        self._is_streaming = False
+
+    def update(self):
+        self.get_torque()
+
+    def get_torque(self):
+        self.update_adc()
+        torque_rating = 100 # 100 Nm = 5V
+        self.torque = (self.volts-2.5-self.bias)/2.5*torque_rating
+        return self.torque
+
+    def calibrate_loadcell(self,Calibrate=True,SaveCal=False):
+        if Calibrate:
+            self.update()
+            voltage_cal = []
+            print("calibrating loadcell")
+            start_time = time()
+            while time() - start_time < 5:
+                self.update()
+                voltage_cal.append(self.volts)
+
+            avg_volt = np.mean(np.array(voltage_cal))
+            bias = avg_volt - 2.5
+            print("Bias: {} V".format(bias))
+            self.update()
+            self.bias = bias
+            if SaveCal:
+                np.save(file=f"./futek_offset.npy", arr=bias)
+        else:
+            bias = np.load('futek_offset.npy')
+            print("Setting Bias: {} V".format(bias))
+            self.bias = bias
+
+        return bias
+    def set_bias(self,bias):
+        self.bias = bias
+        print("Bias set to: {} V".format(bias))
+
+    @property
+    def is_streaming(self) -> bool:
+        return self._is_streaming