added motor control, python control script

Makefile

@@ -15,7 +15,7 @@ DEBUG = 1
 # optimization
 # OPT = -O3 -ffast-math -flto
 # OPT = -O3 -ffast-math
-OPT = -O0 -ffast-math
+OPT = -O0 -ffast-math -u _printf_float -u _scanf_float
 
 #######################################
 # pathes

MotorControl/low_level.c

@@ -162,17 +162,20 @@ void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward
     motor->vel_setpoint = vel_feed_forward;
     motor->current_setpoint = current_feed_forward;
     motor->control_mode = POSITION_CONTROL;
+    printf("POSITION_CONTROL %3.3f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint);
 }
 
 void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_forward) {
     motor->vel_setpoint = vel_setpoint;
     motor->current_setpoint = current_feed_forward;
     motor->control_mode = VELOCITY_CONTROL;
+    printf("VELOCITY_CONTROL %3.3f %3.3f\n", motor->vel_setpoint, motor->current_setpoint);
 }
 
 void set_current_setpoint(Motor_t* motor, float current_setpoint) {
     motor->current_setpoint = current_setpoint;
     motor->control_mode = CURRENT_CONTROL;
+    printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint);
 }
 
 // Initalises the low level motor control and then starts the motor control threads

Src/usbd_cdc_if.c

@@ -44,6 +44,7 @@
 /* Includes ------------------------------------------------------------------*/
 #include "usbd_cdc_if.h"
 /* USER CODE BEGIN INCLUDE */
+#include "low_level.h"
 /* USER CODE END INCLUDE */
 
 /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
@@ -263,6 +264,32 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
   /* USER CODE BEGIN 6 */
   USBD_CDC_SetRxBuffer(&hUsbDeviceFS, &Buf[0]);
   USBD_CDC_ReceivePacket(&hUsbDeviceFS);
+  // check incoming packet type
+  if (Buf[0] == 'p') {
+	  // position control
+	  uint8_t motor_number;
+	  float pos_setpoint, vel_feed_forward, current_feed_forward;
+	  sscanf(Buf, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
+	  if (motor_number < num_motors) {
+		  set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward);
+	  }
+  } else if (Buf[0] == 'v') {
+	  // velocity control
+	  uint8_t motor_number;
+	  float vel_feed_forward, current_feed_forward;
+	  sscanf(Buf, "v %u %f %f", &motor_number, &vel_feed_forward, &current_feed_forward);
+	  if (motor_number < num_motors) {
+		  set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward);
+	  }
+  } else if (Buf[0] == 'c') {
+	  // velocity control
+	  uint8_t motor_number;
+	  float current_feed_forward;
+	  sscanf(Buf, "c %u %f ", &motor_number, &current_feed_forward);
+	  if (motor_number < num_motors) {
+	      set_current_setpoint(&motors[motor_number], current_feed_forward);
+	  }
+  }
   return (USBD_OK);
   /* USER CODE END 6 */ 
 }

tools/.gitignore

@@ -0,0 +1 @@
+*.py[cod]

tools/odrive/usbbulk.py

@@ -0,0 +1,120 @@
+# requires pyusb
+#   pip install --pre pyusb
+
+import usb.core
+import usb.util
+import sys
+import time
+
+from odrive.util import ODriveNotConnectedError
+from odrive.util import USBID_VID_ODRIVE, USBID_PID_ODRIVE
+
+BULK_DEVICE_NOT_FOUND     = 'ODrive BulkDevice Not Found'
+BULK_DEVICE_FOUND         = 'ODrive BulkDevice Found!'
+
+def noprint(x):
+  pass
+
+def poll_odrive_bulk_device(intv=1, printer=noprint):
+  # look for device
+  while 1:
+    time.sleep(intv)
+    try:
+      dev = ODriveBulkDevice()
+      break
+    except:
+      printer(BULK_DEVICE_NOT_FOUND)
+  # found device
+  printer(BULK_DEVICE_FOUND)
+  return dev
+
+class ODriveBulkDevice():
+  def __init__(self, idVendor=None, idProduct=None):
+    # ODrive idVendor
+    if idVendor is None:
+      idVendor = USBID_VID_ODRIVE
+    # ODrive idProduct
+    if idProduct is None:
+      idProduct = USBID_PID_ODRIVE
+    # find our device
+    self.dev = usb.core.find(idVendor=idVendor, idProduct=idProduct)
+    # was it found?
+    if self.dev is None:
+      raise ODriveNotConnectedError()
+
+  ##
+  # information about the connected device
+  ##
+  def info(self):
+    # loop through configurations
+    string = ""
+    for cfg in self.dev:
+      string += "ConfigurationValue {0}\n".format(cfg.bConfigurationValue)
+      for intf in cfg:
+        string += "\tInterfaceNumber {0},{0}\n".format(intf.bInterfaceNumber, intf.bAlternateSetting)
+        for ep in intf:
+          string += "\t\tEndpointAddress {0}\n".format(ep.bEndpointAddress)
+    return string
+
+  def init(self):
+    try:
+      string = ""
+      # detach kernel driver
+      if self.dev.is_kernel_driver_active(1):
+        self.dev.detach_kernel_driver(1)
+        string += "Detached Kernel Driver\n"
+      # set the active configuration. With no arguments, the first
+      # configuration will be the active one
+      self.dev.set_configuration()
+      # get an endpoint instance
+      self.cfg = self.dev.get_active_configuration()
+      self.intf = self.cfg[(1,0)]
+      # write endpoint
+      self.epw = usb.util.find_descriptor(self.intf,
+          # match the first OUT endpoint
+          custom_match = \
+          lambda e: \
+              usb.util.endpoint_direction(e.bEndpointAddress) == \
+              usb.util.ENDPOINT_OUT
+      )
+      assert self.epw is not None
+      string += "EndpointAddress for writing {}\n".format(self.epw.bEndpointAddress)
+      # read endpoint
+      self.epr = usb.util.find_descriptor(self.intf,
+          # match the first IN endpoint
+          custom_match = \
+          lambda e: \
+              usb.util.endpoint_direction(e.bEndpointAddress) == \
+              usb.util.ENDPOINT_IN
+      )
+      assert self.epr is not None
+      string += "EndpointAddress for reading {}\n".format(self.epr.bEndpointAddress)
+      return string
+    except usb.core.USBError:
+      #return -1
+      raise
+
+  def shutdown(self):
+    return 0
+
+  def send(self, usbBuffer):
+    try:
+      ret = self.epw.write(usbBuffer, 0)
+      return ret
+    except usb.core.USBError:
+      #return -1
+      raise
+
+  def recieve(self, bufferLen):
+    try:
+      ret = self.epr.read(bufferLen, 0)
+      return ret
+    except usb.core.USBError:
+      #return -1
+      raise
+
+  def send_max(self):
+    return 64
+
+  def recieve_max(self):
+    return 64

tools/odrive/util.py

@@ -0,0 +1,44 @@
+# requires pyusb
+#   pip install --pre pyusb
+
+import time
+import usb.core
+import usb.util
+
+# Exceptions
+class ODriveError(Exception):
+  pass
+
+class ODriveNotConnectedError(ODriveError):
+  pass
+
+# ODrive generic        1209:0D31
+USBID_VID_ODRIVE        = 0x1209
+USBID_PID_ODRIVE        = 0x0D31
+# ODrive rev 3.1        1209:0D31
+USBID_VID_ODRIVE_3_1    = USBID_VID_ODRIVE
+USBID_PID_ODRIVE_3_1    = USBID_PID_ODRIVE
+USB_DEV_ODRIVE_3_1      = (USBID_VID_ODRIVE_3_1, USBID_PID_ODRIVE_3_1)
+# all devices
+all_devices         = [USB_DEV_ODRIVE_3_1]
+
+# device messages
+DEVICE_NOT_FOUND    = 'ODrive Not Found'
+DEVICE_FOUND        = 'ODrive Found!'
+
+def noprint(x):
+  pass
+
+def poll_odrive_device(devices=all_devices, intv=1, printer=noprint):
+  while 1:
+    time.sleep(intv)
+    for device in devices:
+      try:
+        idVendor  = device[0]
+        idProduct = device[1]
+        dev = usb.core.find(idVendor=idVendor, idProduct=idProduct)
+        if dev is not None:
+          printer(DEVICE_FOUND)
+          return device
+      except:
+        printer(DEVICE_NOT_FOUND)

tools/test_bulk.py

@@ -0,0 +1,51 @@
+#! /usr/bin/env python3
+
+import argparse
+
+def parse_args():
+  parser = argparse.ArgumentParser(description='Talk to a ODrive board over USB bulk channel.')
+  return parser.parse_args()
+
+if __name__ == '__main__':
+  # parse args before other imports
+  args = parse_args()
+
+import sys
+import time
+import threading
+from odrive import usbbulk
+
+running = True
+
+def main(args):
+  global running
+  print("ODrive USB Bulk Communications")
+  print("---------------------------------------------------------------------")
+  print("USAGE:")
+  print("\tPOSITION_CONTROL:\n\t\tp MOTOR_NUMBER POSITION VELOCITY CURRENT")
+  print("\tVELOCITY_CONTROL:\n\t\tv MOTOR_NUMBER VELOCITY CURRENT")
+  print("\tCURRENT_CONTROL:\n\t\tc MOTOR_NUMBER CURRENT")
+  print("---------------------------------------------------------------------")
+  # query device
+  dev = usbbulk.poll_odrive_bulk_device(printer=print)
+  print (dev.info())
+  print (dev.init())
+  # thread
+  thread = threading.Thread(target=recieve_thread, args=[dev])
+  thread.start()
+  while running:
+    try:
+      command = input("Enter ODrive command:\n")
+      print(command)
+      dev.send(command)
+    except:
+      running = False
+
+def recieve_thread(dev):
+  while running:
+    message = dev.recieve(dev.recieve_max())
+    print(bytes(message).decode('ascii'), end='')
+    time.sleep(1)
+
+if __name__ == "__main__":
+   main(args)