diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index c76161ea..f10b6211 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -18,7 +18,7 @@ struct ControllerConfig_t { Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t float pos_gain = 20.0f; // [(counts/s) / counts] float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] - // float vel_gain = 15.0f / 200.0f, // [A/(rad/s)] + // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] float vel_limit = 20000.0f; // [counts/s] }; diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 98f1cd02..78211f41 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -124,7 +124,6 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { axes[motor_number]->controller_.set_current_setpoint(current_setpoint); - respond(response_channel, use_checksum, "ok", motor_number); } } else if (cmd[0] == 'i'){ // Dump device info diff --git a/Firmware/communication/interface_uart.cpp b/Firmware/communication/interface_uart.cpp index 58b4f57d..7d17e528 100644 --- a/Firmware/communication/interface_uart.cpp +++ b/Firmware/communication/interface_uart.cpp @@ -95,7 +95,7 @@ void serve_on_uart() { dma_last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; // Start UART communication thread - osThreadDef(uart_server_thread_def, uart_server_thread, osPriorityNormal, 0, 512); + osThreadDef(uart_server_thread_def, uart_server_thread, osPriorityNormal, 0, 1024 /* the ascii protocol needs considerable stack space */); uart_thread = osThreadCreate(osThread(uart_server_thread_def), NULL); } diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 6fe5df1b..1a1e22ca 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -51,12 +51,22 @@ class AxisTestContext(): self.odrv_ctx = odrv_ctx def test_assert_eq(observed, expected, range=None, accuracy=None): - if range is None and accuracy is None and observed != expected: - raise TestFailed("value mismatch: expected {} but observed {}".format(expected, observed)) - if not range is None and ((observed < expected - range) or (observed > expected + range)): - raise TestFailed("value out of range: expected {}+-{} but observed {}".format(expected, range, observed)) - elif not accuracy is None and ((observed < expected * (1 - accuracy)) or (observed > expected * (1 + accuracy))): - raise TestFailed("value out of range: expected {}+-{}% but observed {}".format(expected, accuracy*100.0, observed)) + sign = lambda x: 1 if x >= 0 else -1 + + # Comparision with absolute range + if not range is None: + if (observed < expected - range) or (observed > expected + range): + raise TestFailed("value out of range: expected {}+-{} but observed {}".format(expected, range, observed)) + + # Comparision with relative range + elif not accuracy is None: + if sign(observed) != sign(expected) or (abs(observed) < abs(expected) * (1 - accuracy)) or (abs(observed) > abs(expected) * (1 + accuracy)): + raise TestFailed("value out of range: expected {}+-{}% but observed {}".format(expected, accuracy*100.0, observed)) + + # Exact comparision + else: + if observed != expected: + raise TestFailed("value mismatch: expected {} but observed {}".format(expected, observed)) def get_errors(axis_ctx: AxisTestContext): errors = [] @@ -154,6 +164,9 @@ def get_max_rpm(axis_ctx: AxisTestContext): rated_rpm = min(base_speed_rpm, axis_ctx.yaml['encoder-max-rpm']) return rated_rpm +def get_sensorless_vel(axis_ctx: AxisTestContext, vel): + return vel * 2 * math.pi / axis_ctx.yaml['encoder-cpr'] * axis_ctx.yaml['motor-pole-pairs'] + class ODriveTest(ABC): """ Tests inheriting from this class get full ownership of the ODrive @@ -669,3 +682,86 @@ class TestVelCtrlVsPosCtrl(DualAxisTest): #init_pos = axis1_ctx.handle.encoder.pos_estimate #axis1_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) #request_state(axis1_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + +# ASCII protocol helper functions +def gcode_calc_checksum(data): + from functools import reduce + return reduce(lambda a, b: a ^ b, data) +def gcode_append_checksum(data): + return data + b'*' + str(gcode_calc_checksum(data)).encode('ascii') +def get_lines(port): + buf = port.get_bytes(512, time.monotonic() + 0.2) + return [line.rstrip(b'\r') for line in buf.split(b'\n') if line.rstrip(b'\r')] + +class TestAsciiProtocol(ODriveTest): + def run_test(self, odrv_ctx: ODriveTestContext, logger): + import odrive.serial_transport + port = odrive.serial_transport.SerialStreamTransport(odrv_ctx.yaml['uart'], 115200) + + # send garbage to throw the device off track + port.process_bytes(b"garbage\r\n\r\0trash\n") + port.process_bytes(b"\n") # start a new clean line + get_lines(port) # flush RX buffer + + # info command without checksum + port.process_bytes(b"i\n") + # check if it reports the serial number (among other things) + lines = get_lines(port) + expected_line = ('Serial number: ' + odrv_ctx.yaml['serial-number']).encode('ascii') + if not expected_line in lines: + raise Exception("expected {} in ASCII protocol response but got {}".format(expected_line, str(lines))) + + # info command with checksum + port.process_bytes(gcode_append_checksum(b"i") + b" ; a useless comment\n") + # check if it reports the serial number with checksum (among other things) + lines = get_lines(port) + expected_line = gcode_append_checksum(('Serial number: ' + odrv_ctx.yaml['serial-number']).encode('ascii')) + if not expected_line in lines: + raise Exception("expected {} in ASCII protocol response but got {}".format(expected_line, str(lines))) + + port.process_bytes(b"p 0 2000 -10 0.002\n") + time.sleep(0.01) # 1ms is too short, 2ms usually works, 10ms for good measure + test_assert_eq(odrv_ctx.handle.axis0.controller.pos_setpoint, 2000, accuracy=0.001) + test_assert_eq(odrv_ctx.handle.axis0.controller.vel_setpoint, -10, accuracy=0.001) + test_assert_eq(odrv_ctx.handle.axis0.controller.current_setpoint, 0.002, accuracy=0.001) + + port.process_bytes(b"v 1 -21.1 0.32\n") + time.sleep(0.01) + test_assert_eq(odrv_ctx.handle.axis1.controller.vel_setpoint, -21.1, accuracy=0.001) + test_assert_eq(odrv_ctx.handle.axis1.controller.current_setpoint, 0.32, accuracy=0.001) + + port.process_bytes(b"c 0 0.1\n") + time.sleep(0.01) + test_assert_eq(odrv_ctx.handle.axis0.controller.current_setpoint, 0.1, accuracy=0.001) + + # write arbitrary parameter + port.process_bytes(b"w axis0.controller.pos_setpoint -123.456 ; comment\n") + time.sleep(0.01) + test_assert_eq(odrv_ctx.handle.axis0.controller.pos_setpoint, -123.456, accuracy=0.001) + + port.process_bytes(b"r axis0.controller.pos_setpoint\n") + lines = get_lines(port) + expected_line = b'-123.4560' + if lines != [expected_line]: + raise Exception("expected {} in ASCII protocol response but got {}".format(expected_line, str(lines))) + + # disable axes + odrv_ctx.handle.axis0.controller.set_pos_setpoint(0, 0, 0) + odrv_ctx.handle.axis1.controller.set_pos_setpoint(0, 0, 0) + request_state(odrv_ctx.axes[0], AXIS_STATE_IDLE) + request_state(odrv_ctx.axes[1], AXIS_STATE_IDLE) + + +class TestSensorlessControl(AxisTest): + def run_test(self, axis_ctx: AxisTestContext, logger): + odrv0.axis0.controller.config.vel_gain = 5 / get_sensorless_vel(axis_ctx, 10000) + odrv0.axis0.controller.config.vel_integrator_gain = 10 / get_sensorless_vel(axis_ctx, 10000) + target_vel = get_sensorless_vel(axis_ctx, 20000) + axis_ctx.handle.controller.set_vel_setpoint(target_vel, 0) + request_state(axis_ctx, AXIS_STATE_SENSORLESS_CONTROL) + # wait for spinup + time.sleep(2) + test_assert_eq(odrv0.axis0.encoder.pll_vel, target_vel, range=2000) + + request_state(axis_ctx, AXIS_STATE_IDLE) diff --git a/tools/run_tests.py b/tools/run_tests.py index 1cb1bcd8..2293c573 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -87,6 +87,12 @@ else: all_tests.append(TestDiscoverAndGotoIdle()) all_tests.append(TestEncoderOffsetCalibration(pass_if_ready=True)) +all_tests.append(TestAsciiProtocol()) +all_tests.append(TestSensorlessControl()) + +#all_tests.append(TestStepDirInput()) +#all_tests.append(TestPWMInput()) + if test_rig_yaml['type'] == 'parallel': #all_tests.append(TestHighVelocity()) all_tests.append(TestHighVelocityInViscousFluid(load_current=35, driver_current=45))