From 2b400958df014a3d8ec396b3e5d57f75fcc60632 Mon Sep 17 00:00:00 2001 From: Max Date: Thu, 22 Apr 2021 15:15:07 +0100 Subject: [PATCH] Update --- odrive_setup.py | 181 ++++++++++++++++++++++++++++++++++++++++++++++ pygame_control.py | 57 +++++++++++++++ 2 files changed, 238 insertions(+) create mode 100644 odrive_setup.py create mode 100644 pygame_control.py diff --git a/odrive_setup.py b/odrive_setup.py new file mode 100644 index 0000000..925be05 --- /dev/null +++ b/odrive_setup.py @@ -0,0 +1,181 @@ +import sys +import time +import odrive +from odrive.enums import * +from fibre.protocol import ChannelBrokenException +import logging + +logging.basicConfig(level='DEBUG') + +odrv = odrive.find_any() + +def erase_previous_config(odrv0: object): + odrv0.erase_configuration() + logging.warning('Oodrv0 config erased') + +def set_pole_pairs(odrv0: object, pole_pairs: int): + # pole pairs = number of magnets in motor / 2 +print(odrv0.axis0.motor.config.pole_pairs) +print(odrv0.axis1.motor.config.pole_pairs) + logging.warning(f'Both channels set to {pole_pairs} pole pairs') + +def unknowns(odrv0): +logging.warning(f"Axis 0 resistance calib max voltage: {odrv0.axis0.motor.config.resistance_calib_max_voltage}") +logging.warning(f"Axis 0 requested current range: {odrv0.axis0.motor.config.requested_current_range}") +logging.warning(f"Axis 0 current control bandwidth: {odrv0.axis0.motor.config.current_control_bandwidth}") +logging.warning(f"Axis 0 torque constant: {odrv0.axis0.motor.config.torque_constant}") +logging.warning(f"Axis 1 resistance calib max voltage: {odrv0.axis1.motor.config.resistance_calib_max_voltage}") +logging.warning(f"Axis 1 requested current range: {odrv0.axis1.motor.config.requested_current_range}") +logging.warning(f"Axis 1 current control bandwidth: {odrv0.axis1.motor.config.current_control_bandwidth}") +logging.warning(f"Axis 1 torque constant: {odrv0.axis1.motor.config.torque_constant}") + +def encoder_setup(odrv0: object, cpr: int): +odrv0.axis0.encoder.config.mode = 0 # ENCODER_MODE_HALL This one doesn't work... +odrv0.axis1.encoder.config.mode = 0 # ENCODER_MODE_HALL This one doesn't work... + logging.warning('Set encoders to INCREMENTAL') +odrv0.axis0.encoder.config.cpr = 4000 +odrv0.axis1.encoder.config.cpr = 4000 + +odrv0.axis0.encoder.config.use_index = True +odrv0.axis1.encoder.config.use_index = True +odrv0.axis0.encoder.config.ignore_illegal_hall_state = True +odrv0.axis1.encoder.config.ignore_illegal_hall_state = True + logging.warning(f'Set encoders to {cpr} cpr') +logging.warning(f"Axis 0 encoder calib scan distance: {odrv0.axis0.encoder.calib_scan_distance}") +logging.warning(f"Axis 1 encoder calib scan distance: {odrv0.axis1.encoder.calib_scan_distance}") +logging.warning(f"Axis 0 encoder bandwidth: {odrv0.axis0.encoder.bandwidth}") +logging.warning(f"Axis 1 encoder bandwidth: {odrv0.axis1.encoder.bandwidth}") + +def controller_setup(odrv0: object, pos_gain: int = 1, vel_limit: int = 0.5): # Low vel limit (1 turn every 2s to avoid crsahing lol) + # https://docs.oodrv0robotics.com/control + odrv0.axis0.controller.config.pos_gain = pos_gain + odrv0.axis1.controller.config.pos_gain = pos_gain + logging.warning(f'Set positional gain to {pos_gain}') + ### NOTE: The VEL gain determines how fast the motor will move from point A to point B, may need to up/ tweak the value + odrv0.axis0.controller.config.vel_gain = 0.02 * odrv0.axis0.motor.config.torque_constant * odrv0.axis0.encoder.config.cpr + odrv0.axis1.controller.config.vel_gain = 0.02 * odrv0.axis1.motor.config.torque_constant * odrv0.axis1.encoder.config.cpr + logging.warning(f'Set velocity gain to 0.02*torque_constant*cpr') + odrv0.axis0.controller.config.vel_integrator_gain = 0.1 * odrv0.axis0.motor.config.torque_constant * odrv0.axis0.encoder.config.cpr + odrv0.axis1.controller.config.vel_integrator_gain = 0.1 * odrv0.axis1.motor.config.torque_constant * odrv0.axis1.encoder.config.cpr + logging.warning(f'Set velocity integrator gain to 0.1*torque_constant*cpr') +odrv0.axis0.controller.config.vel_limit = 10 +odrv0.axis1.controller.config.vel_limit = 10 + logging.warning(f'Set velocity limit to {vel_limit}') + +def set_position_control_mode(odrv0: object): +odrv0.axis0.controller.config.control_mode = CONTROL_MODE_POSITION_CONTROL +odrv0.axis1.controller.config.control_mode = CONTROL_MODE_POSITION_CONTROL + logging.warning(f'Set control_mode to CONTROL_MODE_POSITION_CONTROL') + + odrv0.save_configuration() + logging.warning(f'Saved new config') + + odrv0.reboot() + logging.warning(f'Rebooting odrv0') + time.sleep(1) + +### NOTE: free motor os any load etc. + +def auto_oodrv0_calibration_sequence(odrv0: object): +odrv0.axis0.requested_state = AXIS_STATE_MOTOR_CALIBRATION +odrv0.axis1.requested_state = AXIS_STATE_MOTOR_CALIBRATION + logging.warning(f'Set state to AXIS_STATE_MOTOR_CALIBRATION, sleeping for 5 s.') + time.sleep(10) + + if odrv0.axis0.motor.error != 0: + logging.error(f"Error: Oodrv0 reported an error of {odrv0.axis0.motor.error} while in the state " + "AXIS_STATE_MOTOR_CALIBRATION. Printing out Oodrv0 motor data for " + f"debug:\n {odrv0.axis0.motor}") + + if odrv0.axis1.motor.error != 0: + logging.error(f"Error: Oodrv0 reported an error of {odrv0.axis1.motor.error} while in the state " + "AXIS_STATE_MOTOR_CALIBRATION. Printing out Oodrv0 motor data for " + f"debug:\n {odrv0.axis1.motor}") + +logging.critical(f"Axis 0 phase inductence is: {odrv0.axis0.motor.config.phase_inductance}, check that it is 0.001 -> 0.0001") +logging.critical(f"Axis 1 phase inductence is: {odrv0.axis1.motor.config.phase_inductance}, check that it is 0.001 -> 0.0001") +logging.critical(f"Axis 0 phase resistance is: {odrv0.axis0.motor.config.phase_resistance}, check that it is 0.1 -> 0.5") +logging.critical(f"Axis 1 phase resistance is: {odrv0.axis1.motor.config.phase_resistance}, check that it is 0.1 -> 0.5") + a = input('Ok to continue? [y/N]') + if not (a == 'y' or a == 'Y'): + logging.info('Exiting...') + sys.exit(0) + +odrv0.axis0.motor.config.pre_calibrated = True +odrv0.axis1.motor.config.pre_calibrated = True + logging.warning(f'Setting motors to pre calibrated') + + time.sleep(5) + logging.info("Starting encoder calibration, wait 30s.") +odrv0.axis0.requested_state = AXIS_STATE_ENCODER_OFFSET_CALIBRATION +odrv0.axis1.requested_state = AXIS_STATE_ENCODER_OFFSET_CALIBRATION + time.sleep(30) + + if odrv0.axis0.encoder.error != 0: + logging.error(f"Error: Oodrv0 reported an error of {odrv0.axis0.encoder.error} while in the state " + "AXIS_STATE_MOTOR_CALIBRATION. Printing out Oodrv0 motor data for " + f"debug:\n {odrv0.axis0.encoder}") + + if odrv0.axis1.encoder.error != 0: + logging.error(f"Error: Oodrv0 reported an error of {odrv0.axis1.encoder.error} while in the state " + "AXIS_STATE_MOTOR_CALIBRATION. Printing out Oodrv0 motor data for " + f"debug:\n {odrv0.axis1.encoder}") + +logging.critical(f"Axis 0 encoder offset is: {odrv0.axis0.encoder.config.offset_float}, check that it is 0.5 or 1.5") +logging.critical(f"Axis 1 encoder offset is: {odrv0.axis1.encoder.config.offset_float}, check that it is 0.5 or 1.5") + a = input('Ok to continue? [y/N]') + if not (a == 'y' or a == 'Y'): + logging.info('Exiting...') + sys.exit(0) + +odrv0.axis0.encoder.config.pre_calibrated = True +odrv0.axis1.encoder.config.pre_calibrated = True + logging.warning(f'Setting encoders to pre calibrated') + + odrv0.save_configuration() + logging.warning(f'Saved new config') + + odrv0.reboot() + logging.warning(f'Rebooting odrv0') + time.sleep(1) + +def bootinit_index_search(odrv0): +odrv0.axis0.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH +odrv0.axis1.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH + +def set_closed_loop_ctrl(odrv0: object): +odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL +odrv0.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL + +def set_idle(odrv0: object): +odrv0.axis0.requested_state = AXIS_STATE_IDLE +odrv0.axis1.requested_state = AXIS_STATE_IDLE + +def set_current_limit(odrv0: object): +odrv0.axis0.motor.config.current_lim = 15 +odrv0.axis1.motor.config.current_lim = 15 # Amps, up this to 90 etc later... + +def set_brake_resistance(odrv0): + pass + # odrv0.config.brake_resistance = ? // find out brake resistance + +def goto_xy(x: float, y: float, odrv0: object): + pings_per_cm_0 = 10 # NOTE This is completely ARBITRARY!!! and needs to be calibrated + pings_per_cm_1 = 10 # NOTE This is completely ARBITRARY!!! and needs to be calibrated + + pi = 3.142 + pulley_diameter = 10 # cm + distance_per_revolution = pi * pulley_diameter + + + target_angle_0 = (y / distance_per_revolution) - (x / distance_per_revolution) + target_angle_1 = (y / distance_per_revolution) + (x / distance_per_revolution) # TODO: Derive this matrix + + odrv0.axis0.controller.input_pos = target_angle_0 # NOTE TODO DOO NOT RUN THIS BEFORE VERIFYING ENCODER OFFSET STATUS + odrv0.axis1.controller.input_pos = target_angle_1 + +odrv0.axis0.controller.input_pos = 10 +odrv0.axis1.controller.input_pos = 10 + +odrv0.axis0.controller.input_pos = 0 +odrv0.axis1.controller.input_pos = 0 diff --git a/pygame_control.py b/pygame_control.py new file mode 100644 index 0000000..04b196d --- /dev/null +++ b/pygame_control.py @@ -0,0 +1,57 @@ +import pygame + +pygame.init() +pygame.font.init() +font_name = pygame.font.get_default_font() +font = pygame.font.Font(font_name, 20) +window = pygame.display.set_mode((500, 500)) +pygame.display.set_caption("RAH Visual Controller V0.1") + +run = True +armed = False + +prev_x = 0 +prev_y = 0 + +def update_and_send_pos_to_odrive(): + pass + + +while run: + pygame.time.delay(20) + for event in pygame.event.get(): + if event.type == pygame.QUIT: + run = False + + + mouse_x, mouse_y = pygame.mouse.get_pos() + + keys = pygame.key.get_pressed() + if keys[pygame.K_SPACE]: + armed = True + window.fill((64, 0, 0)) + idkwhatthisis_2 = font.render(f"Armed: {armed}", True, (255, 0, 0)) + prev_x = mouse_x + prev_y = mouse_y + # clear where the striker currently is + else: + armed = False + window.fill((10, 10, 10)) + pygame.draw.circle(window, (255, 0, 0), (prev_x, prev_y), (20)) + idkwhatthisis_2 = font.render(f"Armed: {armed}", True, (200, 200, 200)) + # draw where the striker currently is + + # window.fill((0, 0, 0)) + pygame.draw.circle(window, (255, 0, 0), (mouse_x, mouse_y), (20)) + pygame.draw.rect(window, (0, 255, 0), (mouse_x, 0, 1, 500)) + pygame.draw.rect(window, (0, 128, 255), (0, mouse_y, 500, 1)) + idkwhatthisis = font.render(f"X: {mouse_x} | Y: {mouse_y}", True, (255, 255, 255)) + window.blit(idkwhatthisis, (0, 0)) + window.blit(idkwhatthisis_2, (0, 480)) + pygame.display.update() + + if armed: + update_and_send_pos_to_odrive() + +pygame.font.quit() +pygame.quit()