From 464dad06d115cfb21add19fec597ed12a8e65093 Mon Sep 17 00:00:00 2001
From: Max <Hunt>
Date: Tue, 3 Nov 2020 11:00:54 +0000
Subject: [PATCH] Update

---
 .gitignore           |  7 +++-
 odrive_controller.py | 95 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 100 insertions(+), 2 deletions(-)
 create mode 100644 odrive_controller.py

diff --git a/.gitignore b/.gitignore
index 492aed4..c3715d7 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,7 +5,8 @@
 .LSOverride
 
 # Icon must end with two \r
-Icon

+Icon
+
 
 # Thumbnails
 ._*
@@ -26,6 +27,9 @@ Network Trash Folder
 Temporary Items
 .apdisk
 
+.vscode
+.vscode/*
+
 # ---> Linux
 *~
 
@@ -224,4 +228,3 @@ qtcreator-*
 
 # Catkin custom files
 CATKIN_IGNORE
-
diff --git a/odrive_controller.py b/odrive_controller.py
new file mode 100644
index 0000000..8eca461
--- /dev/null
+++ b/odrive_controller.py
@@ -0,0 +1,95 @@
+import odrive
+import signal
+import logging
+from odrive.enums import (AXIS_STATE_IDLE,
+                          AXIS_STATE_FULL_CALIBRATION_SEQUENCE,
+                          AXIS_STATE_CLOSED_LOOP_CONTROL)
+
+odrive_params = {
+    'current_lim': 10,  # A
+    'vel_lim': 60000,  # Rev/s
+    'accel_lim': 1000000/1.2,
+    'decel_lim': 1000000/1.2,
+    'encoder_cpr': 4000,  # Counts per revolution
+    'desired_velocity': 10000,  # Rev/s
+    'brake_resistance': 50  # Ohms
+}
+
+logging.basicConfig(level=logging.DEBUG)
+
+
+class Endstop:
+    def __init__(self, pin_ix: int):
+        self.endstop_pin = pin_ix
+        # Not sure if this is needed???
+
+
+class Motors:
+    def __init__(self, params: dict, odrive_sn: str = None):
+        self.params = params
+        if odrive_sn:
+            try:
+                self.odrive = odrive.find_any(serial_number=odrive_sn)
+            except Exception as err:
+                logging.error(f"Encountered error: {err}")
+        else:
+            self.odrive = odrive.find_any()
+        self.enable_estop()
+        self.set_parameters()
+        logging.info(f"Odrive ready, voltage: {self.drive.vbus_voltage}V")
+
+    def digital_estop(self):
+        axis0 = self.odrive.axis0
+        axis1 = self.odrive.axis1
+        axis0.requested_state = AXIS_STATE_IDLE
+        axis1.requested_state = AXIS_STATE_IDLE
+        logging.warn('Estop triggered, motors set to Idle')
+
+    def enable_estop(self):
+        signal.signal(signal.SIGINT, self.digital_estop)
+        logging.info('Ctrl+C Estop enabled')
+
+    def set_parameters(self):
+        axis0 = self.drive.axis0
+        axis1 = self.drive.axis1
+        for axis in [axis0, axis1]:
+            axis.motor.config.current_lim = self.params.get('current_lim', 1)
+            axis.encoder.config.cpr = self.params.get('encoder_cpr', 4000)
+            axis.controller.config.vel_limit = self.params.get('vel_lim', 1)
+            axis.trap_traj.config.vel_limit = self.params.get('vel_lim', 1)
+            axis.trap_traj.config.accel_limit = self.params.get('accel_lim', 1)
+            axis.trap_traj.config.decel_limit = self.params.get('decel_lim', 1)
+
+    def run_calibration(self):
+        self.drive.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
+        self.drive.axis1.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
+        while ((self.drive.axis0.current_state != AXIS_STATE_IDLE)
+               or (self.drive.axis1.current_state != AXIS_STATE_IDLE)):
+            pass
+        self.drive.save_configuration()
+
+    def set_idle(self):
+        self.drive.axis0.requested_state = AXIS_STATE_IDLE
+        self.drive.axis1.requested_state = AXIS_STATE_IDLE
+
+    def set_closed_loop(self):
+        self.drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+        self.drive.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+
+    def index_calibration(self):
+        pass
+        # TODO: here it will look for the endstop and set encoder counts to zero
+        # Freeze one motor
+        # Move other motor slowly until endstop activated
+        # https://docs.odriverobotics.com/api/odrive.endstop.config
+
+    def cartesian_to_angle(self, position: dict):
+        '''
+        input dict: poisition = {
+                'x': 0->100,
+                'y': 0->100
+            }
+        '''
+        target_x = position['x']
+        target_y = position['y']
+        pass