Merge branch 'devel' into input-filter

.travis.yml

@@ -45,11 +45,13 @@ env:
   - CONFIG_BOARD_VERSION=v3.4-48V DEPLOY=v3.4-48V
   - CONFIG_BOARD_VERSION=v3.5-24V DEPLOY=v3.5-24V
   - CONFIG_BOARD_VERSION=v3.5-48V DEPLOY=v3.5-48V
+  - CONFIG_BOARD_VERSION=v3.6-24V DEPLOY=v3.6-24V
+  - CONFIG_BOARD_VERSION=v3.6-56V DEPLOY=v3.6-56V
 
   # Various protocol combinations
-  - CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=native-stream CONFIG_UART_PROTOCOL=native
-  - CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=stdout CONFIG_UART_PROTOCOL=stdout
-  - CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=none CONFIG_UART_PROTOCOL=none
+  #- CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=native-stream CONFIG_UART_PROTOCOL=native
+  #- CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=stdout CONFIG_UART_PROTOCOL=stdout
+  #- CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=none CONFIG_UART_PROTOCOL=none
 
 script:
 - "./Firmware/build.sh"

CHANGELOG.md

@@ -2,14 +2,21 @@
 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.
-* Second order setpoint input filter.
+* A release target for ODrive v3.6
 * 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`
+* Lock-in drive feature
+* Script to enable using a hall signal as index edge.
 
 ### Changed
 * Moved `traptraj.A_per_css` to `controller.inertia`
 * Refactored velocity ramp mode into the new general input filtering structure
+* Encoder index search now based on the new lock-in drive feature
 
+### Fixed
+* Encoder index interrupts now disabled when not searching
 
 # Releases
 ## [0.4.8] - 2019-02-25

Firmware/.vscode/launch.json

@@ -15,6 +15,7 @@
                 "interface/stlink-v2.cfg",
                 "target/stm32f4x_stlink.cfg",
             ],
+            "svdFile": "${workspaceRoot}/Board/v3/STM32F40x.svd",
             "cwd": "${workspaceRoot}"
         },
         {

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)

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;
     }
 }
@@ -60,12 +60,10 @@ void Encoder::enc_index_cb() {
 }
 
 void Encoder::set_idx_subscribe(bool override_enable) {
-    if (override_enable || (config_.use_index && !config_.find_idx_on_lockin_only)) {
+    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);
-    }
-
-    if (!config_.use_index || config_.find_idx_on_lockin_only) {
+    } else if (!config_.use_index || config_.find_idx_on_lockin_only) {
         GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin);
     }
 }
@@ -163,9 +161,7 @@ bool Encoder::run_direction_find() {
 // 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_) {
@@ -202,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))
@@ -232,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;
@@ -243,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))
@@ -288,8 +284,8 @@ void Encoder::sample_now() {
         } break;
 
         case MODE_SINCOS: {
-            sincos_sample_s_ = get_adc_voltage(GPIO_3_GPIO_Port, GPIO_3_Pin) / 3.3f;
-            sincos_sample_c_ = get_adc_voltage(GPIO_4_GPIO_Port, GPIO_4_Pin) / 3.3f;
+            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: {

Firmware/MotorControl/encoder.hpp

@@ -37,6 +37,8 @@ public:
         float offset_float = 0.0f; // Sub-count phase alignment offset
         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
@@ -83,6 +85,7 @@ 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
@@ -99,11 +102,12 @@ public:
             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",
@@ -122,9 +126,12 @@ public:
                 make_protocol_property("bandwidth", &config_.bandwidth,
                     [](void* ctx) { static_cast<Encoder*>(ctx)->update_pll_gains(); }, this),
                 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")
         );
     }
 };

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('')

docs/ascii-protocol.md

@@ -93,6 +93,16 @@ c motor current
 
 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
 ```

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
 

tools/odrive/utils.py

@@ -192,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] "

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')

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':

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']},

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