Thomas Byrne
l6470_gimbal_controller_os5
Dependencies: X_NUCLEO_IHM02A1
Diff: main.cpp
- Revision:
- 0:08a64a788d1f
- Child:
- 1:13fb32a7d8ce
--- /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");
+}