From f0782d034a8f856fadc770c74e2cc4f32c876c58 Mon Sep 17 00:00:00 2001 From: Richmond Tran Date: Thu, 10 Feb 2022 17:36:48 -0700 Subject: [PATCH] Rewrote auto_calibrate and user_calibrate function of Odrive_ctrl. Updated user loop --- src/odrive_exo.py | 387 ++++++++++++++++++++++++++-------------------- 1 file changed, 218 insertions(+), 169 deletions(-) diff --git a/src/odrive_exo.py b/src/odrive_exo.py index 62d19f0..3cb1c3a 100755 --- a/src/odrive_exo.py +++ b/src/odrive_exo.py @@ -64,52 +64,79 @@ def user_loop(self): For user control via menu """ while (True): - # key = input("\n1. Calibrate\n2. Position Control Mode\n3. Velocity Control Mode\n4. Voltage Control Mode\n5. Set Limits\n6. Dump Configs + Errors\nChoose a command (Press a number): ") - key = input("\n1. Calibrate\n2. Position Control Mode\n3. Velocity Control Mode\n4. Torque Control Mode\n5. Set Limits\n6. Dump Configs + Errors\nChoose a command (Press a number): ") - + key = input("1. Calibrate\n2. Position Control Mode\n3. Velocity Control Mode\n4. Torque Control Mode\n5. Set Limits\n6. Dump Configs + Errors\nChoose a command (Press a number): ") + print("\n") if key == '1': - # Calibration - key = input("\n1. Auto (Don't do this while motor is connect to arm)\n2. User\nChoose a calibration type: ") + # Calibration ----- + key = input("1. Auto (Don't do this while motor is connect to arm)\n2. User\nChoose a calibration type: ") if key == '1': # Don't do this while connected to arm - #self.odrv_ctrl.auto_calibrate(True) + # self.odrv_ctrl.auto_calibrate() + pass + elif key == '2': + # self.odrv_ctrl.user_calibrate() + pass + else: pass - if key == '2': - self.odrv_ctrl.user_calibrate() elif key == '2': - # Position Control Mode + # Position Control Mode ----- key = '' while key != "q": - print("Current Position [counts]: " + str(self.odrv_ctrl.get_position())) + try: + print("Current Position [turns]: " + str(self.odrv_ctrl.get_position())) + print("\n") + except: + print("Cannot get position estimate") + print("\n") + key = input("Enter a position or press q to quit: ") - if key != 'q': self.odrv_ctrl.set_position(int(key)) - # TODO: Possibly add a delay here to give time for motor to spin so position is read correctly on next loop + if key != 'q': + try: + self.odrv_ctrl.set_position(int(key)) + time.sleep(1) + except: + print("Cannot set position") elif key == '3': - # Velocity Control Mode + # Velocity Control Mode ----- key = '' while key != 'q': - print("Current Position [counts]: " + str(self.odrv_ctrl.get_position())) - print("Current Velocity [counts/s]: " + str(self.odrv_ctrl.get_velocity())) + try: + print("Current Position [turns]: " + str(self.odrv_ctrl.get_position())) + print("Current Velocity [turns/s]: " + str(self.odrv_ctrl.get_velocity())) + print("\n") + except: + print("Cannot get position or velocity estimate") + print("\n") + key = input("Enter a velocity or press q to quit: ") - if key != 'q': self.odrv_ctrl.set_velocity(int(key)) - # TODO: Possibly add a delay here to give time for motor to spin so position is read correctly on next loop - - # # Voltage Control Mode - # elif key == '4': - # # self.odrv_ctrl.set_voltage() - # print("No command") + if key != 'q': + try: + self.odrv_ctrl.set_velocity(int(key)) + time.sleep(1) + except: + print("Cannot set velocity") elif key == '4': - # Torque Control Mode + # Torque Control Mode ----- key = '' while key != 'q': - key = input("Enter a velocity or press q to quit: ") - if key != 'q': self.odrv_ctrl.set_torque(int(key)) - # TODO: Possibly add a delay here to give time for motor to spin so position is read correctly on next loop - - + try: + print("Current Position [turns]: " + str(self.odrv_ctrl.get_position())) + print("\n") + except: + print("Cannot get position estimate") + print("\n") + + key = input("Enter a torque or press q to quit: ") + + if key != 'q': + try: + self.odrv_ctrl.set_torque(int(key)) + time.sleep(1) + except: + print("Cannot set torque") elif key == '5': # Set Limits print("Current Limit: " + str(self.odrv_ctrl.get_current_limit()) + "\nVelocity Limit: " + str(self.odrv_ctrl.get_velocity_limit())), @@ -119,16 +146,16 @@ def user_loop(self): key = input("Enter a new Current Limit: ") self.odrv_ctrl.set_current_limit(int(key)) print("New Current Limit: " + str(self.odrv_ctrl.get_current_limit())) + print("\n") elif key == '2': # Velocity key = input("Enter a new Velocity Limit: ") self.odrv_ctrl.set_velocity_limit(int(key)) print("New Velocity Limit: " + str(self.odrv_ctrl.get_velocity_limit())) + print("\n") else: # Aborts with any other key pass - - elif key == '6': # Dump Configs key = input("Reset Errors? [y/n]: ") @@ -136,9 +163,11 @@ def user_loop(self): self.odrv_ctrl.dump_errors(True) else: self.odrv_ctrl.dump_errors() + print("\n") self.odrv_ctrl.dump_motor_config() + print("\n") self.odrv_ctrl.dump_encoder_config() - self.odrv_ctrl.dump_errors() + print("\n") def exo_loop(self): """ @@ -158,7 +187,7 @@ class ODrive_ctrl: encoder_cpr = 8192 encoder_kv = 192 - # Connection + # Connection ----- def connect(self): print("Finding an ODrive...") @@ -167,6 +196,7 @@ def connect(self): self.axis0 = self.odrv.axis0 self.axis1 = self.odrv.axis1 print("Found an ODrive!") + self.set_requested_state(self.axis0, AXIS_STATE_IDLE) else: print("Couldn't find an ODrive...") @@ -174,87 +204,117 @@ def reboot_odrive(self): print("Rebooting and Reconnecting") try: self.odrv.reboot() - except fibre.ChannelBrokenException: + except: print("Expected connection loss due to reboot") + # except fibre.ChannelBrokenException: + # print("Expected connection loss due to reboot") self.connect() - # Calibration - - # def auto_calibrate(self, debug): - # """ - # Auto Calibration - should only be needed once per life of motor. - # """ - # ##################Calibration Phase 1 - Set Variables################### - # ''' - # Based off https://docs.odriverobotics.com/ - # ''' - # print("Reseting Configuration - Recalibration Required") - # self.odrv.erase_configuration() - # self.reboot_odrive() - - # print("Calibration Phase 1") - # # These 3 values are the ones we had to change from default, they will - # # change with different power supplies: - # # Garage Power Supply: 3,3,5 - # # Turnigy ReaktorPro: 10, 10, 3 (Defaults) - # # - # self.axis0.motor.config.calibration_current = 10 - # self.axis0.motor.config.current_lim = 10 - # # self.axis0.motor.config.resistance_calib_max_voltage = 3 # Not sure if this is needed - # self.axis0.controller.config.vel_limit = 20000 - # self.odrv.config.enable_brake_resistor = True # Set to True if using brake resistor - # self.odrv.config.dc_max_negative_current = -10 # [Amps] - # # self.odrv0.config.max_regen_current = 10 # Not sure if this is needed - - # # Display the changed parameters if in debug mode - # if debug: - # print("Calibration current set to " + str(self.axis0.motor.config.calibration_current)) - # print("Current limit set to " + str(self.axis0.motor.config.current_lim)) - # print("Resistance_calib_max_voltage set to " + str(self.axis0.motor.config.resistance_calib_max_voltage)) - - # print("Calibration Phase 1 Complete.") - # ''' - # At this point setting self.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE - # should make the motor Beep, then spin one direction, then the other. - # Running dump_errors(odrv0) in the odrivetool should show no errors. - # ''' - - # self.set_requested_state(self.axis0, AXIS_STATE_FULL_CALIBRATION_SEQUENCE) - # time.sleep(20) - - # #####################Calibration Phase 2 - Encoder###################### - # """ - # Based off https://docs.odriverobotics.com/encoders - # """ - # print("\nPhase 2\n") - # self.axis0.encoder.config.use_index = True # Done - # self.set_requested_state(self.axis0, AXIS_STATE_ENCODER_INDEX_SEARCH) - # time.sleep(10) - # self.set_requested_state(self.axis0, AXIS_STATE_ENCODER_OFFSET_CALIBRATION) # Error seems to happen here - # time.sleep(10) - # # - # if debug: - # # Should be 0 - # print("axis.error = " + str(self.axis0.error) + " expected = 0") - # # Should be some integer, like -326 or 1364 - # print("offset = " + str(self.axis0.encoder.config.offset) + " expected = Integer (+ or -)") - # # Should be 1 or -1 - # print("direction = " + str(self.axis0.motor.config.direction) + " expected = -1 or +1" ) - - # print("Setting encoder and motor pre-calibrated flags to True...The variable printed below should be true if the odrive agrees calibration worked") - # self.axis0.encoder.config.pre_calibrated = True - # self.axis0.motor.config.pre_calibrated = True - # # Search for an index at startup, i.e. encoder should spin and stop at the same position - # self.axis0.config.startup_encoder_index_search = False - # print("self.axis0.encoder.config.pre_calibrated = ") - # print(self.axis0.encoder.config.pre_calibrated) - - # if self.axis0.encoder.config.pre_calibrated: - # self.odrv.save_configuration() - # self.reboot_odrive() - # print("Done full calibration sequence - successful") - # else: - # print("Done full calibration sequence - not successful") + def save_configuration(self): + try: + self.odrv.save_configuration() + time.sleep(10) + self.reboot_odrive() + time.sleep(10) + except fibre.libfibre.ObjectLostError: + self.reboot_odrive() + time.sleep(10) + + # Calibration ----- + + def auto_calibrate(self): + ''' + Motor Calibration + https://docs.odriverobotics.com/ + ''' + + # Reset Configuration + try: + print("Erasing configuration...") + self.odrv.erase_configuration() + time.sleep(10) + self.reboot_odrive() + time.sleep(10) + except fibre.libfibre.ObjectLostError: + self.reboot_odrive() + time.sleep(10) + + # ----- Set the limit ----- + # self.axis0.motor.config.current_lim [A] + # self.axis0.controller.config.vel_limit [turn/s] + + # TODO Figure out proper limits given motor specs + self.axis0.motor.config.calibration_current = 10 + self.axis0.motor.config.current_lim = 10 + self.axis0.motor.config.resistance_calib_max_voltage = 3 + self.axis0.controller.config.vel_limit = 20000 + + # ----- Set hardware parameters ----- + self.odrv.config.enable_brake_resistor = True + self.odrv.config.dc_max_negative_current = -10 # [Amps] + + self.save_configuration(); + + print(".requested_state: AXIS_STATE_FULL_CALIBRATION_SEQUENCE") + self.set_requested_state(self.axis0, AXIS_STATE_FULL_CALIBRATION_SEQUENCE) + time.sleep(10) + + # Check if motor is calibrated. Important for next step. + print(".motor.is_calibrated: " + str(self.axis0.motor.is_calibrated)) + + ''' + Encoder Calibration + https://docs.odriverobotics.com/encoders + ''' + + # ----- Encoder with index signal ----- + + # Startup Sequence Notes + # self.axis0.motor.config.motor_type = MOTOR_TYPE_HIGH_CURRENT + # self.axis0.encoder.config.calib_range = 0.05 + # self.axis0.motor.config.calibration_current = 10.0 + # self.axis0.motor.config.resistance_calib_max_voltage = 12.0 + # self.axis0.controller.config.vel_limit = 5 + + print(".encoder.config.cpr: " + str(self.axis0.encoder.config.cpr)) + print(".encoder.config.mode: " + str(self.axis0.encoder.config.mode)) + + self.axis0.encoder.config.use_index = True + + print(".requested_state: AXIS_STATE_ENCODER_INDEX_SEARCH") + self.set_requested_state(self.axis0, AXIS_STATE_ENCODER_INDEX_SEARCH) + time.sleep(10) + + print(".requested_state: AXIS_STATE_ENCODER_OFFSET_CALIBRATION") + self.set_requested_state(self.axis0, AXIS_STATE_ENCODER_OFFSET_CALIBRATION) + time.sleep(10) + + # This should be zero + print(".error: " + str(self.axis0.error)) + # This should print a number like -326 or 1364 + print(".encoder.config.phase_offset: " + str(self.axis0.encoder.config.phase_offset)) + # This should print 1 or -1 + print(".encoder.config.direction: " + str(self.axis0.encoder.config.direction)) + + self.axis0.encoder.config.pre_calibrated = True + + # If you would like to search for the index at startup + self.axis0.config.startup_encoder_index_search = False + # If you are looking to start your machine as quickly as possible on bootup + self.axis0.motor.config.pre_calibrated = True + + self.save_configuration() + + print(".requested_state: AXIS_STATE_FULL_CALIBRATION_SEQUENCE") + self.set_requested_state(self.axis0, AXIS_STATE_FULL_CALIBRATION_SEQUENCE) + time.sleep(10) + + print(".encoder.index_found: " + str(self.axis0.encoder.index_found)) + print(".encoder.is_ready: " + str(self.axis0.encoder.is_ready)) + print(".encoder.config.precalibrated: " + str(self.axis0.encoder.config.pre_calibrated)) + + print("Auto-Calibration finished") + print("\n") def user_calibrate(self): """ @@ -266,60 +326,70 @@ def user_calibrate(self): manually rotate motor through the index position (marked with red line on motor) """ + # These 3 values are the ones we had to change from default, they will # change with different power supplies: # Garage Power Supply: 3,3,5 # Turnigy ReaktorPro: 10, 10, 3 (Defaults) # https://docs.google.com/spreadsheets/d/12vzz7XVEK6YNIOqH0jAz51F5VUpc-lJEs3mmkWP1H4Y/edit#gid=0 --> Motor limits - # self.axis0.motor.config.current_lim = 68 # From above link - # self.axis0.controller.config.vel_limit = 50000 # counts/s - self.odrv.config.enable_brake_resistor = True # Set to True if using brake resistor - self.odrv.config.dc_max_negative_current = -10 # [Amps] + # ----- Set the limit ----- + # self.axis0.motor.config.current_lim [A] + # self.axis0.controller.config.vel_limit [turn/s] + # self.axis0.motor.config.calibration_current [A] - # self.axis0.motor.config.calibration_current = 10 // Not sure if this is needed - # self.odrv0.config.max_regen_current = 10 # Not sure if this is needed - # self.axis0.motor.config.resistance_calib_max_voltage = 3 # Not sure if this is needed + # TODO Figure out proper limits given motor specs + self.axis0.motor.config.calibration_current = 10 + self.axis0.motor.config.current_lim = 10 + self.axis0.motor.config.resistance_calib_max_voltage = 3 + self.axis0.controller.config.vel_limit = 20000 - try: - self.odrv.save_configuration() - print("\nIndex Values Before: " + str(self.axis0.encoder.is_ready) + " " + str(self.axis0.encoder.index_found)) - print("Delaying 10 seconds for manual calibration movement") - time.sleep(10) - print("\nIndex Values After: " + str(self.axis0.encoder.is_ready) + " " + str(self.axis0.encoder.index_found)) - except fibre.libfibre.ObjectLostError: - self.connect() + # ----- Set hardware parameters ----- + self.odrv.config.enable_brake_resistor = True + self.odrv.config.dc_max_negative_current = -10 + self.odrv.config.max_regen_current = 10 - self.set_requested_state(self.axis0, AXIS_STATE_FULL_CALIBRATION_SEQUENCE) - time.sleep(20) + self.save_configuration() + + print("Delaying 10 seconds for manual calibration movement") + time.sleep(10) + + print(".encoder.index_found: " + str(self.axis0.encoder.index_found)) + print(".encoder.is_ready: " + str(self.axis0.encoder.is_ready)) + print(".encoder.config.precalibrated: " + str(self.axis0.encoder.config.pre_calibrated)) + + print("User-Calibration finished") + print("\n") - # Getters + # Getters ----- def get_position(self): - return self.axis0.encoder.pos_estimate + try: + return self.axis0.encoder.pos_estimate + except: + print("Cannot get position") def get_velocity(self): - return self.axis0.encoder.vel_estimate + try: + return self.axis0.encoder.vel_estimate + except: + print("Cannot get velocity") def get_current_limit(self): - return self.axis0.motor.config.current_lim + try: + return self.axis0.motor.config.current_lim + except: + print("Cannot get current limit") def get_velocity_limit(self): - return self.axis0.controller.config.vel_limit + try: + return self.axis0.controller.config.vel_limit + except: + print("Cannot get velocity limit") - # Setters + # Setters ----- def set_control_mode(self, axis, mode): - """ - ---------- Outdated ---------- - - 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 - """ - """ https://docs.odriverobotics.com/commands.html https://docs.odriverobotics.com/api/odrive.controller.controlmode @@ -358,56 +428,35 @@ def set_requested_state(self, axis, state): def set_position(self, position): try: self.set_requested_state(self.axis0, AXIS_STATE_CLOSED_LOOP_CONTROL) - # Outdated: self.axis0.controller.config.control_mode = CTRL_MODE_POSITION_CONTROL self.axis0.controller.config.control_mode = CONTROL_MODE_POSITION_CONTROL - # Outdated: self.axis0.controller.pos_setpoint = position # [counts] # .controller.input_pos = self.axis0.controller.input_pos = position # print("Set position to: " + str(position) + " encoder units") print("Set position to: " + str(position) + " turns") except: - pass + print("Cannot set position") def set_velocity(self, velocity): try: self.set_requested_state(self.axis0, AXIS_STATE_CLOSED_LOOP_CONTROL) - # Outdated: self.axis0.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL self.axis0.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL - # Outdated: self.axis0.controller.vel_setpoint = velocity # [counts/sec] # .controller.input_vel = self.axis0.controller.input_vel = velocity # print("Set Velocity to: " + str(velocity) + "counts/s (Non-Ramped)") print("Set velocity to: " + str(velocity) + " turn/s") except: - pass + print("Cannot set velocity") def set_torque(self, torque): try: self.set_requested_state(self.axis0, AXIS_STATE_CLOSED_LOOP_CONTROL) self.axis0.controller.config.control_mode = CONTROL_MODE_TORQUE_CONTROL # .controller.input_torque = - self.axis0.controller.input_vel = torque + self.axis0.controller.input_torque = torque print("Set torque to: " + str(torque) + " torque in Nm") except: - pass - - """ - def set_current(self, current): - # Set the current, proportional to torque - # NOTE: There is no velocity limiting in current control mode, make - # sure to not overrev the motor or exceed max speed for encode. - if (current >= 0 and current <= 68): - self.set_requested_state(self.axis0, AXIS_STATE_CLOSED_LOOP_CONTROL) - self.axis0.controller.config.control_mode = CTRL_MODE_CURRENT_CONTROL - self.axis0.controller.current_setpoint = current - else: - print("Current outside motor limits") - - def set_voltage(self): - self.set_requested_state(self.axis0, AXIS_STATE_CLOSED_LOOP_CONTROL) - self.axis0.controller.config.control_mode = CTRL_MODE_VOLTAGE_CONTROL - """ + print("Cannot set torque") def set_current_limit(self, new_current_lim): self.axis0.motor.config.current_lim = new_current_lim @@ -417,7 +466,7 @@ def set_velocity_limit(self, new_velocity_lim): self.axis0.controller.config.vel_limit = new_velocity_lim print("Set global velocity limit = " + str(new_velocity_lim)) - # Config and Error Dumps + # Config and Error Dumps ----- def dump_errors(self, clear=False): odrive.utils.dump_errors(self.odrv, clear)