diff --git a/.travis.yml b/.travis.yml index 99852c97..0337261e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -33,7 +33,7 @@ install: - export TUP_DIR=$HOME/dl/tup_0.7.5-0~16.04.york0_amd64 - export TUP_ARCHIVE=$HOME/dl/tup_0.7.5-0~16.04.york0_amd64.deb -- export TUP_URL=http://ppa.launchpad.net/jonathonf/tup/ubuntu/pool/main/t/tup/tup_0.7.5-0~16.04.york0_amd64.deb +- export TUP_URL=http://ppa.launchpad.net/jonathonf/tup/ubuntu/pool/main/t/tup/tup_0.7.8-2~16.04.york0_amd64.deb - if [ ! -e $TUP_DIR/bin/tup ]; then wget $TUP_URL -O $TUP_ARCHIVE; dpkg-deb -R $TUP_ARCHIVE $TUP_DIR; fi - export PATH=$PATH:$TUP_DIR/usr/bin diff --git a/Arduino/ODriveArduino/ODriveArduino.cpp b/Arduino/ODriveArduino/ODriveArduino.cpp index 6e4a2748..00fce19a 100644 --- a/Arduino/ODriveArduino/ODriveArduino.cpp +++ b/Arduino/ODriveArduino/ODriveArduino.cpp @@ -38,6 +38,14 @@ void ODriveArduino::SetVelocity(int motor_number, float velocity, float current_ serial_ << "v " << motor_number << " " << velocity << " " << current_feedforward << "\n"; } +void ODriveArduino::SetCurrent(int motor_number, float current) { + serial_ << "c " << motor_number << " " << current << "\n"; +} + +void ODriveArduino::TrapezoidalMove(int motor_number, float position){ + serial_ << "t " << motor_number << " " << position << "\n"; +} + float ODriveArduino::readFloat() { return readString().toFloat(); } diff --git a/Arduino/ODriveArduino/ODriveArduino.h b/Arduino/ODriveArduino/ODriveArduino.h index 524f5a59..86b3aaf1 100644 --- a/Arduino/ODriveArduino/ODriveArduino.h +++ b/Arduino/ODriveArduino/ODriveArduino.h @@ -26,7 +26,8 @@ public: void SetPosition(int motor_number, float position, float velocity_feedforward, float current_feedforward); void SetVelocity(int motor_number, float velocity); void SetVelocity(int motor_number, float velocity, float current_feedforward); - + void SetCurrent(int motor_number, float current); + void TrapezoidalMove(int motor_number, float position); // General params float readFloat(); int32_t readInt(); diff --git a/Arduino/ODriveArduino/examples/ODriveArduinoTest/ODriveArduinoTest.ino b/Arduino/ODriveArduino/examples/ODriveArduinoTest/ODriveArduinoTest.ino index f948f7f6..1e835026 100644 --- a/Arduino/ODriveArduino/examples/ODriveArduinoTest/ODriveArduinoTest.ino +++ b/Arduino/ODriveArduino/examples/ODriveArduinoTest/ODriveArduinoTest.ino @@ -8,6 +8,7 @@ template<> inline Print& operator <<(Print &obj, float arg) { obj.print(a // Serial to the ODrive SoftwareSerial odrive_serial(8, 9); //RX (ODrive TX), TX (ODrive RX) +// Note: you must also connect GND on ODrive to GND on Arduino! // ODrive object ODriveArduino odrive(odrive_serial); diff --git a/CHANGELOG.md b/CHANGELOG.md index 5ab02f4e..b384b7f7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,14 +2,40 @@ Please add a note of your changes below this heading if you make a Pull Request. ### Added -* `dump_errors()` utility function in odrivetool to dump, decode and optionally clear errors. +* Communication watchdog feature. +* `encoder.set_linear_count(count)` function. +* Configurable encoder offset calibration distance and speed:`calib_scan_distance` and `calib_scan_omega` +* Encoder offset calibration debug variable `calib_scan_response` +* Script to enable using a hall signal as index edge. + +### Fixed +* Encoder index interrupts now disabled when not searching # Releases +## [0.4.8] - 2019-02-25 +### Added +* `dump_errors()` utility function in odrivetool to dump, decode and optionally clear errors. +* `f` command to ascii protocol to get encoder position and velocity feedback. +* `q` command to ascii protocol. It is like the old `p` command, but velocity and current mean limits, not feed-forward. +* `ss`, `se`, `sr` commands to ascii protocol, for save config, erase config and reboot. +* `move_incremental` function for relative trajectory moves. +* `encoder.config.ignore_illegal_hall_state` option. +* `encoder.config.enable_phase_interpolation` option. Setting to false may reduce jerky pulsations at low speed when using hall sensor feedback. +* Analog input. Used the same way as the PWM input mappings. +* Voltage limit soft clamping instead of ERROR_MODULATION_MAGNITUDE in gimbal motor closed loop. +* Thermal current limit with linear derating. + +### Changed +* Unified lockin drive modes. Current for index searching and encoder offset calibration now moved to axis.lockin.current. + +### Fixed +* Added required 1.5 cycle phase shift between ADC and PWM, lack thereof caused unstable current controller at high eRPM. + ## [0.4.7] - 2018-11-28 ### Added * Overspeed fault * Current sense saturation fault. -* Supress startup transients by sampling encoder estimate into position setpoint when entering closed loop control. +* Suppress startup transients by sampling encoder estimate into position setpoint when entering closed loop control. * Make step dir gpio pins configurable. * Configuration variable `encoder.config.zero_count_on_find_idx`, true by default. Set to false to leave the initial encoder count to be where the axis was at boot. * Circular position setpoint mode: position setpoints wrapped [0, cpr). Useful for infinite incremental position control. diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index 570c0f29..c4a2f37c 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -6,6 +6,7 @@ "${workspaceRoot}", "${workspaceRoot}/fibre/cpp/include/**", "${workspaceRoot}/MotorControl", + "${workspaceRoot}/communication", "${workspaceRoot}/Drivers/DRV8301", "${workspaceRoot}/Board/v3/Inc", "${workspaceRoot}/Board/v3/Drivers/CMSIS/Include", @@ -16,30 +17,21 @@ "${workspaceRoot}/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/include", - "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", - "${ARM_GCC_ROOT}/arm-none-eabi/include/**", - "${ARM_GCC_ROOT}/lib/gcc/arm-none-eabi/**" + "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F" ], "defines": [ "STM32F405xx", "USE_HAL_DRIVER", "HW_VERSION_MAJOR=3", - "HW_VERSION_MINOR=5", - "HW_VERSION_VOLTAGE=24", + "HW_VERSION_MINOR=6", + "HW_VERSION_VOLTAGE=56", "USB_PROTOCOL_NATIVE", "__weak=\"__attribute__((weak))\"", "__packed=\"__attribute__((__packed__))\"", "__GNUC__" ], "intelliSenseMode": "clang-x64", - "browse": { - "path": [ - "${workspaceRoot}", - "${ARM_GCC_ROOT}" - ], - "limitSymbolsToIncludedHeaders": true - }, - "compilerPath": "${ARM_GCC_ROOT}/bin/arm-none-eabi-gcc.exe -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float", + "compilerPath": "\"${ARM_GCC_ROOT}/bin/arm-none-eabi-gcc.exe\" -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float", "cStandard": "c11", "cppStandard": "c++14" }, @@ -49,6 +41,7 @@ "${workspaceRoot}", "${workspaceRoot}/fibre/cpp/include/**", "${workspaceRoot}/MotorControl", + "${workspaceRoot}/communication", "${workspaceRoot}/Drivers/DRV8301", "${workspaceRoot}/Board/v3/Inc", "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", @@ -92,6 +85,7 @@ "${workspaceRoot}", "${workspaceRoot}/fibre/cpp/include/**", "${workspaceRoot}/MotorControl", + "${workspaceRoot}/communication", "${workspaceRoot}/Drivers/DRV8301", "${workspaceRoot}/Board/v3/Inc", "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", diff --git a/Firmware/Board/v3/Inc/main.h b/Firmware/Board/v3/Inc/main.h index d41b19ee..bfd9888c 100644 --- a/Firmware/Board/v3/Inc/main.h +++ b/Firmware/Board/v3/Inc/main.h @@ -171,7 +171,7 @@ #define CURRENT_MEAS_PERIOD ( (float)2*TIM_1_8_PERIOD_CLOCKS*(TIM_1_8_RCR+1) / (float)TIM_1_8_CLOCK_HZ ) #define CURRENT_MEAS_HZ ( (float)(TIM_1_8_CLOCK_HZ) / (float)(2*TIM_1_8_PERIOD_CLOCKS*(TIM_1_8_RCR+1)) ) -#if HW_VERSION_VOLTAGE == 48 +#if HW_VERSION_VOLTAGE >= 48 #define VBUS_S_DIVIDER_RATIO 19.0f #define VBUS_OVERVOLTAGE_LEVEL 52.0f #elif HW_VERSION_VOLTAGE == 24 diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c index 11154587..ac6c6de9 100644 --- a/Firmware/Board/v3/Src/freertos.c +++ b/Firmware/Board/v3/Src/freertos.c @@ -60,6 +60,7 @@ #include "usb_device.h" extern PCD_HandleTypeDef hpcd_USB_OTG_FS; int odrive_main(void); +int load_configuration(void); /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -187,6 +188,9 @@ void MX_FREERTOS_Init(void) { sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1); init_deferred_interrupts(); + + // Load persistent configuration (or defaults) + load_configuration(); /* USER CODE END RTOS_SEMAPHORES */ /* USER CODE BEGIN RTOS_TIMERS */ diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index f0fe1d3e..7746ae87 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -91,22 +91,34 @@ void NMI_Handler(void) /* USER CODE END NonMaskableInt_IRQn 1 */ } +void get_regs(void** stack_ptr) { + void* volatile r0 __attribute__((unused)) = stack_ptr[0]; + void* volatile r1 __attribute__((unused)) = stack_ptr[1]; + void* volatile r2 __attribute__((unused)) = stack_ptr[2]; + void* volatile r3 __attribute__((unused)) = stack_ptr[3]; + + void* volatile r12 __attribute__((unused)) = stack_ptr[4]; + void* volatile lr __attribute__((unused)) = stack_ptr[5]; // Link register + void* volatile pc __attribute__((unused)) = stack_ptr[6]; // Program counter + void* volatile psr __attribute__((unused)) = stack_ptr[7]; // Program status register + + volatile bool stay_looping = true; + while(stay_looping); +} + /** * @brief This function handles Hard fault interrupt. */ +__attribute__((naked)) void HardFault_Handler(void) { - /* USER CODE BEGIN HardFault_IRQn 0 */ - - /* USER CODE END HardFault_IRQn 0 */ - while (1) - { - /* USER CODE BEGIN W1_HardFault_IRQn 0 */ - /* USER CODE END W1_HardFault_IRQn 0 */ - } - /* USER CODE BEGIN HardFault_IRQn 1 */ - - /* USER CODE END HardFault_IRQn 1 */ + __asm( + " tst lr, #4 \n\t" + " ite eq \n\t" + " mrseq r0, msp \n\t" + " mrsne r0, psp \n\t" + " b get_regs \n\t" + ); } /** diff --git a/Firmware/Makefile b/Firmware/Makefile index 82ae758c..91754f5a 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -40,6 +40,10 @@ erase: erase_config: $(OPENOCD) -c init -c reset\ halt -c flash\ erase_address\ 0x80C0000\ 0x40000 -c reset\ init -c reset\ run -c exit +# Sometimes the STM32 will get it's protection bits set for unknown reasons. Unlock it with this command +unlock: + $(OPENOCD) -c init -c reset\ halt -c stm32f2x\ unlock\ 0 + # The one-time programmable memory stores the board version # has the following format: # - OTP format version (0xFE: version 1) @@ -70,8 +74,8 @@ ifeq ($(OTP_CONFIRM),TRUE) -c 'mwb 0x1fff7801 0x01' -c 'sleep 10' \ -c 'mwb 0x1fff7802 0x01' -c 'sleep 10' \ -c 'mwb 0x1fff7803 3' -c 'sleep 10' \ - -c 'mwb 0x1fff7804 5' -c 'sleep 10' \ - -c 'mwb 0x1fff7805 48' -c 'sleep 10' \ + -c 'mwb 0x1fff7804 6' -c 'sleep 10' \ + -c 'mwb 0x1fff7805 56' -c 'sleep 10' \ -c 'reset run' \ -c exit @echo "OK" diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 13669f71..737c0ee3 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -31,6 +31,7 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config, motor_.axis_ = this; trap_.axis_ = this; decode_step_dir_pins(); + update_watchdog_settings(); min_endstop_.axis_ = this; max_endstop_.axis_ = this; } @@ -94,6 +95,21 @@ void Axis::decode_step_dir_pins() { dir_pin_ = get_gpio_pin_by_pin(config_.dir_gpio_pin); } +// @brief: Setup the watchdog reset value from the configuration watchdog timeout interval. +void Axis::update_watchdog_settings() { + + if(config_.watchdog_timeout <= 0.0f) { // watchdog disabled + watchdog_reset_value_ = 0; + } else if(config_.watchdog_timeout >= UINT32_MAX / (current_meas_hz+1)) { //overflow! + watchdog_reset_value_ = UINT32_MAX; + } else { + watchdog_reset_value_ = static_cast(config_.watchdog_timeout * current_meas_hz); + } + + // Do a feed to avoid instant timeout + watchdog_feed(); +} + // @brief (de)activates step/dir input void Axis::set_step_dir_active(bool active) { if (active) { @@ -149,42 +165,86 @@ bool Axis::do_updates() { return check_for_errors(); } -float Axis::get_temp() { - float adc = adc_measurements_[hw_config_.thermistor_adc_ch]; - float normalized_voltage = adc / adc_full_scale; - return horner_fma(normalized_voltage, thermistor_poly_coeffs, thermistor_num_coeffs); +// @brief Feed the watchdog to prevent watchdog timeouts. +void Axis::watchdog_feed() { + watchdog_current_value_ = watchdog_reset_value_; } -bool Axis::run_sensorless_spin_up() { - // Early Spin-up: spiral up current +// @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired. +bool Axis::watchdog_check() { + // reset value = 0 means watchdog disabled. + if(watchdog_reset_value_ == 0) return true; + + // explicit check here to ensure that we don't underflow back to UINT32_MAX + if(watchdog_current_value_ > 0) { + watchdog_current_value_--; + return true; + } else { + error_ |= ERROR_WATCHDOG_TIMER_EXPIRED; + return false; + } +} + +bool Axis::run_lockin_spin() { + // Spiral up current for softer rotor lock-in + lockin_state_ = LOCKIN_STATE_RAMP; float x = 0.0f; - run_control_loop([&](){ - float phase = wrap_pm_pi(config_.ramp_up_distance * x); - float I_mag = config_.spin_up_current * x; - x += current_meas_period / config_.ramp_up_time; - if (!motor_.update(I_mag, phase)) - return error_ |= ERROR_MOTOR_FAILED, false; + run_control_loop([&]() { + float phase = wrap_pm_pi(config_.lockin.ramp_distance * x); + float I_mag = config_.lockin.current * x; + x += current_meas_period / config_.lockin.ramp_time; + if (!motor_.update(I_mag, phase, 0.0f)) + return false; return x < 1.0f; }); - if (error_ != ERROR_NONE) - return false; - // Late Spin-up: accelerate - float vel = config_.ramp_up_distance / config_.ramp_up_time; - float phase = wrap_pm_pi(config_.ramp_up_distance); - run_control_loop([&](){ - vel += config_.spin_up_acceleration * current_meas_period; + // Spin states + float distance = config_.lockin.ramp_distance; + float phase = wrap_pm_pi(distance); + float vel = distance / config_.lockin.ramp_time; + + // Function of states to check if we are done + auto spin_done = [&](bool vel_override = false) -> bool { + bool done = false; + if (config_.lockin.finish_on_vel || vel_override) + done = done || fabsf(vel) >= fabsf(config_.lockin.vel); + if (config_.lockin.finish_on_distance) + done = done || fabsf(distance) >= fabsf(config_.lockin.finish_distance); + if (config_.lockin.finish_on_enc_idx) + done = done || encoder_.index_found_; + return done; + }; + + // Accelerate + lockin_state_ = LOCKIN_STATE_ACCELERATE; + run_control_loop([&]() { + vel += config_.lockin.accel * current_meas_period; + distance += vel * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); - float I_mag = config_.spin_up_current; - if (!motor_.update(I_mag, phase)) - return error_ |= ERROR_MOTOR_FAILED, false; - return vel < config_.spin_up_target_vel; + + if (!motor_.update(config_.lockin.current, phase, vel)) + return false; + return !spin_done(true); //vel_override to go to next phase }); - // call to controller.reset() that happend when arming means that vel_setpoint - // is zeroed. So we make the setpoint the spinup target for smooth transition. - controller_.vel_setpoint_ = config_.spin_up_target_vel; + if (!encoder_.index_found_) + encoder_.set_idx_subscribe(true); + // Constant speed + if (!spin_done()) { + lockin_state_ = LOCKIN_STATE_CONST_VEL; + vel = config_.lockin.vel; // reset to actual specified vel to avoid small integration error + run_control_loop([&]() { + distance += vel * current_meas_period; + phase = wrap_pm_pi(phase + vel * current_meas_period); + + if (!motor_.update(config_.lockin.current, phase, vel)) + return false; + return !spin_done(); + }); + } + + lockin_state_ = LOCKIN_STATE_INACTIVE; return check_for_errors(); } @@ -198,7 +258,7 @@ bool Axis::run_sensorless_control_loop() { float current_setpoint; if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, ¤t_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; - if (!motor_.update(current_setpoint, sensorless_estimator_.phase_)) + if (!motor_.update(current_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_)) return false; // set_error should update axis.error_ return true; }); @@ -213,9 +273,10 @@ bool Axis::run_closed_loop_control_loop() { // Note that all estimators are updated in the loop prefix in run_control_loop float current_setpoint; if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, ¤t_setpoint)) - return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error - if (!motor_.update(current_setpoint, encoder_.phase_)) - return false; // set_error should update axis.error_ + return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error + float phase_vel = 2*M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs; + if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel)) + return false; // set_error should update axis.error_ // Handle the homing case if (homing_state_ == HOMING_STATE_HOMING) { @@ -309,48 +370,73 @@ void Axis::run_state_machine_loop() { // Note that current_state is a reference to task_chain_[0] - // Validate the state before running it - if (current_state_ > AXIS_STATE_MOTOR_CALIBRATION && !motor_.is_calibrated_) - current_state_ = AXIS_STATE_UNDEFINED; - if (current_state_ > AXIS_STATE_ENCODER_OFFSET_CALIBRATION && !encoder_.is_ready_) - current_state_ = AXIS_STATE_UNDEFINED; - // Run the specified state // Handlers should exit if requested_state != AXIS_STATE_UNDEFINED bool status; switch (current_state_) { - case AXIS_STATE_MOTOR_CALIBRATION: + case AXIS_STATE_MOTOR_CALIBRATION: { status = motor_.run_calibration(); - break; + } break; + + case AXIS_STATE_ENCODER_INDEX_SEARCH: { + if (!motor_.is_calibrated_) + goto invalid_state_label; + if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction==0) + goto invalid_state_label; - case AXIS_STATE_ENCODER_INDEX_SEARCH: status = encoder_.run_index_search(); - break; + } break; + + case AXIS_STATE_ENCODER_DIR_FIND: { + if (!motor_.is_calibrated_) + goto invalid_state_label; + + status = encoder_.run_direction_find(); + } break; case AXIS_STATE_HOMING: status = controller_.home_axis(); break; - case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: + case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: { + if (!motor_.is_calibrated_) + goto invalid_state_label; status = encoder_.run_offset_calibration(); - break; + } break; - case AXIS_STATE_SENSORLESS_CONTROL: - status = run_sensorless_spin_up(); // TODO: restart if desired - if (status) + case AXIS_STATE_LOCKIN_SPIN: { + if (!motor_.is_calibrated_ || motor_.config_.direction==0) + goto invalid_state_label; + status = run_lockin_spin(); + } break; + + case AXIS_STATE_SENSORLESS_CONTROL: { + if (!motor_.is_calibrated_ || motor_.config_.direction==0) + goto invalid_state_label; + status = run_lockin_spin(); // TODO: restart if desired + if (status) { + // call to controller.reset() that happend when arming means that vel_setpoint + // is zeroed. So we make the setpoint the spinup target for smooth transition. + controller_.vel_setpoint_ = config_.lockin.vel; status = run_sensorless_control_loop(); - break; + } + } break; - case AXIS_STATE_CLOSED_LOOP_CONTROL: + case AXIS_STATE_CLOSED_LOOP_CONTROL: { + if (!motor_.is_calibrated_ || motor_.config_.direction==0) + goto invalid_state_label; + if (!encoder_.is_ready_) + goto invalid_state_label; status = run_closed_loop_control_loop(); - break; + } break; - case AXIS_STATE_IDLE: + case AXIS_STATE_IDLE: { run_idle_loop(); status = motor_.arm(); // done with idling - try to arm the motor - break; + } break; default: + invalid_state_label: error_ |= ERROR_INVALID_STATE; status = false; // this will set the state to idle break; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 880d005f..a7f9e5b3 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -27,12 +27,11 @@ public: ERROR_ENCODER_FAILED = 0x100, // Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value ERROR_CONTROLLER_FAILED = 0x200, ERROR_POS_CTRL_DURING_SENSORLESS = 0x400, + ERROR_WATCHDOG_TIMER_EXPIRED = 0x800, ERROR_MIN_ENDSTOP_PRESSED = 0x800, ERROR_MAX_ENDSTOP_PRESSED = 0x1000 }; - // Warning: Do not reorder these enum values. - // The state machine uses ">" comparision on them. enum State_t { AXIS_STATE_UNDEFINED = 0, //(ctx)->update_watchdog_settings(); }, this), make_protocol_property("step_gpio_pin", &config_.step_gpio_pin, [](void* ctx) { static_cast(ctx)->decode_step_dir_pins(); }, this), make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin, [](void* ctx) { static_cast(ctx)->decode_step_dir_pins(); }, this), - make_protocol_property("ramp_up_time", &config_.ramp_up_time), - make_protocol_property("ramp_up_distance", &config_.ramp_up_distance), - make_protocol_property("spin_up_current", &config_.spin_up_current), - make_protocol_property("spin_up_acceleration", &config_.spin_up_acceleration), - make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel) + make_protocol_object("lockin", + make_protocol_property("current", &config_.lockin.current), + make_protocol_property("ramp_time", &config_.lockin.ramp_time), + make_protocol_property("ramp_distance", &config_.lockin.ramp_distance), + make_protocol_property("accel", &config_.lockin.accel), + make_protocol_property("vel", &config_.lockin.vel), + make_protocol_property("finish_distance", &config_.lockin.finish_distance), + make_protocol_property("finish_on_vel", &config_.lockin.finish_on_vel), + make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance), + make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx) + ) ), - make_protocol_function("get_temp", *this, &Axis::get_temp), make_protocol_object("motor", motor_.make_protocol_definitions()), make_protocol_object("controller", controller_.make_protocol_definitions()), make_protocol_object("encoder", encoder_.make_protocol_definitions()), make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()), + make_protocol_object("trap_traj", trap_.make_protocol_definitions()), make_protocol_object("min_endstop", min_endstop_.make_protocol_definitions()), make_protocol_object("max_endstop", max_endstop_.make_protocol_definitions()), - make_protocol_object("trap_traj", trap_.make_protocol_definitions()) + make_protocol_function("watchdog_feed", *this, &Axis::watchdog_feed) ); } }; diff --git a/Firmware/MotorControl/board_config_v3.h b/Firmware/MotorControl/board_config_v3.h index e865102d..58a8b3e2 100644 --- a/Firmware/MotorControl/board_config_v3.h +++ b/Firmware/MotorControl/board_config_v3.h @@ -23,7 +23,6 @@ typedef struct { uint16_t step_gpio_pin; uint16_t dir_gpio_pin; - size_t thermistor_adc_ch; osPriority thread_priority; } AxisHardwareConfig_t; @@ -42,6 +41,7 @@ typedef struct { TIM_HandleTypeDef* timer; uint16_t control_deadline; float shunt_conductance; + size_t inverter_thermistor_adc_ch; } MotorHardwareConfig_t; typedef struct { SPI_HandleTypeDef* spi; @@ -74,7 +74,6 @@ const BoardHardwareConfig_t hw_configs[2] = { { .axis_config = { .step_gpio_pin = 1, .dir_gpio_pin = 2, - .thermistor_adc_ch = 15, .thread_priority = (osPriority)(osPriorityHigh + (osPriority)1), }, .encoder_config = { @@ -92,6 +91,7 @@ const BoardHardwareConfig_t hw_configs[2] = { { .timer = &htim1, .control_deadline = TIM_1_8_PERIOD_CLOCKS, .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + .inverter_thermistor_adc_ch = 15, }, .gate_driver_config = { .spi = &hspi3, @@ -112,11 +112,6 @@ const BoardHardwareConfig_t hw_configs[2] = { { #else .step_gpio_pin = 3, .dir_gpio_pin = 4, -#endif -#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 - .thermistor_adc_ch = 4, -#else - .thermistor_adc_ch = 1, #endif .thread_priority = osPriorityHigh, }, @@ -135,6 +130,11 @@ const BoardHardwareConfig_t hw_configs[2] = { { .timer = &htim8, .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2, .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 + .inverter_thermistor_adc_ch = 4, +#else + .inverter_thermistor_adc_ch = 1, +#endif }, .gate_driver_config = { .spi = &hspi3, diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 53e7a70c..ff27607e 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -56,6 +56,15 @@ void Controller::move_to_pos(float goal_point) { axis_->trap_.config_.decel_limit); traj_start_loop_count_ = axis_->loop_counter_; config_.control_mode = CTRL_MODE_TRAJECTORY_CONTROL; + goal_point_ = goal_point; +} + +void Controller::move_incremental(float displacement, bool from_goal_point = true){ + if(from_goal_point){ + move_to_pos(goal_point_ + displacement); + } else{ + move_to_pos(pos_setpoint_ + displacement); + } } void Controller::start_anticogging_calibration() { @@ -195,8 +204,8 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s Iq += vel_integrator_current_; // Current limiting - float Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current); bool limited = false; + float Ilim = axis_->motor_.effective_current_lim(); if (Iq > Ilim) { limited = true; Iq = Ilim; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index d8e71b7c..6bfeaa81 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -35,7 +35,7 @@ public: float homing_speed = 2000.0f; // [counts/s] }; - Controller(Config_t& config); + explicit Controller(Config_t& config); void reset(); void set_error(Error_t error); @@ -45,6 +45,7 @@ public: // Trajectory-Planned control void move_to_pos(float goal_point); + void move_incremental(float displacement, bool from_goal_point); bool home_axis(); @@ -92,6 +93,8 @@ public: uint32_t traj_start_loop_count_ = 0; + float goal_point_ = 0.0f; + // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( @@ -119,8 +122,9 @@ public: "vel_setpoint", "current_feed_forward"), make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, "current_setpoint"), + make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"), + make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"), make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration), - make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "goal_point"), make_protocol_function("home_axis", *this, &Controller::home_axis) ); } diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 7fb0a63b..95dc91f9 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -9,7 +9,7 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, { update_pll_gains(); - if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL)) { + if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL || config.mode == Encoder::MODE_SINCOS)) { is_ready_ = true; } } @@ -20,8 +20,7 @@ static void enc_index_cb_wrapper(void* ctx) { void Encoder::setup() { HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL); - GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_NOPULL, - GPIO_MODE_IT_RISING, enc_index_cb_wrapper, this); + set_idx_subscribe(); } void Encoder::set_error(Error_t error) { @@ -40,9 +39,8 @@ bool Encoder::do_checks(){ // Triggered when an encoder passes over the "Index" pin // TODO: only arm index edge interrupt when we know encoder has powered up // (maybe by attaching the interrupt on start search, synergistic with following) -// TODO: disable interrupt once we found the index void Encoder::enc_index_cb() { - if (config_.use_index && !index_found_) { + if (config_.use_index) { set_circular_count(0, false); if (config_.zero_count_on_find_idx) set_linear_count(0); // Avoid position control transient after search @@ -56,6 +54,35 @@ void Encoder::enc_index_cb() { } index_found_ = true; } + + // Disable interrupt + GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin); +} + +void Encoder::set_idx_subscribe(bool override_enable) { + if (config_.use_index && (override_enable || !config_.find_idx_on_lockin_only)) { + GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_PULLDOWN, + enc_index_cb_wrapper, this); + } else if (!config_.use_index || config_.find_idx_on_lockin_only) { + GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin); + } +} + +void Encoder::update_pll_gains() { + pll_kp_ = 2.0f * config_.bandwidth; // basic conversion to discrete time + pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped + + // Check that we don't get problems with discrete time approximation + if (!(current_meas_period * pll_kp_ < 1.0f)) { + set_error(ERROR_UNSTABLE_GAIN); + } +} + +void Encoder::check_pre_calibrated() { + if (!is_ready_) + config_.pre_calibrated = false; + if (config_.mode == MODE_INCREMENTAL && !index_found_) + config_.pre_calibrated = false; } // Function that sets the current encoder count to a desired 32-bit value. @@ -90,36 +117,42 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) { cpu_exit_critical(prim); } - -// @brief Slowly turns the motor in one direction until the -// encoder index is found. -// TODO: Do the scan with current, not voltage! bool Encoder::run_index_search() { - float voltage_magnitude; - if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_HIGH_CURRENT) - voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; - else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) - voltage_magnitude = axis_->motor_.config_.calibration_current; - else - return false; - - float omega = (float)(axis_->motor_.config_.direction) * config_.idx_search_speed; - + config_.use_index = true; index_found_ = false; - float phase = 0.0f; - axis_->run_control_loop([&](){ - phase = wrap_pm_pi(phase + omega * current_meas_period); + if (!config_.idx_search_unidirectional && axis_->motor_.config_.direction == 0) { + axis_->motor_.config_.direction = 1; + } - float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); - float v_beta = voltage_magnitude * our_arm_sin_f32(phase); - if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) - return false; // error set inside enqueue_voltage_timings - axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH); + bool orig_finish_on_enc_idx = axis_->config_.lockin.finish_on_enc_idx; + axis_->config_.lockin.finish_on_enc_idx = true; + bool status = axis_->run_lockin_spin(); + axis_->config_.lockin.finish_on_enc_idx = orig_finish_on_enc_idx; + return status; +} - // continue until the index is found - return !index_found_; - }); - return true; +bool Encoder::run_direction_find() { + int32_t init_enc_val = shadow_count_; + bool orig_finish_on_distance = axis_->config_.lockin.finish_on_distance; + axis_->config_.lockin.finish_on_distance = true; + axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic + bool status = axis_->run_lockin_spin(); + axis_->config_.lockin.finish_on_distance = orig_finish_on_distance; + + if (status) { + // Check response and direction + if (shadow_count_ > init_enc_val + 8) { + // motor same dir as encoder + axis_->motor_.config_.direction = 1; + } else if (shadow_count_ < init_enc_val - 8) { + // motor opposite dir as encoder + axis_->motor_.config_.direction = -1; + } else { + axis_->motor_.config_.direction = 0; + } + } + + return status; } // @brief Turns the motor in one direction for a bit and then in the other @@ -128,9 +161,7 @@ bool Encoder::run_index_search() { // TODO: Do the scan with current, not voltage! bool Encoder::run_offset_calibration() { static const float start_lock_duration = 1.0f; - static const float scan_omega = 4.0f * M_PI; - static const float scan_distance = 16.0f * M_PI; - static const int num_steps = (int)(scan_distance / scan_omega * (float)current_meas_hz); + static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz); // Require index found if enabled if (config_.use_index && !index_found_) { @@ -167,7 +198,7 @@ bool Encoder::run_offset_calibration() { // scan forward i = 0; axis_->run_control_loop([&](){ - float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f); + float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f); float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) @@ -197,9 +228,9 @@ bool Encoder::run_offset_calibration() { //TODO avoid recomputing elec_rad_per_enc every time // Check CPR float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); - float expected_encoder_delta = scan_distance / elec_rad_per_enc; - float actual_encoder_delta_abs = fabsf(shadow_count_-init_enc_val); - if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) + float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc; + calib_scan_response_ = fabsf(shadow_count_-init_enc_val); + if(fabsf(calib_scan_response_ - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) { set_error(ERROR_CPR_OUT_OF_RANGE); return false; @@ -208,7 +239,7 @@ bool Encoder::run_offset_calibration() { // scan backwards i = 0; axis_->run_control_loop([&](){ - float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f); + float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f); float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) @@ -242,13 +273,24 @@ static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) { } } -void Encoder::update_pll_gains() { - pll_kp_ = 2.0f * config_.bandwidth; // basic conversion to discrete time - pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped +void Encoder::sample_now() { + switch (config_.mode) { + case MODE_INCREMENTAL: { + tim_cnt_sample_ = (int16_t)hw_config_.timer->Instance->CNT; + } break; - // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * pll_kp_ < 1.0f)) { - set_error(ERROR_UNSTABLE_GAIN); + case MODE_HALL: { + // do nothing: samples already captured in general GPIO capture + } break; + + case MODE_SINCOS: { + sincos_sample_s_ = (get_adc_voltage(GPIO_3_GPIO_Port, GPIO_3_Pin) / 3.3f) - 0.5f; + sincos_sample_c_ = (get_adc_voltage(GPIO_4_GPIO_Port, GPIO_4_Pin) / 3.3f) - 0.5f; + } break; + + default: { + set_error(ERROR_UNSUPPORTED_ENCODER_MODE); + } break; } } @@ -259,7 +301,7 @@ bool Encoder::update() { case MODE_INCREMENTAL: { //TODO: use count_in_cpr_ instead as shadow_count_ can overflow //or use 64 bit - int16_t delta_enc_16 = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)shadow_count_; + int16_t delta_enc_16 = (int16_t)tim_cnt_sample_ - (int16_t)shadow_count_; delta_enc = (int32_t)delta_enc_16; //sign extend } break; @@ -271,10 +313,23 @@ bool Encoder::update() { if (delta_enc > 3) delta_enc -= 6; } else { - set_error(ERROR_ILLEGAL_HALL_STATE); - return false; + if (!config_.ignore_illegal_hall_state) { + set_error(ERROR_ILLEGAL_HALL_STATE); + return false; + } } } break; + + case MODE_SINCOS: { + float phase = fast_atan2(sincos_sample_s_, sincos_sample_c_); + int fake_count = (int)(1000.0f * phase); + //CPR = 6283 = 2pi * 1k + + delta_enc = fake_count - count_in_cpr_; + delta_enc = mod(delta_enc, 6283); + if (delta_enc > 6283/2) + delta_enc -= 6283; + } break; default: { set_error(ERROR_UNSUPPORTED_ENCODER_MODE); @@ -308,7 +363,7 @@ bool Encoder::update() { //// run encoder count interpolation int32_t corrected_enc = count_in_cpr_ - config_.offset; // if we are stopped, make sure we don't randomly drift - if (snap_to_zero_vel) { + if (snap_to_zero_vel || !config_.enable_phase_interpolation) { interpolation_ = 0.5f; // reset interpolation if encoder edge comes } else if (delta_enc > 0) { diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 78f8f4f6..c2d32841 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -19,7 +19,8 @@ public: enum Mode_t { MODE_INCREMENTAL, - MODE_HALL + MODE_HALL, + MODE_SINCOS }; struct Config_t { @@ -30,13 +31,18 @@ public: // be determined by run_offset_calibration. // In this case the encoder will enter ready // state as soon as the index is found. - float idx_search_speed = 10.0f; // [rad/s electrical] bool zero_count_on_find_idx = true; int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, int32_t offset = 0; // Offset between encoder count and rotor electrical phase float offset_float = 0.0f; // Sub-count phase alignment offset - float calib_range = 0.02f; + bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state + float calib_range = 0.02f; // Accuracy required to pass encoder cpr check + float calib_scan_distance = 16.0f * M_PI; // rad electrical + float calib_scan_omega = 4.0f * M_PI; // rad/s electrical float bandwidth = 1000.0f; + bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state + bool idx_search_unidirectional = false; // Only allow index search in known direction + bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111 }; Encoder(const EncoderHardwareConfig_t& hw_config, @@ -47,17 +53,21 @@ public: bool do_checks(); void enc_index_cb(); + void set_idx_subscribe(bool override_enable = false); + void update_pll_gains(); + void check_pre_calibrated(); void set_linear_count(int32_t count); void set_circular_count(int32_t count, bool update_offset); bool calib_enc_offset(float voltage_magnitude); - bool scan_for_enc_idx(float omega, float voltage_magnitude); bool run_index_search(); + bool run_direction_find(); bool run_offset_calibration(); + void sample_now(); bool update(); - void update_pll_gains(); + const EncoderHardwareConfig_t& hw_config_; Config_t& config_; @@ -75,39 +85,53 @@ public: float vel_estimate_ = 0.0f; // [count/s] float pll_kp_ = 0.0f; // [count/s / count] float pll_ki_ = 0.0f; // [(count/s^2) / count] + float calib_scan_response_ = 0.0f; // debug report from offset calib + int16_t tim_cnt_sample_ = 0; // // Updated by low_level pwm_adc_cb uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC + float sincos_sample_s_ = 0.0f; + float sincos_sample_c_ = 0.0f; // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_property("error", &error_), - make_protocol_ro_property("is_ready", &is_ready_), - make_protocol_ro_property("index_found", const_cast(&index_found_)), + make_protocol_property("is_ready", &is_ready_), + make_protocol_property("index_found", const_cast(&index_found_)), make_protocol_property("shadow_count", &shadow_count_), make_protocol_property("count_in_cpr", &count_in_cpr_), make_protocol_property("interpolation", &interpolation_), - make_protocol_property("phase", &phase_), + make_protocol_ro_property("phase", &phase_), make_protocol_property("pos_estimate", &pos_estimate_), make_protocol_property("pos_cpr", &pos_cpr_), - make_protocol_property("hall_state", &hall_state_), + make_protocol_ro_property("hall_state", &hall_state_), make_protocol_property("vel_estimate", &vel_estimate_), + make_protocol_ro_property("calib_scan_response", &calib_scan_response_), // make_protocol_property("pll_kp", &pll_kp_), // make_protocol_property("pll_ki", &pll_ki_), make_protocol_object("config", make_protocol_property("mode", &config_.mode), - make_protocol_property("use_index", &config_.use_index), - make_protocol_property("pre_calibrated", &config_.pre_calibrated), - make_protocol_property("idx_search_speed", &config_.idx_search_speed), + make_protocol_property("use_index", &config_.use_index, + [](void* ctx) { static_cast(ctx)->set_idx_subscribe(); }, this), + make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only, + [](void* ctx) { static_cast(ctx)->set_idx_subscribe(); }, this), + make_protocol_property("pre_calibrated", &config_.pre_calibrated, + [](void* ctx) { static_cast(ctx)->check_pre_calibrated(); }, this), make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx), make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset), make_protocol_property("offset_float", &config_.offset_float), + make_protocol_property("enable_phase_interpolation", &config_.enable_phase_interpolation), make_protocol_property("bandwidth", &config_.bandwidth, [](void* ctx) { static_cast(ctx)->update_pll_gains(); }, this), - make_protocol_property("calib_range", &config_.calib_range) - ) + make_protocol_property("calib_range", &config_.calib_range), + make_protocol_property("calib_scan_distance", &config_.calib_scan_distance), + make_protocol_property("calib_scan_omega", &config_.calib_scan_omega), + make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional), + make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state) + ), + make_protocol_function("set_linear_count", *this, &Encoder::set_linear_count, "count") ); } }; diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index b40b3274..6125c99c 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -125,7 +125,7 @@ bool safety_critical_disarm_motor_pwm(Motor& motor) { void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]) { uint32_t mask = cpu_enter_critical(); if (!brake_resistor_armed) { - motor.armed_state_ = Motor::ARMED_STATE_ARMED; + motor.armed_state_ = Motor::ARMED_STATE_DISARMED; } motor.hw_config_.timer->Instance->CCR1 = timings[0]; @@ -539,6 +539,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { axis.motor_.current_meas_.phC = current - axis.motor_.DC_calib_.phC; } // Prepare hall readings + // TODO move this to inside encoder update function decode_hall_samples(axis.encoder_, GPIO_port_samples[axis_num]); // Trigger axis thread axis.signal_current_meas(); @@ -553,18 +554,30 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } void tim_update_cb(TIM_HandleTypeDef* htim) { - int portsamples_arr; + + // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current + // If we are counting down, we just sampled in SVM vector 7, with zero current + bool counting_down = htim->Instance->CR1 & TIM_CR1_DIR; + if (counting_down) + return; + + int sample_ch; + Axis* axis; if (htim == &htim1) { - portsamples_arr = 0; + sample_ch = 0; + axis = axes[0]; } else if (htim == &htim8) { - portsamples_arr = 1; + sample_ch = 1; + axis = axes[1]; } else { low_level_fault(Motor::ERROR_UNEXPECTED_TIMER_CALLBACK); return; } + axis->encoder_.sample_now(); + for (int i = 0; i < num_GPIO; ++i) { - GPIO_port_samples[portsamples_arr][i] = GPIOs_to_samp[i]->IDR; + GPIO_port_samples[sample_ch][i] = GPIOs_to_samp[i]->IDR; } } @@ -713,4 +726,33 @@ void pwm_in_cb(int channel, uint32_t timestamp) { last_timestamp[gpio_num - 1] = timestamp; last_pin_state[gpio_num - 1] = current_pin_state; last_sample_valid[gpio_num - 1] = true; -} \ No newline at end of file +} + + +/* Analog speed control input */ + +static void update_analog_endpoint(const struct PWMMapping_t *map, int gpio) +{ + float fraction = get_adc_voltage(get_gpio_port_by_pin(gpio), get_gpio_pin_by_pin(gpio)) / 3.3f; + float value = map->min + (fraction * (map->max - map->min)); + get_endpoint(map->endpoint)->set_from_float(value); +} + +static void analog_polling_thread(void *) +{ + while (true) { + for (int i = 0; i < GPIO_COUNT; i++) { + struct PWMMapping_t *map = &board_config.analog_mappings[i]; + + if (is_endpoint_ref_valid(map->endpoint)) + update_analog_endpoint(map, i + 1); + } + osDelay(10); + } +} + +void start_analog_thread() +{ + osThreadDef(thread_def, analog_polling_thread, osPriorityLow, 0, 4*512); + osThreadCreate(osThread(thread_def), NULL); +} diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 3a3225de..503b98e1 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -51,6 +51,7 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, void start_general_purpose_adc(); float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); void pwm_in_init(); +void start_analog_thread(); void update_brake_current(); diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index e7299c1e..6ad902d9 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -51,7 +51,7 @@ void save_configuration(void) { } } -void load_configuration(void) { +extern "C" int load_configuration(void) { // Try to load configs if (NVM_init() || ConfigFormat::safe_load_config( @@ -81,6 +81,7 @@ void load_configuration(void) { } else { user_config_loaded_ = true; } + return user_config_loaded_; } void erase_configuration(void) { @@ -107,7 +108,7 @@ void enter_dfu_mode() { extern "C" { int odrive_main(void); -void vApplicationStackOverflowHook(void) { +void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) { for (;;); // TODO: safe action } void vApplicationIdleHook(void) { @@ -126,8 +127,6 @@ void vApplicationIdleHook(void) { } int odrive_main(void) { - // Load persistent configuration (or defaults) - load_configuration(); #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 if (board_config.enable_i2c_instead_of_can) { @@ -226,6 +225,8 @@ int odrive_main(void) { axes[i]->start_thread(); } + start_analog_thread(); + system_stats_.fully_booted = true; return 0; } diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 7cee19b0..6fa01cc8 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -143,14 +143,54 @@ void Motor::set_error(Motor::Error_t error){ update_brake_current(); } +float Motor::get_inverter_temp() { + float adc = adc_measurements_[hw_config_.inverter_thermistor_adc_ch]; + float normalized_voltage = adc / adc_full_scale; + return horner_fma(normalized_voltage, thermistor_poly_coeffs, thermistor_num_coeffs); +} + +bool Motor::update_thermal_limits() { + float fet_temp = get_inverter_temp(); + float temp_margin = config_.inverter_temp_limit_upper - fet_temp; + float derating_range = config_.inverter_temp_limit_upper - config_.inverter_temp_limit_lower; + thermal_current_lim_ = config_.current_lim * (temp_margin / derating_range); + if (!(thermal_current_lim_ >= 0.0f)) { //Funny polarity to also catch NaN + thermal_current_lim_ = 0.0f; + } + if (fet_temp > config_.inverter_temp_limit_upper + 5) { + set_error(ERROR_INVERTER_OVER_TEMP); + return false; + } + return true; +} + bool Motor::do_checks() { if (!check_DRV_fault()) { set_error(ERROR_DRV_FAULT); return false; } + if (!update_thermal_limits()) { + //error already set in function + return false; + } return true; } +float Motor::effective_current_lim() { + // Configured limit + float current_lim = config_.current_lim; + // Hardware limit + if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) { + current_lim = std::min(current_lim, 0.98f*one_by_sqrt3*vbus_voltage); + } else { + current_lim = std::min(current_lim, axis_->motor_.current_control_.max_allowed_current); + } + // Thermal limit + current_lim = std::min(current_lim, thermal_current_lim_); + + return current_lim; +} + void Motor::log_timing(TimingLog_t log_idx) { static const uint16_t clocks_per_cnt = (uint16_t)((float)TIM_1_8_CLOCK_HZ / (float)TIM_APB1_CLOCK_HZ); uint16_t timing = clocks_per_cnt * htim13.Instance->CNT; // TODO: Use a hw_config @@ -236,7 +276,7 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { config_.phase_inductance = L; // TODO arbitrary values set for now - if (L < 1e-6f || L > 2500e-6f) + if (L < 2e-6f || L > 4000e-6f) return set_error(ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE), false; return true; } @@ -282,17 +322,16 @@ bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { return true; } -// TODO: This doesn't update brake current // We should probably make FOC Current call FOC Voltage to avoid duplication. -bool Motor::FOC_voltage(float v_d, float v_q, float phase) { - float c = our_arm_cos_f32(phase); - float s = our_arm_sin_f32(phase); +bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) { + float c = our_arm_cos_f32(pwm_phase); + float s = our_arm_sin_f32(pwm_phase); float v_alpha = c*v_d - s*v_q; float v_beta = c*v_q + s*v_d; return enqueue_voltage_timings(v_alpha, v_beta); } -bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { +bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase) { // Syntactic sugar CurrentControl_t& ictrl = current_control_; @@ -310,11 +349,12 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC); // Park transform - float c = our_arm_cos_f32(phase); - float s = our_arm_sin_f32(phase); - float Id = c * Ialpha + s * Ibeta; - float Iq = c * Ibeta - s * Ialpha; - ictrl.Iq_measured = Iq; + float c_I = our_arm_cos_f32(I_phase); + float s_I = our_arm_sin_f32(I_phase); + float Id = c_I * Ialpha + s_I * Ibeta; + float Iq = c_I * Ibeta - s_I * Ialpha; + ictrl.Iq_measured += ictrl.I_measured_report_filter_k * (Iq - ictrl.Iq_measured); + ictrl.Id_measured += ictrl.I_measured_report_filter_k * (Id - ictrl.Id_measured); // Current error float Ierr_d = Id_des - Id; @@ -348,8 +388,10 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { ictrl.Ibus = mod_d * Id + mod_q * Iq; // Inverse park transform - float mod_alpha = c * mod_d - s * mod_q; - float mod_beta = c * mod_q + s * mod_d; + float c_p = our_arm_cos_f32(pwm_phase); + float s_p = our_arm_sin_f32(pwm_phase); + float mod_alpha = c_p * mod_d - s_p * mod_q; + float mod_beta = c_p * mod_q + s_p * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) ictrl.final_v_alpha = mod_to_V * mod_alpha; @@ -364,19 +406,22 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { } -bool Motor::update(float current_setpoint, float phase) { +bool Motor::update(float current_setpoint, float phase, float phase_vel) { current_setpoint *= config_.direction; phase *= config_.direction; + phase_vel *= config_.direction; + + float pwm_phase = phase + 1.5f * current_meas_period * phase_vel; // Execute current command // TODO: move this into the mot if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if(!FOC_current(0.0f, current_setpoint, phase)){ + if(!FOC_current(0.0f, current_setpoint, phase, pwm_phase)){ return false; } } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) { //In gimbal motor mode, current is reinterptreted as voltage. - if(!FOC_voltage(0.0f, current_setpoint, phase)) + if(!FOC_voltage(0.0f, current_setpoint, pwm_phase)) return false; } else { set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE); diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 2fe384e5..279e6848 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -21,7 +21,8 @@ public: ERROR_MODULATION_MAGNITUDE = 0x0080, ERROR_BRAKE_DEADTIME_VIOLATION = 0x0100, ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200, - ERROR_CURRENT_SENSE_SATURATION = 0x0400 + ERROR_CURRENT_SENSE_SATURATION = 0x0400, + ERROR_INVERTER_OVER_TEMP = 0x0800 }; enum MotorType_t { @@ -46,6 +47,8 @@ public: float final_v_beta; // [V] float Iq_setpoint; // [A] float Iq_measured; // [A] + float Id_measured; // [A] + float I_measured_report_filter_k; float max_allowed_current; // [A] float overcurrent_trip_level; // [A] }; @@ -60,7 +63,7 @@ public: float resistance_calib_max_voltage = 2.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. float phase_inductance = 0.0f; // to be set by measure_phase_inductance float phase_resistance = 0.0f; // to be set by measure_phase_resistance - int32_t direction = 1; // 1 or -1 + int32_t direction = 0; // 1 or -1 (0 = unspecified) MotorType_t motor_type = MOTOR_TYPE_HIGH_CURRENT; // Read out max_allowed_current to see max supported value for current_lim. // float current_lim = 70.0f; //[A] @@ -68,6 +71,8 @@ public: // Value used to compute shunt amplifier gains float requested_current_range = 60.0f; // [A] float current_control_bandwidth = 1000.0f; // [rad/s] + float inverter_temp_limit_lower = 100; + float inverter_temp_limit_upper = 120; }; enum TimingLog_t { @@ -106,6 +111,9 @@ public: bool check_DRV_fault(); void set_error(Error_t error); bool do_checks(); + float get_inverter_temp(); + bool update_thermal_limits(); + float effective_current_lim(); void log_timing(TimingLog_t log_idx); float phase_current_from_adcval(uint32_t ADCValue); bool measure_phase_resistance(float test_current, float max_voltage); @@ -113,9 +121,9 @@ public: bool run_calibration(); bool enqueue_modulation_timings(float mod_alpha, float mod_beta); bool enqueue_voltage_timings(float v_alpha, float v_beta); - bool FOC_voltage(float v_d, float v_q, float phase); - bool FOC_current(float Id_des, float Iq_des, float phase); - bool update(float current_setpoint, float phase); + bool FOC_voltage(float v_d, float v_q, float pwm_phase); + bool FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase); + bool update(float current_setpoint, float phase, float phase_vel); const MotorHardwareConfig_t& hw_config_; const GateDriverHardwareConfig_t gate_driver_config_; @@ -154,11 +162,14 @@ public: .final_v_beta = 0.0f, .Iq_setpoint = 0.0f, .Iq_measured = 0.0f, + .Id_measured = 0.0f, + .I_measured_report_filter_k = 1.0f, .max_allowed_current = 0.0f, .overcurrent_trip_level = 0.0f, }; DRV8301_FaultType_e drv_fault_ = DRV8301_FaultType_NoFault; DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup) + float thermal_current_lim_ = 10.0f; //[A] // Communication protocol definitions auto make_protocol_definitions() { @@ -171,6 +182,8 @@ public: make_protocol_property("DC_calib_phB", &DC_calib_.phB), make_protocol_property("DC_calib_phC", &DC_calib_.phC), make_protocol_property("phase_current_rev_gain", &phase_current_rev_gain_), + make_protocol_ro_property("thermal_current_lim", &thermal_current_lim_), + make_protocol_function("get_inverter_temp", *this, &Motor::get_inverter_temp), make_protocol_object("current_control", make_protocol_property("p_gain", ¤t_control_.p_gain), make_protocol_property("i_gain", ¤t_control_.i_gain), @@ -181,6 +194,8 @@ public: make_protocol_property("final_v_beta", ¤t_control_.final_v_beta), make_protocol_property("Iq_setpoint", ¤t_control_.Iq_setpoint), make_protocol_property("Iq_measured", ¤t_control_.Iq_measured), + make_protocol_property("Id_measured", ¤t_control_.Id_measured), + make_protocol_property("I_measured_report_filter_k", ¤t_control_.I_measured_report_filter_k), make_protocol_ro_property("max_allowed_current", ¤t_control_.max_allowed_current), make_protocol_ro_property("overcurrent_trip_level", ¤t_control_.overcurrent_trip_level) ), @@ -212,6 +227,8 @@ public: make_protocol_property("direction", &config_.direction), make_protocol_property("motor_type", &config_.motor_type), make_protocol_property("current_lim", &config_.current_lim), + make_protocol_property("inverter_temp_limit_lower", &config_.inverter_temp_limit_lower), + make_protocol_property("inverter_temp_limit_upper", &config_.inverter_temp_limit_upper), make_protocol_property("requested_current_range", &config_.requested_current_range), make_protocol_property("current_control_bandwidth", &config_.current_control_bandwidth, [](void* ctx) { static_cast(ctx)->update_current_controller_gains(); }, this) diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 74cce4ac..5804eb14 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -77,11 +77,12 @@ struct BoardConfig_t { float brake_resistance = 0.47f; // [ohm] #endif float dc_bus_undervoltage_trip_level = 8.0f; // * ) } }; - SensorlessEstimator(Config_t& config); + explicit SensorlessEstimator(Config_t& config); bool update(); diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index 42dac0ef..fe5f3fec 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -9,13 +9,14 @@ public: float decel_limit = 5000.0f; // [count/s^2] float A_per_css = 0.0f; // [A/(count/s^2)] }; + struct Step_t { float Y; float Yd; float Ydd; }; - TrapezoidalTrajectory(Config_t& config); + explicit TrapezoidalTrajectory(Config_t& config); bool planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax); Step_t eval(float t); diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 49c325b3..88392af1 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -31,6 +31,14 @@ elseif boardversion == "v3.5-48V" then boarddir = 'Board/v3' FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=5" FLAGS += "-DHW_VERSION_VOLTAGE=48" +elseif boardversion == "v3.6-24V" then + boarddir = 'Board/v3' + FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=6" + FLAGS += "-DHW_VERSION_VOLTAGE=24" +elseif boardversion == "v3.6-56V" then + boarddir = 'Board/v3' + FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=6" + FLAGS += "-DHW_VERSION_VOLTAGE=56" elseif boardversion == "" then error("board version not specified - take a look at tup.config.default") else @@ -91,10 +99,6 @@ FLAGS += '-mfpu=fpv4-sp-d16' FLAGS += '-mfloat-abi=hard' FLAGS += { '-Wall', '-Wdouble-promotion', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'} --- debug build -FLAGS += '-g -gdwarf-2' - - -- linker flags LDFLAGS += '-T'..boarddir..'/STM32F405RGTx_FLASH.ld' LDFLAGS += '-L'..boarddir..'/Drivers/CMSIS/Lib' -- lib dir @@ -102,10 +106,15 @@ LDFLAGS += '-lc -lm -lnosys -larm_cortexM4lf_math' -- libs LDFLAGS += '-mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float -Wl,--cref -Wl,--gc-sections' LDFLAGS += '-Wl,--undefined=uxTopUsedPriority' +-- debug build +if tup.getconfig("DEBUG") == "true" then + FLAGS += '-g -gdwarf-2' + OPT += '-Og' +else + OPT += '-O2' +end -- common flags for ASM, C and C++ -OPT += '-Og' --- OPT += '-O0' OPT += '-ffast-math -fno-finite-math-only' tup.append_table(FLAGS, OPT) tup.append_table(LDFLAGS, OPT) diff --git a/Firmware/build.lua b/Firmware/build.lua index 8962e403..d4c7aad4 100644 --- a/Firmware/build.lua +++ b/Firmware/build.lua @@ -56,14 +56,14 @@ function GCCToolchain(prefix, builddir, compiler_flags, linker_flags) compiler_flags += '-fstack-usage' end - gcc_generic_compiler = function(compiler, compiler_flags, gen_su_file, src, flags, includes, outputs) + local gcc_generic_compiler = function(compiler, compiler_flags, gen_su_file, src, flags, includes, outputs) -- convert include list to flags inc_flags = {} for _,inc in pairs(includes) do inc_flags += "-I"..inc end -- todo: vary build directory - obj_file = builddir.."/"..src:gsub("/","_")..".o" + obj_file = builddir.."/obj/"..src:gsub("/","_")..".o" outputs.object_files += obj_file if gen_su_file then su_file = builddir.."/"..src:gsub("/","_")..".su" @@ -162,7 +162,7 @@ function build(args) outputs.includes = {} for _,inc in pairs(args.includes) do - table.insert(outputs.includes, tup.nodevariable(inc)) + table.insert(outputs.includes, inc) end if args.name != nil then all_packages[args.name] = outputs diff --git a/Firmware/build.sh b/Firmware/build.sh index 1622fa69..8f3a0730 100755 --- a/Firmware/build.sh +++ b/Firmware/build.sh @@ -15,6 +15,7 @@ export CONFIG_STRICT=true rm -rdf build mkdir -p build env | grep ^CONFIG > tup.config +tup init tup generate ./tup_build.sh bash -xe ./tup_build.sh diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 0981cc1d..1e1c9ba4 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -99,7 +99,28 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& vel_feed_forward = 0.0f; if (numscan < 4) current_feed_forward = 0.0f; - axes[motor_number]->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward); + Axis* axis = axes[motor_number]; + axis->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward); + axis->watchdog_feed(); + } + + } else if (cmd[0] == 'q') { // position control with limits + unsigned motor_number; + float pos_setpoint, vel_limit, current_lim; + int numscan = sscanf(cmd, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, ¤t_lim); + if (numscan < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + Axis* axis = axes[motor_number]; + axis->controller_.pos_setpoint_ = pos_setpoint; + if (numscan >= 3) + axis->controller_.config_.vel_limit = vel_limit; + if (numscan >= 4) + axis->motor_.config_.current_lim = current_lim; + + axis->watchdog_feed(); } } else if (cmd[0] == 'v') { // velocity control @@ -113,7 +134,9 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else { if (numscan < 3) current_feed_forward = 0.0f; - axes[motor_number]->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward); + Axis* axis = axes[motor_number]; + axis->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward); + axis->watchdog_feed(); } } else if (cmd[0] == 'c') { // current control @@ -125,7 +148,9 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { - axes[motor_number]->controller_.set_current_setpoint(current_setpoint); + Axis* axis = axes[motor_number]; + axis->controller_.set_current_setpoint(current_setpoint); + axis->watchdog_feed(); } } else if (cmd[0] == 't') { // trapezoidal trajectory @@ -137,13 +162,29 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { - axes[motor_number]->controller_.move_to_pos(goal_point); + Axis* axis = axes[motor_number]; + axis->controller_.move_to_pos(goal_point); + axis->watchdog_feed(); + } + + } else if (cmd[0] == 'f') { // feedback + unsigned motor_number; + int numscan = sscanf(cmd, "f %u", &motor_number); + if (numscan < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + respond(response_channel, use_checksum, "%f %f", + (double)axes[motor_number]->encoder_.pos_estimate_, + (double)axes[motor_number]->encoder_.vel_estimate_); } } else if (cmd[0] == 'h') { // Help respond(response_channel, use_checksum, "Please see documentation for more details"); respond(response_channel, use_checksum, ""); respond(response_channel, use_checksum, "Available commands syntax reference:"); + respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim"); respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff"); respond(response_channel, use_checksum, "Velocity: v axis vel I-ff"); respond(response_channel, use_checksum, "Current: c axis I"); @@ -151,6 +192,10 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state"); respond(response_channel, use_checksum, "Read: r property"); respond(response_channel, use_checksum, "Write: w property value"); + respond(response_channel, use_checksum, ""); + respond(response_channel, use_checksum, "Save config: ss"); + respond(response_channel, use_checksum, "Erase config: se"); + respond(response_channel, use_checksum, "Reboot: sr"); } else if (cmd[0] == 'i'){ // Dump device info // respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature()); @@ -160,6 +205,15 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", FW_VERSION_MAJOR, FW_VERSION_MINOR, FW_VERSION_REVISION); respond(response_channel, use_checksum, "Serial number: %s", serial_number_str); + } else if (cmd[0] == 's'){ // System + if(cmd[1] == 's') { // Save config + save_configuration(); + } else if (cmd[1] == 'e'){ // Erase config + erase_configuration(); + } else if (cmd[1] == 'b'){ // Reboot + NVIC_SystemReset(); + } + } else if (cmd[0] == 'r') { // read property char name[MAX_LINE_LENGTH]; int numscan = sscanf(cmd, "r %" TO_STR(MAX_LINE_LENGTH) "s", name); @@ -196,6 +250,17 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } } + }else if (cmd[0] == 'u') { // Update axis watchdog. + unsigned motor_number; + int numscan = sscanf(cmd, "u %u", &motor_number); + if(numscan < 1){ + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + }else { + axes[motor_number]->watchdog_feed(); + } + } else if (cmd[0] != 0) { respond(response_channel, use_checksum, "unknown command"); } diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 625a3cf7..79982e64 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -84,7 +84,7 @@ void init_communication(void) { printf("hi!\r\n"); // Start command handling thread - osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 6000 /* in 32-bit words */); // TODO: fix stack issues + osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 8000 /* in 32-bit words */); // TODO: fix stack issues comm_thread = osThreadCreate(osThread(task_cmd_parse), NULL); while (!endpoint_list_valid) @@ -163,8 +163,11 @@ static inline auto make_obj_tree() { make_protocol_object("gpio2_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[1])), make_protocol_object("gpio3_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[2])), #endif - make_protocol_object("gpio4_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[3])) - ), + make_protocol_object("gpio4_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[3])), + + make_protocol_object("gpio3_analog_mapping", make_protocol_definitions(board_config.analog_mappings[2])), + make_protocol_object("gpio4_analog_mapping", make_protocol_definitions(board_config.analog_mappings[3])) + ), make_protocol_object("axis0", axes[0]->make_protocol_definitions()), make_protocol_object("axis1", axes[1]->make_protocol_definitions()), make_protocol_object("can", can1_ctx.make_protocol_definitions()), diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp index cd44c907..036a8203 100644 --- a/Firmware/communication/interface_usb.cpp +++ b/Firmware/communication/interface_usb.cpp @@ -177,6 +177,6 @@ void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair) { void start_usb_server() { // Start USB communication thread - osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, 512); + osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, 1024); usb_thread = osThreadCreate(osThread(usb_server_thread_def), NULL); } diff --git a/Firmware/fibre/cpp/include/fibre/protocol.hpp b/Firmware/fibre/cpp/include/fibre/protocol.hpp index 4b503e68..498d5172 100644 --- a/Firmware/fibre/cpp/include/fibre/protocol.hpp +++ b/Firmware/fibre/cpp/include/fibre/protocol.hpp @@ -82,61 +82,19 @@ typedef struct { uint16_t endpoint_id; } endpoint_ref_t; +#include template::value>> -inline size_t write_le(T value, uint8_t* buffer); +inline size_t write_le(T value, uint8_t* buffer){ + //TODO: add static_assert that this is still a little endian machine + std::memcpy(&buffer[0], &value, sizeof(value)); + return sizeof(value); +} template -inline size_t read_le(T* value, const uint8_t* buffer); - -template<> -inline size_t write_le(bool value, uint8_t* buffer) { - buffer[0] = value ? 1 : 0; - return 1; -} - -template<> -inline size_t write_le(uint8_t value, uint8_t* buffer) { - buffer[0] = value; - return 1; -} - -template<> -inline size_t write_le(uint16_t value, uint8_t* buffer) { - buffer[0] = (value >> 0) & 0xff; - buffer[1] = (value >> 8) & 0xff; - return 2; -} - -template<> -inline size_t write_le(uint32_t value, uint8_t* buffer) { - buffer[0] = (value >> 0) & 0xff; - buffer[1] = (value >> 8) & 0xff; - buffer[2] = (value >> 16) & 0xff; - buffer[3] = (value >> 24) & 0xff; - return 4; -} - -template<> -inline size_t write_le(int32_t value, uint8_t* buffer) { - buffer[0] = (value >> 0) & 0xff; - buffer[1] = (value >> 8) & 0xff; - buffer[2] = (value >> 16) & 0xff; - buffer[3] = (value >> 24) & 0xff; - return 4; -} - -template<> -inline size_t write_le(uint64_t value, uint8_t* buffer) { - buffer[0] = (value >> 0) & 0xff; - buffer[1] = (value >> 8) & 0xff; - buffer[2] = (value >> 16) & 0xff; - buffer[3] = (value >> 24) & 0xff; - buffer[4] = (value >> 32) & 0xff; - buffer[5] = (value >> 40) & 0xff; - buffer[6] = (value >> 48) & 0xff; - buffer[7] = (value >> 56) & 0xff; - return 8; +typename std::enable_if_t::value, size_t> +write_le(T value, uint8_t* buffer) { + return write_le>(value, buffer); } template<> @@ -148,65 +106,17 @@ inline size_t write_le(float value, uint8_t* buffer) { } template -typename std::enable_if_t::value, size_t> -write_le(T value, uint8_t* buffer) { - return write_le>(value, buffer); -} - -template<> -inline size_t read_le(bool* value, const uint8_t* buffer) { - *value = buffer[0]; - return 1; -} - -template<> -inline size_t read_le(uint8_t* value, const uint8_t* buffer) { - *value = buffer[0]; - return 1; -} - -template<> -inline size_t read_le(uint16_t* value, const uint8_t* buffer) { - *value = (static_cast(buffer[0]) << 0) | - (static_cast(buffer[1]) << 8); - return 2; -} - -template<> -inline size_t read_le(int32_t* value, const uint8_t* buffer) { - *value = (static_cast(buffer[0]) << 0) | - (static_cast(buffer[1]) << 8) | - (static_cast(buffer[2]) << 16) | - (static_cast(buffer[3]) << 24); - return 4; -} - -template<> -inline size_t read_le(uint32_t* value, const uint8_t* buffer) { - *value = (static_cast(buffer[0]) << 0) | - (static_cast(buffer[1]) << 8) | - (static_cast(buffer[2]) << 16) | - (static_cast(buffer[3]) << 24); - return 4; -} - -template<> -inline size_t read_le(uint64_t* value, const uint8_t* buffer) { - *value = (static_cast(buffer[0]) << 0) | - (static_cast(buffer[1]) << 8) | - (static_cast(buffer[2]) << 16) | - (static_cast(buffer[3]) << 24) | - (static_cast(buffer[4]) << 32) | - (static_cast(buffer[5]) << 40) | - (static_cast(buffer[6]) << 48) | - (static_cast(buffer[7]) << 56); - return 8; +inline size_t read_le(T* value, const uint8_t* buffer){ + // TODO: add static_assert that this is still a little endian machine + std::memcpy(value, buffer, sizeof(*value)); + return sizeof(*value); } template<> inline size_t read_le(float* value, const uint8_t* buffer) { static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); + return read_le(reinterpret_cast(value), buffer); } @@ -499,6 +409,14 @@ inline constexpr const char* get_default_json_modifier() { return "\"type\":\"float\",\"access\":\"rw\""; } template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"int64\",\"access\":\"r\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"int64\",\"access\":\"rw\""; +} +template<> inline constexpr const char* get_default_json_modifier() { return "\"type\":\"uint64\",\"access\":\"r\""; } diff --git a/Firmware/fibre/tupfiles/build.lua b/Firmware/fibre/tupfiles/build.lua index 4a0ea105..8b4a4510 100644 --- a/Firmware/fibre/tupfiles/build.lua +++ b/Firmware/fibre/tupfiles/build.lua @@ -5,7 +5,7 @@ function GCCToolchain(prefix, builddir, compiler_flags, linker_flags) -- add some default compiler flags compiler_flags += '-fstack-usage' - gcc_generic_compiler = function(compiler, compiler_flags, gen_su_file, src, flags, includes, outputs) + local gcc_generic_compiler = function(compiler, compiler_flags, gen_su_file, src, flags, includes, outputs) -- resolve source path src = tostring(src) diff --git a/Firmware/sampler.py b/Firmware/sampler.py index 08dc126e..75ebbe55 100644 --- a/Firmware/sampler.py +++ b/Firmware/sampler.py @@ -1,9 +1,11 @@ #!/usr/bin/python2 # run openocd (0.9.0) with : -# $ openocd -f stlink-v2-1.cfg -f stm32f4x.cfg &> /dev/null" +# $ openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg &> /dev/null & # then run # $ python2 sampler.py path_to_myelf_with_symbols +# ctrl-c to stop sampling. +# To terminate the openocd session, enter command "fg" then do ctrl-c. import sys import time @@ -111,10 +113,10 @@ if __name__ == '__main__': cur = time.time() if cur - start > 1.0: - tmp = sorted(countmap.items(), key=operator.itemgetter(1), reverse=True) + tmp = sorted(countmap.items(), key=operator.itemgetter(1)) #, reverse=True) for k, v in tmp: - # print('{:05.2f}% {}'.format((v * 100.) / total, k)) - print('{:06.2f} clocks : {}'.format((v * 8192) / total, k)) + print('{:05.2f}% {}'.format((v * 100.) / total, k)) + # print('{:06.2f} clocks : {}'.format((v * 10500) / total, k)) start = cur print('{} Samples'.format(total)) print('') diff --git a/Firmware/tup.config.default b/Firmware/tup.config.default index 60d2500d..5c2c4822 100644 --- a/Firmware/tup.config.default +++ b/Firmware/tup.config.default @@ -3,6 +3,7 @@ #CONFIG_BOARD_VERSION=v3.5-24V CONFIG_USB_PROTOCOL=native CONFIG_UART_PROTOCOL=ascii +CONFIG_DEBUG=false # Uncomment this to error on compilation warnings #CONFIG_STRICT=true diff --git a/LICENSE.md b/LICENSE.md new file mode 100644 index 00000000..977309e4 --- /dev/null +++ b/LICENSE.md @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2016-2018 ODrive Robotics + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/ODrive_Workspace.code-workspace b/ODrive_Workspace.code-workspace index d858dcc3..3d990eaf 100644 --- a/ODrive_Workspace.code-workspace +++ b/ODrive_Workspace.code-workspace @@ -11,6 +11,16 @@ } ], "settings": { + + "c-cpp-flylint.cppcheck.includePaths": [ + "${workspaceRoot}", + "${workspaceRoot}/fibre/cpp/include/fibre", + "${workspaceRoot}/communication", + "${workspaceRoot}/MotorControl", + ], + "c-cpp-flylint.cppcheck.platform": "avr8", + "c-cpp-flylint.cppcheck.standard": ["c99","c++14"], + "files.associations": { "memory": "cpp", "utility": "cpp", diff --git a/docs/Gemfile.lock b/docs/Gemfile.lock index 1c6d972e..860837ad 100644 --- a/docs/Gemfile.lock +++ b/docs/Gemfile.lock @@ -70,7 +70,7 @@ GEM listen (= 3.1.5) mercenary (~> 0.3) minima (= 2.4.0) - nokogiri (>= 1.8.1, < 2.0) + nokogiri (>= 1.8.5, < 2.0) rouge (= 2.2.1) terminal-table (~> 1.4) github-pages-health-check (1.4.0) @@ -81,7 +81,7 @@ GEM typhoeus (~> 1.3) html-pipeline (2.7.1) activesupport (>= 2) - nokogiri (>= 1.4) + nokogiri (>= 1.8.5) http_parser.rb (0.6.0) i18n (0.9.5) concurrent-ruby (~> 1.0) @@ -207,7 +207,7 @@ GEM minitest (5.11.3) multipart-post (2.0.0) net-dns (0.8.0) - nokogiri (1.8.2) + nokogiri (>= 1.8.5) mini_portile2 (~> 2.3.0) octokit (4.8.0) sawyer (~> 0.8.0, >= 0.5.3) diff --git a/docs/ascii-protocol.md b/docs/ascii-protocol.md index 951ec214..a0757e2f 100644 --- a/docs/ascii-protocol.md +++ b/docs/ascii-protocol.md @@ -7,7 +7,7 @@ * **Windows:** Use the Zadig utility to set the ODrive's driver to "usbser". Windows will then make the device available as COM port. You can use [PuTTY](https://www.chiark.greenend.org.uk/~sgtatham/putty/) to manually send commands or open the COM port using your favorite programming language * **Linux/macOS:** Run `/dev/tty*` to list all serial ports. The ODrive will show up as `/dev/ttyACM0` on Linux and `/dev/tty.usbmodem[...]` on macOS. Once you know the name, you can use `screen /dev/ttyACM0` (with the correct name) to send commands manually or open the device using your favorite programming language. Serial ports on Unix can be opened, written to and read from like a normal file. * **Via UART:** Connect the ODrive's TX (GPIO1) to your host's RX. Connect your ODrive's RX (GPIO2) to your host's TX. The logic level of the ODrive is 3.3V. - * **Arduino:** You can use the [ODrive Arduino library](https://github.com/madcowswe/ODriveArduino) to talk to the ODrive. + * **Arduino:** You can use the [ODrive Arduino library](https://github.com/madcowswe/ODrive/tree/master/Arduino/ODriveArduino) to talk to the ODrive. * **Windows/Linux/macOS:** You can use an FTDI USB-UART cable to connect to the ODrive. ## Command format @@ -36,7 +36,23 @@ Example: `t 0 -20000` For general moving around of the axis, this is the recommended command. +This command updates the watchdog timer for the motor. + #### Motor Position command +For basic use where you send one setpoint at at a time, use the `q` command. +If you have a realtime controller that is streaming setpoints and tracking a trajectory, use the `p` command. + +``` +q motor position velocity_lim current_lim +``` +* `q` for position +* `motor` is the motor number, `0` or `1`. +* `position` is the desired position, in encoder counts. +* `velocity_lim` is the velocity limit, in counts/s (optional). +* `current_lim` is the current limit, in A (optional). + +Example: `q 0 -20000 10000 10` + ``` p motor position velocity_ff current_ff ``` @@ -50,6 +66,7 @@ Example: `p 0 -20000 0 0` Note that if you don't know what feed-forward is or what it's used for, simply omit it. +This command updates the watchdog timer for the motor. #### Motor Velocity command ``` @@ -64,6 +81,8 @@ Example: `v 0 1000 0` Note that if you don't know what feed-forward is or what it's used for, simply omit it. +This command updates the watchdog timer for the motor. + #### Motor Current command ``` c motor current @@ -72,6 +91,29 @@ c motor current * `motor` is the motor number, `0` or `1`. * `current` is the desired current in A. +This command updates the watchdog timer for the motor. + +#### Request feedback +``` +f motor + +response: +pos vel +``` +* `f` for feedback +* `pos` is the encoder position in counts (float) +* `vel` is the encoder velocity in counts/s (float) + +#### Update motor watchdog +``` +u motor +``` +* `u` for /u/pdate. +* `motor` is the motor number, `0` or `1`. + +This command updates the watchdog timer for the motor, without changing any +setpoints. + #### Parameter reading/writing Not all parameters can be accessed via the ASCII protocol but at least all parameters with float and integer type are supported. @@ -90,3 +132,8 @@ Not all parameters can be accessed via the ASCII protocol but at least all param * `property` name of the property, as seen in ODrive Tool * `value` text representation of the value to be written * Example: `w axis0.controller.pos_setpoint -123.456` + +#### System commands: +* `ss` - Save config +* `se` - Erase config +* `sr` - Reboot diff --git a/docs/commands.md b/docs/commands.md index 32b7769d..e164b884 100644 --- a/docs/commands.md +++ b/docs/commands.md @@ -79,7 +79,7 @@ An upcoming feature will enable automatic tuning. Until then, here is a rough tu * Back down `vel_gain` to 50% of the vibrating value. * Increase `pos_gain` by around 30% per iteration until you see some overshoot. * Back down `pos_gain` until you do not have overshoot anymore. -* The integrator is not easily tuned, nor is it strictly required. Tune at your own discretion. +* The integrator can be set to `0.5 * bandwidth * vel_gain`, where `bandwidth` is the overall resulting tracking bandwidth of your system. Say your tuning made it track commands with a settling time of 100ms: this means the bandwidth was 1/100ms or 10. In this case you should set the `vel_integrator_gain = 0.5 * 10 * vel_gain`. ## System monitoring commands diff --git a/docs/developer-guide.md b/docs/developer-guide.md index ecf3e1ea..04671b48 100644 --- a/docs/developer-guide.md +++ b/docs/developer-guide.md @@ -5,7 +5,7 @@ As such it assumes that you know things like how to use Git, what a compiler is, The official releases are maintained on the `master` branch. However since you are a developer, you are encouraged to use the `devel` branch, as it contains the latest features. -The project is under active development, so make sure to check the [Changelog](CHANGELOG.md) to keep track of updates. +The project is under active development, so make sure to check the [Changelog](../CHANGELOG.md) to keep track of updates. ### Table of contents @@ -70,7 +70,8 @@ sudo pacman -S tup #### Mac First install [Homebrew](https://brew.sh/). Then you can run these commands in Terminal: ```bash -brew cask install gcc-arm-embedded +brew tap osx-cross/arm +brew install arm-gcc-bin brew cask install osxfuse && brew install tup brew install openocd ``` @@ -80,7 +81,9 @@ __Note__: make sure these programs are not only installed but also added to your Some instructions in this document may assume that you're using a bash command prompt, such as the Windows 10 built-in bash or [Git](https://git-scm.com/download/win) bash. -* [ARM compiler](https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads) __Note__: After installing, create an environment variable named `ARM_GCC_ROOT` whose value is the path you installed to. e.g. `C:\Program Files (x86)\GNU Tools Arm Embedded\7 2018-q2-update`. This variable is used to locate include files for the c/c++ Visual Studio Code extension. +* [ARM compiler](https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads) + * __Note 1__: After installing, create an environment variable named `ARM_GCC_ROOT` whose value is the path you installed to. e.g. `C:\Program Files (x86)\GNU Tools Arm Embedded\7 2018-q2-update`. This variable is used to locate include files for the c/c++ Visual Studio Code extension. + * __Note 2__: 8-2018-q4-major seems to have a bug on Windows. Please use 7-2018-q2-update. * [Tup](http://gittup.org/tup/index.html) * [Make for Windows](http://gnuwin32.sourceforge.net/packages/make.htm) * [OpenOCD](http://gnuarmeclipse.github.io/openocd/install/). Also follow the instructions on the ST-LINK/V2 drivers. diff --git a/docs/getting-started.md b/docs/getting-started.md index d9cc3dd6..8060bbb3 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -40,7 +40,7 @@ permalink: / -* A power supply (12V-24V for the 24V board variant, 12V-48V for the 48V board variant). A battery is also fine. +* A power supply (12V-24V for the 24V board variant, 12V-48V for the 48V board variant). A battery is also fine. Some advice on choosing a power supply can be found [here](https://things-in-motion.blogspot.com/2018/12/how-to-select-right-power-source-for.html).
What voltage variant do I have?
On all ODrives shipped July 2018 or after have a silkscreen label clearly indicating the voltage variant. @@ -56,14 +56,19 @@ All non-power I/O is 3.3V output and 5V tolerant on input, on ODrive v3.3 and ne ### Wiring up the motors * Connect the motor phases into the 3-phase screw terminals. It is not recommended to use a clip-on connector such as an alligator clip, as this can cause issues with the phase resistance/inductance measurements. -* Connect the power source to the DC terminals. Make sure to pay attention to the polarity. -* Do not apply power just yet. ### Wiring up the encoders Connect the encoder(s) to J4. The A,B phases are required, and the Z (index pulse) is optional. The A,B and Z lines have 3.3k pull up resistors, for use with open-drain encoder outputs. For single ended push-pull signals with weak drive current (\<4mA), you may want to desolder the pull-ups. ![Image of ODrive all hooked up](https://docs.google.com/drawings/d/e/2PACX-1vTCD0P40Cd-wvD7Fl8UYEaxp3_UL81oI4qUVqrrCJPi6tkJeSs2rsffIXQRpdu6rNZs6-2mRKKYtILG/pub?w=1716&h=1281) +### Safety & Power UP +
+ Always think safety before powering up the ODrive if motors are attached. Consider what might happen if the motor spins as soon as power is applied. +
+* Unlike some devices, the ODrive does not recieve power over the USB port so the 24/48 volt power input is required even just to communicate with it using USB. It is ok to power up the ODrive before or after connecting the USB cable. +* To power up the ODrive, connect the power source to the DC terminals. Make sure to pay attention to the polarity. A small spark is normal. This is caused by the capacitors charging up. + ## Downloading and Installing Tools Most instructions in this guide refer to a utility called `odrivetool`, so you should install that first. @@ -189,6 +194,7 @@ This is the resistance of the brake resistor. If you are not using it, you may s `odrv0.axis0.motor.config.pole_pairs` This is the number of **magnet poles** in the rotor, **divided by two**. To find this, you can simply count the number of permanent magnets in the rotor, if you can see them. _Note: this is not the same as the number of coils in the stator._ If you can't see them, try sliding a magnet around the rotor, and counting how many times it stops. This will be the number of **pole pairs**. If you use a magnetic piece of metal instead of a magnet, you will get the number of **magnet poles**. + `odrv0.axis0.motor.config.motor_type` This is the type of motor being used. Currently two types of motors are supported: High-current motors (`MOTOR_TYPE_HIGH_CURRENT`) and gimbal motors (`MOTOR_TYPE_GIMBAL`).
Which motor_type to choose?
@@ -255,6 +261,7 @@ You can also directly control the current of the motor, which is proportional to ### Trajectory control +While in position control mode, use the `move_to_pos` or `move_incremental` functions. See the **Usage** section for details
This mode lets you smoothly accelerate, coast, and decelerate the axis from one position to another. With raw position control, the controller simply tries to go to the setpoint as quickly as possible. Using a trajectory lets you tune the feedback gains more aggressively to reject disturbance, while keeping smooth motion. ![Taptraj](TrapTrajPosVel.PNG)
@@ -284,10 +291,22 @@ Keep in mind that you must still set your safety limits as before. I recommend #### Usage Use the `move_to_pos` function to move to an absolute position: ``` -..controller.move_to_pos() +..controller.move_to_pos(your_absolute_pos) ``` +Use the `move_incremental` function to move to a relative position. +To set the goal relative to the current actual position, use `from_goal_point = False` +To set the goal relative to the previous destination, use `from_goal_point = True` +``` +..controller.move_incremental(pos_increment, from_goal_point) +``` + +You can also execute a move with the [appropriate ascii command](ascii-protocol.md#motor-trajectory-command). + ### Circular position control + +To enable Circular position control, set `axis.controller.config.setpoints_in_cpr = True` + This mode is useful for continuos incremental position movement. For example a robot rolling indefinitely, or an extruder motor or conveyor belt moving with controlled increments indefinitely. In the regular position mode, the `pos_setpoint` would grow to a very large value and would lose precision due to floating point rounding. @@ -312,6 +331,18 @@ You can now control the current with `axis.controller.current_setpoint = 3` [A]. *Note: There is no velocity limiting in current control mode. Make sure that you don't overrev the motor, or exceed the max speed for your encoder.* + +## Watchdog Timer +Each axis has a configurable watchdog timer that can stop the motors if the +control connection to the ODrive is interrupted. + +Each axis has a configurable watchdog timeout: `axis.config.watchdog_timeout`, +measured in seconds. A value of `0` disables the watchdog functionality. Any value +`> 0` will stop the motors if the watchdog has not been fed in the configured +time interval. + +The watchdog is fed using the `axis.watchdog_feed()` method of each axis. + ## What's next? You can now: * See what other [commands and parameters](commands.md) are available, including setting tuning parameters for better performance. diff --git a/docs/hoverboard.md b/docs/hoverboard.md index 2e9f6cf4..9641ae6d 100644 --- a/docs/hoverboard.md +++ b/docs/hoverboard.md @@ -18,6 +18,8 @@ You may wire the motor phases in any order into a motor connector on the ODrive, | Green | Z | | Black | GND | +Note: In order to ber compatible with encoder inputs, the ODrive doesn't have any filtering capacitors on the pins where the hall sensors connect. Therefore to get a reliable hall signal, it is recommended that you add some filter capacitors to these pins. You can see instructions [here](https://discourse.odriverobotics.com/t/encoder-error-error-illegal-hall-state/1047/7?u=madcowswe). + ### Hoverboard motor configuration Standard 6.5 inch hoverboard hub motors have 30 permanent magnet poles, and thus 15 pole pairs. If you have a different motor you need to count the magnets or have a reliable datasheet for this information. @@ -118,7 +120,7 @@ odrv0.axis0.requested_state = AXIS_STATE_IDLE Hopefully you got your motor to spin! Feel free to repeat all of the above for the other axis if appropriate. ### PWM input -If you want to drive your hoverboard wheels around with an RC remote contro you can use the [RC PWM input](interfaces.md#rc-pwm-input). There is more information in that link. +If you want to drive your hoverboard wheels around with an RC remote control you can use the [RC PWM input](interfaces.md#rc-pwm-input). There is more information in that link. Lets use GPIO 3/4 for the velocity inputs so that we don't have to disable UART. Then let's map the full stick range of these inputs to some suitable velocity setpoint range. We also have to reboot to activate the PWM input. @@ -159,6 +161,9 @@ odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL odrv0.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL ``` +### Safety +Be sure to setup the Failsafe feature on your RC Receiver so that if connection is lost between the remote and the receiver, the receiver outputs 0 and 0 for the velocity setpoint of both axes (or whatever is safest for your configuration). Also note that if the receiver turns off (loss of power, etc) or if the signal from the receiver to the ODrive is lost (wire comes unplugged, etc), the ODrive will continue the last commanded velocity setpoint. There is currently no timeout function in the ODrive for PWM inputs. + ### Automatic startup Try to reboot and then activate AXIS_STATE_CLOSED_LOOP_CONTROL on both axis. Check that everything is operational and works as expected. If so, you can now make the ODrive turn on the motor power automatically after booting. This is useful if you are going to be running the ODrive without a PC or other logic board. diff --git a/docs/interfaces.md b/docs/interfaces.md index ff0ec9ac..3f5a7dbd 100644 --- a/docs/interfaces.md +++ b/docs/interfaces.md @@ -33,6 +33,7 @@ The ODrive can be controlled over various ports and protocols. If you're comfort (*) ODrive v3.5 and later Notes: +* You must also connect GND between ODrive and your other board. * ODrive v3.3 and onward have 5V tolerant GPIO pins. * ODrive v3.5 and later have some noise supression filters on the default step/dir pins * You can change the step/dir pins using `axis.config._gpio_pin`. @@ -127,6 +128,8 @@ As an example, we'll configure GPIO4 to control the angle of axis 0. We want the ``` 5. With the ODrive powered off, connect the RC receiver ground to the ODrive's GND and one of the RC receiver signals to GPIO4. You may try to power the receiver from the ODrive's 5V supply if it doesn't draw too much power. Power up the the RC transmitter. You should now be able to control axis 0 from one of the RC sticks. +Be sure to setup the Failsafe feature on your RC Receiver so that if connection is lost between the remote and the receiver, the receiver outputs 0 for the velocity setpoint of both axes (or whatever is safest for your configuration). Also note that if the receiver turns off (loss of power, etc) or if the signal from the receiver to the ODrive is lost (wire comes unplugged, etc), the ODrive will continue the last commanded velocity setpoint. There is currently no timeout function in the ODrive for PWM inputs. + ## Ports Note: when you use an existing library you don't have to deal with the specifics described in this section. diff --git a/docs/odrivetool.md b/docs/odrivetool.md index 18dc7b0d..52b648bb 100644 --- a/docs/odrivetool.md +++ b/docs/odrivetool.md @@ -116,6 +116,7 @@ You can use the DfuSe app from ST. 1. Force the ODrive into DFU mode, as per the instructions above "How to force DFU mode". 1. In the top left it should now be connected to "STM Device in DFU Mode". 1. If it doesn't appear, it may be because the driver is set to libusb by Zadig. We need to set it back to the original driver. Follow [these instructions](https://github.com/pbatard/libwdi/wiki/FAQ#Help_Zadig_replaced_the_driver_for_the_wrong_device_How_do_I_restore_it). + 2. If, after doing the above step, the ODrive still installs itself as a libusb device in Device Manager, you can try to delete the libusb driver (this is OK, since we can use Zadig to install it again). You can simply delete the file `C:\Windows\System32\drivers\libusb0.sys`. 1. In the bottom right section called "Upgrade or Verify Action" click the button "Choose...". 1. Locate the `ODriveFirmware.dfu` we made before. 1. Click button "Upgrade". @@ -135,9 +136,40 @@ In the Firmware directory, after finishing building the firmware: sudo dfu-util -a 0 -s 0x08000000 -D build/ODriveFirmware.bin ``` -#### MacOS -**This section needs more detail. Please consider adding detail if you got it to work.** -You may be able to use [dfu-util](http://dfu-util.sourceforge.net/) to upgrade the firmware. The command should be similar to the Linux instructions. +#### macOS + +First, you need to install the arm development tools to copy the binary into the appropriate format. + +```text +$ brew cask install gcc-arm-embedded +``` + +Then convert the binary to .bin format + +```text +$ arm-none-eabi-objcopy -O binary ODriveFirmware_v3.5-48V.elf ODriveFirmware_v3.5-48V.bin +``` + +Install `dfu-util`: + +```text +$ sudo port install dfu-util # via MacPorts; for HomeBrew use "brew install dfu-util" +``` + +Find the correct device serial number to use: + +```text +$ dfu-util --list # list the DFU capable devices +[...] +Found DFU: [0483:df11] ver=2200, devnum=5, cfg=1, intf=0, path="20-2", alt=0, + name="@Internal Flash /0x08000000/04*016Kg,01*064Kg,07*128Kg", serial="388237123123" +``` + +Finally, flash the firmware using the found serial number: + +```text +$ sudo dfu-util -S 388237123123 -a 0 -s 0x08000000 -D ODriveFirmware_v3.5-48V.bin +``` ## Flashing with an STLink diff --git a/docs/troubleshooting.md b/docs/troubleshooting.md index c5556d02..7795ed00 100644 --- a/docs/troubleshooting.md +++ b/docs/troubleshooting.md @@ -20,6 +20,7 @@ The following sections will give some guidance on the most common errors. You ma * Axis error flags defined [here](../Firmware/MotorControl/axis.hpp). * Motor error flags defined [here](../Firmware/MotorControl/motor.hpp). * Encoder error flags defined [here](../Firmware/MotorControl/encoder.hpp). +* Controller error flags defined [here](../Firmware/MotorControl/controller.hpp). * Sensorless estimator error flags defined [here](../Firmware/MotorControl/sensorless_estimator.hpp). ## Common Axis Errors @@ -36,9 +37,9 @@ You can monitor your PUS voltage using liveplotter in odrive tool by entering `s * `ERROR_DC_BUS_OVER_VOLTAGE = 0x04` -Confirm that you have a break resistor of the correct value connected securly and that `odrv0.config.brake_resistance` is set to the value of your break resistor. +Confirm that you have a brake resistor of the correct value connected securly and that `odrv0.config.brake_resistance` is set to the value of your brake resistor. -You can monitor your PUS voltage using liveplotter in odrive tool by entering `start_liveplotter(lambda: [odrv0.vbus_voltage])`. If during a move you see the voltage rise above your PSU's nominal set voltage then you have your break resistance set too low. This may happen if you are using long wires or small gauge wires to connect your break resistor to your odrive which will added extra resistance. This extra resistance needs to be accounted for to prevent this voltage spike. If you have checked all your connections you can also try increasing your break resistance by ~ 0.01 Ohm at a time to a maximum of 0.05 greater than your break resistor value. +You can monitor your PUS voltage using liveplotter in odrive tool by entering `start_liveplotter(lambda: [odrv0.vbus_voltage])`. If during a move you see the voltage rise above your PSU's nominal set voltage then you have your brake resistance set too low. This may happen if you are using long wires or small gauge wires to connect your brake resistor to your odrive which will added extra resistance. This extra resistance needs to be accounted for to prevent this voltage spike. If you have checked all your connections you can also try increasing your brake resistance by ~ 0.01 Ohm at a time to a maximum of 0.05 greater than your brake resistor value. ## Common Motor Errors @@ -58,7 +59,8 @@ Some motors will have a considerably different phase resistance and inductance t In general, you need ```text -resistance_calib_max_voltage > calibration_current * phase_resistance`. +resistance_calib_max_voltage > calibration_current * phase_resistance +resistance_calib_max_voltage < 0.5 * vbus_voltage ``` * `ERROR_DRV_FAULT = 0x0008` @@ -72,6 +74,12 @@ power supply of the DRV8301 gate driver chips, thus tripping its under-voltage f To resolve this issue you can limit the M0 current to 40A. The lowest current at which the DRV fault was observed is 45A on one test motor and 50A on another test motor. Refer to [this post](https://discourse.odriverobotics.com/t/drv-fault-on-odrive-v3-4/558) for instructions for a hardware fix. +* `ERROR_MODULATION_MAGNITUDE = 0x0080` + +The bus voltage was insufficent to push the requested current through the motor. Reduce `motor.config.calibration_current` and/or `motor.config.current_lim`, for errors at calibration-time and closed loop control respectively. + +For gimbal motors, it is recommended to set the calibration_current and current_lim to half your bus voltage, or less. + ## Common Encoder Errors * `ERROR_CPR_OUT_OF_RANGE = 0x02` diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index ffff2ed1..616662b6 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -10,6 +10,8 @@ AXIS_STATE_SENSORLESS_CONTROL = 5 AXIS_STATE_ENCODER_INDEX_SEARCH = 6 AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7 AXIS_STATE_CLOSED_LOOP_CONTROL = 8 +AXIS_STATE_LOCKIN_SPIN = 9 +AXIS_STATE_ENCODER_DIR_FIND = 10 AXIS_STATE_HOMING = 9 class errors: @@ -26,6 +28,7 @@ class errors: ERROR_ENCODER_FAILED = 0x100 # Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value ERROR_CONTROLLER_FAILED = 0x200 ERROR_POS_CTRL_DURING_SENSORLESS = 0x400 + ERROR_WATCHDOG_TIMER_EXPIRED = 0x800 class motor: ERROR_NONE = 0 @@ -62,6 +65,7 @@ CTRL_MODE_VOLTAGE_CONTROL = 0 CTRL_MODE_CURRENT_CONTROL = 1 CTRL_MODE_VELOCITY_CONTROL = 2 CTRL_MODE_POSITION_CONTROL = 3 +CTRL_MODE_TRAJECTORY_CONTROL = 4 ENCODER_MODE_INCREMENTAL = 0 ENCODER_MODE_HALL = 1 diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index be8a5f4b..f5ce0c7f 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -32,9 +32,10 @@ class OperationAbortedException(Exception): pass def dump_errors(odrv, clear=False): - axes = [axis for name, axis in odrv._remote_attributes.items() if 'axis' in name] - for num, axis in enumerate(axes): - print('Axis{}:'.format(num)) + axes = [(name, axis) for name, axis in odrv._remote_attributes.items() if 'axis' in name] + axes.sort() + for name, axis in axes: + print(name) # Flatten axis and submodules # (name, remote_obj, errorcode) @@ -59,7 +60,7 @@ def dump_errors(odrv, clear=False): else: print(prefix + _VT100Colors['green'] + "no error" + _VT100Colors['default']) -data_rate = 100 +data_rate = 10 plot_rate = 10 num_samples = 1000 def start_liveplotter(get_var_callback): @@ -106,6 +107,7 @@ def start_liveplotter(get_var_callback): while not cancellation_token.is_set(): plt.clf() plt.plot(vals) + plt.legend(list(range(len(vals)))) fig.canvas.draw() fig.canvas.start_event_loop(1/plot_rate) @@ -190,17 +192,6 @@ def usb_burn_in_test(get_var_callback, cancellation_token): print("read {} values".format(i)) threading.Thread(target=fetch_data, daemon=True).start() -def setup_udev_rules(logger): - if platform.system() != 'Linux': - logger.error("This command only makes sense on Linux") - if os.getuid() != 0: - logger.warn("you should run this as root, otherwise it will probably not work") - with open('/etc/udev/rules.d/91-odrive.rules', 'w') as file: - file.write('SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d3[0-9]", MODE="0666"\n') - subprocess.check_call(["udevadm", "control", "--reload-rules"]) - subprocess.check_call(["udevadm", "trigger"]) - logger.info('udev rules configured successfully') - def yes_no_prompt(question, default=None): if default is None: question += " [y/n] " diff --git a/tools/odrive/version.py b/tools/odrive/version.py index 5a2827a3..a96e9b4e 100644 --- a/tools/odrive/version.py +++ b/tools/odrive/version.py @@ -3,6 +3,7 @@ import re import subprocess import os import sys +import platform def version_str_to_tuple(version_string): """ @@ -78,3 +79,15 @@ if __name__ == '__main__': args.output.write('#define FW_VERSION_MINOR {}\n'.format(minor)) args.output.write('#define FW_VERSION_REVISION {}\n'.format(revision)) args.output.write('#define FW_VERSION_UNRELEASED {}\n'.format(1 if unreleased else 0)) + +def setup_udev_rules(logger): + if platform.system() != 'Linux': + logger.error("This command only makes sense on Linux") + return + if os.getuid() != 0: + logger.warn("you should run this as root, otherwise it will probably not work") + with open('/etc/udev/rules.d/91-odrive.rules', 'w') as file: + file.write('SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d3[0-9]", MODE="0666", ENV{ID_MM_DEVICE_IGNORE}="1"\n') + subprocess.check_call(["udevadm", "control", "--reload-rules"]) + subprocess.check_call(["udevadm", "trigger"]) + logger.info('udev rules configured successfully') diff --git a/tools/odrivetool b/tools/odrivetool index c6632e90..5c3af5b7 100755 --- a/tools/odrivetool +++ b/tools/odrivetool @@ -177,7 +177,7 @@ try: rate_test(my_odrive) elif args.command == 'udev-setup': - from odrive.utils import setup_udev_rules + from odrive.version import setup_udev_rules setup_udev_rules(logger) elif args.command == 'generate-code': diff --git a/tools/setup.py b/tools/setup.py index b2cfeb74..986d9d0d 100644 --- a/tools/setup.py +++ b/tools/setup.py @@ -91,11 +91,10 @@ if creating_package: if not creating_package: import platform if platform.system() == 'Linux': - import odrive.utils from fibre.utils import Logger try: - odrive.utils.setup_udev_rules(Logger()) - except PermissionError: + odrive.version.setup_udev_rules(Logger()) + except Exception: print("Warning: could not set up udev rules. Run `sudo odrivetool udev-setup` to try again.") try: @@ -117,6 +116,7 @@ try: 'requests', # Used to by DFU to load firmware files 'IntelHex', # Used to by DFU to download firmware from github 'matplotlib', # Required to run the liveplotter + 'monotonic', # For compatibility with older python versions 'pywin32 >= 222; platform_system == "Windows"' # Required for fancy terminal features on Windows ], package_data={'': ['version.txt']}, diff --git a/tools/setup_hall_as_index.py b/tools/setup_hall_as_index.py new file mode 100644 index 00000000..c01de4ff --- /dev/null +++ b/tools/setup_hall_as_index.py @@ -0,0 +1,97 @@ + +import odrive +from odrive.utils import dump_errors +from odrive.enums import * +import time + +print("Finding an odrive...") +odrv = odrive.find_any() + +# axes = [odrv.axis0, odrv.axis1]; +axes = [odrv.axis0]; + +flip_index_search_direction = False +save_and_reboot = True + +print("Setting config...") +# Settings to protect battery +odrv.config.dc_bus_overvoltage_trip_level = 14.8 +odrv.config.dc_bus_undervoltage_trip_level = 8.0 +odrv.config.brake_resistance = 0 +for ax in axes: + ax.motor.config.requested_current_range = 25 + ax.motor.config.calibration_current = 10 + ax.motor.config.current_lim = 10 + ax.motor.config.resistance_calib_max_voltage = 4 + ax.motor.config.pole_pairs = 10 + + ax.encoder.config.cpr = 4096 + ax.encoder.config.use_index = True + ax.encoder.config.find_idx_on_lockin_only = True + ax.encoder.config.idx_search_unidirectional = True + + ax.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL + ax.controller.config.vel_limit = 10000 + ax.controller.config.vel_gain = 0.002205736003816127 + ax.controller.config.vel_integrator_gain = 0.022057360038161278 + ax.controller.config.pos_gain = 26 + + ax.config.lockin.current = 10 + ax.config.lockin.ramp_distance = 3.14 + ax.config.lockin.vel = 15 + ax.config.lockin.accel = 10 + ax.config.lockin.finish_distance = 30 + +def wait_and_exit_on_error(ax): + while ax.current_state != AXIS_STATE_IDLE: + time.sleep(0.1) + if ax.error != errors.axis.ERROR_NONE: + dump_errors(odrv, True) + exit() + +for axnum, ax in enumerate(axes): + print("Calibrating motor {}...".format(axnum)) + ax.requested_state = AXIS_STATE_MOTOR_CALIBRATION + wait_and_exit_on_error(ax) + + print("Checking motor {} direction...".format(axnum)) + ax.requested_state = AXIS_STATE_ENCODER_DIR_FIND + wait_and_exit_on_error(ax) + print(" Direction is {}".format(ax.motor.config.direction)) + + if flip_index_search_direction: + ax.config.lockin.ramp_distance = -ax.config.lockin.ramp_distance + ax.config.lockin.vel = -ax.config.lockin.vel + ax.config.lockin.accel = -ax.config.lockin.accel + + print("Searching for index on motor {}...".format(axnum)) + ax.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH + wait_and_exit_on_error(ax) + if (not ax.encoder.index_found): + print("Failed finding index! Quitting.") + exit() + + print("Calibrating encoder offset on motor {}...".format(axnum)) + ax.requested_state = AXIS_STATE_ENCODER_OFFSET_CALIBRATION + wait_and_exit_on_error(ax) + if (not ax.encoder.is_ready): + print("Failed to calibrate encoder! Quitting") + exit() + + # If we get here there were no errors, so let's commit the values + ax.motor.config.pre_calibrated = True + ax.encoder.config.pre_calibrated = True + + # Uncomment this if you wish to automatically run index search and closed loop control on boot + # ax.config.startup_encoder_index_search = True + # ax.config.startup_closed_loop_control = True + +#Everything should be good to go here, so let's save and reboot +print("") +print("All operations successful!") +if save_and_reboot: + odrv.save_configuration() + try: + odrv.reboot() + except odrive.fibre.ChannelBrokenException: + pass