![](/media/cache/group/ST_logo_2020_blue_V_cmyk.jpg.50x50_q85.jpg)
Motion control example for 3 motors.
Dependencies: X_NUCLEO_IHM03A1 mbed
Fork of IHM03A1_ExampleFor3Motors by
This application provides an example of usage of three X-NUCLEO-IHM03A1 High Power Stepper Motor Control Expansion Boards.
It shows how to use three stepper motors connected to the three expansion boards by:
- moving each motor independently;
- moving several motors synchronously;
- monitoring the status of the three motors;
- handling interrupts triggered by all motor drivers;
- getting and setting a motor driver parameter;
- etc.
For the hardware configuration of the expansion boards, please refer to the X_NUCLEO_IHM03A1 home web page.
main.cpp@5:05b73855a2e1, 2018-10-18 (annotated)
- Committer:
- Davidroid
- Date:
- Thu Oct 18 13:41:14 2018 +0000
- Revision:
- 5:05b73855a2e1
- Parent:
- 2:c29a38e427f6
Update with the latest release of the libraries.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nucleosam | 0:36024a9bd220 | 1 | /** |
nucleosam | 0:36024a9bd220 | 2 | ****************************************************************************** |
nucleosam | 0:36024a9bd220 | 3 | * @file main.cpp |
nucleosam | 0:36024a9bd220 | 4 | * @author IPC Rennes |
nucleosam | 0:36024a9bd220 | 5 | * @version V1.0.0 |
nucleosam | 0:36024a9bd220 | 6 | * @date March 18th, 2016 |
nucleosam | 0:36024a9bd220 | 7 | * @brief mbed test application for the STMicroelectronics X-NUCLEO-IHM03A1 |
nucleosam | 0:36024a9bd220 | 8 | * Motor Control Expansion Boards: control of 3 motors with 3 |
nucleosam | 0:36024a9bd220 | 9 | * expansions boards. |
nucleosam | 0:36024a9bd220 | 10 | ****************************************************************************** |
nucleosam | 0:36024a9bd220 | 11 | * @attention |
nucleosam | 0:36024a9bd220 | 12 | * |
nucleosam | 0:36024a9bd220 | 13 | * <h2><center>© COPYRIGHT(c) 2016 STMicroelectronics</center></h2> |
nucleosam | 0:36024a9bd220 | 14 | * |
nucleosam | 0:36024a9bd220 | 15 | * Redistribution and use in source and binary forms, with or without modification, |
nucleosam | 0:36024a9bd220 | 16 | * are permitted provided that the following conditions are met: |
nucleosam | 0:36024a9bd220 | 17 | * 1. Redistributions of source code must retain the above copyright notice, |
nucleosam | 0:36024a9bd220 | 18 | * this list of conditions and the following disclaimer. |
nucleosam | 0:36024a9bd220 | 19 | * 2. Redistributions in binary form must reproduce the above copyright notice, |
nucleosam | 0:36024a9bd220 | 20 | * this list of conditions and the following disclaimer in the documentation |
nucleosam | 0:36024a9bd220 | 21 | * and/or other materials provided with the distribution. |
nucleosam | 0:36024a9bd220 | 22 | * 3. Neither the name of STMicroelectronics nor the names of its contributors |
nucleosam | 0:36024a9bd220 | 23 | * may be used to endorse or promote products derived from this software |
nucleosam | 0:36024a9bd220 | 24 | * without specific prior written permission. |
nucleosam | 0:36024a9bd220 | 25 | * |
nucleosam | 0:36024a9bd220 | 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
nucleosam | 0:36024a9bd220 | 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
nucleosam | 0:36024a9bd220 | 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
nucleosam | 0:36024a9bd220 | 29 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE |
nucleosam | 0:36024a9bd220 | 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
nucleosam | 0:36024a9bd220 | 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR |
nucleosam | 0:36024a9bd220 | 32 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
nucleosam | 0:36024a9bd220 | 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, |
nucleosam | 0:36024a9bd220 | 34 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
nucleosam | 0:36024a9bd220 | 35 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
nucleosam | 0:36024a9bd220 | 36 | * |
nucleosam | 0:36024a9bd220 | 37 | ****************************************************************************** |
nucleosam | 0:36024a9bd220 | 38 | */ |
nucleosam | 0:36024a9bd220 | 39 | |
nucleosam | 0:36024a9bd220 | 40 | |
nucleosam | 0:36024a9bd220 | 41 | /* Includes ------------------------------------------------------------------*/ |
nucleosam | 0:36024a9bd220 | 42 | |
nucleosam | 0:36024a9bd220 | 43 | /* mbed specific header files. */ |
nucleosam | 0:36024a9bd220 | 44 | #include "mbed.h" |
nucleosam | 0:36024a9bd220 | 45 | |
nucleosam | 0:36024a9bd220 | 46 | /* Helper header files. */ |
nucleosam | 0:36024a9bd220 | 47 | #include "DevSPI.h" |
nucleosam | 0:36024a9bd220 | 48 | |
nucleosam | 0:36024a9bd220 | 49 | /* Component specific header files. */ |
Davidroid | 5:05b73855a2e1 | 50 | #include "PowerStep01.h" |
nucleosam | 0:36024a9bd220 | 51 | |
nucleosam | 0:36024a9bd220 | 52 | /* Variables -----------------------------------------------------------------*/ |
nucleosam | 0:36024a9bd220 | 53 | |
nucleosam | 0:36024a9bd220 | 54 | /* Initialization parameters of the motor connected to the expansion board. */ |
nucleosam | 0:36024a9bd220 | 55 | /* Current mode. */ |
Davidroid | 5:05b73855a2e1 | 56 | powerstep01_init_u_t init_device_parameters = |
nucleosam | 0:36024a9bd220 | 57 | { |
nucleosam | 0:36024a9bd220 | 58 | /* common parameters */ |
nucleosam | 0:36024a9bd220 | 59 | .cm.cp.cmVmSelection = POWERSTEP01_CM_VM_CURRENT, // enum powerstep01_CmVm_t |
nucleosam | 0:36024a9bd220 | 60 | 582, // Acceleration rate in step/s2, range 14.55 to 59590 steps/s^2 |
nucleosam | 0:36024a9bd220 | 61 | 582, // Deceleration rate in step/s2, range 14.55 to 59590 steps/s^2 |
nucleosam | 0:36024a9bd220 | 62 | 488, // Maximum speed in step/s, range 15.25 to 15610 steps/s |
nucleosam | 0:36024a9bd220 | 63 | 0, // Minimum speed in step/s, range 0 to 976.3 steps/s |
nucleosam | 0:36024a9bd220 | 64 | POWERSTEP01_LSPD_OPT_OFF, // Low speed optimization bit, enum powerstep01_LspdOpt_t |
nucleosam | 0:36024a9bd220 | 65 | 244.16, // Full step speed in step/s, range 7.63 to 15625 steps/s |
nucleosam | 0:36024a9bd220 | 66 | POWERSTEP01_BOOST_MODE_OFF, // Boost of the amplitude square wave, enum powerstep01_BoostMode_t |
nucleosam | 0:36024a9bd220 | 67 | 281.25, // Overcurrent threshold settings via enum powerstep01_OcdTh_t |
nucleosam | 0:36024a9bd220 | 68 | STEP_MODE_1_16, // Step mode settings via enum motorStepMode_t |
nucleosam | 0:36024a9bd220 | 69 | POWERSTEP01_SYNC_SEL_DISABLED, // Synch. Mode settings via enum powerstep01_SyncSel_t |
nucleosam | 0:36024a9bd220 | 70 | (POWERSTEP01_ALARM_EN_OVERCURRENT| |
nucleosam | 0:36024a9bd220 | 71 | POWERSTEP01_ALARM_EN_THERMAL_SHUTDOWN| |
nucleosam | 0:36024a9bd220 | 72 | POWERSTEP01_ALARM_EN_THERMAL_WARNING| |
nucleosam | 0:36024a9bd220 | 73 | POWERSTEP01_ALARM_EN_UVLO| |
nucleosam | 0:36024a9bd220 | 74 | POWERSTEP01_ALARM_EN_STALL_DETECTION| |
nucleosam | 0:36024a9bd220 | 75 | POWERSTEP01_ALARM_EN_SW_TURN_ON| |
nucleosam | 0:36024a9bd220 | 76 | POWERSTEP01_ALARM_EN_WRONG_NPERF_CMD), // Alarm settings via bitmap enum powerstep01_AlarmEn_t |
nucleosam | 0:36024a9bd220 | 77 | POWERSTEP01_IGATE_64mA, // Gate sink/source current via enum powerstep01_Igate_t |
nucleosam | 0:36024a9bd220 | 78 | POWERSTEP01_TBOOST_0ns, // Duration of the overboost phase during gate turn-off via enum powerstep01_Tboost_t |
nucleosam | 0:36024a9bd220 | 79 | POWERSTEP01_TCC_500ns, // Controlled current time via enum powerstep01_Tcc_t |
nucleosam | 0:36024a9bd220 | 80 | POWERSTEP01_WD_EN_DISABLE, // External clock watchdog, enum powerstep01_WdEn_t |
nucleosam | 0:36024a9bd220 | 81 | POWERSTEP01_TBLANK_375ns, // Duration of the blanking time via enum powerstep01_TBlank_t |
nucleosam | 0:36024a9bd220 | 82 | POWERSTEP01_TDT_125ns, // Duration of the dead time via enum powerstep01_Tdt_t |
nucleosam | 0:36024a9bd220 | 83 | /* current mode parameters */ |
nucleosam | 0:36024a9bd220 | 84 | 328.12, // Hold torque in mV, range from 7.8mV to 1000 mV |
nucleosam | 0:36024a9bd220 | 85 | 328.12, // Running torque in mV, range from 7.8mV to 1000 mV |
nucleosam | 0:36024a9bd220 | 86 | 328.12, // Acceleration torque in mV, range from 7.8mV to 1000 mV |
nucleosam | 0:36024a9bd220 | 87 | 328.12, // Deceleration torque in mV, range from 7.8mV to 1000 mV |
nucleosam | 0:36024a9bd220 | 88 | POWERSTEP01_TOFF_FAST_8us, //Maximum fast decay time , enum powerstep01_ToffFast_t |
nucleosam | 0:36024a9bd220 | 89 | POWERSTEP01_FAST_STEP_12us, //Maximum fall step time , enum powerstep01_FastStep_t |
nucleosam | 0:36024a9bd220 | 90 | 3.0, // Minimum on-time in us, range 0.5us to 64us |
nucleosam | 0:36024a9bd220 | 91 | 21.0, // Minimum off-time in us, range 0.5us to 64us |
nucleosam | 0:36024a9bd220 | 92 | POWERSTEP01_CONFIG_INT_16MHZ_OSCOUT_2MHZ, // Clock setting , enum powerstep01_ConfigOscMgmt_t |
nucleosam | 0:36024a9bd220 | 93 | POWERSTEP01_CONFIG_SW_HARD_STOP, // External switch hard stop interrupt mode, enum powerstep01_ConfigSwMode_t |
nucleosam | 0:36024a9bd220 | 94 | POWERSTEP01_CONFIG_TQ_REG_TVAL_USED, // External torque regulation enabling , enum powerstep01_ConfigEnTqReg_t |
nucleosam | 0:36024a9bd220 | 95 | POWERSTEP01_CONFIG_VS_COMP_DISABLE, // Motor Supply Voltage Compensation enabling , enum powerstep01_ConfigEnVscomp_t |
nucleosam | 0:36024a9bd220 | 96 | POWERSTEP01_CONFIG_OC_SD_DISABLE, // Over current shutwdown enabling, enum powerstep01_ConfigOcSd_t |
nucleosam | 0:36024a9bd220 | 97 | POWERSTEP01_CONFIG_UVLOVAL_LOW, // UVLO Threshold via powerstep01_ConfigUvLoVal_t |
nucleosam | 0:36024a9bd220 | 98 | POWERSTEP01_CONFIG_VCCVAL_15V, // VCC Val, enum powerstep01_ConfigVccVal_t |
nucleosam | 0:36024a9bd220 | 99 | POWERSTEP01_CONFIG_TSW_048us, // Switching period, enum powerstep01_ConfigTsw_t |
nucleosam | 2:c29a38e427f6 | 100 | POWERSTEP01_CONFIG_PRED_DISABLE // Predictive current enabling , enum powerstep01_ConfigPredEn_t |
nucleosam | 0:36024a9bd220 | 101 | }; |
nucleosam | 0:36024a9bd220 | 102 | |
nucleosam | 0:36024a9bd220 | 103 | /* Motor Control Component. */ |
Davidroid | 5:05b73855a2e1 | 104 | PowerStep01 *motor1; |
Davidroid | 5:05b73855a2e1 | 105 | PowerStep01 *motor2; |
Davidroid | 5:05b73855a2e1 | 106 | PowerStep01 *motor3; |
nucleosam | 0:36024a9bd220 | 107 | |
nucleosam | 0:36024a9bd220 | 108 | /* Functions -----------------------------------------------------------------*/ |
nucleosam | 0:36024a9bd220 | 109 | |
nucleosam | 0:36024a9bd220 | 110 | /** |
nucleosam | 0:36024a9bd220 | 111 | * @brief This is an example of user handler for the flag interrupt. |
nucleosam | 0:36024a9bd220 | 112 | * @param None |
nucleosam | 0:36024a9bd220 | 113 | * @retval None |
nucleosam | 0:36024a9bd220 | 114 | * @note If needed, implement it, and then attach and enable it: |
Davidroid | 5:05b73855a2e1 | 115 | * + motor->attach_flag_irq(&myFlagIRQHandler); |
Davidroid | 5:05b73855a2e1 | 116 | * + motor->enable_flag_irq(); |
nucleosam | 0:36024a9bd220 | 117 | * To disable it: |
nucleosam | 0:36024a9bd220 | 118 | * + motor->DisbleFlagIRQ(); |
nucleosam | 0:36024a9bd220 | 119 | */ |
nucleosam | 0:36024a9bd220 | 120 | void myFlagIRQHandler(void) |
nucleosam | 0:36024a9bd220 | 121 | { |
nucleosam | 0:36024a9bd220 | 122 | /* Set ISR flag. */ |
Davidroid | 5:05b73855a2e1 | 123 | PowerStep01::isrFlag = TRUE; |
nucleosam | 0:36024a9bd220 | 124 | |
Davidroid | 5:05b73855a2e1 | 125 | motor1->fetch_and_clear_all_status(); |
Davidroid | 5:05b73855a2e1 | 126 | PowerStep01 *motor; |
nucleosam | 0:36024a9bd220 | 127 | motor = motor1; |
nucleosam | 0:36024a9bd220 | 128 | unsigned int statusRegister; |
nucleosam | 0:36024a9bd220 | 129 | |
nucleosam | 0:36024a9bd220 | 130 | printf(" WARNING: \"FLAG\" interrupt triggered.\r\n"); |
nucleosam | 0:36024a9bd220 | 131 | /* Get the value of the status register. */ |
Davidroid | 5:05b73855a2e1 | 132 | for (uint8_t loop = 0; loop<PowerStep01::get_nb_devices();loop++) |
nucleosam | 0:36024a9bd220 | 133 | { |
nucleosam | 0:36024a9bd220 | 134 | if (loop==1) motor = motor2; |
nucleosam | 0:36024a9bd220 | 135 | if (loop==2) motor = motor3; |
Davidroid | 5:05b73855a2e1 | 136 | statusRegister = motor->get_fetched_status(); |
nucleosam | 0:36024a9bd220 | 137 | printf(" Motor%d:\r\n",loop+1); |
nucleosam | 0:36024a9bd220 | 138 | /* Check HIZ flag: if set, power brigdes are disabled */ |
nucleosam | 0:36024a9bd220 | 139 | if ((statusRegister & POWERSTEP01_STATUS_HIZ)==POWERSTEP01_STATUS_HIZ) |
nucleosam | 0:36024a9bd220 | 140 | { |
nucleosam | 0:36024a9bd220 | 141 | // HIZ state |
nucleosam | 0:36024a9bd220 | 142 | printf(" HiZ state.\r\n"); |
nucleosam | 0:36024a9bd220 | 143 | } |
nucleosam | 0:36024a9bd220 | 144 | /* Check BUSY flag: if not set, a command is under execution */ |
nucleosam | 0:36024a9bd220 | 145 | if ((statusRegister & POWERSTEP01_STATUS_BUSY)==0) |
nucleosam | 0:36024a9bd220 | 146 | { |
nucleosam | 0:36024a9bd220 | 147 | // BUSY |
nucleosam | 0:36024a9bd220 | 148 | printf(" Busy.\r\n"); |
nucleosam | 0:36024a9bd220 | 149 | } |
nucleosam | 0:36024a9bd220 | 150 | /* Check SW_F flag: if not set, the SW input is opened */ |
nucleosam | 0:36024a9bd220 | 151 | if ((statusRegister & POWERSTEP01_STATUS_SW_F )!=0) |
nucleosam | 0:36024a9bd220 | 152 | { |
nucleosam | 0:36024a9bd220 | 153 | // SW closed (connected to ground) |
nucleosam | 0:36024a9bd220 | 154 | printf(" SW closed (connected to ground).\r\n"); |
nucleosam | 0:36024a9bd220 | 155 | } |
nucleosam | 0:36024a9bd220 | 156 | /* Check SW_EN bit */ |
nucleosam | 0:36024a9bd220 | 157 | if ((statusRegister & POWERSTEP01_STATUS_SW_EVN)== |
nucleosam | 0:36024a9bd220 | 158 | POWERSTEP01_STATUS_SW_EVN) |
nucleosam | 0:36024a9bd220 | 159 | { |
nucleosam | 0:36024a9bd220 | 160 | // SW turn_on event |
nucleosam | 0:36024a9bd220 | 161 | printf(" SW turn_on event.\r\n"); |
nucleosam | 0:36024a9bd220 | 162 | } |
nucleosam | 0:36024a9bd220 | 163 | if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)== |
nucleosam | 0:36024a9bd220 | 164 | POWERSTEP01_STATUS_MOT_STATUS_STOPPED) |
nucleosam | 0:36024a9bd220 | 165 | { |
nucleosam | 0:36024a9bd220 | 166 | // MOTOR STOPPED |
nucleosam | 0:36024a9bd220 | 167 | printf(" Stopped.\r\n"); |
nucleosam | 0:36024a9bd220 | 168 | } |
nucleosam | 0:36024a9bd220 | 169 | else |
nucleosam | 0:36024a9bd220 | 170 | { |
nucleosam | 0:36024a9bd220 | 171 | if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)== |
nucleosam | 0:36024a9bd220 | 172 | POWERSTEP01_STATUS_MOT_STATUS_ACCELERATION) |
nucleosam | 0:36024a9bd220 | 173 | { |
nucleosam | 0:36024a9bd220 | 174 | // MOTOR ACCELERATION |
nucleosam | 0:36024a9bd220 | 175 | printf(" Accelerating "); |
nucleosam | 0:36024a9bd220 | 176 | } |
nucleosam | 0:36024a9bd220 | 177 | else if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)== |
nucleosam | 0:36024a9bd220 | 178 | POWERSTEP01_STATUS_MOT_STATUS_DECELERATION) |
nucleosam | 0:36024a9bd220 | 179 | { |
nucleosam | 0:36024a9bd220 | 180 | // MOTOR DECELERATION |
nucleosam | 0:36024a9bd220 | 181 | printf(" Decelerating "); |
nucleosam | 0:36024a9bd220 | 182 | } |
nucleosam | 0:36024a9bd220 | 183 | else if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)== |
nucleosam | 0:36024a9bd220 | 184 | POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD) |
nucleosam | 0:36024a9bd220 | 185 | { |
nucleosam | 0:36024a9bd220 | 186 | // MOTOR RUNNING AT CONSTANT SPEED |
nucleosam | 0:36024a9bd220 | 187 | printf(" Steady running "); |
nucleosam | 0:36024a9bd220 | 188 | } |
nucleosam | 0:36024a9bd220 | 189 | /* Check direction bit */ |
nucleosam | 0:36024a9bd220 | 190 | if ((statusRegister & POWERSTEP01_STATUS_DIR)==0) |
nucleosam | 0:36024a9bd220 | 191 | { |
nucleosam | 0:36024a9bd220 | 192 | // StepperMotor::BWD |
nucleosam | 0:36024a9bd220 | 193 | printf(" in backward direction.\r\n"); |
nucleosam | 0:36024a9bd220 | 194 | } |
nucleosam | 0:36024a9bd220 | 195 | else |
nucleosam | 0:36024a9bd220 | 196 | { |
nucleosam | 0:36024a9bd220 | 197 | // StepperMotor::FWD |
nucleosam | 0:36024a9bd220 | 198 | printf(" in forward direction.\r\n"); |
nucleosam | 0:36024a9bd220 | 199 | } |
nucleosam | 0:36024a9bd220 | 200 | } |
nucleosam | 0:36024a9bd220 | 201 | /* Check Command Error flag: if set, the command received by SPI can't be */ |
nucleosam | 0:36024a9bd220 | 202 | /* performed. This occurs for instance when a move command is sent to the */ |
nucleosam | 0:36024a9bd220 | 203 | /* Powerstep01 while it is already running */ |
nucleosam | 0:36024a9bd220 | 204 | if ((statusRegister & POWERSTEP01_STATUS_CMD_ERROR)== |
nucleosam | 0:36024a9bd220 | 205 | POWERSTEP01_STATUS_CMD_ERROR) |
nucleosam | 0:36024a9bd220 | 206 | { |
nucleosam | 0:36024a9bd220 | 207 | // Command Error |
nucleosam | 0:36024a9bd220 | 208 | printf(" Non-performable command detected.\r\n"); |
nucleosam | 0:36024a9bd220 | 209 | } |
nucleosam | 0:36024a9bd220 | 210 | /* Check Step mode clock flag: if set, the device is working in step clock mode */ |
nucleosam | 0:36024a9bd220 | 211 | if ((statusRegister & POWERSTEP01_STATUS_STCK_MOD)== |
nucleosam | 0:36024a9bd220 | 212 | POWERSTEP01_STATUS_STCK_MOD) |
nucleosam | 0:36024a9bd220 | 213 | { |
nucleosam | 0:36024a9bd220 | 214 | //Step clock mode enabled |
nucleosam | 0:36024a9bd220 | 215 | printf(" Step clock mode enabled.\r\n"); |
nucleosam | 0:36024a9bd220 | 216 | } |
nucleosam | 0:36024a9bd220 | 217 | /* Check UVLO flag: if not set, there is an undervoltage lock-out */ |
nucleosam | 0:36024a9bd220 | 218 | if ((statusRegister & POWERSTEP01_STATUS_UVLO)==0) |
nucleosam | 0:36024a9bd220 | 219 | { |
nucleosam | 0:36024a9bd220 | 220 | //Undervoltage lock-out |
nucleosam | 0:36024a9bd220 | 221 | printf(" undervoltage lock-out.\r\n"); |
nucleosam | 0:36024a9bd220 | 222 | } |
nucleosam | 0:36024a9bd220 | 223 | /* Check UVLO ADC flag: if not set, there is an ADC undervoltage lock-out */ |
nucleosam | 0:36024a9bd220 | 224 | if ((statusRegister & POWERSTEP01_STATUS_UVLO_ADC)==0) |
nucleosam | 0:36024a9bd220 | 225 | { |
nucleosam | 0:36024a9bd220 | 226 | //ADC undervoltage lock-out |
nucleosam | 2:c29a38e427f6 | 227 | printf(" ADC undervoltage lock-out:\r\n"); |
nucleosam | 2:c29a38e427f6 | 228 | printf(" Expected with default IHM03A1 HW configuration.\r\n"); |
nucleosam | 0:36024a9bd220 | 229 | } |
nucleosam | 0:36024a9bd220 | 230 | /* Check thermal STATUS flags: if set, the thermal status is not normal */ |
nucleosam | 0:36024a9bd220 | 231 | if ((statusRegister & POWERSTEP01_STATUS_TH_STATUS)!=0) |
nucleosam | 0:36024a9bd220 | 232 | { |
nucleosam | 0:36024a9bd220 | 233 | //thermal status: 1: Warning, 2: Bridge shutdown, 3: Device shutdown |
nucleosam | 0:36024a9bd220 | 234 | if (((statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11)==1) |
nucleosam | 0:36024a9bd220 | 235 | { |
nucleosam | 0:36024a9bd220 | 236 | printf(" Thermal status - Warning.\r\n"); |
nucleosam | 0:36024a9bd220 | 237 | } |
nucleosam | 0:36024a9bd220 | 238 | else if (((statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11)==2) |
nucleosam | 0:36024a9bd220 | 239 | { |
nucleosam | 0:36024a9bd220 | 240 | printf(" Thermal status - Bridge shutdown.\r\n"); |
nucleosam | 0:36024a9bd220 | 241 | } |
nucleosam | 0:36024a9bd220 | 242 | else if (((statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11)==3) |
nucleosam | 0:36024a9bd220 | 243 | { |
nucleosam | 0:36024a9bd220 | 244 | printf(" Thermal status - Device shutdown.\r\n"); |
nucleosam | 0:36024a9bd220 | 245 | } |
nucleosam | 0:36024a9bd220 | 246 | } |
nucleosam | 0:36024a9bd220 | 247 | /* Check OCD flag: if not set, there is an overcurrent detection */ |
nucleosam | 0:36024a9bd220 | 248 | if ((statusRegister & POWERSTEP01_STATUS_OCD)==0) |
nucleosam | 0:36024a9bd220 | 249 | { |
nucleosam | 0:36024a9bd220 | 250 | //Overcurrent detection |
nucleosam | 0:36024a9bd220 | 251 | printf(" Overcurrent detection.\r\n"); |
nucleosam | 0:36024a9bd220 | 252 | } |
nucleosam | 0:36024a9bd220 | 253 | /* Check STALL_A flag: if not set, there is a Stall condition on bridge A */ |
nucleosam | 0:36024a9bd220 | 254 | if ((statusRegister & POWERSTEP01_STATUS_STALL_A)==0) |
nucleosam | 0:36024a9bd220 | 255 | { |
nucleosam | 0:36024a9bd220 | 256 | //Bridge A stalled |
nucleosam | 0:36024a9bd220 | 257 | printf(" Bridge A stalled.\r\n"); |
nucleosam | 0:36024a9bd220 | 258 | } |
nucleosam | 0:36024a9bd220 | 259 | /* Check STALL_B flag: if not set, there is a Stall condition on bridge B */ |
nucleosam | 0:36024a9bd220 | 260 | if ((statusRegister & POWERSTEP01_STATUS_STALL_B)==0) |
nucleosam | 0:36024a9bd220 | 261 | { |
nucleosam | 0:36024a9bd220 | 262 | //Bridge B stalled |
nucleosam | 0:36024a9bd220 | 263 | printf(" Bridge B stalled.\r\n"); |
nucleosam | 0:36024a9bd220 | 264 | } |
nucleosam | 0:36024a9bd220 | 265 | } |
nucleosam | 0:36024a9bd220 | 266 | /* Reset ISR flag. */ |
Davidroid | 5:05b73855a2e1 | 267 | PowerStep01::isrFlag = FALSE; |
nucleosam | 0:36024a9bd220 | 268 | } |
nucleosam | 0:36024a9bd220 | 269 | |
nucleosam | 0:36024a9bd220 | 270 | /** |
nucleosam | 0:36024a9bd220 | 271 | * @brief This is an example of user handler for the busy interrupt. |
nucleosam | 0:36024a9bd220 | 272 | * @param None |
nucleosam | 0:36024a9bd220 | 273 | * @retval None |
nucleosam | 0:36024a9bd220 | 274 | * @note If needed, implement it, and then attach and enable it: |
Davidroid | 5:05b73855a2e1 | 275 | * + motor->attach_busy_irq(&myBusyIRQHandler); |
Davidroid | 5:05b73855a2e1 | 276 | * + motor->enable_busy_irq(); |
nucleosam | 0:36024a9bd220 | 277 | * To disable it: |
nucleosam | 0:36024a9bd220 | 278 | * + motor->DisbleBusyIRQ(); |
nucleosam | 0:36024a9bd220 | 279 | */ |
nucleosam | 0:36024a9bd220 | 280 | void myBusyIRQHandler(void) |
nucleosam | 0:36024a9bd220 | 281 | { |
nucleosam | 0:36024a9bd220 | 282 | /* Set ISR flag. */ |
Davidroid | 5:05b73855a2e1 | 283 | PowerStep01::isrFlag = TRUE; |
nucleosam | 0:36024a9bd220 | 284 | |
nucleosam | 0:36024a9bd220 | 285 | /* Reset ISR flag. */ |
Davidroid | 5:05b73855a2e1 | 286 | PowerStep01::isrFlag = FALSE; |
nucleosam | 0:36024a9bd220 | 287 | } |
nucleosam | 0:36024a9bd220 | 288 | |
nucleosam | 0:36024a9bd220 | 289 | /** |
nucleosam | 0:36024a9bd220 | 290 | * @brief This is an example of error handler. |
nucleosam | 0:36024a9bd220 | 291 | * @param[in] error Number of the error |
nucleosam | 0:36024a9bd220 | 292 | * @retval None |
nucleosam | 2:c29a38e427f6 | 293 | * @note If needed, implement it, and then attach it: |
Davidroid | 5:05b73855a2e1 | 294 | * + motor->attach_error_handler(&myErrorHandler); |
nucleosam | 0:36024a9bd220 | 295 | */ |
nucleosam | 0:36024a9bd220 | 296 | void myErrorHandler(uint16_t error) |
nucleosam | 0:36024a9bd220 | 297 | { |
nucleosam | 0:36024a9bd220 | 298 | /* Printing to the console. */ |
nucleosam | 0:36024a9bd220 | 299 | printf("Error %d detected\r\n\n", error); |
nucleosam | 0:36024a9bd220 | 300 | |
nucleosam | 0:36024a9bd220 | 301 | /* Infinite loop */ |
nucleosam | 0:36024a9bd220 | 302 | while(1) |
nucleosam | 0:36024a9bd220 | 303 | { |
nucleosam | 0:36024a9bd220 | 304 | } |
nucleosam | 0:36024a9bd220 | 305 | } |
nucleosam | 0:36024a9bd220 | 306 | |
Davidroid | 5:05b73855a2e1 | 307 | void wait_for_all_devices_not_busy(void) |
nucleosam | 0:36024a9bd220 | 308 | { |
nucleosam | 0:36024a9bd220 | 309 | /* Wait while at least one is active */ |
Davidroid | 5:05b73855a2e1 | 310 | while (motor1->is_device_busy()|motor2->is_device_busy()|motor3->is_device_busy()); |
nucleosam | 0:36024a9bd220 | 311 | } |
nucleosam | 0:36024a9bd220 | 312 | |
nucleosam | 0:36024a9bd220 | 313 | /* Main ----------------------------------------------------------------------*/ |
nucleosam | 0:36024a9bd220 | 314 | |
nucleosam | 0:36024a9bd220 | 315 | int main() |
nucleosam | 0:36024a9bd220 | 316 | { |
nucleosam | 0:36024a9bd220 | 317 | int32_t pos; |
nucleosam | 0:36024a9bd220 | 318 | uint32_t myMaxSpeed; |
nucleosam | 0:36024a9bd220 | 319 | uint32_t myMinSpeed; |
nucleosam | 0:36024a9bd220 | 320 | uint16_t myAcceleration; |
nucleosam | 0:36024a9bd220 | 321 | uint16_t myDeceleration; |
nucleosam | 0:36024a9bd220 | 322 | uint32_t unsignedIntegerValue; |
nucleosam | 0:36024a9bd220 | 323 | float floatValue; |
nucleosam | 0:36024a9bd220 | 324 | |
nucleosam | 0:36024a9bd220 | 325 | /* Printing to the console. */ |
nucleosam | 0:36024a9bd220 | 326 | printf("STARTING MAIN PROGRAM\r\n"); |
nucleosam | 0:36024a9bd220 | 327 | printf(" Reminder:\r\n"); |
nucleosam | 0:36024a9bd220 | 328 | printf(" The position unit is in agreement to the step mode.\r\n"); |
nucleosam | 0:36024a9bd220 | 329 | printf(" The speed, acceleration or deceleration unit\r\n"); |
nucleosam | 0:36024a9bd220 | 330 | printf(" do not depend on the step mode and the step unit is a full step.\r\n"); |
nucleosam | 0:36024a9bd220 | 331 | |
nucleosam | 0:36024a9bd220 | 332 | //----- Initialization |
nucleosam | 0:36024a9bd220 | 333 | /* Initializing SPI bus. */ |
nucleosam | 0:36024a9bd220 | 334 | DevSPI dev_spi(D11, D12, D13); |
nucleosam | 0:36024a9bd220 | 335 | |
nucleosam | 0:36024a9bd220 | 336 | /* Initializing Motor Control Component. */ |
Davidroid | 5:05b73855a2e1 | 337 | motor1 = new PowerStep01(D2, D4, D8, D9, D10, dev_spi); |
Davidroid | 5:05b73855a2e1 | 338 | motor2 = new PowerStep01(D2, D4, D8, D9, D10, dev_spi); |
Davidroid | 5:05b73855a2e1 | 339 | motor3 = new PowerStep01(D2, D4, D8, D9, D10, dev_spi); |
Davidroid | 5:05b73855a2e1 | 340 | if (motor1->init(&init_device_parameters) != COMPONENT_OK) exit(EXIT_FAILURE); |
Davidroid | 5:05b73855a2e1 | 341 | if (motor2->init(&init_device_parameters) != COMPONENT_OK) exit(EXIT_FAILURE); |
Davidroid | 5:05b73855a2e1 | 342 | if (motor3->init(&init_device_parameters) != COMPONENT_OK) exit(EXIT_FAILURE); |
nucleosam | 0:36024a9bd220 | 343 | |
nucleosam | 0:36024a9bd220 | 344 | /* Attaching and enabling interrupt handlers. */ |
Davidroid | 5:05b73855a2e1 | 345 | motor1->attach_flag_irq(&myFlagIRQHandler); |
Davidroid | 5:05b73855a2e1 | 346 | motor1->enable_flag_irq(); |
Davidroid | 5:05b73855a2e1 | 347 | motor1->attach_busy_irq(&myBusyIRQHandler); |
Davidroid | 5:05b73855a2e1 | 348 | motor1->enable_busy_irq(); |
Davidroid | 5:05b73855a2e1 | 349 | motor2->attach_flag_irq(&myFlagIRQHandler); |
Davidroid | 5:05b73855a2e1 | 350 | motor2->enable_flag_irq(); |
Davidroid | 5:05b73855a2e1 | 351 | motor2->attach_busy_irq(&myBusyIRQHandler); |
Davidroid | 5:05b73855a2e1 | 352 | motor2->enable_busy_irq(); |
Davidroid | 5:05b73855a2e1 | 353 | motor3->attach_flag_irq(&myFlagIRQHandler); |
Davidroid | 5:05b73855a2e1 | 354 | motor3->enable_flag_irq(); |
Davidroid | 5:05b73855a2e1 | 355 | motor3->attach_busy_irq(&myBusyIRQHandler); |
Davidroid | 5:05b73855a2e1 | 356 | motor3->enable_busy_irq(); |
nucleosam | 0:36024a9bd220 | 357 | |
nucleosam | 0:36024a9bd220 | 358 | /* Attaching an error handler */ |
Davidroid | 5:05b73855a2e1 | 359 | motor1->attach_error_handler(&myErrorHandler); |
Davidroid | 5:05b73855a2e1 | 360 | motor2->attach_error_handler(&myErrorHandler); |
Davidroid | 5:05b73855a2e1 | 361 | motor3->attach_error_handler(&myErrorHandler); |
nucleosam | 0:36024a9bd220 | 362 | |
nucleosam | 0:36024a9bd220 | 363 | /* Printing to the console. */ |
nucleosam | 0:36024a9bd220 | 364 | printf("Motor Control Application Example for 3 Motors\r\n"); |
nucleosam | 0:36024a9bd220 | 365 | |
nucleosam | 0:36024a9bd220 | 366 | /* Request motor 1 to go to position 3200 and print to the console */ |
nucleosam | 0:36024a9bd220 | 367 | printf("--> Request motor1 to go to position 3200.\r\n"); |
Davidroid | 5:05b73855a2e1 | 368 | motor1->go_to(3200); |
nucleosam | 0:36024a9bd220 | 369 | |
nucleosam | 0:36024a9bd220 | 370 | /* Wait for motor 2 ends moving */ |
Davidroid | 5:05b73855a2e1 | 371 | motor1->wait_while_active(); |
nucleosam | 0:36024a9bd220 | 372 | |
nucleosam | 0:36024a9bd220 | 373 | /* Get current position of motor 1 and print to the console */ |
Davidroid | 5:05b73855a2e1 | 374 | pos = motor1->get_position(); |
nucleosam | 0:36024a9bd220 | 375 | printf(" Motor1 position: %d.\r\n", pos); |
nucleosam | 0:36024a9bd220 | 376 | |
nucleosam | 0:36024a9bd220 | 377 | /* Wait for 2 seconds */ |
nucleosam | 0:36024a9bd220 | 378 | wait_ms(2000); |
nucleosam | 0:36024a9bd220 | 379 | |
nucleosam | 0:36024a9bd220 | 380 | /* If the read position of motor 1 is 3200 */ |
nucleosam | 0:36024a9bd220 | 381 | /* Request motor 2 to go to the same position */ |
nucleosam | 0:36024a9bd220 | 382 | if (pos == 3200) |
nucleosam | 0:36024a9bd220 | 383 | { |
nucleosam | 0:36024a9bd220 | 384 | /* Set current position of motor 1 to be its mark position*/ |
nucleosam | 0:36024a9bd220 | 385 | printf(" Set mark to current position of motor1.\r\n"); |
Davidroid | 5:05b73855a2e1 | 386 | motor1->set_mark(); |
nucleosam | 0:36024a9bd220 | 387 | |
nucleosam | 0:36024a9bd220 | 388 | /* Request motor 2 to Go to the same position and print to the console */ |
nucleosam | 0:36024a9bd220 | 389 | printf("--> Request motor2 to go to position 3200.\r\n"); |
Davidroid | 5:05b73855a2e1 | 390 | motor2->go_to(pos); |
nucleosam | 0:36024a9bd220 | 391 | |
nucleosam | 0:36024a9bd220 | 392 | /* Wait for motor 2 ends moving */ |
Davidroid | 5:05b73855a2e1 | 393 | motor2->wait_while_active(); |
nucleosam | 0:36024a9bd220 | 394 | } |
nucleosam | 0:36024a9bd220 | 395 | |
nucleosam | 0:36024a9bd220 | 396 | /* Get current position of motor 2 and print to the console */ |
Davidroid | 5:05b73855a2e1 | 397 | pos = motor2->get_position(); |
nucleosam | 0:36024a9bd220 | 398 | printf(" Motor2 position: %d.\r\n", pos); |
nucleosam | 0:36024a9bd220 | 399 | |
nucleosam | 0:36024a9bd220 | 400 | /* If the read position of motor 2 is 3200 */ |
nucleosam | 0:36024a9bd220 | 401 | /* Request motor 3 to go to the same position */ |
nucleosam | 0:36024a9bd220 | 402 | if (pos == 3200) |
nucleosam | 0:36024a9bd220 | 403 | { |
nucleosam | 0:36024a9bd220 | 404 | /* Request motor 3 to Go to the same position and print to the console */ |
nucleosam | 0:36024a9bd220 | 405 | printf("--> Request motor3 to go to position 3200.\r\n"); |
Davidroid | 5:05b73855a2e1 | 406 | motor3->go_to(pos); |
nucleosam | 0:36024a9bd220 | 407 | |
nucleosam | 0:36024a9bd220 | 408 | /* Wait for motor 3 ends moving */ |
Davidroid | 5:05b73855a2e1 | 409 | motor3->wait_while_active(); |
nucleosam | 0:36024a9bd220 | 410 | } |
nucleosam | 0:36024a9bd220 | 411 | |
nucleosam | 0:36024a9bd220 | 412 | /* Get current position of motor 3 and print to the console */ |
Davidroid | 5:05b73855a2e1 | 413 | pos = motor3->get_position(); |
nucleosam | 0:36024a9bd220 | 414 | printf(" Motor3 position: %d.\r\n", pos); |
nucleosam | 0:36024a9bd220 | 415 | |
nucleosam | 0:36024a9bd220 | 416 | /* Wait for 1s */ |
nucleosam | 0:36024a9bd220 | 417 | wait_ms(1000); |
nucleosam | 0:36024a9bd220 | 418 | |
nucleosam | 0:36024a9bd220 | 419 | if (pos == 3200) |
nucleosam | 0:36024a9bd220 | 420 | { |
nucleosam | 0:36024a9bd220 | 421 | /* Request all motors to go home and print to the console */ |
nucleosam | 0:36024a9bd220 | 422 | printf(" Request all motors to go home.\r\n"); |
Davidroid | 5:05b73855a2e1 | 423 | motor1->queue_commands(POWERSTEP01_GO_HOME,0); |
Davidroid | 5:05b73855a2e1 | 424 | motor2->queue_commands(POWERSTEP01_GO_HOME,0); |
Davidroid | 5:05b73855a2e1 | 425 | motor3->queue_commands(POWERSTEP01_GO_HOME,0); |
Davidroid | 5:05b73855a2e1 | 426 | motor1->send_queued_commands(); |
nucleosam | 0:36024a9bd220 | 427 | |
nucleosam | 0:36024a9bd220 | 428 | /* Wait for all motors ends moving */ |
Davidroid | 5:05b73855a2e1 | 429 | wait_for_all_devices_not_busy(); |
nucleosam | 0:36024a9bd220 | 430 | } |
nucleosam | 0:36024a9bd220 | 431 | |
nucleosam | 0:36024a9bd220 | 432 | /* Wait for 1s */ |
nucleosam | 0:36024a9bd220 | 433 | wait_ms(1000); |
nucleosam | 0:36024a9bd220 | 434 | |
nucleosam | 0:36024a9bd220 | 435 | /* Request motor 1 to Goto position -3200 and print to the console */ |
nucleosam | 0:36024a9bd220 | 436 | printf("--> Request motor1 to go to position -3200.\r\n"); |
Davidroid | 5:05b73855a2e1 | 437 | motor1->go_to(-3200); |
nucleosam | 0:36024a9bd220 | 438 | |
nucleosam | 0:36024a9bd220 | 439 | /* Wait for motor 1 ends moving */ |
Davidroid | 5:05b73855a2e1 | 440 | motor1->wait_while_active(); |
nucleosam | 0:36024a9bd220 | 441 | |
nucleosam | 0:36024a9bd220 | 442 | /* Get current position of motor 1 and print to the console */ |
Davidroid | 5:05b73855a2e1 | 443 | pos = motor1->get_position(); |
nucleosam | 0:36024a9bd220 | 444 | printf(" Motor1 position: %d.\r\n", pos); |
nucleosam | 0:36024a9bd220 | 445 | |
nucleosam | 0:36024a9bd220 | 446 | /* If the read position of motor 1 is -3200 */ |
nucleosam | 0:36024a9bd220 | 447 | /* Request motor 2 to go to the same position */ |
nucleosam | 0:36024a9bd220 | 448 | if (pos == -3200) |
nucleosam | 0:36024a9bd220 | 449 | { |
nucleosam | 0:36024a9bd220 | 450 | /* Request motor 2 to go to the same position and print to the console */ |
nucleosam | 0:36024a9bd220 | 451 | printf("--> Request motor2 to go to position -3200.\r\n"); |
Davidroid | 5:05b73855a2e1 | 452 | motor2->go_to(pos); |
nucleosam | 0:36024a9bd220 | 453 | |
nucleosam | 0:36024a9bd220 | 454 | /* Wait for motor 2 ends moving */ |
Davidroid | 5:05b73855a2e1 | 455 | motor2->wait_while_active(); |
nucleosam | 0:36024a9bd220 | 456 | } |
nucleosam | 0:36024a9bd220 | 457 | |
nucleosam | 0:36024a9bd220 | 458 | /* Get current position of motor 2 and print to the console */ |
Davidroid | 5:05b73855a2e1 | 459 | pos = motor2->get_position(); |
nucleosam | 0:36024a9bd220 | 460 | printf(" Motor2 position: %d.\r\n", pos); |
nucleosam | 0:36024a9bd220 | 461 | |
nucleosam | 0:36024a9bd220 | 462 | /* If the read position of motor 2 is -3200 */ |
nucleosam | 0:36024a9bd220 | 463 | /* Request motor 3 to go to the same position */ |
nucleosam | 0:36024a9bd220 | 464 | if (pos == -3200) |
nucleosam | 0:36024a9bd220 | 465 | { |
nucleosam | 0:36024a9bd220 | 466 | /* Request motor 3 to go to the same position and print to the console */ |
nucleosam | 0:36024a9bd220 | 467 | printf("--> Request motor3 to go to position -3200.\r\n"); |
Davidroid | 5:05b73855a2e1 | 468 | motor3->go_to(pos); |
nucleosam | 0:36024a9bd220 | 469 | |
nucleosam | 0:36024a9bd220 | 470 | /* Wait for motor 3 ends moving */ |
Davidroid | 5:05b73855a2e1 | 471 | motor3->wait_while_active(); |
nucleosam | 0:36024a9bd220 | 472 | } |
nucleosam | 0:36024a9bd220 | 473 | |
nucleosam | 0:36024a9bd220 | 474 | /* Get current position of motor 3 and print to the console */ |
Davidroid | 5:05b73855a2e1 | 475 | pos = motor3->get_position(); |
nucleosam | 0:36024a9bd220 | 476 | printf(" Motor3 position: %d.\r\n", pos); |
nucleosam | 0:36024a9bd220 | 477 | |
nucleosam | 0:36024a9bd220 | 478 | /* Wait for 1s */ |
nucleosam | 0:36024a9bd220 | 479 | wait_ms(1000); |
nucleosam | 0:36024a9bd220 | 480 | |
nucleosam | 0:36024a9bd220 | 481 | if (pos == -3200) |
nucleosam | 0:36024a9bd220 | 482 | { |
nucleosam | 0:36024a9bd220 | 483 | /* Set current position of motor 3 to be its mark position*/ |
nucleosam | 0:36024a9bd220 | 484 | printf(" Set mark to current position of motor3.\r\n"); |
Davidroid | 5:05b73855a2e1 | 485 | motor3->set_mark(); |
nucleosam | 0:36024a9bd220 | 486 | |
nucleosam | 0:36024a9bd220 | 487 | /* Request all motors to go home and print to the console */ |
nucleosam | 0:36024a9bd220 | 488 | printf("--> Request all motors to go home.\r\n"); |
Davidroid | 5:05b73855a2e1 | 489 | motor1->queue_commands(POWERSTEP01_GO_HOME,0); |
Davidroid | 5:05b73855a2e1 | 490 | motor2->queue_commands(POWERSTEP01_GO_HOME,0); |
Davidroid | 5:05b73855a2e1 | 491 | motor3->queue_commands(POWERSTEP01_GO_HOME,0); |
Davidroid | 5:05b73855a2e1 | 492 | motor1->send_queued_commands(); |
nucleosam | 0:36024a9bd220 | 493 | |
nucleosam | 0:36024a9bd220 | 494 | /* Wait for all device ends moving */ |
Davidroid | 5:05b73855a2e1 | 495 | wait_for_all_devices_not_busy(); |
nucleosam | 0:36024a9bd220 | 496 | } |
nucleosam | 0:36024a9bd220 | 497 | |
nucleosam | 0:36024a9bd220 | 498 | /* Wait for 1s */ |
nucleosam | 0:36024a9bd220 | 499 | wait_ms(1000); |
nucleosam | 0:36024a9bd220 | 500 | |
nucleosam | 0:36024a9bd220 | 501 | /* Request motor 1 and motor 3 to go their mark position */ |
nucleosam | 0:36024a9bd220 | 502 | printf("--> Request motor1 and motor3 to go to their marked position.\r\n"); |
Davidroid | 5:05b73855a2e1 | 503 | motor1->queue_commands(POWERSTEP01_GO_MARK,0); |
Davidroid | 5:05b73855a2e1 | 504 | motor2->queue_commands(POWERSTEP01_NOP,0); |
Davidroid | 5:05b73855a2e1 | 505 | motor3->queue_commands(POWERSTEP01_GO_MARK,0); |
Davidroid | 5:05b73855a2e1 | 506 | motor1->send_queued_commands(); |
nucleosam | 0:36024a9bd220 | 507 | |
nucleosam | 0:36024a9bd220 | 508 | /* Wait for motor 1 and 2 ends moving */ |
Davidroid | 5:05b73855a2e1 | 509 | wait_for_all_devices_not_busy(); |
nucleosam | 0:36024a9bd220 | 510 | |
nucleosam | 0:36024a9bd220 | 511 | /* Wait for 1s */ |
nucleosam | 0:36024a9bd220 | 512 | wait_ms(1000); |
nucleosam | 0:36024a9bd220 | 513 | |
nucleosam | 0:36024a9bd220 | 514 | /* Request motor 1 to run in StepperMotor::FWD direction at 400 steps/s*/ |
nucleosam | 0:36024a9bd220 | 515 | printf("--> Request motor1 to run at 400 steps/s in forward direction.\r\n"); |
Davidroid | 5:05b73855a2e1 | 516 | motor1->run(StepperMotor::FWD, 400); |
nucleosam | 0:36024a9bd220 | 517 | |
nucleosam | 0:36024a9bd220 | 518 | /* Wait for device to reach the targeted speed */ |
Davidroid | 5:05b73855a2e1 | 519 | while((motor1->read_status_register() & POWERSTEP01_STATUS_MOT_STATUS)!= |
nucleosam | 0:36024a9bd220 | 520 | POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD) |
nucleosam | 0:36024a9bd220 | 521 | { |
nucleosam | 0:36024a9bd220 | 522 | /* Record the reached speed in step/s rounded to integer */ |
Davidroid | 5:05b73855a2e1 | 523 | unsignedIntegerValue = motor1->get_speed(); |
nucleosam | 0:36024a9bd220 | 524 | /* Print reached speed to the console in step/s */ |
nucleosam | 0:36024a9bd220 | 525 | printf(" motor1 reached Speed: %d step/s.\r\n", unsignedIntegerValue); |
nucleosam | 0:36024a9bd220 | 526 | wait_ms(50); |
nucleosam | 0:36024a9bd220 | 527 | } |
nucleosam | 0:36024a9bd220 | 528 | |
nucleosam | 0:36024a9bd220 | 529 | /* Record the reached speed in step/s */ |
Davidroid | 5:05b73855a2e1 | 530 | floatValue = motor1->get_analog_value(POWERSTEP01_SPEED); |
nucleosam | 0:36024a9bd220 | 531 | /* Print reached speed to the console in step/s */ |
nucleosam | 0:36024a9bd220 | 532 | printf(" motor1 reached Speed: %f step/s.\r\n", floatValue); |
nucleosam | 0:36024a9bd220 | 533 | |
nucleosam | 0:36024a9bd220 | 534 | /* Request motor 2 to run in StepperMotor::FWD direction at 300 steps/s*/ |
nucleosam | 0:36024a9bd220 | 535 | /* Request motor 3 to run in StepperMotor::FWD direction at 200 steps/s*/ |
nucleosam | 0:36024a9bd220 | 536 | /* and start at same time. */ |
nucleosam | 0:36024a9bd220 | 537 | printf("--> Request motor2 and motor3 to run respectively in forward direction\r\n"); |
nucleosam | 0:36024a9bd220 | 538 | printf(" at 300 steps/s and 200 steps/s and start at same time.\r\n"); |
Davidroid | 5:05b73855a2e1 | 539 | motor1->queue_commands(POWERSTEP01_NOP,0); |
Davidroid | 5:05b73855a2e1 | 540 | motor2->queue_commands((uint8_t)POWERSTEP01_RUN|(uint8_t)StepperMotor::FWD,PowerStep01::speed_steps_s_to_reg_val(300)); |
Davidroid | 5:05b73855a2e1 | 541 | motor3->queue_commands((uint8_t)POWERSTEP01_RUN|(uint8_t)StepperMotor::FWD,PowerStep01::speed_steps_s_to_reg_val(200)); |
Davidroid | 5:05b73855a2e1 | 542 | motor1->send_queued_commands(); |
nucleosam | 0:36024a9bd220 | 543 | |
nucleosam | 0:36024a9bd220 | 544 | /* Wait for device to reach the targeted speed */ |
Davidroid | 5:05b73855a2e1 | 545 | while(((motor2->read_status_register() & POWERSTEP01_STATUS_MOT_STATUS)!= |
nucleosam | 0:36024a9bd220 | 546 | POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)|| |
Davidroid | 5:05b73855a2e1 | 547 | ((motor3->read_status_register() & POWERSTEP01_STATUS_MOT_STATUS)!= |
nucleosam | 0:36024a9bd220 | 548 | POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)); |
nucleosam | 0:36024a9bd220 | 549 | |
nucleosam | 0:36024a9bd220 | 550 | /* Record the reached speed in step/s */ |
Davidroid | 5:05b73855a2e1 | 551 | floatValue = motor2->get_analog_value(POWERSTEP01_SPEED); |
nucleosam | 0:36024a9bd220 | 552 | /* Print reached speed to the console in step/s */ |
nucleosam | 0:36024a9bd220 | 553 | printf(" motor2 reached Speed: %f step/s.\r\n", floatValue); |
nucleosam | 0:36024a9bd220 | 554 | |
nucleosam | 0:36024a9bd220 | 555 | /* Record the reached speed in step/s */ |
Davidroid | 5:05b73855a2e1 | 556 | floatValue = motor3->get_analog_value(POWERSTEP01_SPEED); |
nucleosam | 0:36024a9bd220 | 557 | /* Print reached speed to the console in step/s */ |
nucleosam | 0:36024a9bd220 | 558 | printf(" motor3 reached Speed: %f step/s.\r\n", floatValue); |
nucleosam | 0:36024a9bd220 | 559 | |
nucleosam | 0:36024a9bd220 | 560 | /* Wait for 3s */ |
nucleosam | 0:36024a9bd220 | 561 | wait_ms(3000); |
nucleosam | 0:36024a9bd220 | 562 | |
nucleosam | 0:36024a9bd220 | 563 | /* Request motor 2 to make a soft stop */ |
nucleosam | 0:36024a9bd220 | 564 | printf("--> Request motor2 to stop softly\r\n"); |
Davidroid | 5:05b73855a2e1 | 565 | motor2->soft_stop(); |
nucleosam | 0:36024a9bd220 | 566 | |
nucleosam | 0:36024a9bd220 | 567 | /* Wait for motor 2 end moving */ |
Davidroid | 5:05b73855a2e1 | 568 | motor2->wait_while_active(); |
nucleosam | 0:36024a9bd220 | 569 | |
nucleosam | 0:36024a9bd220 | 570 | /* Request motor 1 and 3 to make a hard stop */ |
nucleosam | 0:36024a9bd220 | 571 | printf("--> Request motor1 and motor3 to stop immediately\r\n"); |
Davidroid | 5:05b73855a2e1 | 572 | motor1->queue_commands(POWERSTEP01_HARD_STOP,0); |
Davidroid | 5:05b73855a2e1 | 573 | motor2->queue_commands(POWERSTEP01_NOP,0); |
Davidroid | 5:05b73855a2e1 | 574 | motor3->queue_commands(POWERSTEP01_HARD_STOP,0); |
Davidroid | 5:05b73855a2e1 | 575 | motor1->send_queued_commands(); |
nucleosam | 0:36024a9bd220 | 576 | |
nucleosam | 0:36024a9bd220 | 577 | /* Wait for both motors end moving */ |
Davidroid | 5:05b73855a2e1 | 578 | wait_for_all_devices_not_busy(); |
nucleosam | 0:36024a9bd220 | 579 | |
nucleosam | 0:36024a9bd220 | 580 | /* Request all motors to go home and print to the console */ |
nucleosam | 0:36024a9bd220 | 581 | printf("--> Request all motors to go home.\r\n"); |
Davidroid | 5:05b73855a2e1 | 582 | motor1->queue_commands(POWERSTEP01_GO_HOME,0); |
Davidroid | 5:05b73855a2e1 | 583 | motor2->queue_commands(POWERSTEP01_GO_HOME,0); |
Davidroid | 5:05b73855a2e1 | 584 | motor3->queue_commands(POWERSTEP01_GO_HOME,0); |
Davidroid | 5:05b73855a2e1 | 585 | motor1->send_queued_commands(); |
nucleosam | 0:36024a9bd220 | 586 | |
nucleosam | 0:36024a9bd220 | 587 | /* Wait for all device ends moving */ |
Davidroid | 5:05b73855a2e1 | 588 | wait_for_all_devices_not_busy(); |
nucleosam | 0:36024a9bd220 | 589 | |
nucleosam | 0:36024a9bd220 | 590 | /* Get acceleration, deceleration, Maxspeed and MinSpeed of motor 1*/ |
Davidroid | 5:05b73855a2e1 | 591 | myMaxSpeed= motor1->get_raw_parameter(POWERSTEP01_MAX_SPEED); |
Davidroid | 5:05b73855a2e1 | 592 | myAcceleration = motor1->get_raw_parameter(POWERSTEP01_ACC); |
Davidroid | 5:05b73855a2e1 | 593 | myDeceleration = motor1->get_raw_parameter(POWERSTEP01_DEC); |
Davidroid | 5:05b73855a2e1 | 594 | myMinSpeed = motor1->get_raw_parameter(POWERSTEP01_MIN_SPEED); |
nucleosam | 0:36024a9bd220 | 595 | |
nucleosam | 0:36024a9bd220 | 596 | /* Select 1/16 microstepping mode for motor 1 */ |
nucleosam | 0:36024a9bd220 | 597 | printf(" Set 1/16 microstepping mode for motor1.\r\n"); |
Davidroid | 5:05b73855a2e1 | 598 | motor1->set_step_mode(StepperMotor::STEP_MODE_1_16); |
nucleosam | 0:36024a9bd220 | 599 | |
nucleosam | 0:36024a9bd220 | 600 | /* Select 1/8 microstepping mode for motor 2 */ |
nucleosam | 0:36024a9bd220 | 601 | printf(" Set 1/8 microstepping mode for motor2.\r\n"); |
Davidroid | 5:05b73855a2e1 | 602 | motor2->set_step_mode(StepperMotor::STEP_MODE_1_8); |
nucleosam | 0:36024a9bd220 | 603 | |
nucleosam | 0:36024a9bd220 | 604 | /* Set speed and acceleration of motor 2 */ |
nucleosam | 0:36024a9bd220 | 605 | /* Do not scale with microstepping mode */ |
Davidroid | 5:05b73855a2e1 | 606 | motor2->set_raw_parameter(POWERSTEP01_ACC, myAcceleration); |
Davidroid | 5:05b73855a2e1 | 607 | motor2->set_raw_parameter(POWERSTEP01_DEC, myDeceleration); |
Davidroid | 5:05b73855a2e1 | 608 | motor2->set_raw_parameter(POWERSTEP01_MIN_SPEED, myMinSpeed); |
Davidroid | 5:05b73855a2e1 | 609 | motor2->set_raw_parameter(POWERSTEP01_MAX_SPEED, myMaxSpeed); |
nucleosam | 0:36024a9bd220 | 610 | |
nucleosam | 0:36024a9bd220 | 611 | /* Select ful step mode for motor 3 */ |
nucleosam | 0:36024a9bd220 | 612 | printf(" Set ful step mode for motor3.\r\n"); |
Davidroid | 5:05b73855a2e1 | 613 | motor3->set_step_mode(StepperMotor::STEP_MODE_FULL); |
nucleosam | 0:36024a9bd220 | 614 | |
nucleosam | 0:36024a9bd220 | 615 | /* Set speed and acceleration of motor 3 */ |
nucleosam | 0:36024a9bd220 | 616 | /* Do not scale with microstepping mode */ |
Davidroid | 5:05b73855a2e1 | 617 | motor3->set_raw_parameter(POWERSTEP01_ACC, myAcceleration); |
Davidroid | 5:05b73855a2e1 | 618 | motor3->set_raw_parameter(POWERSTEP01_DEC, myDeceleration); |
Davidroid | 5:05b73855a2e1 | 619 | motor3->set_raw_parameter(POWERSTEP01_MIN_SPEED, myMinSpeed); |
Davidroid | 5:05b73855a2e1 | 620 | motor3->set_raw_parameter(POWERSTEP01_MAX_SPEED, myMaxSpeed); |
nucleosam | 0:36024a9bd220 | 621 | |
nucleosam | 0:36024a9bd220 | 622 | /* Printing to the console. */ |
nucleosam | 0:36024a9bd220 | 623 | printf("--> Infinite Loop...\r\n"); |
nucleosam | 0:36024a9bd220 | 624 | /* Infinite loop */ |
nucleosam | 0:36024a9bd220 | 625 | while(1) |
nucleosam | 0:36024a9bd220 | 626 | { |
nucleosam | 0:36024a9bd220 | 627 | /* motor 1 is using 1/16 microstepping mode */ |
nucleosam | 0:36024a9bd220 | 628 | /* motor 2 is using 1/8 microstepping mode */ |
nucleosam | 0:36024a9bd220 | 629 | /* motor 3 is using full step mode */ |
nucleosam | 0:36024a9bd220 | 630 | /* position is in microsteps */ |
Davidroid | 5:05b73855a2e1 | 631 | motor1->queue_commands(POWERSTEP01_GO_TO,-3200); |
Davidroid | 5:05b73855a2e1 | 632 | motor2->queue_commands(POWERSTEP01_GO_TO,1600); |
Davidroid | 5:05b73855a2e1 | 633 | motor3->queue_commands(POWERSTEP01_GO_TO,-200); |
Davidroid | 5:05b73855a2e1 | 634 | motor1->send_queued_commands(); |
nucleosam | 0:36024a9bd220 | 635 | |
nucleosam | 0:36024a9bd220 | 636 | /* Wait for all device ends moving */ |
Davidroid | 5:05b73855a2e1 | 637 | wait_for_all_devices_not_busy(); |
nucleosam | 0:36024a9bd220 | 638 | |
Davidroid | 5:05b73855a2e1 | 639 | motor1->queue_commands(POWERSTEP01_GO_TO,3200); |
Davidroid | 5:05b73855a2e1 | 640 | motor2->queue_commands(POWERSTEP01_GO_TO,-1600); |
Davidroid | 5:05b73855a2e1 | 641 | motor3->queue_commands(POWERSTEP01_GO_TO,200); |
Davidroid | 5:05b73855a2e1 | 642 | motor1->send_queued_commands(); |
nucleosam | 0:36024a9bd220 | 643 | |
nucleosam | 0:36024a9bd220 | 644 | /* Wait for all device ends moving */ |
Davidroid | 5:05b73855a2e1 | 645 | wait_for_all_devices_not_busy(); |
nucleosam | 0:36024a9bd220 | 646 | } |
nucleosam | 0:36024a9bd220 | 647 | } |
nucleosam | 0:36024a9bd220 | 648 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ |