l6470_gimbal_controller_os5
Dependencies: X_NUCLEO_IHM02A1
Diff: main.cpp
- Revision:
- 0:08a64a788d1f
- Child:
- 1:13fb32a7d8ce
diff -r 000000000000 -r 08a64a788d1f main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Mar 29 23:51:53 2021 +0000 @@ -0,0 +1,205 @@ +/* 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"); +}