Thomas Byrne / Mbed OS l6470_gimbal_controller

l6470_gimbal_controller_os5

Dependencies:   X_NUCLEO_IHM02A1

main.cpp@1:13fb32a7d8ce, 2021-03-30 (annotated)

Committer:
tom_astranis
Date:
Tue Mar 30 00:35:56 2021 +0000
Revision:
1:13fb32a7d8ce
Parent:
0:08a64a788d1f 
Ported to mbed os 5 to use mbed studio

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ersatzavian 0:08a64a788d1f 1 /* Includes ------------------------------------------------------------------*/
ersatzavian 0:08a64a788d1f 2
ersatzavian 0:08a64a788d1f 3 /* mbed specific header files. */
ersatzavian 0:08a64a788d1f 4 #include "mbed.h"
ersatzavian 0:08a64a788d1f 5
ersatzavian 0:08a64a788d1f 6 /* Helper header files. */
ersatzavian 0:08a64a788d1f 7 #include "DevSPI.h"
ersatzavian 0:08a64a788d1f 8
ersatzavian 0:08a64a788d1f 9 /* Expansion Board specific header files. */
ersatzavian 0:08a64a788d1f 10 #include "XNucleoIHM02A1.h"
ersatzavian 0:08a64a788d1f 11
ersatzavian 0:08a64a788d1f 12
ersatzavian 0:08a64a788d1f 13 /* Definitions ---------------------------------------------------------------*/
ersatzavian 0:08a64a788d1f 14
ersatzavian 0:08a64a788d1f 15 // Max time to run looking for a stall to mark home position
tom_astranis 1:13fb32a7d8ce 16 #define HOME_TIMEOUT 5s
ersatzavian 0:08a64a788d1f 17
ersatzavian 0:08a64a788d1f 18 // LED period / 2
tom_astranis 1:13fb32a7d8ce 19 #define LED_ON_TIME 250ms
ersatzavian 0:08a64a788d1f 20
ersatzavian 0:08a64a788d1f 21
ersatzavian 0:08a64a788d1f 22 /* Variables -----------------------------------------------------------------*/
ersatzavian 0:08a64a788d1f 23
ersatzavian 0:08a64a788d1f 24 /* Motor Control Expansion Board. */
ersatzavian 0:08a64a788d1f 25 XNucleoIHM02A1 *motor_shield;
ersatzavian 0:08a64a788d1f 26
ersatzavian 0:08a64a788d1f 27 /* Initialization parameters of the motors connected to the expansion board.
ersatzavian 0:08a64a788d1f 28 Motor: https://www.amazon.com/100-Planetary-Gearbox-Stepper-Torque/dp/B00PNEQPCU/ref=psdc_306577011_t1_B00PNEQFAM
ersatzavian 0:08a64a788d1f 29 Rated current: 0.67 A
ersatzavian 0:08a64a788d1f 30 Phase resistance: 9.2 Ω
ersatzavian 0:08a64a788d1f 31 Steps per rev: 200 -> 100:1 gearbox -> 20,000 steps per output shaft rev.
ersatzavian 0:08a64a788d1f 32
ersatzavian 0:08a64a788d1f 33 Very good brief on the L6470 and what the below parameters do:
ersatzavian 0:08a64a788d1f 34 https://www.mouser.com/simple_stepper_motor/
ersatzavian 0:08a64a788d1f 35 */
ersatzavian 0:08a64a788d1f 36 L6470_init_t init[L6470DAISYCHAINSIZE] = {
ersatzavian 0:08a64a788d1f 37 /* First Motor. */
ersatzavian 0:08a64a788d1f 38 {
ersatzavian 0:08a64a788d1f 39 12.0, /* Motor supply voltage in V. */
ersatzavian 0:08a64a788d1f 40 200, /* Min number of steps per revolution for the motor. */
ersatzavian 0:08a64a788d1f 41 0.5, /* Max motor phase voltage in A. */
ersatzavian 0:08a64a788d1f 42 6.0, /* Max motor phase voltage in V. */
ersatzavian 0:08a64a788d1f 43 300.0, /* Motor initial speed [step/s]. */
ersatzavian 0:08a64a788d1f 44 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
ersatzavian 0:08a64a788d1f 45 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
ersatzavian 0:08a64a788d1f 46 992.0, /* Motor maximum speed [step/s]. */
ersatzavian 0:08a64a788d1f 47 0.0, /* Motor minimum speed [step/s]. */
ersatzavian 0:08a64a788d1f 48 0.0, /* Motor full-step speed threshold [step/s]. */
ersatzavian 0:08a64a788d1f 49 6.0, /* Holding kval [V]. */
ersatzavian 0:08a64a788d1f 50 6.0, /* Constant speed kval [V]. */
ersatzavian 0:08a64a788d1f 51 6.0, /* Acceleration starting kval [V]. */
ersatzavian 0:08a64a788d1f 52 6.0, /* Deceleration starting kval [V]. */
ersatzavian 0:08a64a788d1f 53 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
ersatzavian 0:08a64a788d1f 54 392.1569e-6, /* Start slope [s/step]. */
ersatzavian 0:08a64a788d1f 55 643.1372e-6, /* Acceleration final slope [s/step]. */
ersatzavian 0:08a64a788d1f 56 643.1372e-6, /* Deceleration final slope [s/step]. */
ersatzavian 0:08a64a788d1f 57 0, /* Thermal compensation factor (range [0, 15]). */
ersatzavian 0:08a64a788d1f 58 400.0, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
ersatzavian 0:08a64a788d1f 59 100.0, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
ersatzavian 0:08a64a788d1f 60 StepperMotor::STEP_MODE_FULL, /* Step mode selection. */
ersatzavian 0:08a64a788d1f 61 0xFF, /* Alarm conditions enable. */
ersatzavian 0:08a64a788d1f 62 0x2E88 /* Ic configuration. */
ersatzavian 0:08a64a788d1f 63 },
ersatzavian 0:08a64a788d1f 64
ersatzavian 0:08a64a788d1f 65 /* Second Motor. */
ersatzavian 0:08a64a788d1f 66 {
ersatzavian 0:08a64a788d1f 67 12.0, /* Motor supply voltage in V. */
ersatzavian 0:08a64a788d1f 68 200, /* Min number of steps per revolution for the motor. */
ersatzavian 0:08a64a788d1f 69 0.5, /* Max motor phase voltage in A. */
ersatzavian 0:08a64a788d1f 70 6.0, /* Max motor phase voltage in V. */
ersatzavian 0:08a64a788d1f 71 300.0, /* Motor initial speed [step/s]. */
ersatzavian 0:08a64a788d1f 72 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
ersatzavian 0:08a64a788d1f 73 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
ersatzavian 0:08a64a788d1f 74 992.0, /* Motor maximum speed [step/s]. */
ersatzavian 0:08a64a788d1f 75 0.0, /* Motor minimum speed [step/s]. */
ersatzavian 0:08a64a788d1f 76 0.0, /* Motor full-step speed threshold [step/s]. */
ersatzavian 0:08a64a788d1f 77 6.0, /* Holding kval [V]. */
ersatzavian 0:08a64a788d1f 78 6.0, /* Constant speed kval [V]. */
ersatzavian 0:08a64a788d1f 79 6.0, /* Acceleration starting kval [V]. */
ersatzavian 0:08a64a788d1f 80 6.0, /* Deceleration starting kval [V]. */
ersatzavian 0:08a64a788d1f 81 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
ersatzavian 0:08a64a788d1f 82 392.1569e-6, /* Start slope [s/step]. */
ersatzavian 0:08a64a788d1f 83 643.1372e-6, /* Acceleration final slope [s/step]. */
ersatzavian 0:08a64a788d1f 84 643.1372e-6, /* Deceleration final slope [s/step]. */
ersatzavian 0:08a64a788d1f 85 0, /* Thermal compensation factor (range [0, 15]). */
ersatzavian 0:08a64a788d1f 86 400.0, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
ersatzavian 0:08a64a788d1f 87 100.0, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
ersatzavian 0:08a64a788d1f 88 StepperMotor::STEP_MODE_FULL, /* Step mode selection. */
ersatzavian 0:08a64a788d1f 89 0xFF, /* Alarm conditions enable. */
ersatzavian 0:08a64a788d1f 90 0x2E88 /* Ic configuration. */
ersatzavian 0:08a64a788d1f 91 }
ersatzavian 0:08a64a788d1f 92 };
ersatzavian 0:08a64a788d1f 93
ersatzavian 0:08a64a788d1f 94 bool flag_set = false;
ersatzavian 0:08a64a788d1f 95 bool timeout_set = false;
ersatzavian 0:08a64a788d1f 96 bool found_limit = false;
ersatzavian 0:08a64a788d1f 97
ersatzavian 0:08a64a788d1f 98 void flag_handler()
ersatzavian 0:08a64a788d1f 99 {
ersatzavian 0:08a64a788d1f 100 flag_set = true;
ersatzavian 0:08a64a788d1f 101 }
ersatzavian 0:08a64a788d1f 102
ersatzavian 0:08a64a788d1f 103 void timeout_handler()
ersatzavian 0:08a64a788d1f 104 {
ersatzavian 0:08a64a788d1f 105 timeout_set = true;
ersatzavian 0:08a64a788d1f 106 }
ersatzavian 0:08a64a788d1f 107
ersatzavian 0:08a64a788d1f 108 void limit_sw_handler()
ersatzavian 0:08a64a788d1f 109 {
ersatzavian 0:08a64a788d1f 110 found_limit = true;
ersatzavian 0:08a64a788d1f 111 }
ersatzavian 0:08a64a788d1f 112
ersatzavian 0:08a64a788d1f 113 /* Main ----------------------------------------------------------------------*/
ersatzavian 0:08a64a788d1f 114
ersatzavian 0:08a64a788d1f 115 int main()
ersatzavian 0:08a64a788d1f 116 {
ersatzavian 0:08a64a788d1f 117 /*----- Initialization. -----*/
ersatzavian 0:08a64a788d1f 118
ersatzavian 0:08a64a788d1f 119 /* Initializing SPI bus. */
ersatzavian 0:08a64a788d1f 120 #ifdef TARGET_STM32F429
ersatzavian 0:08a64a788d1f 121 DevSPI dev_spi(D11, D12, D13);
ersatzavian 0:08a64a788d1f 122 #else
ersatzavian 0:08a64a788d1f 123 DevSPI dev_spi(D11, D12, D3);
ersatzavian 0:08a64a788d1f 124 #endif
ersatzavian 0:08a64a788d1f 125
ersatzavian 0:08a64a788d1f 126 DigitalOut led1(LED1);
ersatzavian 0:08a64a788d1f 127
ersatzavian 0:08a64a788d1f 128 Timeout t;
ersatzavian 0:08a64a788d1f 129
ersatzavian 0:08a64a788d1f 130 /* Initializing Motor Control Expansion Board. */
ersatzavian 0:08a64a788d1f 131 motor_shield = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
ersatzavian 0:08a64a788d1f 132
ersatzavian 0:08a64a788d1f 133 /* Building a list of motor control components. */
ersatzavian 0:08a64a788d1f 134 L6470 **motors = motor_shield->get_components();
ersatzavian 0:08a64a788d1f 135
ersatzavian 0:08a64a788d1f 136 /*----- Setting home position - run at constant speed until stall -----*/
ersatzavian 0:08a64a788d1f 137 printf("Attempting to home.\r\n");
ersatzavian 0:08a64a788d1f 138
ersatzavian 0:08a64a788d1f 139 motors[0]->attach_flag_irq(&flag_handler);
ersatzavian 0:08a64a788d1f 140 motors[0]->enable_flag_irq();
ersatzavian 0:08a64a788d1f 141
ersatzavian 0:08a64a788d1f 142 // Run at a specified speed (in steps/s).
ersatzavian 0:08a64a788d1f 143 // Goal is to stall and halt using the IRQ, and set the zero position there.
ersatzavian 0:08a64a788d1f 144 // Otherwise, time out
tom_astranis 1:13fb32a7d8ce 145 t.attach(&timeout_handler, HOME_TIMEOUT);
ersatzavian 0:08a64a788d1f 146 // Run "backward" so the farthest "backward" is the zero position.
ersatzavian 0:08a64a788d1f 147 motors[0]->run(StepperMotor::BWD, 20000);
ersatzavian 0:08a64a788d1f 148
ersatzavian 0:08a64a788d1f 149 while(!(flag_set or timeout_set)) {
ersatzavian 0:08a64a788d1f 150 led1 = !led1;
tom_astranis 1:13fb32a7d8ce 151 ThisThread::sleep_for(LED_ON_TIME);
ersatzavian 0:08a64a788d1f 152 }
ersatzavian 0:08a64a788d1f 153
ersatzavian 0:08a64a788d1f 154 // We're either there or timed out; halt the timeout and hard stop the motor
ersatzavian 0:08a64a788d1f 155 t.detach();
ersatzavian 0:08a64a788d1f 156 motors[0]->hard_stop();
ersatzavian 0:08a64a788d1f 157
ersatzavian 0:08a64a788d1f 158 if (flag_set) {
ersatzavian 0:08a64a788d1f 159 printf("Flag was set, status register: 0x%02X\r\n", motors[0]->get_status());
ersatzavian 0:08a64a788d1f 160 flag_set = false;
ersatzavian 0:08a64a788d1f 161 }
ersatzavian 0:08a64a788d1f 162
ersatzavian 0:08a64a788d1f 163 if (timeout_set) {
ersatzavian 0:08a64a788d1f 164 printf("Timeout happened, home position probably invalid\r\n");
ersatzavian 0:08a64a788d1f 165 timeout_set = false;
ersatzavian 0:08a64a788d1f 166 }
ersatzavian 0:08a64a788d1f 167
ersatzavian 0:08a64a788d1f 168 // Set home position and print current position to console to confirm
ersatzavian 0:08a64a788d1f 169 motors[0]->set_home();
ersatzavian 0:08a64a788d1f 170 printf("Home set. Current Position: %d\r\n", motors[0]->get_position());
ersatzavian 0:08a64a788d1f 171
ersatzavian 0:08a64a788d1f 172 /*----- Setting far limit (mark) position - run at constant speed until stall -----*/
tom_astranis 1:13fb32a7d8ce 173 t.attach(&timeout_handler, HOME_TIMEOUT);
ersatzavian 0:08a64a788d1f 174 motors[0]->run(StepperMotor::FWD, 20000);
ersatzavian 0:08a64a788d1f 175
ersatzavian 0:08a64a788d1f 176 while(!(flag_set or timeout_set)) {
ersatzavian 0:08a64a788d1f 177 led1 = !led1;
tom_astranis 1:13fb32a7d8ce 178 ThisThread::sleep_for(LED_ON_TIME);
ersatzavian 0:08a64a788d1f 179 }
ersatzavian 0:08a64a788d1f 180
ersatzavian 0:08a64a788d1f 181 // We're either there or timed out; halt the timeout and hard stop the motor
ersatzavian 0:08a64a788d1f 182 t.detach();
ersatzavian 0:08a64a788d1f 183 motors[0]->hard_stop();
ersatzavian 0:08a64a788d1f 184
ersatzavian 0:08a64a788d1f 185 if (flag_set) {
ersatzavian 0:08a64a788d1f 186 printf("Flag was set, status register: 0x%02X\r\n", motors[0]->get_status());
ersatzavian 0:08a64a788d1f 187 flag_set = false;
ersatzavian 0:08a64a788d1f 188 }
ersatzavian 0:08a64a788d1f 189
ersatzavian 0:08a64a788d1f 190 if (timeout_set) {
ersatzavian 0:08a64a788d1f 191 printf("Timeout happened, mark position probably invalid\r\n");
ersatzavian 0:08a64a788d1f 192 timeout_set = false;
ersatzavian 0:08a64a788d1f 193 }
ersatzavian 0:08a64a788d1f 194
ersatzavian 0:08a64a788d1f 195 motors[0]->set_mark();
ersatzavian 0:08a64a788d1f 196 printf("Marked far limit. Current Position: %d\r\n", motors[0]->get_position());
ersatzavian 0:08a64a788d1f 197
ersatzavian 0:08a64a788d1f 198 /*---- Now center the actuator -----*/
ersatzavian 0:08a64a788d1f 199 motors[0]->go_to(motors[0]->get_mark()/2);
ersatzavian 0:08a64a788d1f 200
ersatzavian 0:08a64a788d1f 201
ersatzavian 0:08a64a788d1f 202 /*----- High Impedance State. -----*/
ersatzavian 0:08a64a788d1f 203 motors[0]->hard_hiz();
ersatzavian 0:08a64a788d1f 204 printf("Motor Centered, Bridge High Z.\r\n");
ersatzavian 0:08a64a788d1f 205 }