mirror of
https://github.com/odriverobotics/ODrive.git
synced 2026-09-20 22:55:00 +08:00
Merge branch 'devel' into property-hooks
This commit is contained in:
Oskar Weigl
@@ -4,9 +4,16 @@ Please add a note of your changes below this heading if you make a Pull Request.
|
||||
# Unreleased
|
||||
|
||||
### Added
|
||||
* **Trapezoidal Trajectory Planner**
|
||||
* Hook to execute protocol property written callback
|
||||
* -Wdouble-promotion warning to compilation
|
||||
|
||||
### Changed
|
||||
* Make python tools compatible with python 2.7 (so it can be used with ROS)
|
||||
* Threading API constructor can't take the daemon parameter, so all thread creation had to be expanded out.
|
||||
* `TimeoutError` isn't defined, but it makes for more readable code, so I defined it as an OSError subclass.
|
||||
* `ModuleNotFoundError` is replaced by the older ImportError.
|
||||
* Print function imported from future
|
||||
* Using new hooks to calculate:
|
||||
* `motor.config.current_control_bandwidth`
|
||||
* This depricates `motor.set_current_control_bandwidth()`
|
||||
@@ -27,8 +34,10 @@ Please add a note of your changes below this heading if you make a Pull Request.
|
||||
* Hardcoded maximum inductance now 2500 uH.
|
||||
|
||||
### Fixed
|
||||
* Incorrect shifting of offset during index callback
|
||||
* Once you got an axis error `ERROR_INVALID_STATE` you could never clear it
|
||||
* Char to int conversion to read motornum on arduino example
|
||||
* GPIO above #5 would not be used correctly in some cases
|
||||
|
||||
## [0.4.2] - 2018-08-04
|
||||
### Added
|
||||
|
||||
+13
-16
@@ -4,6 +4,7 @@
|
||||
"name": "Win32",
|
||||
"includePath": [
|
||||
"${workspaceRoot}",
|
||||
"${workspaceRoot}/fibre/cpp/include/**",
|
||||
"${workspaceRoot}/MotorControl",
|
||||
"${workspaceRoot}/Drivers/DRV8301",
|
||||
"${workspaceRoot}/Board/v3/Inc",
|
||||
@@ -16,12 +17,8 @@
|
||||
"${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",
|
||||
"c:/program files (x86)/gnu tools arm embedded/6 2017-q1-update/arm-none-eabi/include/c++/6.3.1",
|
||||
"c:/program files (x86)/gnu tools arm embedded/6 2017-q1-update/arm-none-eabi/include/c++/6.3.1/arm-none-eabi",
|
||||
"c:/program files (x86)/gnu tools arm embedded/6 2017-q1-update/arm-none-eabi/include/c++/6.3.1/backward",
|
||||
"c:/program files (x86)/gnu tools arm embedded/6 2017-q1-update/lib/gcc/arm-none-eabi/6.3.1/include",
|
||||
"c:/program files (x86)/gnu tools arm embedded/6 2017-q1-update/lib/gcc/arm-none-eabi/6.3.1/include-fixed",
|
||||
"c:/program files (x86)/gnu tools arm embedded/6 2017-q1-update/arm-none-eabi/include"
|
||||
"${ARM_GCC_ROOT}/arm-none-eabi/include/**",
|
||||
"${ARM_GCC_ROOT}/lib/gcc/arm-none-eabi/**"
|
||||
],
|
||||
"defines": [
|
||||
"STM32F405xx",
|
||||
@@ -38,10 +35,11 @@
|
||||
"browse": {
|
||||
"path": [
|
||||
"${workspaceRoot}",
|
||||
"C:/Program Files (x86)/GNU Tools ARM Embedded"
|
||||
"${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",
|
||||
"cStandard": "c11",
|
||||
"cppStandard": "c++14"
|
||||
},
|
||||
@@ -49,6 +47,7 @@
|
||||
"name": "Linux",
|
||||
"includePath": [
|
||||
"${workspaceRoot}",
|
||||
"${workspaceRoot}/fibre/cpp/include/**",
|
||||
"${workspaceRoot}/MotorControl",
|
||||
"${workspaceRoot}/Drivers/DRV8301",
|
||||
"${workspaceRoot}/Board/v3/Inc",
|
||||
@@ -61,8 +60,8 @@
|
||||
"${workspaceRoot}/Board/v3/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy",
|
||||
"${workspaceRoot}/Board/v3/Drivers/CMSIS/Device/ST/STM32F4xx/Include",
|
||||
"${workspaceRoot}/Board/v3/Drivers/CMSIS/Include",
|
||||
"/usr/lib/gcc/arm-none-eabi/4.9.3/include",
|
||||
"/usr/lib/arm-none-eabi/include"
|
||||
"${ARM_GCC_ROOT}/arm-none-eabi/include/**",
|
||||
"${ARM_GCC_ROOT}/lib/gcc/arm-none-eabi/**"
|
||||
],
|
||||
"defines": [
|
||||
"STM32F405xx",
|
||||
@@ -78,8 +77,7 @@
|
||||
"browse": {
|
||||
"path": [
|
||||
"${workspaceRoot}",
|
||||
"/usr/lib/gcc/arm-none-eabi/4.9.3/include",
|
||||
"/usr/lib/arm-none-eabi/include"
|
||||
"${ARM_GCC_ROOT}"
|
||||
],
|
||||
"limitSymbolsToIncludedHeaders": true,
|
||||
"databaseFilename": ""
|
||||
@@ -92,6 +90,7 @@
|
||||
"name": "Mac",
|
||||
"includePath": [
|
||||
"${workspaceRoot}",
|
||||
"${workspaceRoot}/fibre/cpp/include/**",
|
||||
"${workspaceRoot}/MotorControl",
|
||||
"${workspaceRoot}/Drivers/DRV8301",
|
||||
"${workspaceRoot}/Board/v3/Inc",
|
||||
@@ -104,10 +103,8 @@
|
||||
"${workspaceRoot}/Board/v3/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy",
|
||||
"${workspaceRoot}/Board/v3/Drivers/CMSIS/Device/ST/STM32F4xx/Include",
|
||||
"${workspaceRoot}/Board/v3/Drivers/CMSIS/Include",
|
||||
"/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/arm-none-eabi/include",
|
||||
"/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/arm-none-eabi/include/c++/6.3.1",
|
||||
"/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/arm-none-eabi/include/c++/6.3.1/arm-none-eabi",
|
||||
"/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/lib/gcc/arm-none-eabi/6.3.1/include"
|
||||
"${ARM_GCC_ROOT}/arm-none-eabi/include/**",
|
||||
"${ARM_GCC_ROOT}/lib/gcc/arm-none-eabi/**"
|
||||
],
|
||||
"defines": [
|
||||
"STM32F405xx",
|
||||
@@ -123,7 +120,7 @@
|
||||
"browse": {
|
||||
"path": [
|
||||
"${workspaceRoot}",
|
||||
"/usr/local/Caskroom/gcc-arm-embedded/"
|
||||
"${ARM_GCC_ROOT}"
|
||||
],
|
||||
"limitSymbolsToIncludedHeaders": true,
|
||||
"databaseFilename": ""
|
||||
|
||||
@@ -285,6 +285,15 @@ GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin){
|
||||
case 4: return GPIO_4_GPIO_Port; break;
|
||||
#ifdef GPIO_5_GPIO_Port
|
||||
case 5: return GPIO_5_GPIO_Port; break;
|
||||
#endif
|
||||
#ifdef GPIO_6_GPIO_Port
|
||||
case 6: return GPIO_6_GPIO_Port; break;
|
||||
#endif
|
||||
#ifdef GPIO_7_GPIO_Port
|
||||
case 7: return GPIO_7_GPIO_Port; break;
|
||||
#endif
|
||||
#ifdef GPIO_8_GPIO_Port
|
||||
case 8: return GPIO_8_GPIO_Port; break;
|
||||
#endif
|
||||
default: return GPIO_1_GPIO_Port;
|
||||
}
|
||||
@@ -298,6 +307,15 @@ uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin){
|
||||
case 4: return GPIO_4_Pin; break;
|
||||
#ifdef GPIO_5_Pin
|
||||
case 5: return GPIO_5_Pin; break;
|
||||
#endif
|
||||
#ifdef GPIO_6_Pin
|
||||
case 6: return GPIO_6_Pin; break;
|
||||
#endif
|
||||
#ifdef GPIO_7_Pin
|
||||
case 7: return GPIO_7_Pin; break;
|
||||
#endif
|
||||
#ifdef GPIO_8_Pin
|
||||
case 8: return GPIO_8_Pin; break;
|
||||
#endif
|
||||
default: return GPIO_1_Pin;
|
||||
}
|
||||
|
||||
+1
-1
@@ -1,6 +1,6 @@
|
||||
The MIT License (MIT)
|
||||
|
||||
Copyright (c) 2016 Oskar Weigl (madcowswe)
|
||||
Copyright (c) 2016-2018 Oskar Weigl
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
|
||||
@@ -7,22 +7,25 @@
|
||||
#include "odrive_main.h"
|
||||
|
||||
Axis::Axis(const AxisHardwareConfig_t& hw_config,
|
||||
AxisConfig_t& config,
|
||||
Config_t& config,
|
||||
Encoder& encoder,
|
||||
SensorlessEstimator& sensorless_estimator,
|
||||
Controller& controller,
|
||||
Motor& motor)
|
||||
Motor& motor,
|
||||
TrapezoidalTrajectory& trap)
|
||||
: hw_config_(hw_config),
|
||||
config_(config),
|
||||
encoder_(encoder),
|
||||
sensorless_estimator_(sensorless_estimator),
|
||||
controller_(controller),
|
||||
motor_(motor)
|
||||
motor_(motor),
|
||||
trap_(trap)
|
||||
{
|
||||
encoder_.axis_ = this;
|
||||
sensorless_estimator_.axis_ = this;
|
||||
controller_.axis_ = this;
|
||||
motor_.axis_ = this;
|
||||
trap_.axis_ = this;
|
||||
}
|
||||
|
||||
static void step_cb_wrapper(void* ctx) {
|
||||
@@ -179,7 +182,7 @@ bool Axis::run_sensorless_spin_up() {
|
||||
bool Axis::run_sensorless_control_loop() {
|
||||
set_step_dir_enabled(config_.enable_step_dir);
|
||||
run_control_loop([this](){
|
||||
if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL)
|
||||
if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL)
|
||||
return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false;
|
||||
|
||||
// Note that all estimators are updated in the loop prefix in run_control_loop
|
||||
|
||||
@@ -5,40 +5,6 @@
|
||||
#error "This file should not be included directly. Include odrive_main.h instead."
|
||||
#endif
|
||||
|
||||
// Warning: Do not reorder these enum values.
|
||||
// The state machine uses ">" comparision on them.
|
||||
enum AxisState_t {
|
||||
AXIS_STATE_UNDEFINED = 0, //<! will fall through to idle
|
||||
AXIS_STATE_IDLE = 1, //<! disable PWM and do nothing
|
||||
AXIS_STATE_STARTUP_SEQUENCE = 2, //<! the actual sequence is defined by the config.startup_... flags
|
||||
AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3, //<! run all calibration procedures, then idle
|
||||
AXIS_STATE_MOTOR_CALIBRATION = 4, //<! run motor calibration
|
||||
AXIS_STATE_SENSORLESS_CONTROL = 5, //<! run sensorless control
|
||||
AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
|
||||
AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
|
||||
AXIS_STATE_CLOSED_LOOP_CONTROL = 8 //<! run closed loop control
|
||||
};
|
||||
|
||||
struct AxisConfig_t {
|
||||
bool startup_motor_calibration = false; //<! run motor calibration at startup, skip otherwise
|
||||
bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
|
||||
// this only has an effect if encoder.config.use_index is also true
|
||||
bool startup_encoder_offset_calibration = false; //<! run encoder offset calibration after startup, skip otherwise
|
||||
bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
|
||||
bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
|
||||
bool enable_step_dir = false; //<! enable step/dir input after calibration
|
||||
// For M0 this has no effect if enable_uart is true
|
||||
|
||||
float counts_per_step = 2.0f;
|
||||
|
||||
// Spinup settings
|
||||
float ramp_up_time = 0.4f; // [s]
|
||||
float ramp_up_distance = 4 * M_PI; // [rad]
|
||||
float spin_up_current = 10.0f; // [A]
|
||||
float spin_up_acceleration = 400.0f; // [rad/s^2]
|
||||
float spin_up_target_vel = 400.0f; // [rad/s]
|
||||
};
|
||||
|
||||
class Axis {
|
||||
public:
|
||||
enum Error_t {
|
||||
@@ -56,16 +22,51 @@ public:
|
||||
ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
|
||||
};
|
||||
|
||||
// Warning: Do not reorder these enum values.
|
||||
// The state machine uses ">" comparision on them.
|
||||
enum State_t {
|
||||
AXIS_STATE_UNDEFINED = 0, //<! will fall through to idle
|
||||
AXIS_STATE_IDLE = 1, //<! disable PWM and do nothing
|
||||
AXIS_STATE_STARTUP_SEQUENCE = 2, //<! the actual sequence is defined by the config.startup_... flags
|
||||
AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3, //<! run all calibration procedures, then idle
|
||||
AXIS_STATE_MOTOR_CALIBRATION = 4, //<! run motor calibration
|
||||
AXIS_STATE_SENSORLESS_CONTROL = 5, //<! run sensorless control
|
||||
AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
|
||||
AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
|
||||
AXIS_STATE_CLOSED_LOOP_CONTROL = 8 //<! run closed loop control
|
||||
};
|
||||
|
||||
struct Config_t {
|
||||
bool startup_motor_calibration = false; //<! run motor calibration at startup, skip otherwise
|
||||
bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
|
||||
// this only has an effect if encoder.config.use_index is also true
|
||||
bool startup_encoder_offset_calibration = false; //<! run encoder offset calibration after startup, skip otherwise
|
||||
bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
|
||||
bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
|
||||
bool enable_step_dir = false; //<! enable step/dir input after calibration
|
||||
// For M0 this has no effect if enable_uart is true
|
||||
|
||||
float counts_per_step = 2.0f;
|
||||
|
||||
// Spinup settings
|
||||
float ramp_up_time = 0.4f; // [s]
|
||||
float ramp_up_distance = 4 * M_PI; // [rad]
|
||||
float spin_up_current = 10.0f; // [A]
|
||||
float spin_up_acceleration = 400.0f; // [rad/s^2]
|
||||
float spin_up_target_vel = 400.0f; // [rad/s]
|
||||
};
|
||||
|
||||
enum thread_signals {
|
||||
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
|
||||
};
|
||||
|
||||
Axis(const AxisHardwareConfig_t& hw_config,
|
||||
AxisConfig_t& config,
|
||||
Config_t& config,
|
||||
Encoder& encoder,
|
||||
SensorlessEstimator& sensorless_estimator,
|
||||
Controller& controller,
|
||||
Motor& motor);
|
||||
Motor& motor,
|
||||
TrapezoidalTrajectory& trap);
|
||||
|
||||
void setup();
|
||||
void start_thread();
|
||||
@@ -144,12 +145,13 @@ public:
|
||||
void run_state_machine_loop();
|
||||
|
||||
const AxisHardwareConfig_t& hw_config_;
|
||||
AxisConfig_t& config_;
|
||||
Config_t& config_;
|
||||
|
||||
Encoder& encoder_;
|
||||
SensorlessEstimator& sensorless_estimator_;
|
||||
Controller& controller_;
|
||||
Motor& motor_;
|
||||
TrapezoidalTrajectory& trap_;
|
||||
|
||||
osThreadId thread_id_;
|
||||
volatile bool thread_id_valid_ = false;
|
||||
@@ -157,9 +159,9 @@ public:
|
||||
// variables exposed on protocol
|
||||
Error_t error_ = ERROR_NONE;
|
||||
bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir
|
||||
AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
|
||||
AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
|
||||
AxisState_t& current_state_ = task_chain_[0];
|
||||
State_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
|
||||
State_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
|
||||
State_t& current_state_ = task_chain_[0];
|
||||
uint32_t loop_counter_ = 0;
|
||||
|
||||
// Communication protocol definitions
|
||||
@@ -188,7 +190,8 @@ public:
|
||||
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("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()),
|
||||
make_protocol_object("trap_traj", trap_.make_protocol_definitions())
|
||||
);
|
||||
}
|
||||
};
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#include "odrive_main.h"
|
||||
|
||||
|
||||
Controller::Controller(ControllerConfig_t& config) :
|
||||
Controller::Controller(Config_t& config) :
|
||||
config_(config)
|
||||
{}
|
||||
|
||||
@@ -44,6 +44,15 @@ void Controller::set_current_setpoint(float current_setpoint) {
|
||||
#endif
|
||||
}
|
||||
|
||||
void Controller::move_to_pos(float goal_point) {
|
||||
axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_,
|
||||
axis_->trap_.config_.vel_limit,
|
||||
axis_->trap_.config_.accel_limit,
|
||||
axis_->trap_.config_.decel_limit);
|
||||
traj_start_loop_count_ = axis_->loop_counter_;
|
||||
config_.control_mode = CTRL_MODE_TRAJECTORY_CONTROL;
|
||||
}
|
||||
|
||||
void Controller::start_anticogging_calibration() {
|
||||
// Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
|
||||
if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) {
|
||||
@@ -82,7 +91,28 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate)
|
||||
bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) {
|
||||
// Only runs if anticogging_.calib_anticogging is true; non-blocking
|
||||
anticogging_calibration(pos_estimate, vel_estimate);
|
||||
|
||||
float anticogging_pos = pos_estimate;
|
||||
|
||||
// Trajectory control
|
||||
if (config_.control_mode == CTRL_MODE_TRAJECTORY_CONTROL) {
|
||||
// Note: uint32_t loop count delta is OK across overflow
|
||||
// Beware of negative deltas, as they will not be well behaved due to uint!
|
||||
float t = (axis_->loop_counter_ - traj_start_loop_count_) * current_meas_period;
|
||||
if (t > axis_->trap_.Tf_) {
|
||||
// Drop into position control mode when done to avoid problems on loop counter delta overflow
|
||||
config_.control_mode = CTRL_MODE_POSITION_CONTROL;
|
||||
// pos_setpoint already set by trajectory
|
||||
vel_setpoint_ = 0.0f;
|
||||
current_setpoint_ = 0.0f;
|
||||
} else {
|
||||
TrapezoidalTrajectory::Step_t traj_step = axis_->trap_.eval(t);
|
||||
pos_setpoint_ = traj_step.Y;
|
||||
vel_setpoint_ = traj_step.Yd;
|
||||
current_setpoint_ = traj_step.Ydd * axis_->trap_.config_.A_per_css;
|
||||
}
|
||||
anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate
|
||||
}
|
||||
|
||||
// Position control
|
||||
// TODO Decide if we want to use encoder or pll position here
|
||||
float vel_des = vel_setpoint_;
|
||||
@@ -103,7 +133,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
|
||||
// We get the current position and apply a current feed-forward
|
||||
// ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1)
|
||||
if (anticogging_.use_anticogging) {
|
||||
Iq += anticogging_.cogging_map[mod(static_cast<int>(pos_estimate), axis_->encoder_.config_.cpr)];
|
||||
Iq += anticogging_.cogging_map[mod(static_cast<int>(anticogging_pos), axis_->encoder_.config_.cpr)];
|
||||
}
|
||||
|
||||
float v_err = vel_des - vel_estimate;
|
||||
|
||||
@@ -5,32 +5,36 @@
|
||||
#error "This file should not be included directly. Include odrive_main.h instead."
|
||||
#endif
|
||||
|
||||
// Note: these should be sorted from lowest level of control to
|
||||
// highest level of control, to allow "<" style comparisons.
|
||||
typedef enum {
|
||||
CTRL_MODE_VOLTAGE_CONTROL = 0,
|
||||
CTRL_MODE_CURRENT_CONTROL = 1,
|
||||
CTRL_MODE_VELOCITY_CONTROL = 2,
|
||||
CTRL_MODE_POSITION_CONTROL = 3
|
||||
} Motor_control_mode_t;
|
||||
|
||||
struct ControllerConfig_t {
|
||||
Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t
|
||||
float pos_gain = 20.0f; // [(counts/s) / counts]
|
||||
float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)]
|
||||
// float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] <sensorless example>
|
||||
float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)]
|
||||
float vel_limit = 20000.0f; // [counts/s]
|
||||
};
|
||||
|
||||
class Controller {
|
||||
public:
|
||||
Controller(ControllerConfig_t& config);
|
||||
// Note: these should be sorted from lowest level of control to
|
||||
// highest level of control, to allow "<" style comparisons.
|
||||
enum ControlMode_t{
|
||||
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
|
||||
};
|
||||
|
||||
struct Config_t {
|
||||
ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t
|
||||
float pos_gain = 20.0f; // [(counts/s) / counts]
|
||||
float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)]
|
||||
// float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] <sensorless example>
|
||||
float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)]
|
||||
float vel_limit = 20000.0f; // [counts/s]
|
||||
};
|
||||
|
||||
Controller(Config_t& config);
|
||||
void reset();
|
||||
|
||||
void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward);
|
||||
void set_vel_setpoint(float vel_setpoint, float current_feed_forward);
|
||||
void set_current_setpoint(float current_setpoint);
|
||||
|
||||
// Trajectory-Planned control
|
||||
void move_to_pos(float goal_point);
|
||||
|
||||
// TODO: make this more similar to other calibration loops
|
||||
void start_anticogging_calibration();
|
||||
@@ -38,7 +42,7 @@ public:
|
||||
|
||||
bool update(float pos_estimate, float vel_estimate, float* current_setpoint);
|
||||
|
||||
ControllerConfig_t& config_;
|
||||
Config_t& config_;
|
||||
Axis* axis_ = nullptr; // set by Axis constructor
|
||||
|
||||
// TODO: anticogging overhaul:
|
||||
@@ -71,6 +75,8 @@ public:
|
||||
float vel_integrator_current_ = 0.0f; // [A]
|
||||
float current_setpoint_ = 0.0f; // [A]
|
||||
|
||||
uint32_t traj_start_loop_count_ = 0;
|
||||
|
||||
// Communication protocol definitions
|
||||
auto make_protocol_definitions() {
|
||||
return make_protocol_member_list(
|
||||
@@ -91,6 +97,7 @@ 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, "goal_point"),
|
||||
make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration)
|
||||
);
|
||||
}
|
||||
|
||||
@@ -97,9 +97,9 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) {
|
||||
// TODO: Do the scan with current, not voltage!
|
||||
bool Encoder::run_index_search() {
|
||||
float voltage_magnitude;
|
||||
if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT)
|
||||
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_TYPE_GIMBAL)
|
||||
else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL)
|
||||
voltage_magnitude = axis_->motor_.config_.calibration_current;
|
||||
else
|
||||
return false;
|
||||
@@ -144,9 +144,9 @@ bool Encoder::run_offset_calibration() {
|
||||
shadow_count_ = count_in_cpr_;
|
||||
|
||||
float voltage_magnitude;
|
||||
if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT)
|
||||
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_TYPE_GIMBAL)
|
||||
else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL)
|
||||
voltage_magnitude = axis_->motor_.config_.calibration_current;
|
||||
else
|
||||
return false;
|
||||
|
||||
@@ -11,9 +11,10 @@
|
||||
BoardConfig_t board_config;
|
||||
Encoder::Config_t encoder_configs[AXIS_COUNT];
|
||||
SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT];
|
||||
ControllerConfig_t controller_configs[AXIS_COUNT];
|
||||
MotorConfig_t motor_configs[AXIS_COUNT];
|
||||
AxisConfig_t axis_configs[AXIS_COUNT];
|
||||
Controller::Config_t controller_configs[AXIS_COUNT];
|
||||
Motor::Config_t motor_configs[AXIS_COUNT];
|
||||
Axis::Config_t axis_configs[AXIS_COUNT];
|
||||
TrapezoidalTrajectory::Config_t trap_configs[AXIS_COUNT];
|
||||
bool user_config_loaded_;
|
||||
|
||||
SystemStats_t system_stats_ = { 0 };
|
||||
@@ -24,9 +25,10 @@ typedef Config<
|
||||
BoardConfig_t,
|
||||
Encoder::Config_t[AXIS_COUNT],
|
||||
SensorlessEstimator::Config_t[AXIS_COUNT],
|
||||
ControllerConfig_t[AXIS_COUNT],
|
||||
MotorConfig_t[AXIS_COUNT],
|
||||
AxisConfig_t[AXIS_COUNT]> ConfigFormat;
|
||||
Controller::Config_t[AXIS_COUNT],
|
||||
Motor::Config_t[AXIS_COUNT],
|
||||
TrapezoidalTrajectory::Config_t[AXIS_COUNT],
|
||||
Axis::Config_t[AXIS_COUNT]> ConfigFormat;
|
||||
|
||||
void save_configuration(void) {
|
||||
if (ConfigFormat::safe_store_config(
|
||||
@@ -35,6 +37,7 @@ void save_configuration(void) {
|
||||
&sensorless_configs,
|
||||
&controller_configs,
|
||||
&motor_configs,
|
||||
&trap_configs,
|
||||
&axis_configs)) {
|
||||
//printf("saving configuration failed\r\n"); osDelay(5);
|
||||
} else {
|
||||
@@ -51,15 +54,17 @@ void load_configuration(void) {
|
||||
&sensorless_configs,
|
||||
&controller_configs,
|
||||
&motor_configs,
|
||||
&trap_configs,
|
||||
&axis_configs)) {
|
||||
//If loading failed, restore defaults
|
||||
board_config = BoardConfig_t();
|
||||
for (size_t i = 0; i < AXIS_COUNT; ++i) {
|
||||
encoder_configs[i] = Encoder::Config_t();
|
||||
sensorless_configs[i] = SensorlessEstimator::Config_t();
|
||||
controller_configs[i] = ControllerConfig_t();
|
||||
motor_configs[i] = MotorConfig_t();
|
||||
axis_configs[i] = AxisConfig_t();
|
||||
controller_configs[i] = Controller::Config_t();
|
||||
motor_configs[i] = Motor::Config_t();
|
||||
trap_configs[i] = TrapezoidalTrajectory::Config_t();
|
||||
axis_configs[i] = Axis::Config_t();
|
||||
}
|
||||
} else {
|
||||
user_config_loaded_ = true;
|
||||
@@ -162,8 +167,9 @@ int odrive_main(void) {
|
||||
Motor *motor = new Motor(hw_configs[i].motor_config,
|
||||
hw_configs[i].gate_driver_config,
|
||||
motor_configs[i]);
|
||||
TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]);
|
||||
axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i],
|
||||
*encoder, *sensorless_estimator, *controller, *motor);
|
||||
*encoder, *sensorless_estimator, *controller, *motor, *trap);
|
||||
}
|
||||
|
||||
// Start ADC for temperature measurements and user measurements
|
||||
|
||||
@@ -7,7 +7,7 @@
|
||||
|
||||
Motor::Motor(const MotorHardwareConfig_t& hw_config,
|
||||
const GateDriverHardwareConfig_t& gate_driver_config,
|
||||
MotorConfig_t& config) :
|
||||
Config_t& config) :
|
||||
hw_config_(hw_config),
|
||||
gate_driver_config_(gate_driver_config),
|
||||
config_(config),
|
||||
@@ -293,10 +293,11 @@ bool Motor::FOC_voltage(float v_d, float v_q, float phase) {
|
||||
}
|
||||
|
||||
bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
|
||||
Current_control_t* ictrl = ¤t_control_;
|
||||
// Syntactic sugar
|
||||
CurrentControl_t& ictrl = current_control_;
|
||||
|
||||
// For Reporting
|
||||
ictrl->Iq_setpoint = Iq_des;
|
||||
ictrl.Iq_setpoint = Iq_des;
|
||||
|
||||
// Clarke transform
|
||||
float Ialpha = -current_meas_.phB - current_meas_.phC;
|
||||
@@ -307,7 +308,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
|
||||
float s = arm_sin_f32(phase);
|
||||
float Id = c * Ialpha + s * Ibeta;
|
||||
float Iq = c * Ibeta - s * Ialpha;
|
||||
ictrl->Iq_measured = Iq;
|
||||
ictrl.Iq_measured = Iq;
|
||||
|
||||
// Current error
|
||||
float Ierr_d = Id_des - Id;
|
||||
@@ -315,8 +316,8 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
|
||||
|
||||
// TODO look into feed forward terms (esp omega, since PI pole maps to RL tau)
|
||||
// Apply PI control
|
||||
float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain;
|
||||
float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain;
|
||||
float Vd = ictrl.v_current_control_integral_d + Ierr_d * ictrl.p_gain;
|
||||
float Vq = ictrl.v_current_control_integral_q + Ierr_q * ictrl.p_gain;
|
||||
|
||||
float mod_to_V = (2.0f / 3.0f) * vbus_voltage;
|
||||
float V_to_mod = 1.0f / mod_to_V;
|
||||
@@ -330,23 +331,23 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
|
||||
mod_d *= mod_scalefactor;
|
||||
mod_q *= mod_scalefactor;
|
||||
// TODO make decayfactor configurable
|
||||
ictrl->v_current_control_integral_d *= 0.99f;
|
||||
ictrl->v_current_control_integral_q *= 0.99f;
|
||||
ictrl.v_current_control_integral_d *= 0.99f;
|
||||
ictrl.v_current_control_integral_q *= 0.99f;
|
||||
} else {
|
||||
ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period);
|
||||
ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period);
|
||||
ictrl.v_current_control_integral_d += Ierr_d * (ictrl.i_gain * current_meas_period);
|
||||
ictrl.v_current_control_integral_q += Ierr_q * (ictrl.i_gain * current_meas_period);
|
||||
}
|
||||
|
||||
// Compute estimated bus current
|
||||
ictrl->Ibus = mod_d * Id + mod_q * Iq;
|
||||
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;
|
||||
|
||||
// Report final applied voltage in stationary frame (for sensorles estimator)
|
||||
ictrl->final_v_alpha = mod_to_V * mod_alpha;
|
||||
ictrl->final_v_beta = mod_to_V * mod_beta;
|
||||
ictrl.final_v_alpha = mod_to_V * mod_alpha;
|
||||
ictrl.final_v_beta = mod_to_V * mod_beta;
|
||||
|
||||
// Apply SVM
|
||||
if (!enqueue_modulation_timings(mod_alpha, mod_beta))
|
||||
|
||||
@@ -7,51 +7,6 @@
|
||||
|
||||
#include "drv8301.h"
|
||||
|
||||
typedef enum {
|
||||
MOTOR_TYPE_HIGH_CURRENT = 0,
|
||||
// MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented
|
||||
MOTOR_TYPE_GIMBAL = 2
|
||||
} Motor_type_t;
|
||||
|
||||
typedef struct {
|
||||
float phB;
|
||||
float phC;
|
||||
} Iph_BC_t;
|
||||
|
||||
typedef struct {
|
||||
float p_gain; // [V/A]
|
||||
float i_gain; // [V/As]
|
||||
float v_current_control_integral_d; // [V]
|
||||
float v_current_control_integral_q; // [V]
|
||||
float Ibus; // DC bus current [A]
|
||||
// Voltage applied at end of cycle:
|
||||
float final_v_alpha; // [V]
|
||||
float final_v_beta; // [V]
|
||||
float Iq_setpoint;
|
||||
float Iq_measured;
|
||||
float max_allowed_current;
|
||||
} Current_control_t;
|
||||
|
||||
// NOTE: for gimbal motors, all units of A are instead V.
|
||||
// example: vel_gain is [V/(count/s)] instead of [A/(count/s)]
|
||||
// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor.
|
||||
typedef struct {
|
||||
bool pre_calibrated = false; // can be set to true to indicate that all values here are valid
|
||||
int32_t pole_pairs = 7;
|
||||
float calibration_current = 10.0f; // [A]
|
||||
float resistance_calib_max_voltage = 1.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
|
||||
Motor_type_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]
|
||||
float current_lim = 10.0f; //[A]
|
||||
// Value used to compute shunt amplifier gains
|
||||
float requested_current_range = 70.0f; // [A]
|
||||
float current_control_bandwidth = 1000.0f; // [rad/s]
|
||||
} MotorConfig_t;
|
||||
|
||||
class Motor {
|
||||
public:
|
||||
enum Error_t {
|
||||
@@ -68,6 +23,51 @@ public:
|
||||
ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200
|
||||
};
|
||||
|
||||
enum MotorType_t {
|
||||
MOTOR_TYPE_HIGH_CURRENT = 0,
|
||||
// MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented
|
||||
MOTOR_TYPE_GIMBAL = 2
|
||||
};
|
||||
|
||||
struct Iph_BC_t {
|
||||
float phB;
|
||||
float phC;
|
||||
};
|
||||
|
||||
struct CurrentControl_t{
|
||||
float p_gain; // [V/A]
|
||||
float i_gain; // [V/As]
|
||||
float v_current_control_integral_d; // [V]
|
||||
float v_current_control_integral_q; // [V]
|
||||
float Ibus; // DC bus current [A]
|
||||
// Voltage applied at end of cycle:
|
||||
float final_v_alpha; // [V]
|
||||
float final_v_beta; // [V]
|
||||
float Iq_setpoint;
|
||||
float Iq_measured;
|
||||
float max_allowed_current;
|
||||
};
|
||||
|
||||
// NOTE: for gimbal motors, all units of A are instead V.
|
||||
// example: vel_gain is [V/(count/s)] instead of [A/(count/s)]
|
||||
// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor.
|
||||
struct Config_t {
|
||||
bool pre_calibrated = false; // can be set to true to indicate that all values here are valid
|
||||
int32_t pole_pairs = 7;
|
||||
float calibration_current = 10.0f; // [A]
|
||||
float resistance_calib_max_voltage = 1.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
|
||||
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]
|
||||
float current_lim = 10.0f; //[A]
|
||||
// Value used to compute shunt amplifier gains
|
||||
float requested_current_range = 70.0f; // [A]
|
||||
float current_control_bandwidth = 1000.0f; // [rad/s]
|
||||
};
|
||||
|
||||
enum TimingLog_t {
|
||||
TIMING_LOG_GENERAL,
|
||||
TIMING_LOG_ADC_CB_I,
|
||||
@@ -90,7 +90,7 @@ public:
|
||||
|
||||
Motor(const MotorHardwareConfig_t& hw_config,
|
||||
const GateDriverHardwareConfig_t& gate_driver_config,
|
||||
MotorConfig_t& config);
|
||||
Config_t& config);
|
||||
|
||||
bool arm();
|
||||
void disarm();
|
||||
@@ -117,7 +117,7 @@ public:
|
||||
|
||||
const MotorHardwareConfig_t& hw_config_;
|
||||
const GateDriverHardwareConfig_t gate_driver_config_;
|
||||
MotorConfig_t& config_;
|
||||
Config_t& config_;
|
||||
Axis* axis_ = nullptr; // set by Axis constructor
|
||||
|
||||
//private:
|
||||
@@ -142,7 +142,7 @@ public:
|
||||
Iph_BC_t current_meas_ = {0.0f, 0.0f};
|
||||
Iph_BC_t DC_calib_ = {0.0f, 0.0f};
|
||||
float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup)
|
||||
Current_control_t current_control_ = {
|
||||
CurrentControl_t current_control_ = {
|
||||
.p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement
|
||||
.i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement
|
||||
.v_current_control_integral_d = 0.0f,
|
||||
|
||||
@@ -109,6 +109,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_c
|
||||
#include <sensorless_estimator.hpp>
|
||||
#include <controller.hpp>
|
||||
#include <motor.hpp>
|
||||
#include <trapTraj.hpp>
|
||||
#include <axis.hpp>
|
||||
#include <communication/communication.h>
|
||||
|
||||
|
||||
@@ -0,0 +1,94 @@
|
||||
#include <math.h>
|
||||
#include "odrive_main.h"
|
||||
#include "utils.h"
|
||||
|
||||
// A sign function where input 0 has positive sign (not 0)
|
||||
float sign_hard(float val) {
|
||||
return (std::signbit(val)) ? -1.0f : 1.0f;
|
||||
}
|
||||
|
||||
// Symbol Description
|
||||
// Ta, Tv and Td Duration of the stages of the AL profile
|
||||
// Xi and Vi Adapted initial conditions for the AL profile
|
||||
// Xf Position set-point
|
||||
// s Direction (sign) of the trajectory
|
||||
// Vmax, Amax, Dmax and jmax Kinematic bounds
|
||||
// Ar, Dr and Vr Reached values of acceleration and velocity
|
||||
|
||||
TrapezoidalTrajectory::TrapezoidalTrajectory(Config_t& config) : config_(config) {}
|
||||
|
||||
bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
|
||||
float Vmax, float Amax, float Dmax) {
|
||||
float dX = Xf - Xi; // Distance to travel
|
||||
float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance
|
||||
float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement
|
||||
float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any)
|
||||
Ar_ = s * Amax; // Maximum Acceleration (signed)
|
||||
Dr_ = -s * Dmax; // Maximum Deceleration (signed)
|
||||
Vr_ = s * Vmax; // Maximum Velocity (signed)
|
||||
|
||||
// If we start with a speed faster than cruising, then we need to decel instead of accel
|
||||
// aka "double deceleration move" in the paper
|
||||
if ((s * Vi) > (s * Vr_)) {
|
||||
Ar_ = -s * Amax;
|
||||
}
|
||||
|
||||
// Time to accel/decel to/from Vr (cruise speed)
|
||||
Ta_ = (Vr_ - Vi) / Ar_;
|
||||
Td_ = -Vr_ / Dr_;
|
||||
|
||||
// Integral of velocity ramps over the full accel and decel times to get
|
||||
// minimum displacement required to reach cuising speed
|
||||
float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;
|
||||
|
||||
// Are we displacing enough to reach cruising speed?
|
||||
if (s*dX < s*dXmin) {
|
||||
// Short move (triangle profile)
|
||||
Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
|
||||
Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
|
||||
Td_ = std::max(0.0f, -Vr_ / Dr_);
|
||||
Tv_ = 0.0f;
|
||||
} else {
|
||||
// Long move (trapezoidal profile)
|
||||
Tv_ = (dX - dXmin) / Vr_;
|
||||
}
|
||||
|
||||
// Fill in the rest of the values used at evaluation-time
|
||||
Tf_ = Ta_ + Tv_ + Td_;
|
||||
Xi_ = Xi;
|
||||
Xf_ = Xf;
|
||||
Vi_ = Vi;
|
||||
yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phase
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) {
|
||||
Step_t trajStep;
|
||||
if (t < 0.0f) { // Initial Condition
|
||||
trajStep.Y = Xi_;
|
||||
trajStep.Yd = Vi_;
|
||||
trajStep.Ydd = 0.0f;
|
||||
} else if (t < Ta_) { // Accelerating
|
||||
trajStep.Y = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t);
|
||||
trajStep.Yd = Vi_ + Ar_*t;
|
||||
trajStep.Ydd = Ar_;
|
||||
} else if (t < Ta_ + Tv_) { // Coasting
|
||||
trajStep.Y = yAccel_ + Vr_*(t - Ta_);
|
||||
trajStep.Yd = Vr_;
|
||||
trajStep.Ydd = 0.0f;
|
||||
} else if (t < Tf_) { // Deceleration
|
||||
float td = t - Tf_;
|
||||
trajStep.Y = Xf_ + 0.5f*Dr_*SQ(td);
|
||||
trajStep.Yd = Dr_*td;
|
||||
trajStep.Ydd = Dr_;
|
||||
} else if (t >= Tf_) { // Final Condition
|
||||
trajStep.Y = Xf_;
|
||||
trajStep.Yd = 0.0f;
|
||||
trajStep.Ydd = 0.0f;
|
||||
} else {
|
||||
// TODO: report error here
|
||||
}
|
||||
|
||||
return trajStep;
|
||||
}
|
||||
@@ -0,0 +1,53 @@
|
||||
#ifndef _TRAP_TRAJ_H
|
||||
#define _TRAP_TRAJ_H
|
||||
|
||||
class TrapezoidalTrajectory {
|
||||
public:
|
||||
struct Config_t {
|
||||
float vel_limit = 20000.0f; // [count/s]
|
||||
float accel_limit = 5000.0f; // [count/s^2]
|
||||
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);
|
||||
bool planTrapezoidal(float Xf, float Xi, float Vi,
|
||||
float Vmax, float Amax, float Dmax);
|
||||
Step_t eval(float t);
|
||||
|
||||
auto make_protocol_definitions() {
|
||||
return make_protocol_member_list(
|
||||
make_protocol_object("config",
|
||||
make_protocol_property("vel_limit", &config_.vel_limit),
|
||||
make_protocol_property("accel_limit", &config_.accel_limit),
|
||||
make_protocol_property("decel_limit", &config_.decel_limit),
|
||||
make_protocol_property("A_per_css", &config_.A_per_css)
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
Axis* axis_ = nullptr; // set by Axis constructor
|
||||
Config_t& config_;
|
||||
|
||||
float Xi_;
|
||||
float Xf_;
|
||||
float Vi_;
|
||||
|
||||
float Ar_;
|
||||
float Vr_;
|
||||
float Dr_;
|
||||
|
||||
float Ta_;
|
||||
float Tv_;
|
||||
float Td_;
|
||||
float Tf_;
|
||||
|
||||
float yAccel_;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -63,6 +63,8 @@ extern "C" {
|
||||
#define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y))
|
||||
#define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y))
|
||||
|
||||
#define SQ(x) ((x) * (x))
|
||||
|
||||
static const float one_by_sqrt3 = 0.57735026919f;
|
||||
static const float two_by_sqrt3 = 1.15470053838f;
|
||||
static const float sqrt3_by_2 = 0.86602540378f;
|
||||
|
||||
@@ -89,7 +89,7 @@ FLAGS += '-mthumb'
|
||||
FLAGS += '-mcpu=cortex-m4'
|
||||
FLAGS += '-mfpu=fpv4-sp-d16'
|
||||
FLAGS += '-mfloat-abi=hard'
|
||||
FLAGS += { '-Wall', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'}
|
||||
FLAGS += { '-Wall', '-Wdouble-promotion', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'}
|
||||
|
||||
-- debug build
|
||||
FLAGS += '-g -gdwarf-2'
|
||||
@@ -155,6 +155,7 @@ build{
|
||||
'MotorControl/encoder.cpp',
|
||||
'MotorControl/controller.cpp',
|
||||
'MotorControl/sensorless_estimator.cpp',
|
||||
'MotorControl/trapTraj.cpp',
|
||||
'MotorControl/main.cpp',
|
||||
'communication/communication.cpp',
|
||||
'communication/ascii_protocol.cpp',
|
||||
|
||||
@@ -128,6 +128,18 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
|
||||
axes[motor_number]->controller_.set_current_setpoint(current_setpoint);
|
||||
}
|
||||
|
||||
} else if (cmd[0] == 't') { // trapezoidal trajectory
|
||||
unsigned motor_number;
|
||||
float goal_point;
|
||||
int numscan = sscanf(cmd, "t %u %f", &motor_number, &goal_point);
|
||||
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 {
|
||||
axes[motor_number]->controller_.move_to_pos(goal_point);
|
||||
}
|
||||
|
||||
} else if (cmd[0] == 'h') { // Help
|
||||
respond(response_channel, use_checksum, "Please see documentation for more details");
|
||||
respond(response_channel, use_checksum, "");
|
||||
|
||||
@@ -592,10 +592,10 @@ private:
|
||||
template<typename T>
|
||||
struct format_traits_t;
|
||||
|
||||
template<> struct format_traits_t<float> { using type = void;
|
||||
static constexpr const char * fmt = "%f";
|
||||
static constexpr const char * fmtp = "%f";
|
||||
};
|
||||
// template<> struct format_traits_t<float> { using type = void;
|
||||
// static constexpr const char * fmt = "%f";
|
||||
// static constexpr const char * fmtp = "%f";
|
||||
// };
|
||||
template<> struct format_traits_t<int32_t> { using type = void;
|
||||
static constexpr const char * fmt = "%ld";
|
||||
static constexpr const char * fmtp = "%ld";
|
||||
@@ -626,8 +626,15 @@ static bool to_string(const T& value, char * buffer, size_t length, int) {
|
||||
snprintf(buffer, length, format_traits_t<T>::fmtp, value);
|
||||
return true;
|
||||
}
|
||||
// Special case for float because printf promotes float to double, and we get warnings
|
||||
template<typename T>
|
||||
static bool to_string(const float& value, char * buffer, size_t length, int) {
|
||||
snprintf(buffer, length, "%f", (double)value);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
//__attribute__((__unused__))
|
||||
static bool to_string(const bool& value, char * buffer, size_t length, int) {
|
||||
buffer[0] = value ? '1' : '0';
|
||||
buffer[1] = 0;
|
||||
@@ -642,7 +649,11 @@ template<typename T, typename = typename format_traits_t<T>::type>
|
||||
static bool from_string(const char * buffer, size_t length, T* property, int) {
|
||||
return sscanf(buffer, format_traits_t<T>::fmt, property) == 1;
|
||||
}
|
||||
//__attribute__((__unused__))
|
||||
// Special case for float because printf promotes float to double, and we get warnings
|
||||
template<typename T>
|
||||
static bool from_string(const char * buffer, size_t length, float* property, int) {
|
||||
return sscanf(buffer, "%f", property) == 1;
|
||||
}
|
||||
template<typename T>
|
||||
static bool from_string(const char * buffer, size_t length, bool* property, int) {
|
||||
int val;
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
|
||||
from .discovery import find_any, find_all
|
||||
from .utils import Event, Logger
|
||||
from .utils import Event, Logger, TimeoutError
|
||||
from .protocol import ChannelBrokenException, ChannelDamagedException
|
||||
from .shell import launch_shell
|
||||
|
||||
@@ -11,7 +11,7 @@ import fibre.protocol
|
||||
import fibre.utils
|
||||
import fibre.remote_object
|
||||
from fibre.utils import Event, Logger
|
||||
from fibre.protocol import ChannelBrokenException
|
||||
from fibre.protocol import ChannelBrokenException, TimeoutError
|
||||
|
||||
# Load all installed transport layers
|
||||
|
||||
@@ -20,25 +20,25 @@ channel_types = {}
|
||||
try:
|
||||
import fibre.usbbulk_transport
|
||||
channel_types['usb'] = fibre.usbbulk_transport.discover_channels
|
||||
except ModuleNotFoundError:
|
||||
except ImportError:
|
||||
pass
|
||||
|
||||
try:
|
||||
import fibre.serial_transport
|
||||
channel_types['serial'] = fibre.serial_transport.discover_channels
|
||||
except ModuleNotFoundError:
|
||||
except ImportError:
|
||||
pass
|
||||
|
||||
try:
|
||||
import fibre.tcp_transport
|
||||
channel_types['tcp'] = fibre.tcp_transport.discover_channels
|
||||
except ModuleNotFoundError:
|
||||
except ImportError:
|
||||
pass
|
||||
|
||||
try:
|
||||
import fibre.udp_transport
|
||||
channel_types['udp'] = fibre.udp_transport.discover_channels
|
||||
except ModuleNotFoundError:
|
||||
except ImportError:
|
||||
pass
|
||||
|
||||
def noprint(text):
|
||||
@@ -102,9 +102,10 @@ def find_all(path, serial_number,
|
||||
prefix = search_spec.split(':')[0]
|
||||
the_rest = ':'.join(search_spec.split(':')[1:])
|
||||
if prefix in channel_types:
|
||||
threading.Thread(target=channel_types[prefix],
|
||||
args=(the_rest, serial_number, did_discover_channel, search_cancellation_token, channel_termination_token, logger),
|
||||
daemon=True).start()
|
||||
t = threading.Thread(target=channel_types[prefix],
|
||||
args=(the_rest, serial_number, did_discover_channel, search_cancellation_token, channel_termination_token, logger))
|
||||
t.daemon = True
|
||||
t.start()
|
||||
else:
|
||||
raise Exception("Invalid path spec \"{}\"".format(search_spec))
|
||||
|
||||
|
||||
@@ -6,7 +6,7 @@ import sys
|
||||
import threading
|
||||
import traceback
|
||||
#import fibre.utils
|
||||
from fibre.utils import Event, wait_any
|
||||
from fibre.utils import Event, wait_any, TimeoutError
|
||||
|
||||
import abc
|
||||
if sys.version_info >= (3, 4):
|
||||
@@ -65,7 +65,6 @@ def calc_crc16(remainder, value):
|
||||
#print(hex(calc_crc8(0x12, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37])))
|
||||
#print(hex(calc_crc16(0xfeef, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37])))
|
||||
|
||||
|
||||
class DeviceInitException(Exception):
|
||||
pass
|
||||
|
||||
@@ -256,7 +255,9 @@ class Channel(PacketSink):
|
||||
self._logger.debug("receiver thread is exiting: " + traceback.format_exc())
|
||||
finally:
|
||||
self._channel_broken.set()
|
||||
threading.Thread(target=receiver_thread, daemon=True).start()
|
||||
t = threading.Thread(target=receiver_thread)
|
||||
t.daemon = True
|
||||
t.start()
|
||||
|
||||
def remote_endpoint_operation(self, endpoint_id, input, expect_ack, output_length):
|
||||
if input is None:
|
||||
|
||||
@@ -10,6 +10,7 @@ import traceback
|
||||
import serial
|
||||
import serial.tools.list_ports
|
||||
import fibre
|
||||
from fibre.utils import TimeoutError
|
||||
|
||||
# TODO: make this customizable
|
||||
DEFAULT_BAUDRATE = 115200
|
||||
|
||||
@@ -4,7 +4,7 @@ import socket
|
||||
import time
|
||||
import traceback
|
||||
import fibre.protocol
|
||||
from fibre.utils import wait_any
|
||||
from fibre.utils import wait_any, TimeoutError
|
||||
|
||||
def noprint(x):
|
||||
pass
|
||||
|
||||
@@ -8,6 +8,7 @@ import time
|
||||
import fibre.protocol
|
||||
import traceback
|
||||
import platform
|
||||
from fibre.utils import TimeoutError
|
||||
|
||||
# Currently we identify fibre-enabled devices by VID,PID
|
||||
# TODO: identify by USB descriptors
|
||||
|
||||
@@ -12,10 +12,16 @@ try:
|
||||
# TODO: we should win32console anyway so we could just omit colorama
|
||||
import colorama
|
||||
colorama.init()
|
||||
except ModuleNotFoundError:
|
||||
except ImportError:
|
||||
print("Could not init terminal features.")
|
||||
sys.stdout.flush()
|
||||
pass
|
||||
|
||||
if sys.version_info < (3, 3):
|
||||
class TimeoutError(Exception):
|
||||
pass
|
||||
else:
|
||||
TimeoutError = TimeoutError
|
||||
|
||||
def get_serial_number_str(device):
|
||||
if hasattr(device, 'serial_number'):
|
||||
@@ -92,7 +98,10 @@ class Event():
|
||||
def delayed_trigger():
|
||||
if not self.wait(timeout=timeout):
|
||||
self.set()
|
||||
threading.Thread(target=delayed_trigger, daemon=True).start()
|
||||
threading.Thread(target=delayed_trigger)
|
||||
t.daemon = True
|
||||
t.start()
|
||||
|
||||
|
||||
def wait_any(timeout=None, *events):
|
||||
"""
|
||||
|
||||
@@ -24,6 +24,18 @@ command *42 ; comment [new line character]
|
||||
|
||||
## Command Reference
|
||||
|
||||
#### Motor trajectory command
|
||||
```
|
||||
t motor destination
|
||||
```
|
||||
* `t` for trajectory
|
||||
* `motor` is the motor number, `0` or `1`.
|
||||
* `destination` is the goal position, in encoder counts.
|
||||
|
||||
Example: `t 0 -20000`
|
||||
|
||||
For general moving around of the axis, this is the recommended command.
|
||||
|
||||
#### Motor Position command
|
||||
```
|
||||
p motor position velocity_ff current_ff
|
||||
|
||||
@@ -16,6 +16,7 @@ Before doing the VSCode setup, make sure you've installed all of your [prerequis
|
||||
* Include Autocomplete
|
||||
* Path Autocomplete
|
||||
* Auto Comment Blocks
|
||||
1. Create an environment variable named `ARM_GCC_ROOT` whose value is the location of the `GNU Arm Embedded Toolchain` (.e.g `C:\Program Files (x86)\GNU Tools Arm Embedded\7 2018-q2-update`) that you installed in the prerequisites section of the developer's guide.
|
||||
1. Restart VSCode
|
||||
1. Open the VSCode Workspace file, which is located in the root of the ODrive repository. It is called `ODrive_Workspace.code-workspace`. The first time you open it, VSCode will install some dependencies. If it fails, you may need to [change your proxy settings](https://code.visualstudio.com/docs/getstarted/settings).
|
||||
|
||||
|
||||
@@ -32,7 +32,7 @@ The recommended tools for ODrive development are:
|
||||
|
||||
* **make**: Used to invoke tup
|
||||
* **Tup**: The build system used to invoke the compile commands
|
||||
* **ARM GNU Compiler**: For cross-comiling code
|
||||
* **ARM GNU Compiler**: For cross-compiling code
|
||||
* **ARM GDB**: For debugging the code and stepping through on the device
|
||||
* **OpenOCD**: For flashing the ODrive with the STLink/v2 programmer
|
||||
* **Python**: For running the Python tools
|
||||
@@ -80,7 +80,7 @@ __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)
|
||||
* [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.
|
||||
* [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.
|
||||
|
||||
+72
-51
@@ -12,6 +12,7 @@ permalink: /
|
||||
- [Hardware Requirements](#hardware-requirements)
|
||||
- [Wiring up the ODrive](#wiring-up-the-odrive)
|
||||
- [Downloading and Installing Tools](#downloading-and-installing-tools)
|
||||
- [Firmware](#firmware)
|
||||
- [Start `odrivetool`](#start-odrivetool)
|
||||
- [Configure M0](#configure-m0)
|
||||
- [Position control of M0](#position-control-of-m0)
|
||||
@@ -21,8 +22,7 @@ permalink: /
|
||||
|
||||
## Hardware Requirements
|
||||
|
||||
You will need:
|
||||
|
||||
### You will need:
|
||||
* One or two [brushless motors](https://docs.google.com/spreadsheets/d/12vzz7XVEK6YNIOqH0jAz51F5VUpc-lJEs3mmkWP1H4Y). It is fine, even recommended, to start testing with just a single motor and encoder.
|
||||
* One or two [quadrature incremental encoder(s)](encoders)
|
||||
* A power resistor. A good starting point would be the 50W resistor included with your ODrive.
|
||||
@@ -47,25 +47,26 @@ You will need:
|
||||
</div></details>
|
||||
|
||||
## Wiring up the ODrive
|
||||
|
||||
<div class="alert">
|
||||
Make sure you have a good mechanical connection between the encoder and the motor, slip can cause disasterous oscillations or runaway.
|
||||
Make sure you have a good mechanical connection between the encoder and the motor, slip can cause disastrous oscillations or runaway.
|
||||
</div>
|
||||
|
||||
All non-power I/O is 3.3V output and 5V tolerant on input, on ODrive v3.3 and newer.
|
||||
|
||||
1. Wire up the motor phases into the 3-phase screw terminals, and the power resistor to the AUX terminal. Wire up the power source to the DC terminal, make sure to pay attention to the polarity. Do not apply power just yet.
|
||||
### 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.
|
||||
|
||||
2. Wire up 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.
|
||||
### 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.
|
||||
|
||||
|
||||
|
||||
## Downloading and Installing Tools
|
||||
|
||||
Most instructions in this guide refer to a utility called `odrivetool`, so you should install that first.
|
||||
|
||||
### Windows
|
||||
|
||||
1. Install Python 3. We recommend the Anaconda distribution because it packs a lot of useful scientific tools, however you can also install the standalone python.
|
||||
* __Anaconda__: Download the installer from [here](https://www.anaconda.com/download/#windows). Execute the downloaded file and follow the instructions.
|
||||
* __Standalone Python__: Download the installer from [here](https://www.python.org/downloads/). Execute the downloaded file and follow the instructions.
|
||||
@@ -117,23 +118,24 @@ Try step 5 again
|
||||
|
||||
|
||||
### Linux
|
||||
|
||||
1. [Install Python 3](https://www.python.org/downloads/).
|
||||
2. Install the ODrive tools by opening a terminal and typing `pip install odrive` <kbd>Enter</kbd>
|
||||
3. __Linux__: set up USB permissions
|
||||
3. Set up USB permissions
|
||||
```bash
|
||||
echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d[0-9][0-9]", MODE="0666"' | sudo tee /etc/udev/rules.d/91-odrive.rules
|
||||
sudo udevadm control --reload-rules
|
||||
sudo udevadm trigger
|
||||
```
|
||||
|
||||
## Firmware
|
||||
#### ODrive v3.5 and later
|
||||
Your board should come preflashed with firmware. If you run into problems, follow the instructions [here](odrivetool.md#device-firmware-update) on the DFU procedure before you continue.
|
||||
|
||||
#### ODrive v3.4 and earlier
|
||||
Your board does **not** come preflashed with any firmware. Follow the instructions [here](odrivetool.md#device-firmware-update) on the STP Link procedure before you continue.
|
||||
|
||||
## Start `odrivetool`
|
||||
|
||||
<div class="note" markdown="span">__ODrive v3.5 and later:__ Your board should come preflashed with firmware. If you run into problems, follow the instructions [here](odrivetool.md#device-firmware-update) on the DFU procedure before you continue.</div>
|
||||
|
||||
<div class="note" markdown="span">__ODrive v3.4 and earlier:__ Your board does __not__ come preflashed with any firmware. Follow the instructions [here](odrivetool.md#device-firmware-update) on the STP Link procedure before you continue.</div>
|
||||
|
||||
To launch the main interactive ODrive tool, type `odrivetool` <kbd>Enter</kbd>. Connect your ODrive and wait for the tool to find it. Now you can for instance type `odrv0.vbus_voltage` <kbd>Enter</kbd> to inpect the boards main supply voltage.
|
||||
To launch the main interactive ODrive tool, type `odrivetool` <kbd>Enter</kbd>. Connect your ODrive and wait for the tool to find it. Now you can, for instance type `odrv0.vbus_voltage` <kbd>Enter</kbd> to inpect the boards main supply voltage.
|
||||
It should look something like this:
|
||||
|
||||
```text
|
||||
@@ -148,63 +150,77 @@ Out[1]: 11.97055721282959
|
||||
|
||||
The tool you're looking at is a fully capable Python command prompt, so you can type any valid python code.
|
||||
|
||||
You can read more about the odrivetool [here](odrivetool.md).
|
||||
You can read more about `odrivetool` [here](odrivetool.md).
|
||||
|
||||
## Configure M0
|
||||
|
||||
<div class="alert" markdown="span">Read this section carefully, else you risk breaking something.</div>
|
||||
<div class="note" markdown="span">There is a [separate guide](hoverboard.md) specifically for hoverboard motors.</div>
|
||||
|
||||
### 1. Set the limits:
|
||||
<details><summary markdown="span">Wait, how do I set these?</summary><div markdown="block">
|
||||
|
||||
In the previous step we started `odrivetool`. In there, you can assign variables directly by name.
|
||||
|
||||
For instance, to set the current limit of M0 to 10A you would type: `odrv0.axis0.motor.config.current_lim = 10` <kbd>Enter</kbd>
|
||||
</div></details>
|
||||
|
||||
* The current limit: `odrv0.axis0.motor.config.current_lim` [A]. The default current limit, for safety reasons, is set to 10A. This is quite weak, and good for making sure the drive is stable. Once you have tuned the drive, you can increase this to 75A to get some performance. Note that above 75A, you must change the current amplifier gains. You do this by requesting a different current range. i.e. for 90A on M0: 'odrv0.axis0.motor.config.requested_current_range = 90' [A], then save the configeration and reboot as the gains are written out to the DRV (MOSFET driver) only during startup.
|
||||
* Note: The motor current and the current drawn from the power supply is not the same in general. You should not look at the power supply current to see what is going on with the motor current.
|
||||
<details><summary markdown="span">Ok so tell me how it actually works then...</summary><div markdown="block">
|
||||
The current in the motor is only connected to the current in the power supply _sometimes_ and other times it just cycles out of one phase and back in the other. This is what the modulation magnitude is (sometimes people call this duty cycle, but that's a bit confusing because we use SVM not straight PWM). When the modulation magnitude is 0, the average voltage seen across the motor phases is 0, and the motor current is never connected to the power supply. When the magnitude is 100%, it is always connected, and at 50% it's connected half the time, and cycled in just the motor half the time.
|
||||
#### Current limit
|
||||
`odrv0.axis0.motor.config.current_lim` [A].
|
||||
The default current limit, for safety reasons, is set to 10A. This is quite weak, but good for making sure the drive is stable. Once you have tuned the oDrive, you can increase this to 75A to increase performance. Note that above 75A, you must change the current amplifier gains. You do this by requesting a different current range. i.e. for 90A on M0: `odrv0.axis0.motor.config.requested_current_range = 90` [A], then save the configuration and reboot as the gains are written out to the DRV (MOSFET driver) only during startup.
|
||||
|
||||
The largest effect on modulation magnitude is speed. There are other smaller factors, but in general: if the motor is still it's not unreasonable to have 50A in the motor from 5A on the power supply. When the motor is spinning close to top speed, the power supply current and the motor current will be somewhat close to each other.
|
||||
*Note: The motor current and the current drawn from the power supply is not the same in general. You should not look at the power supply current to see what is going on with the motor current.*
|
||||
|
||||
<details><summary markdown="span">Ok, so tell me how it actually works then...</summary><div markdown="block">
|
||||
The current in the motor is only connected to the current in the power supply _sometimes_ and other times it just cycles out of one phase and back in the other. This is what the modulation magnitude is (sometimes people call this duty cycle, but that's a bit confusing because we use SVM not straight PWM). When the modulation magnitude is 0, the average voltage seen across the motor phases is 0, and the motor current is never connected to the power supply. When the magnitude is 100%, it is always connected, and at 50% it's connected half the time, and cycled in just the motor half the time.
|
||||
|
||||
The largest effect on modulation magnitude is speed. There are other smaller factors, but in general: if the motor is still it's not unreasonable to have 50A in the motor from 5A on the power supply. When the motor is spinning close to top speed, the power supply current and the motor current will be somewhat close to each other.
|
||||
</div></details>
|
||||
* The velocity limit: `odrv0.axis0.controller.config.vel_limit` [counts/s]. The motor will be limited to this speed; again the default value is quite slow.
|
||||
* You can change `odrv0.axis0.motor.config.calibration_current` [A] to the largest value you feel comfortable leaving running through the motor continously when the motor is stationary. If you are using a small motor (i.e. 15A current rated) you may need to reduce `calibration_current` to a value smaller than the default.
|
||||
#### Velocity limit
|
||||
`odrv0.axis0.controller.config.vel_limit` [counts/s].
|
||||
The motor will be limited to this speed. Again the default value is quite slow.
|
||||
#### Calibration current
|
||||
You can change `odrv0.axis0.motor.config.calibration_current` [A] to the largest value you feel comfortable leaving running through the motor continuously when the motor is stationary. If you are using a small motor (i.e. 15A current rated) you may need to reduce `calibration_current` to a value smaller than the default.
|
||||
|
||||
### 2. Set other hardware parameters:
|
||||
### 2. Set other hardware parameters
|
||||
`odrv0.config.brake_resistance` [Ohm]
|
||||
This is the resistance of the brake resistor. If you are not using it, you may set it to `0`. Note that there may be some extra resistance in your wiring and in the screw terminals, so if you are getting issues while braking you may want to increase this parameter by around 0.05 ohm.
|
||||
|
||||
`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`).
|
||||
|
||||
* `odrv0.config.brake_resistance` [Ohm]: This is the resistance of the brake resistor. If you are not using it, you may set it to `0`. Note that there may be some extra resistance in your wiring and in the screw terminals, so if you are getting issues while braking you may want to increase this parameter by around 0.05 ohm.
|
||||
* `odrv0.axis0.motor.config.pole_pairs`: This is the number of **magnet poles** in the rotor, **divided by two**. 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._
|
||||
* `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`).
|
||||
<details><summary markdown="span">Which <code>motor_type</code> to choose?</summary><div markdown="block">
|
||||
|
||||
<details><summary markdown="span">Which `motor_type` to choose?</summary><div markdown="block">
|
||||
If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For low-current gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. Do not use `MOTOR_TYPE_GIMBAL` on a motor that is not a gimbal motor, as it may overheat the motor or the ODrive.
|
||||
If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For low-current gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. Do not use `MOTOR_TYPE_GIMBAL` on a motor that is not a gimbal motor, as it may overheat the motor or the ODrive.
|
||||
|
||||
**Further detail:**
|
||||
If 100's of mA of current noise is "small" for you, you can choose `MOTOR_TYPE_HIGH_CURRENT`.
|
||||
If 100's of mA of current noise is "large" for you, and you do not intend to spin the motor very fast (omega * L << R), and the motor is fairly large resistance (1 ohm or larger), you can chose `MOTOR_TYPE_GIMBAL`.
|
||||
If 100's of mA current noise is "large" for you, _and_ you intend to spin the motor fast, then you need to replace the shunt resistors on the ODrive.
|
||||
</div></details>
|
||||
**Further detail:**
|
||||
If 100's of mA of current noise is "small" for you, you can choose `MOTOR_TYPE_HIGH_CURRENT`.
|
||||
If 100's of mA of current noise is "large" for you, and you do not intend to spin the motor very fast (Ω * L << R), and the motor is fairly large resistance (1 ohm or larger), you can chose `MOTOR_TYPE_GIMBAL`.
|
||||
If 100's of mA current noise is "large" for you, _and_ you intend to spin the motor fast, then you need to replace the shunt resistors on the ODrive.
|
||||
|
||||
Note: When using gimbal motors, `current_lim` and `calibration_current` actually mean "voltage limit" and "calibration voltage", since we don't use current feedback. This means that if you set it to 10, it means 10V, despite the name of the parameter.
|
||||
</div></details> <br>
|
||||
|
||||
* _if using encoder_: `odrv0.axis0.encoder.config.cpr`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. Usually this is indicated in the datasheet of your encoder.
|
||||
* _if not using encoder_:
|
||||
* If you wish to run in sensorless mode, please see [Setting up Sensorless](commands.md#setting-up-sensorless).
|
||||
* If you are using hall sensor feedback, please see the [hoverboard motor example](hoverboard.md).
|
||||
*Note: When using gimbal motors,* `current_lim` *and* `calibration_current` *actually mean "voltage limit" and "calibration voltage", since we don't use current feedback. This means that if you set it to 10, it means 10V, despite the name of the parameter.*
|
||||
|
||||
#### If using encoder
|
||||
`odrv0.axis0.encoder.config.cpr`: Encoder Count Per Revolution [CPR]
|
||||
This is 4x the Pulse Per Revolution (PPR) value. Usually this is indicated in the datasheet of your encoder.
|
||||
#### If not using encoder
|
||||
* If you wish to run in sensorless mode, please see [Setting up sensorless](commands.md#setting-up-sensorless).
|
||||
* If you are using hall sensor feedback, please see the [hoverboard motor example](hoverboard.md).
|
||||
|
||||
|
||||
### 3. Save configuration.
|
||||
You can save all `.config` parameters to persistent memory such that the ODrive remembers them between power cycles.
|
||||
### 3. Save configuration
|
||||
You can save all `.config` parameters to persistent memory so the ODrive remembers them between power cycles.
|
||||
* `odrv0.save_configuration()` <kbd>Enter</kbd>.
|
||||
|
||||
Due to a [known issue](https://github.com/madcowswe/ODrive/issues/183) it is strongly recommended that you reboot following every save of your configuration using `odrv0.reboot()`.
|
||||
|
||||
|
||||
## Position control of M0
|
||||
|
||||
Let's get motor 0 up and running. The procedure for motor 1 is exactly the same, so feel free to replace read "axis1" wherever it says "axis0".
|
||||
Let's get motor 0 up and running. The procedure for motor 1 is exactly the same, so feel free to substitute `axis0` wherever it says `axis0`.
|
||||
|
||||
1. Type `odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE` <kbd>Enter</kbd>. After about 2 seconds should hear a beep. Then the motor will turn slowly in one direction for a few seconds, then back in the other direction.
|
||||
|
||||
@@ -214,12 +230,13 @@ Let's get motor 0 up and running. The procedure for motor 1 is exactly the same,
|
||||
|
||||
The startup procedure is demonstrated [here](https://www.youtube.com/watch?v=VCX1bA2xnuY).
|
||||
|
||||
**Note**: the rotor must be allowed to rotate without any biased load during startup. That means mass and weak friction loads are fine, but gravity or spring loads are not okay. Also note that in the video, the motors spin after initalisation, but in the current software the default behaviour is not like that.
|
||||
*Note: the rotor must be allowed to rotate without any biased load during startup. That means mass and weak friction loads are fine, but gravity or spring loads are not okay. Also note that in the video, the motors spin after initialization, but in the current software the default behaviour is not like that.*
|
||||
|
||||
<details><summary markdown="span">Help, something isn't working!</summary><div markdown="block">
|
||||
|
||||
Check the encoder wiring and that the encoder is firmly connected to the motor. Check the value of `hex(odrv0.axis0.error)` and then refer to the [error code documentation](troubleshooting.md#error-codes) for details.
|
||||
|
||||
Once you have understood the error and fixed its cause, you may clear the error state (`odrv0.axis0.error = 0` <kbd>Enter</kbd>) and retry. You may also need to clear the error state of other subcomponents (e.g. `odrv0.axis0.motor.error`).
|
||||
Once you understand the error and have fixed its cause, you may clear the error state with (`odrv0.axis0.error = 0` <kbd>Enter</kbd>) and retry. You may also need to clear the error state of other subcomponents (e.g. `odrv0.axis0.motor.error = 0`).
|
||||
</div></details>
|
||||
|
||||
2. Type `odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL` <kbd>Enter</kbd>. From now on the ODrive will try to hold the motor's position. If you try to turn it by hand, it will fight you gently. That is unless you bump up `odrv0.axis0.motor.config.current_lim`, in which case it will fight you more fiercely.
|
||||
@@ -227,13 +244,17 @@ Let's get motor 0 up and running. The procedure for motor 1 is exactly the same,
|
||||
|
||||
### Other control modes
|
||||
The ODrive also supports velocity control and current (torque) control.
|
||||
* **Velocity control**: Set `odrv0.axis0.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL`. You can now control the velocity with `odrv0.axis0.controller.vel_setpoint = 5000`. Units are counts/s.
|
||||
* **Current control**: Set `odrv0.axis0.controller.config.control_mode = CTRL_MODE_CURRENT_CONTROL`. You can now control the current with `odrv0.axis0.controller.current_setpoint = 3`. Units are 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.
|
||||
#### Velocity control
|
||||
Set `odrv0.axis0.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL`.
|
||||
You can now control the velocity with `odrv0.axis0.controller.vel_setpoint = 5000` [count/s].
|
||||
#### Current control
|
||||
Set `odrv0.axis0.controller.config.control_mode = CTRL_MODE_CURRENT_CONTROL`.
|
||||
You can now control the current with `odrv0.axis0.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.*
|
||||
|
||||
## What's next?
|
||||
|
||||
You can now:
|
||||
|
||||
* See what other [commands and parameters](commands.md) are available, including setting tuning parameters for better performance.
|
||||
* Control the ODrive from your own program or hook it up to an existing system through one of it's [interfaces](interfaces.md).
|
||||
* See how you can improve the behavior during the startup procedure, like [bypassing encoder calibration](encoders.md#encoder-with-index-signal).
|
||||
|
||||
+7
-29
@@ -143,36 +143,14 @@ This procedure is only necessary for ODrive v3.4 or earlier. You will need an ST
|
||||
Power up the ODrive.
|
||||
4. Open up a terminal and navigate to the directory where the firmware is.
|
||||
5. Run the following command (replace `ODriveFirmware_v3.4-24V.elf` with the name of your firmware file):
|
||||
```
|
||||
~/Downloads $ openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c reset\ halt -c flash\ write_image\ erase\ ODriveFirmware_v3.4-24V.elf -c reset\ run -c exit
|
||||
Open On-Chip Debugger 0.10.0
|
||||
Licensed under GNU GPL v2
|
||||
For bug reports, read
|
||||
http://openocd.org/doc/doxygen/bugs.html
|
||||
Info : auto-selecting first available session transport "hla_swd". To override use 'transport select <transport>'.
|
||||
Info : The selected transport took over low-level target control. The results might differ compared to plain JTAG/SWD
|
||||
adapter speed: 2000 kHz
|
||||
adapter_nsrst_delay: 100
|
||||
none separate
|
||||
Info : Unable to match requested speed 2000 kHz, using 1800 kHz
|
||||
Info : Unable to match requested speed 2000 kHz, using 1800 kHz
|
||||
Info : clock speed 1800 kHz
|
||||
Info : STLINK v2 JTAG v17 API v2 SWIM v4 VID 0x0483 PID 0x3748
|
||||
Info : using stlink api v2
|
||||
Info : Target voltage: 3.236027
|
||||
Info : stm32f4x.cpu: hardware has 6 breakpoints, 4 watchpoints
|
||||
adapter speed: 2000 kHz
|
||||
target halted due to debug-request, current mode: Thread
|
||||
xPSR: 0x01000000 pc: 0x08009224 msp: 0x20020000
|
||||
auto erase enabled
|
||||
Info : device id = 0x10076413
|
||||
Info : flash size = 1024kbytes
|
||||
target halted due to breakpoint, current mode: Thread
|
||||
xPSR: 0x61000000 pc: 0x20000046 msp: 0x20020000
|
||||
Warn : no flash bank found for address 10000000
|
||||
```
|
||||
openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase ODriveFirmware_v3.4-24V.elf" -c "reset run" -c exit
|
||||
```
|
||||
|
||||
If everything worked correctly, you should see something similar to this towards the end of the printout:
|
||||
```
|
||||
wrote 262144 bytes from file ODriveFirmware_v3.4-24V.elf in 10.194110s (25.113 KiB/s)
|
||||
adapter speed: 2000 kHz
|
||||
```
|
||||
```
|
||||
|
||||
If something doesn't work, make sure `openocd` is in your `PATH` variable, check that the wires are connected properly and try with elevated privileges.
|
||||
|
||||
|
||||
@@ -0,0 +1,225 @@
|
||||
# Copyright (c) 2018 Paul Guénette
|
||||
# Copyright (c) 2018 Oskar Weigl
|
||||
|
||||
# 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.
|
||||
|
||||
# This algorithm is based on:
|
||||
# FIR filter-based online jerk-constrained trajectory generation
|
||||
# https://www.researchgate.net/profile/Richard_Bearee/publication/304358769_FIR_filter-based_online_jerk-controlled_trajectory_generation/links/5770ccdd08ae10de639c0ff7/FIR-filter-based-online-jerk-controlled-trajectory-generation.pdf
|
||||
|
||||
import numpy as np
|
||||
import math
|
||||
import matplotlib.pyplot as plt
|
||||
import random
|
||||
|
||||
# Symbol Description
|
||||
# Ta, Tv and Td Duration of the stages of the AL profile
|
||||
# Xi and Vi Adapted initial conditions for the AL profile
|
||||
# Xf Position set-point
|
||||
# s Direction (sign) of the trajectory
|
||||
# Vmax, Amax, Dmax and jmax Kinematic bounds
|
||||
# Ar, Dr and Vr Reached values of acceleration and velocity
|
||||
|
||||
# Test scales:
|
||||
pos_range = 10000.0
|
||||
Vmax_range = 8000.0
|
||||
Amax_range = 10000.0
|
||||
plot_range = 10000.0
|
||||
|
||||
|
||||
def PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax):
|
||||
dX = Xf - Xi # Distance to travel
|
||||
stop_dist = Vi**2 / (2*Dmax) # Minimum stopping distance
|
||||
dXstop = np.sign(Vi)*stop_dist # Minimum stopping displacement
|
||||
s = np.sign(dX - dXstop) # Sign of coast velocity (if any)
|
||||
Ar = s*Amax # Maximum Acceleration (signed)
|
||||
Dr = -s*Dmax # Maximum Deceleration (signed)
|
||||
Vr = s*Vmax # Maximum Velocity (signed)
|
||||
|
||||
# If we start with a speed faster than cruising, then we need to decel instead of accel
|
||||
# aka "double deceleration move" in the paper
|
||||
if s*Vi > s*Vr:
|
||||
print("Handbrake!")
|
||||
Ar = -s*Amax
|
||||
|
||||
# Time to accel/decel to/from Vr (cruise speed)
|
||||
Ta = (Vr-Vi)/Ar
|
||||
Td = -Vr/Dr
|
||||
|
||||
# Integral of velocity ramps over the full accel and decel times to get
|
||||
# minimum displacement required to reach cuising speed
|
||||
dXmin = Ta*(Vr+Vi)/2.0 + Td*(Vr)/2.0
|
||||
|
||||
# Are we displacing enough to reach cruising speed?
|
||||
if s*dX < s*dXmin:
|
||||
print("Short Move:")
|
||||
# From paper:
|
||||
# Vr = s*math.sqrt((-(Vi**2/Ar)-2*dX)/(1/Dr-1/Ar))
|
||||
# Simplified for less divisions:
|
||||
Vr = s*math.sqrt((Dr*Vi**2 + 2*Ar*Dr*dX) / (Dr-Ar))
|
||||
Ta = max(0, (Vr - Vi)/Ar)
|
||||
Td = max(0, -Vr/Dr)
|
||||
Tv = 0
|
||||
else:
|
||||
print("Long move:")
|
||||
Tv = (dX - dXmin)/Vr # Coasting time
|
||||
|
||||
Tf = Ta+Tv+Td
|
||||
|
||||
print("Xi: {:.2f}\tXf: {:.2f}\tVi: {:.2f}".format(Xi, Xf, Vi))
|
||||
print("Amax: {:.2f}\tVmax: {:.2f}\tDmax: {:.2f}".format(Amax, Vmax, Dmax))
|
||||
print("dX: {:.2f}\tdXst: {:.2f}\tdXmin: {:.2f}".format(dX, dXstop, dXmin))
|
||||
print("Ar: {:.2f}\tVr: {:.2f}\tDr: {:.2f}".format(Ar, Vr, Dr))
|
||||
print("Ta: {:.2f}\tTv: {:.2f}\tTd: {:.2f}".format(Ta, Tv, Td))
|
||||
|
||||
return (Ar, Vr, Dr, Ta, Tv, Td, Tf)
|
||||
|
||||
def EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf):
|
||||
# Create the time series and preallocate the position, velocity, and acceleration arrays
|
||||
t_traj = np.arange(0, Tf+0.1, 1/10000)
|
||||
y = [None]*len(t_traj)
|
||||
yd = [None]*len(t_traj)
|
||||
ydd = [None]*len(t_traj)
|
||||
|
||||
# We only know acceleration (Ar and Dr), so we integrate to create
|
||||
# the velocity and position curves
|
||||
y_Accel = Xi + Vi*Ta + 0.5*Ar*Ta**2
|
||||
|
||||
for i in range(len(t_traj)):
|
||||
t = t_traj[i]
|
||||
if t < 0: # Initial conditions
|
||||
y[i] = Xi
|
||||
yd[i] = Vi
|
||||
ydd[i] = 0
|
||||
elif t < Ta: # Acceleration
|
||||
y[i] = Xi + Vi*t + 0.5*Ar*t**2
|
||||
yd[i] = Vi + Ar*t
|
||||
ydd[i] = Ar
|
||||
elif t < Ta+Tv: # Coasting
|
||||
y[i] = y_Accel + Vr*(t-Ta)
|
||||
yd[i] = Vr
|
||||
ydd[i] = 0
|
||||
elif t < Tf: # Deceleration
|
||||
td = t-Tf
|
||||
y[i] = Xf + 0*td + 0.5*Dr*td**2
|
||||
yd[i] = 0 + Dr*td
|
||||
ydd[i] = Dr
|
||||
elif t >= Tf: # Final condition
|
||||
y[i] = Xf
|
||||
yd[i] = 0
|
||||
ydd[i] = 0
|
||||
else:
|
||||
raise ValueError("t = {} is outside of considered range".format(t))
|
||||
|
||||
dy = np.diff(y)
|
||||
dy_max = np.max(np.abs(dy))
|
||||
dyd = np.diff(yd)
|
||||
dyd_max = np.max(np.abs(dyd))
|
||||
print("dy_max: {:.2f}\tdyd_max: {:.2f}".format(dy_max, dyd_max))
|
||||
|
||||
error = False
|
||||
if dy_max/pos_range > 0.001:
|
||||
print("---------- Bad Pos Continuity --------------------")
|
||||
error = True
|
||||
if dyd_max/Vmax_range > 0.001:
|
||||
print("---------- Bad Vel Continuity --------------------")
|
||||
error = True
|
||||
if abs(Xi-y[0]) > 0.0001:
|
||||
print("---------- Bad Initial Position --------------------")
|
||||
error = True
|
||||
if abs(Xf-y[-1]) > 0.0001:
|
||||
print("---------- Bad Final Position --------------------")
|
||||
error = True
|
||||
if abs(Vi-yd[0]) > 0.0001:
|
||||
print("---------- Bad Initial Velocity --------------------")
|
||||
error = True
|
||||
if abs(yd[-1]) > 0.0001:
|
||||
print("---------- Bad Final Velocity --------------------")
|
||||
error = True
|
||||
|
||||
if error:
|
||||
import ipdb; ipdb.set_trace()
|
||||
|
||||
return (y, yd, ydd, t_traj)
|
||||
|
||||
def graphical_test():
|
||||
numRows = 3
|
||||
numCols = 5
|
||||
fig, axes = plt.subplots(numRows, numCols)
|
||||
random.seed(3) # Repeatable tests by using specific seed
|
||||
for x in range(numRows*numCols):
|
||||
rownow = int(x/numCols)
|
||||
colnow = x % numCols
|
||||
print("row: {}, col: {}".format(rownow, colnow))
|
||||
|
||||
Vmax = random.uniform(0.1*Vmax_range, Vmax_range)
|
||||
Amax = random.uniform(0.1*Amax_range, Amax_range)
|
||||
Dmax = Amax
|
||||
Xf = random.uniform(-pos_range, pos_range)
|
||||
Xi = random.uniform(-pos_range, pos_range)
|
||||
if random.random() <= 0.5:
|
||||
Vi = random.uniform(-Vmax*1.5, Vmax*1.5)
|
||||
else:
|
||||
Vi = 0
|
||||
|
||||
(Ar, Vr, Dr, Ta, Tv, Td, Tf) = PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax)
|
||||
(Y, Yd, Ydd, t) = EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf)
|
||||
|
||||
# Plotting
|
||||
ax1 = axes[rownow, colnow]
|
||||
# Vel limits (draw first for clearer z-order)
|
||||
ax1.plot([t[0], t[-1]], [Vmax, Vmax], 'g--')
|
||||
ax1.plot([t[0], t[-1]], [-Vmax, -Vmax], 'g--')
|
||||
|
||||
ax1.plot(t, Y) # Pos
|
||||
ax1.plot(t, Yd) # Vel
|
||||
ax1.plot(0, Xi, 'bo') # Pos Initial
|
||||
ax1.plot(0, Vi, 'ro') # Vel Initial
|
||||
## TODO: pull out Ta+Td+Td from planner for correct plot points
|
||||
ax1.plot(t[-1]-0.1, Xf, 'b*') # Pos Final
|
||||
ax1.plot(t[-1]-0.1, 0, 'r*') # Vel Final
|
||||
|
||||
ax1.set_ylim(-plot_range, plot_range)
|
||||
|
||||
print()
|
||||
|
||||
plt.show()
|
||||
|
||||
def large_test():
|
||||
random.seed(1) # Repeatable tests by using specific seed
|
||||
for x in range(100):
|
||||
print("Test {}".format(x))
|
||||
Vmax = random.uniform(0.1*Vmax_range, Vmax_range)
|
||||
Amax = random.uniform(0.1*Amax_range, Amax_range)
|
||||
Dmax = Amax
|
||||
Xf = random.uniform(-pos_range, pos_range)
|
||||
Xi = random.uniform(-pos_range, pos_range)
|
||||
if random.random() <= 0.5:
|
||||
Vi = random.uniform(-Vmax*1.5, Vmax*1.5)
|
||||
else:
|
||||
Vi = 0
|
||||
|
||||
(Ar, Vr, Dr, Ta, Tv, Td, Tf) = PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax)
|
||||
(Y, Yd, Ydd, t) = EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf)
|
||||
|
||||
print()
|
||||
|
||||
if __name__ == '__main__':
|
||||
large_test()
|
||||
graphical_test()
|
||||
+6
-2
@@ -3,6 +3,7 @@
|
||||
Tool for flashing .hex files to the ODrive via the STM built-in USB DFU mode.
|
||||
"""
|
||||
|
||||
from __future__ import print_function
|
||||
import argparse
|
||||
import sys
|
||||
import time
|
||||
@@ -206,7 +207,7 @@ def show_deferred_message(message, cancellation_token):
|
||||
time.sleep(1)
|
||||
if not cancellation_token.is_set():
|
||||
print(message)
|
||||
t = threading.Thread(target=show_message_thread, args=(message, cancellation_token), daemon=True)
|
||||
t = threading.Thread(target=show_message_thread, args=(message, cancellation_token))
|
||||
t.daemon = True
|
||||
t.start()
|
||||
|
||||
@@ -436,7 +437,10 @@ def launch_dfu(args, logger, cancellation_token):
|
||||
def find_device_in_dfu_mode_thread():
|
||||
devices[0] = find_device_in_dfu_mode(serial_number, find_odrive_cancellation_token)
|
||||
find_odrive_cancellation_token.set()
|
||||
threading.Thread(target=find_device_in_dfu_mode_thread, daemon=True).start()
|
||||
t = threading.Thread(target=find_device_in_dfu_mode_thread)
|
||||
t.daemon = True
|
||||
t.start()
|
||||
|
||||
|
||||
# Scan for ODrives not in DFU mode
|
||||
# We only scan on USB because DFU is only implemented over USB
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
|
||||
from __future__ import print_function
|
||||
import subprocess
|
||||
import shlex
|
||||
import math
|
||||
|
||||
+10
-3
@@ -1,3 +1,4 @@
|
||||
from __future__ import print_function
|
||||
|
||||
import sys
|
||||
import time
|
||||
@@ -12,7 +13,7 @@ try:
|
||||
import win32console
|
||||
import colorama
|
||||
colorama.init()
|
||||
except ModuleNotFoundError:
|
||||
except ImportError:
|
||||
print("Could not init terminal features.")
|
||||
print("Refer to install instructions at http://docs.odriverobotics.com/#downloading-and-installing-tools")
|
||||
sys.stdout.flush()
|
||||
@@ -72,8 +73,14 @@ def start_liveplotter(get_var_callback):
|
||||
fig.canvas.draw()
|
||||
fig.canvas.start_event_loop(1/plot_rate)
|
||||
|
||||
threading.Thread(target=fetch_data, daemon=True).start()
|
||||
threading.Thread(target=plot_data, daemon=True).start()
|
||||
fetch_t = threading.Thread(target=fetch_data)
|
||||
fetch_t.daemon = True
|
||||
fetch_t.start()
|
||||
|
||||
plot_t = threading.Thread(target=plot_data)
|
||||
plot_t.daemon = True
|
||||
plot_t.start()
|
||||
|
||||
|
||||
return cancellation_token;
|
||||
#plot_data()
|
||||
|
||||
+2
-1
@@ -35,7 +35,8 @@ def for_all_parallel(objects, get_name, callback):
|
||||
# Start a thread for each element in the list
|
||||
all_threads = []
|
||||
for element in objects:
|
||||
thread = threading.Thread(target=run_callback, args=(element,), daemon=True)
|
||||
thread = threading.Thread(target=run_callback, args=(element,))
|
||||
thread.daemon = True
|
||||
thread.start()
|
||||
all_threads.append(thread)
|
||||
|
||||
|
||||
+8
-1
@@ -48,6 +48,10 @@ from setuptools import setup
|
||||
import os
|
||||
import sys
|
||||
|
||||
if sys.version_info < (3, 3):
|
||||
import exceptions
|
||||
PermissionError = exceptions.OSError
|
||||
|
||||
creating_package = "sdist" in sys.argv
|
||||
|
||||
# Load version from Git tag
|
||||
@@ -78,7 +82,10 @@ if creating_package:
|
||||
fibre_link = os.path.join(os.path.dirname(
|
||||
os.path.realpath(__file__)), "fibre")
|
||||
if not os.path.exists(fibre_link):
|
||||
os.symlink(fibre_src, fibre_link, True)
|
||||
if sys.version_info > (3, 3):
|
||||
os.symlink(fibre_src, fibre_link, target_is_directory=True)
|
||||
else:
|
||||
os.symlink(fibre_src, fibre_link)
|
||||
|
||||
# TODO: find a better place for this
|
||||
if not creating_package:
|
||||
|
||||
Reference in New Issue
Block a user