diff --git a/Makefile b/Makefile index 7cd56961..740f1799 100644 --- a/Makefile +++ b/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 diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c index 8339900d..ac35040a 100644 --- a/MotorControl/low_level.c +++ b/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 diff --git a/Src/usbd_cdc_if.c b/Src/usbd_cdc_if.c index a631b97b..65044d9e 100644 --- a/Src/usbd_cdc_if.c +++ b/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, ¤t_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, ¤t_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, ¤t_feed_forward); + if (motor_number < num_motors) { + set_current_setpoint(&motors[motor_number], current_feed_forward); + } + } return (USBD_OK); /* USER CODE END 6 */ } diff --git a/tools/.gitignore b/tools/.gitignore new file mode 100644 index 00000000..9886e1ed --- /dev/null +++ b/tools/.gitignore @@ -0,0 +1 @@ +*.py[cod] diff --git a/tools/odrive/__init__.py b/tools/odrive/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/tools/odrive/usbbulk.py b/tools/odrive/usbbulk.py new file mode 100644 index 00000000..7a48cd2e --- /dev/null +++ b/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 diff --git a/tools/odrive/util.py b/tools/odrive/util.py new file mode 100644 index 00000000..e776f6c3 --- /dev/null +++ b/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) diff --git a/tools/test_bulk.py b/tools/test_bulk.py new file mode 100644 index 00000000..9901d272 --- /dev/null +++ b/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)