main.cpp

00001 /* Includes ------------------------------------------------------------------*/
00002 
00003 /* mbed specific header files. */
00004 #include "mbed.h"
00005 
00006 /* Helper header files. */
00007 #include "DevSPI.h"
00008 
00009 /* Expansion Board specific header files. */
00010 #include "XNucleoIHM02A1.h"
00011 
00012 
00013 /* Definitions ---------------------------------------------------------------*/
00014 
00015 // Max time to run looking for a stall to mark home position
00016 #define HOME_TIMEOUT 5s
00017 
00018 // LED period / 2
00019 #define LED_ON_TIME 250ms
00020 
00021 
00022 /* Variables -----------------------------------------------------------------*/
00023 
00024 /* Motor Control Expansion Board. */
00025 XNucleoIHM02A1 *motor_shield;
00026 
00027 /* Initialization parameters of the motors connected to the expansion board. 
00028    Motor: https://www.amazon.com/100-Planetary-Gearbox-Stepper-Torque/dp/B00PNEQPCU/ref=psdc_306577011_t1_B00PNEQFAM
00029    Rated current: 0.67 A
00030    Phase resistance: 9.2 Ω
00031    Steps per rev: 200 -> 100:1 gearbox -> 20,000 steps per output shaft rev.
00032    
00033    Very good brief on the L6470 and what the below parameters do:
00034    https://www.mouser.com/simple_stepper_motor/
00035  */
00036 L6470_init_t init[L6470DAISYCHAINSIZE] = {
00037     /* First Motor. */
00038     {
00039         12.0,                          /* Motor supply voltage in V. */
00040         200,                           /* Min number of steps per revolution for the motor. */
00041         0.5,                           /* Max motor phase voltage in A. */
00042         6.0,                           /* Max motor phase voltage in V. */
00043         300.0,                         /* Motor initial speed [step/s]. */
00044         500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00045         500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00046         992.0,                         /* Motor maximum speed [step/s]. */
00047         0.0,                           /* Motor minimum speed [step/s]. */
00048         0.0,                           /* Motor full-step speed threshold [step/s]. */
00049         6.0,                           /* Holding kval [V]. */
00050         6.0,                           /* Constant speed kval [V]. */
00051         6.0,                           /* Acceleration starting kval [V]. */
00052         6.0,                           /* Deceleration starting kval [V]. */
00053         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00054         392.1569e-6,                   /* Start slope [s/step]. */
00055         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00056         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00057         0,                             /* Thermal compensation factor (range [0, 15]). */
00058         400.0,                         /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00059         100.0,                         /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00060         StepperMotor::STEP_MODE_FULL,  /* Step mode selection. */
00061         0xFF,                          /* Alarm conditions enable. */
00062         0x2E88                         /* Ic configuration. */
00063     },
00064 
00065     /* Second Motor. */
00066     {
00067         12.0,                          /* Motor supply voltage in V. */
00068         200,                           /* Min number of steps per revolution for the motor. */
00069         0.5,                           /* Max motor phase voltage in A. */
00070         6.0,                           /* Max motor phase voltage in V. */
00071         300.0,                         /* Motor initial speed [step/s]. */
00072         500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00073         500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00074         992.0,                         /* Motor maximum speed [step/s]. */
00075         0.0,                           /* Motor minimum speed [step/s]. */
00076         0.0,                           /* Motor full-step speed threshold [step/s]. */
00077         6.0,                           /* Holding kval [V]. */
00078         6.0,                           /* Constant speed kval [V]. */
00079         6.0,                           /* Acceleration starting kval [V]. */
00080         6.0,                           /* Deceleration starting kval [V]. */
00081         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00082         392.1569e-6,                   /* Start slope [s/step]. */
00083         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00084         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00085         0,                             /* Thermal compensation factor (range [0, 15]). */
00086         400.0,                         /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00087         100.0,                         /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00088         StepperMotor::STEP_MODE_FULL,  /* Step mode selection. */
00089         0xFF,                          /* Alarm conditions enable. */
00090         0x2E88                         /* Ic configuration. */
00091     }
00092 };
00093 
00094 bool flag_set = false;
00095 bool timeout_set = false;
00096 bool found_limit = false;
00097 
00098 void flag_handler() 
00099 {
00100     flag_set = true;
00101 }
00102 
00103 void timeout_handler()
00104 {
00105     timeout_set = true;
00106 }
00107     
00108 void limit_sw_handler()
00109 {
00110     found_limit = true;
00111 }
00112 
00113 /* Main ----------------------------------------------------------------------*/
00114 
00115 int main()
00116 {
00117     /*----- Initialization. -----*/
00118 
00119     /* Initializing SPI bus. */
00120 #ifdef TARGET_STM32F429
00121     DevSPI dev_spi(D11, D12, D13);
00122 #else
00123     DevSPI dev_spi(D11, D12, D3);
00124 #endif
00125 
00126     DigitalOut led1(LED1);
00127         
00128     Timeout t;
00129 
00130     /* Initializing Motor Control Expansion Board. */
00131     motor_shield = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
00132 
00133     /* Building a list of motor control components. */
00134     L6470 **motors = motor_shield->get_components();
00135 
00136     /*----- Setting home position - run at constant speed until stall -----*/
00137     printf("Attempting to home.\r\n");
00138     
00139     motors[0]->attach_flag_irq(&flag_handler);
00140     motors[0]->enable_flag_irq();
00141         
00142     // Run at a specified speed (in steps/s). 
00143     //   Goal is to stall and halt using the IRQ, and set the zero position there.
00144     //   Otherwise, time out
00145     t.attach(&timeout_handler, HOME_TIMEOUT);
00146     // Run "backward" so the farthest "backward" is the zero position.
00147     motors[0]->run(StepperMotor::BWD, 20000);
00148     
00149     while(!(flag_set or timeout_set)) {
00150         led1 = !led1;
00151         ThisThread::sleep_for(LED_ON_TIME);
00152     }
00153     
00154     // We're either there or timed out; halt the timeout and hard stop the motor
00155     t.detach();
00156     motors[0]->hard_stop();
00157     
00158     if (flag_set) {
00159         printf("Flag was set, status register: 0x%02X\r\n", motors[0]->get_status());
00160         flag_set = false;
00161     }
00162     
00163     if (timeout_set) {
00164         printf("Timeout happened, home position probably invalid\r\n");
00165         timeout_set = false;
00166     }
00167    
00168     // Set home position and print current position to console to confirm
00169     motors[0]->set_home();
00170     printf("Home set. Current Position: %d\r\n", motors[0]->get_position());
00171     
00172     /*----- Setting far limit (mark) position - run at constant speed until stall -----*/
00173     t.attach(&timeout_handler, HOME_TIMEOUT);
00174     motors[0]->run(StepperMotor::FWD, 20000);
00175     
00176     while(!(flag_set or timeout_set)) {
00177         led1 = !led1;
00178         ThisThread::sleep_for(LED_ON_TIME);
00179     }
00180     
00181     // We're either there or timed out; halt the timeout and hard stop the motor
00182     t.detach();
00183     motors[0]->hard_stop();
00184     
00185     if (flag_set) {
00186         printf("Flag was set, status register: 0x%02X\r\n", motors[0]->get_status());
00187         flag_set = false;
00188     }
00189     
00190     if (timeout_set) {
00191         printf("Timeout happened, mark position probably invalid\r\n");
00192         timeout_set = false;
00193     }
00194     
00195     motors[0]->set_mark();
00196     printf("Marked far limit. Current Position: %d\r\n", motors[0]->get_position());
00197     
00198     /*---- Now center the actuator -----*/
00199     motors[0]->go_to(motors[0]->get_mark()/2);
00200     
00201 
00202     /*----- High Impedance State. -----*/
00203     motors[0]->hard_hiz();
00204     printf("Motor Centered, Bridge High Z.\r\n");
00205 }