l6470_gimbal_controller_os5
Dependencies: X_NUCLEO_IHM02A1
main.cpp
- Committer:
- ersatzavian
- Date:
- 2021-03-29
- Revision:
- 0:08a64a788d1f
- Child:
- 1:13fb32a7d8ce
File content as of revision 0:08a64a788d1f:
/* Includes ------------------------------------------------------------------*/ /* mbed specific header files. */ #include "mbed.h" /* Helper header files. */ #include "DevSPI.h" /* Expansion Board specific header files. */ #include "XNucleoIHM02A1.h" /* Definitions ---------------------------------------------------------------*/ // Max time to run looking for a stall to mark home position #define HOME_TIMEOUT_S 5 // LED period / 2 #define LED_ON_S 0.25 /* Variables -----------------------------------------------------------------*/ /* Motor Control Expansion Board. */ XNucleoIHM02A1 *motor_shield; /* Initialization parameters of the motors connected to the expansion board. Motor: https://www.amazon.com/100-Planetary-Gearbox-Stepper-Torque/dp/B00PNEQPCU/ref=psdc_306577011_t1_B00PNEQFAM Rated current: 0.67 A Phase resistance: 9.2 Ω Steps per rev: 200 -> 100:1 gearbox -> 20,000 steps per output shaft rev. Very good brief on the L6470 and what the below parameters do: https://www.mouser.com/simple_stepper_motor/ */ L6470_init_t init[L6470DAISYCHAINSIZE] = { /* First Motor. */ { 12.0, /* Motor supply voltage in V. */ 200, /* Min number of steps per revolution for the motor. */ 0.5, /* Max motor phase voltage in A. */ 6.0, /* Max motor phase voltage in V. */ 300.0, /* Motor initial speed [step/s]. */ 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */ 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */ 992.0, /* Motor maximum speed [step/s]. */ 0.0, /* Motor minimum speed [step/s]. */ 0.0, /* Motor full-step speed threshold [step/s]. */ 6.0, /* Holding kval [V]. */ 6.0, /* Constant speed kval [V]. */ 6.0, /* Acceleration starting kval [V]. */ 6.0, /* Deceleration starting kval [V]. */ 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */ 392.1569e-6, /* Start slope [s/step]. */ 643.1372e-6, /* Acceleration final slope [s/step]. */ 643.1372e-6, /* Deceleration final slope [s/step]. */ 0, /* Thermal compensation factor (range [0, 15]). */ 400.0, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */ 100.0, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */ StepperMotor::STEP_MODE_FULL, /* Step mode selection. */ 0xFF, /* Alarm conditions enable. */ 0x2E88 /* Ic configuration. */ }, /* Second Motor. */ { 12.0, /* Motor supply voltage in V. */ 200, /* Min number of steps per revolution for the motor. */ 0.5, /* Max motor phase voltage in A. */ 6.0, /* Max motor phase voltage in V. */ 300.0, /* Motor initial speed [step/s]. */ 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */ 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */ 992.0, /* Motor maximum speed [step/s]. */ 0.0, /* Motor minimum speed [step/s]. */ 0.0, /* Motor full-step speed threshold [step/s]. */ 6.0, /* Holding kval [V]. */ 6.0, /* Constant speed kval [V]. */ 6.0, /* Acceleration starting kval [V]. */ 6.0, /* Deceleration starting kval [V]. */ 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */ 392.1569e-6, /* Start slope [s/step]. */ 643.1372e-6, /* Acceleration final slope [s/step]. */ 643.1372e-6, /* Deceleration final slope [s/step]. */ 0, /* Thermal compensation factor (range [0, 15]). */ 400.0, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */ 100.0, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */ StepperMotor::STEP_MODE_FULL, /* Step mode selection. */ 0xFF, /* Alarm conditions enable. */ 0x2E88 /* Ic configuration. */ } }; bool flag_set = false; bool timeout_set = false; bool found_limit = false; void flag_handler() { flag_set = true; } void timeout_handler() { timeout_set = true; } void limit_sw_handler() { found_limit = true; } /* Main ----------------------------------------------------------------------*/ int main() { /*----- Initialization. -----*/ /* Initializing SPI bus. */ #ifdef TARGET_STM32F429 DevSPI dev_spi(D11, D12, D13); #else DevSPI dev_spi(D11, D12, D3); #endif DigitalOut led1(LED1); Timeout t; /* Initializing Motor Control Expansion Board. */ motor_shield = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi); /* Building a list of motor control components. */ L6470 **motors = motor_shield->get_components(); /*----- Setting home position - run at constant speed until stall -----*/ printf("Attempting to home.\r\n"); motors[0]->attach_flag_irq(&flag_handler); motors[0]->enable_flag_irq(); // Run at a specified speed (in steps/s). // Goal is to stall and halt using the IRQ, and set the zero position there. // Otherwise, time out t.attach(&timeout_handler, HOME_TIMEOUT_S); // Run "backward" so the farthest "backward" is the zero position. motors[0]->run(StepperMotor::BWD, 20000); while(!(flag_set or timeout_set)) { led1 = !led1; wait_ms(LED_ON_S * 1000); } // We're either there or timed out; halt the timeout and hard stop the motor t.detach(); motors[0]->hard_stop(); if (flag_set) { printf("Flag was set, status register: 0x%02X\r\n", motors[0]->get_status()); flag_set = false; } if (timeout_set) { printf("Timeout happened, home position probably invalid\r\n"); timeout_set = false; } // Set home position and print current position to console to confirm motors[0]->set_home(); printf("Home set. Current Position: %d\r\n", motors[0]->get_position()); /*----- Setting far limit (mark) position - run at constant speed until stall -----*/ t.attach(&timeout_handler, HOME_TIMEOUT_S); motors[0]->run(StepperMotor::FWD, 20000); while(!(flag_set or timeout_set)) { led1 = !led1; wait_ms(LED_ON_S * 1000); } // We're either there or timed out; halt the timeout and hard stop the motor t.detach(); motors[0]->hard_stop(); if (flag_set) { printf("Flag was set, status register: 0x%02X\r\n", motors[0]->get_status()); flag_set = false; } if (timeout_set) { printf("Timeout happened, mark position probably invalid\r\n"); timeout_set = false; } motors[0]->set_mark(); printf("Marked far limit. Current Position: %d\r\n", motors[0]->get_position()); /*---- Now center the actuator -----*/ motors[0]->go_to(motors[0]->get_mark()/2); /*----- High Impedance State. -----*/ motors[0]->hard_hiz(); printf("Motor Centered, Bridge High Z.\r\n"); }