main.cpp

00001 #include "mbed.h"
00002 #include "DevSPI.h"
00003 #include "XNucleoIHM02A1.h"
00004 
00005 // after online publish
00006 
00007 //   MBED PUBLISH -A //////////////////////////
00008 //   MBED PUBLISH -A //////////////////////////
00009 //   MBED PUBLISH -A //////////////////////////
00010 
00011 #define Hovno 0
00012 #define Hovnaa 10
00013 
00014 
00015 #define MPR_1 4 /* Number of movements per revolution. */
00016 #define STEPS_1 (400 * 128)   /* 1 revolution given a 400 steps motor configured at 1/128 microstep mode. */
00017 #define STEPS_2 (STEPS_1 * 2)  /* 1 revolution given a 400 steps motor configured at 1/128 microstep mode. */
00018 #define DELAY_1 1000     /* Delay in milliseconds. */
00019 #define DELAY_2 2000    /* Delay in milliseconds. */
00020 #define DELAY_3 5000    /* Delay in milliseconds. */
00021 
00022 
00023 
00024 XNucleoIHM02A1 *x_nucleo_ihm02a1;  /* Motor Control Expansion Board. */
00025 
00026 /* Initialization parameters of the motors connected to the expansion board. */
00027 L6470_init_t init[L6470DAISYCHAINSIZE] = {
00028     /* First Motor. */
00029     {
00030         9.0,                           /* Motor supply voltage in V. */
00031         400,                           /* Min number of steps per revolution for the motor. */
00032         1.7,                           /* Max motor phase voltage in A. */
00033         3.06,                          /* Max motor phase voltage in V. */
00034         300.0,                         /* Motor initial speed [step/s]. */
00035         500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00036         500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00037         992.0,                         /* Motor maximum speed [step/s]. */
00038         0.0,                           /* Motor minimum speed [step/s]. */
00039         602.7,                         /* Motor full-step speed threshold [step/s]. */
00040         3.06,                          /* Holding kval [V]. */
00041         3.06,                          /* Constant speed kval [V]. */
00042         3.06,                          /* Acceleration starting kval [V]. */
00043         3.06,                          /* Deceleration starting kval [V]. */
00044         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00045         392.1569e-6,                   /* Start slope [s/step]. */
00046         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00047         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00048         0,                             /* Thermal compensation factor (range [0, 15]). */
00049         3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00050         3.06 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00051         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00052         0xFF,                          /* Alarm conditions enable. */
00053         0x2E88                         /* Ic configuration. */
00054     },
00055 
00056     /* Second Motor. */
00057     {
00058         9.0,                           /* Motor supply voltage in V. */
00059         400,                           /* Min number of steps per revolution for the motor. */
00060         1.7,                           /* Max motor phase voltage in A. */
00061         3.06,                          /* Max motor phase voltage in V. */
00062         300.0,                         /* Motor initial speed [step/s]. */
00063         500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00064         500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00065         992.0,                         /* Motor maximum speed [step/s]. */
00066         0.0,                           /* Motor minimum speed [step/s]. */
00067         602.7,                         /* Motor full-step speed threshold [step/s]. */
00068         3.06,                          /* Holding kval [V]. */
00069         3.06,                          /* Constant speed kval [V]. */
00070         3.06,                          /* Acceleration starting kval [V]. */
00071         3.06,                          /* Deceleration starting kval [V]. */
00072         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00073         392.1569e-6,                   /* Start slope [s/step]. */
00074         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00075         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00076         0,                             /* Thermal compensation factor (range [0, 15]). */
00077         3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00078         3.06 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00079         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00080         0xFF,                          /* Alarm conditions enable. */
00081         0x2E88                         /* Ic configuration. */
00082     }
00083 };
00084 
00085 
00086 /* Main ----------------------------------------------------------------------*/
00087 
00088 int main()
00089 {
00090     /*----- Initialization. -----*/
00091 
00092     /* Initializing SPI bus. */
00093 #ifdef TARGET_STM32F429
00094     DevSPI dev_spi(D11, D12, D13);
00095 #else
00096     DevSPI dev_spi(D11, D12, D3);
00097 #endif
00098 
00099 
00100     x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);      /* Initializing Motor Control Expansion Board. */
00101     L6470 **motors = x_nucleo_ihm02a1->get_components();       /* Building a list of motor control components. */
00102 
00103 
00104     printf("Motor Control Application Example for 2 Motors\r\n\n");
00105     printf("--> Setting home position.\r\n");    /* Printing to the console. */
00106     motors[0]->set_home(); /* Setting the home position. */
00107     wait_ms(DELAY_1);
00108 
00109     int position = motors[0]->get_position();     /* Getting the current position. */
00110     printf("--> Getting the current position: %d\r\n", position);
00111     wait_ms(DELAY_1);
00112 
00113     printf("--> Moving forward %d steps.\r\n", STEPS_1);
00114     motors[0]->move(StepperMotor::FWD, STEPS_1);      /* Moving. */
00115     motors[0]->wait_while_active();  /* Waiting while active. */
00116     position = motors[0]->get_position();     /* Getting the current position. */
00117     printf("--> Getting the current position: %d\r\n", position);
00118 
00119 
00120     printf("--> Marking the current position.\r\n");
00121     motors[0]->set_mark();     /* Marking the current position. */
00122     wait_ms(DELAY_1);
00123 
00124 
00125     printf("--> Moving backward %d steps.\r\n", STEPS_2);
00126     motors[0]->move(StepperMotor::BWD, STEPS_2);   /* Moving. */
00127     motors[0]->wait_while_active(); /* Waiting while active. */
00128     wait_ms(DELAY_1);     /* Waiting. */
00129     position = motors[0]->get_position();  /* Getting the current position. */
00130     printf("--> Getting the current position: %d\r\n", position);
00131     wait_ms(DELAY_1);
00132 
00133 
00134     printf("--> Going to marked position.\r\n");  /* Printing to the console. */
00135     motors[0]->go_mark(); /* Going to marked position. */
00136     motors[0]->wait_while_active();  /* Waiting while active. */
00137     wait_ms(DELAY_1);   /* Waiting. */
00138     position = motors[0]->get_position();   /* Getting the current position. */
00139     printf("--> Getting the current position: %d\r\n", position);
00140     wait_ms(DELAY_1);
00141 
00142 
00143     printf("--> Going to home position.\r\n");
00144     motors[0]->go_home();  /* Going to home position. */
00145     motors[0]->wait_while_active(); /* Waiting while active. */
00146     wait_ms(DELAY_1);
00147     position = motors[0]->get_position(); /* Getting the current position. */
00148     printf("--> Getting the current position: %d\r\n", position);  /* Printing to the console. */
00149     wait_ms(DELAY_1);  /* Waiting. */
00150 
00151     printf("--> Halving the microsteps.\r\n");
00152 
00153     init[0].step_sel = (init[0].step_sel > 0 ? init[0].step_sel -  1 : init[0].step_sel); /* Halving the microsteps. */
00154     if (!motors[0]->set_step_mode((StepperMotor::step_mode_t) init[0].step_sel))
00155         {
00156             printf("    Step Mode not allowed.\r\n");
00157         }
00158     wait_ms(DELAY_1);
00159     
00160 
00161     printf("--> Setting home position.\r\n");
00162     motors[0]->set_home();  /* Setting the home position. */
00163     wait_ms(DELAY_1);
00164     position = motors[0]->get_position();    /* Getting the current position. */
00165     printf("--> Getting the current position: %d\r\n", position);
00166     wait_ms(DELAY_1);
00167 
00168 
00169     printf("--> Moving forward %d steps.\r\n", STEPS_1);
00170     motors[0]->move(StepperMotor::FWD, STEPS_1); /* Moving. */
00171     motors[0]->wait_while_active();    /* Waiting while active. */
00172     position = motors[0]->get_position();   /* Getting the current position. */
00173     printf("--> Getting the current position: %d\r\n", position);
00174 
00175 
00176     printf("--> Marking the current position.\r\n");
00177     motors[0]->set_mark();    /* Marking the current position. */
00178     wait_ms(DELAY_2);
00179 
00180 
00181     /*----- Running together for a certain amount of time. -----*/
00182     printf("--> Running together for %d seconds.\r\n", DELAY_3 / 1000);
00183     for (int m = 0; m < L6470DAISYCHAINSIZE; m++)     /* Preparing each motor to perform a run at a specified speed. */
00184         {
00185             motors[m]->prepare_run(StepperMotor::BWD, 400);
00186         }
00187     x_nucleo_ihm02a1->perform_prepared_actions();   /* Performing the action on each motor at the same time. */
00188     wait_ms(DELAY_3);
00189 
00190 
00191     /*----- Increasing the speed while running. -----*/
00192     for (int m = 0; m < L6470DAISYCHAINSIZE; m++)       /* Preparing each motor to perform a run at a specified speed. */
00193         {
00194             motors[m]->prepare_get_speed();
00195         }
00196     uint32_t* results = x_nucleo_ihm02a1->perform_prepared_actions();      /* Performing the action on each motor at the same time. */
00197     printf("    Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
00198     printf("--> Doublig the speed while running again for %d seconds.\r\n", DELAY_3 / 1000);
00199     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) /* Preparing each motor to perform a run at a specified speed. */
00200         {
00201             motors[m]->prepare_run(StepperMotor::BWD, results[m] << 1);
00202         }
00203 
00204 
00205     results = x_nucleo_ihm02a1->perform_prepared_actions();      /* Performing the action on each motor at the same time. */
00206     wait_ms(DELAY_3);
00207 
00208 
00209 
00210     for (int m = 0; m < L6470DAISYCHAINSIZE; m++)      /* Preparing each motor to perform a run at a specified speed. */
00211         {
00212             motors[m]->prepare_get_speed();
00213         }
00214     results = x_nucleo_ihm02a1->perform_prepared_actions();      /* Performing the action on each motor at the same time. */
00215     printf("    Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
00216     wait_ms(DELAY_1);
00217 
00218 
00219     /*----- Hard Stop. -----*/
00220     printf("--> Hard Stop.\r\n");
00221 
00222 
00223     for (int m = 0; m < L6470DAISYCHAINSIZE; m++)     /* Preparing each motor to perform a hard stop. */
00224         {
00225             motors[m]->prepare_hard_stop();
00226         }
00227     x_nucleo_ihm02a1->perform_prepared_actions();        /* Performing the action on each motor at the same time. */
00228     wait_ms(DELAY_2);
00229 
00230 
00231     /*----- Doing a full revolution on each motor, one after the other. -----*/
00232     printf("--> Doing a full revolution on each motor, one after the other.\r\n");
00233 
00234 
00235     for (int m = 0; m < L6470DAISYCHAINSIZE; m++)      /* Doing a full revolution on each motor, one after the other. */
00236         {
00237             for (int i = 0; i < MPR_1; i++)
00238                 {
00239                     int steps = (int) (((int) init[m].fullstepsperrevolution * pow(2.0f, init[m].step_sel)) / MPR_1);             /* Computing the number of steps. */
00240                     motors[m]->move(StepperMotor::FWD, steps);/* Moving. */
00241                     motors[m]->wait_while_active();  /* Waiting while active. */
00242                     wait_ms(DELAY_1);
00243                 }
00244         }
00245     wait_ms(DELAY_2);
00246 
00247 
00248     /*----- High Impedance State. -----*/
00249 
00250     printf("--> High Impedance State.\r\n");
00251 
00252 
00253     for (int m = 0; m < L6470DAISYCHAINSIZE; m++)       /* Preparing each motor to set High Impedance State. */
00254         {
00255             motors[m]->prepare_hard_hiz();
00256         }
00257     x_nucleo_ihm02a1->perform_prepared_actions();         /* Performing the action on each motor at the same time. */
00258     wait_ms(DELAY_2);
00259 }