![](/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@0:36024a9bd220, 2016-04-05 (annotated)
- Committer:
- nucleosam
- Date:
- Tue Apr 05 15:33:37 2016 +0000
- Revision:
- 0:36024a9bd220
- Child:
- 1:1d98c151c8eb
Motion control example for 3 motors: initial version.
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. */ |
nucleosam | 0:36024a9bd220 | 50 | #include "powerstep01_class.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. */ |
nucleosam | 0:36024a9bd220 | 56 | powerstep01_Init_u_t initDeviceParameters = |
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 | 0:36024a9bd220 | 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. */ |
nucleosam | 0:36024a9bd220 | 104 | POWERSTEP01 *motor1; |
nucleosam | 0:36024a9bd220 | 105 | POWERSTEP01 *motor2; |
nucleosam | 0:36024a9bd220 | 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: |
nucleosam | 0:36024a9bd220 | 115 | * + motor->AttachFlagIRQ(&FlagIRQHandler); |
nucleosam | 0:36024a9bd220 | 116 | * + motor->EnableFlagIRQ(); |
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. */ |
nucleosam | 0:36024a9bd220 | 123 | POWERSTEP01::isrFlag = TRUE; |
nucleosam | 0:36024a9bd220 | 124 | |
nucleosam | 0:36024a9bd220 | 125 | motor1->FetchAndClearAllStatus(); |
nucleosam | 0:36024a9bd220 | 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. */ |
nucleosam | 0:36024a9bd220 | 132 | for (uint8_t loop = 0; loop<POWERSTEP01::GetNbDevices();loop++) |
nucleosam | 0:36024a9bd220 | 133 | { |
nucleosam | 0:36024a9bd220 | 134 | if (loop==1) motor = motor2; |
nucleosam | 0:36024a9bd220 | 135 | if (loop==2) motor = motor3; |
nucleosam | 0:36024a9bd220 | 136 | statusRegister = motor->GetFetchedStatus(); |
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 | 0:36024a9bd220 | 227 | printf(" ADC undervoltage lock-out.\r\n"); |
nucleosam | 0:36024a9bd220 | 228 | } |
nucleosam | 0:36024a9bd220 | 229 | /* Check thermal STATUS flags: if set, the thermal status is not normal */ |
nucleosam | 0:36024a9bd220 | 230 | if ((statusRegister & POWERSTEP01_STATUS_TH_STATUS)!=0) |
nucleosam | 0:36024a9bd220 | 231 | { |
nucleosam | 0:36024a9bd220 | 232 | //thermal status: 1: Warning, 2: Bridge shutdown, 3: Device shutdown |
nucleosam | 0:36024a9bd220 | 233 | if (((statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11)==1) |
nucleosam | 0:36024a9bd220 | 234 | { |
nucleosam | 0:36024a9bd220 | 235 | printf(" Thermal status - Warning.\r\n"); |
nucleosam | 0:36024a9bd220 | 236 | } |
nucleosam | 0:36024a9bd220 | 237 | else if (((statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11)==2) |
nucleosam | 0:36024a9bd220 | 238 | { |
nucleosam | 0:36024a9bd220 | 239 | printf(" Thermal status - Bridge shutdown.\r\n"); |
nucleosam | 0:36024a9bd220 | 240 | } |
nucleosam | 0:36024a9bd220 | 241 | else if (((statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11)==3) |
nucleosam | 0:36024a9bd220 | 242 | { |
nucleosam | 0:36024a9bd220 | 243 | printf(" Thermal status - Device shutdown.\r\n"); |
nucleosam | 0:36024a9bd220 | 244 | } |
nucleosam | 0:36024a9bd220 | 245 | } |
nucleosam | 0:36024a9bd220 | 246 | /* Check OCD flag: if not set, there is an overcurrent detection */ |
nucleosam | 0:36024a9bd220 | 247 | if ((statusRegister & POWERSTEP01_STATUS_OCD)==0) |
nucleosam | 0:36024a9bd220 | 248 | { |
nucleosam | 0:36024a9bd220 | 249 | //Overcurrent detection |
nucleosam | 0:36024a9bd220 | 250 | printf(" Overcurrent detection.\r\n"); |
nucleosam | 0:36024a9bd220 | 251 | } |
nucleosam | 0:36024a9bd220 | 252 | /* Check STALL_A flag: if not set, there is a Stall condition on bridge A */ |
nucleosam | 0:36024a9bd220 | 253 | if ((statusRegister & POWERSTEP01_STATUS_STALL_A)==0) |
nucleosam | 0:36024a9bd220 | 254 | { |
nucleosam | 0:36024a9bd220 | 255 | //Bridge A stalled |
nucleosam | 0:36024a9bd220 | 256 | printf(" Bridge A stalled.\r\n"); |
nucleosam | 0:36024a9bd220 | 257 | } |
nucleosam | 0:36024a9bd220 | 258 | /* Check STALL_B flag: if not set, there is a Stall condition on bridge B */ |
nucleosam | 0:36024a9bd220 | 259 | if ((statusRegister & POWERSTEP01_STATUS_STALL_B)==0) |
nucleosam | 0:36024a9bd220 | 260 | { |
nucleosam | 0:36024a9bd220 | 261 | //Bridge B stalled |
nucleosam | 0:36024a9bd220 | 262 | printf(" Bridge B stalled.\r\n"); |
nucleosam | 0:36024a9bd220 | 263 | } |
nucleosam | 0:36024a9bd220 | 264 | } |
nucleosam | 0:36024a9bd220 | 265 | /* Reset ISR flag. */ |
nucleosam | 0:36024a9bd220 | 266 | POWERSTEP01::isrFlag = FALSE; |
nucleosam | 0:36024a9bd220 | 267 | } |
nucleosam | 0:36024a9bd220 | 268 | |
nucleosam | 0:36024a9bd220 | 269 | /** |
nucleosam | 0:36024a9bd220 | 270 | * @brief This is an example of user handler for the busy interrupt. |
nucleosam | 0:36024a9bd220 | 271 | * @param None |
nucleosam | 0:36024a9bd220 | 272 | * @retval None |
nucleosam | 0:36024a9bd220 | 273 | * @note If needed, implement it, and then attach and enable it: |
nucleosam | 0:36024a9bd220 | 274 | * + motor->AttachBusyIRQ(&FlagIRQHandler); |
nucleosam | 0:36024a9bd220 | 275 | * + motor->EnableBusyIRQ(); |
nucleosam | 0:36024a9bd220 | 276 | * To disable it: |
nucleosam | 0:36024a9bd220 | 277 | * + motor->DisbleBusyIRQ(); |
nucleosam | 0:36024a9bd220 | 278 | */ |
nucleosam | 0:36024a9bd220 | 279 | void myBusyIRQHandler(void) |
nucleosam | 0:36024a9bd220 | 280 | { |
nucleosam | 0:36024a9bd220 | 281 | /* Set ISR flag. */ |
nucleosam | 0:36024a9bd220 | 282 | POWERSTEP01::isrFlag = TRUE; |
nucleosam | 0:36024a9bd220 | 283 | |
nucleosam | 0:36024a9bd220 | 284 | /* Reset ISR flag. */ |
nucleosam | 0:36024a9bd220 | 285 | POWERSTEP01::isrFlag = FALSE; |
nucleosam | 0:36024a9bd220 | 286 | } |
nucleosam | 0:36024a9bd220 | 287 | |
nucleosam | 0:36024a9bd220 | 288 | /** |
nucleosam | 0:36024a9bd220 | 289 | * @brief This is an example of error handler. |
nucleosam | 0:36024a9bd220 | 290 | * @param[in] error Number of the error |
nucleosam | 0:36024a9bd220 | 291 | * @retval None |
nucleosam | 0:36024a9bd220 | 292 | */ |
nucleosam | 0:36024a9bd220 | 293 | void myErrorHandler(uint16_t error) |
nucleosam | 0:36024a9bd220 | 294 | { |
nucleosam | 0:36024a9bd220 | 295 | /* Printing to the console. */ |
nucleosam | 0:36024a9bd220 | 296 | printf("Error %d detected\r\n\n", error); |
nucleosam | 0:36024a9bd220 | 297 | |
nucleosam | 0:36024a9bd220 | 298 | /* Infinite loop */ |
nucleosam | 0:36024a9bd220 | 299 | while(1) |
nucleosam | 0:36024a9bd220 | 300 | { |
nucleosam | 0:36024a9bd220 | 301 | } |
nucleosam | 0:36024a9bd220 | 302 | } |
nucleosam | 0:36024a9bd220 | 303 | |
nucleosam | 0:36024a9bd220 | 304 | void WaitForAllDevicesNotBusy(void) |
nucleosam | 0:36024a9bd220 | 305 | { |
nucleosam | 0:36024a9bd220 | 306 | /* Wait while at least one is active */ |
nucleosam | 0:36024a9bd220 | 307 | while (motor1->IsDeviceBusy()|motor2->IsDeviceBusy()|motor3->IsDeviceBusy()); |
nucleosam | 0:36024a9bd220 | 308 | } |
nucleosam | 0:36024a9bd220 | 309 | |
nucleosam | 0:36024a9bd220 | 310 | /* Main ----------------------------------------------------------------------*/ |
nucleosam | 0:36024a9bd220 | 311 | |
nucleosam | 0:36024a9bd220 | 312 | int main() |
nucleosam | 0:36024a9bd220 | 313 | { |
nucleosam | 0:36024a9bd220 | 314 | int32_t pos; |
nucleosam | 0:36024a9bd220 | 315 | uint32_t myMaxSpeed; |
nucleosam | 0:36024a9bd220 | 316 | uint32_t myMinSpeed; |
nucleosam | 0:36024a9bd220 | 317 | uint16_t myAcceleration; |
nucleosam | 0:36024a9bd220 | 318 | uint16_t myDeceleration; |
nucleosam | 0:36024a9bd220 | 319 | uint32_t unsignedIntegerValue; |
nucleosam | 0:36024a9bd220 | 320 | float floatValue; |
nucleosam | 0:36024a9bd220 | 321 | |
nucleosam | 0:36024a9bd220 | 322 | /* Printing to the console. */ |
nucleosam | 0:36024a9bd220 | 323 | printf("STARTING MAIN PROGRAM\r\n"); |
nucleosam | 0:36024a9bd220 | 324 | printf(" Reminder:\r\n"); |
nucleosam | 0:36024a9bd220 | 325 | printf(" The position unit is in agreement to the step mode.\r\n"); |
nucleosam | 0:36024a9bd220 | 326 | printf(" The speed, acceleration or deceleration unit\r\n"); |
nucleosam | 0:36024a9bd220 | 327 | printf(" do not depend on the step mode and the step unit is a full step.\r\n"); |
nucleosam | 0:36024a9bd220 | 328 | |
nucleosam | 0:36024a9bd220 | 329 | //----- Initialization |
nucleosam | 0:36024a9bd220 | 330 | /* Initializing SPI bus. */ |
nucleosam | 0:36024a9bd220 | 331 | DevSPI dev_spi(D11, D12, D13); |
nucleosam | 0:36024a9bd220 | 332 | |
nucleosam | 0:36024a9bd220 | 333 | /* Initializing Motor Control Component. */ |
nucleosam | 0:36024a9bd220 | 334 | motor1 = new POWERSTEP01(D2, D4, D8, D9, D10, dev_spi); |
nucleosam | 0:36024a9bd220 | 335 | motor2 = new POWERSTEP01(D2, D4, D8, D9, D10, dev_spi); |
nucleosam | 0:36024a9bd220 | 336 | motor3 = new POWERSTEP01(D2, D4, D8, D9, D10, dev_spi); |
nucleosam | 0:36024a9bd220 | 337 | if (motor1->Init(&initDeviceParameters) != COMPONENT_OK) exit(EXIT_FAILURE); |
nucleosam | 0:36024a9bd220 | 338 | if (motor2->Init(&initDeviceParameters) != COMPONENT_OK) exit(EXIT_FAILURE); |
nucleosam | 0:36024a9bd220 | 339 | if (motor3->Init(&initDeviceParameters) != COMPONENT_OK) exit(EXIT_FAILURE); |
nucleosam | 0:36024a9bd220 | 340 | |
nucleosam | 0:36024a9bd220 | 341 | /* Attaching and enabling interrupt handlers. */ |
nucleosam | 0:36024a9bd220 | 342 | motor1->AttachFlagIRQ(&myFlagIRQHandler); |
nucleosam | 0:36024a9bd220 | 343 | motor1->EnableFlagIRQ(); |
nucleosam | 0:36024a9bd220 | 344 | motor1->AttachBusyIRQ(&myBusyIRQHandler); |
nucleosam | 0:36024a9bd220 | 345 | motor1->EnableBusyIRQ(); |
nucleosam | 0:36024a9bd220 | 346 | motor2->AttachFlagIRQ(&myFlagIRQHandler); |
nucleosam | 0:36024a9bd220 | 347 | motor2->EnableFlagIRQ(); |
nucleosam | 0:36024a9bd220 | 348 | motor2->AttachBusyIRQ(&myBusyIRQHandler); |
nucleosam | 0:36024a9bd220 | 349 | motor2->EnableBusyIRQ(); |
nucleosam | 0:36024a9bd220 | 350 | motor3->AttachFlagIRQ(&myFlagIRQHandler); |
nucleosam | 0:36024a9bd220 | 351 | motor3->EnableFlagIRQ(); |
nucleosam | 0:36024a9bd220 | 352 | motor3->AttachBusyIRQ(&myBusyIRQHandler); |
nucleosam | 0:36024a9bd220 | 353 | motor3->EnableBusyIRQ(); |
nucleosam | 0:36024a9bd220 | 354 | |
nucleosam | 0:36024a9bd220 | 355 | /* Attaching an error handler */ |
nucleosam | 0:36024a9bd220 | 356 | motor1->AttachErrorHandler(&myErrorHandler); |
nucleosam | 0:36024a9bd220 | 357 | motor2->AttachErrorHandler(&myErrorHandler); |
nucleosam | 0:36024a9bd220 | 358 | motor3->AttachErrorHandler(&myErrorHandler); |
nucleosam | 0:36024a9bd220 | 359 | |
nucleosam | 0:36024a9bd220 | 360 | /* Printing to the console. */ |
nucleosam | 0:36024a9bd220 | 361 | printf("Motor Control Application Example for 3 Motors\r\n"); |
nucleosam | 0:36024a9bd220 | 362 | |
nucleosam | 0:36024a9bd220 | 363 | /* Request motor 1 to go to position 3200 and print to the console */ |
nucleosam | 0:36024a9bd220 | 364 | printf("--> Request motor1 to go to position 3200.\r\n"); |
nucleosam | 0:36024a9bd220 | 365 | motor1->GoTo(3200); |
nucleosam | 0:36024a9bd220 | 366 | |
nucleosam | 0:36024a9bd220 | 367 | /* Wait for motor 2 ends moving */ |
nucleosam | 0:36024a9bd220 | 368 | motor1->WaitWhileActive(); |
nucleosam | 0:36024a9bd220 | 369 | |
nucleosam | 0:36024a9bd220 | 370 | /* Get current position of motor 1 and print to the console */ |
nucleosam | 0:36024a9bd220 | 371 | pos = motor1->GetPosition(); |
nucleosam | 0:36024a9bd220 | 372 | printf(" Motor1 position: %d.\r\n", pos); |
nucleosam | 0:36024a9bd220 | 373 | |
nucleosam | 0:36024a9bd220 | 374 | /* Wait for 2 seconds */ |
nucleosam | 0:36024a9bd220 | 375 | wait_ms(2000); |
nucleosam | 0:36024a9bd220 | 376 | |
nucleosam | 0:36024a9bd220 | 377 | /* If the read position of motor 1 is 3200 */ |
nucleosam | 0:36024a9bd220 | 378 | /* Request motor 2 to go to the same position */ |
nucleosam | 0:36024a9bd220 | 379 | if (pos == 3200) |
nucleosam | 0:36024a9bd220 | 380 | { |
nucleosam | 0:36024a9bd220 | 381 | /* Set current position of motor 1 to be its mark position*/ |
nucleosam | 0:36024a9bd220 | 382 | printf(" Set mark to current position of motor1.\r\n"); |
nucleosam | 0:36024a9bd220 | 383 | motor1->SetMark(); |
nucleosam | 0:36024a9bd220 | 384 | |
nucleosam | 0:36024a9bd220 | 385 | /* Request motor 2 to Go to the same position and print to the console */ |
nucleosam | 0:36024a9bd220 | 386 | printf("--> Request motor2 to go to position 3200.\r\n"); |
nucleosam | 0:36024a9bd220 | 387 | motor2->GoTo(pos); |
nucleosam | 0:36024a9bd220 | 388 | |
nucleosam | 0:36024a9bd220 | 389 | /* Wait for motor 2 ends moving */ |
nucleosam | 0:36024a9bd220 | 390 | motor2->WaitWhileActive(); |
nucleosam | 0:36024a9bd220 | 391 | } |
nucleosam | 0:36024a9bd220 | 392 | |
nucleosam | 0:36024a9bd220 | 393 | /* Get current position of motor 2 and print to the console */ |
nucleosam | 0:36024a9bd220 | 394 | pos = motor2->GetPosition(); |
nucleosam | 0:36024a9bd220 | 395 | printf(" Motor2 position: %d.\r\n", pos); |
nucleosam | 0:36024a9bd220 | 396 | |
nucleosam | 0:36024a9bd220 | 397 | /* If the read position of motor 2 is 3200 */ |
nucleosam | 0:36024a9bd220 | 398 | /* Request motor 3 to go to the same position */ |
nucleosam | 0:36024a9bd220 | 399 | if (pos == 3200) |
nucleosam | 0:36024a9bd220 | 400 | { |
nucleosam | 0:36024a9bd220 | 401 | /* Request motor 3 to Go to the same position and print to the console */ |
nucleosam | 0:36024a9bd220 | 402 | printf("--> Request motor3 to go to position 3200.\r\n"); |
nucleosam | 0:36024a9bd220 | 403 | motor3->GoTo(pos); |
nucleosam | 0:36024a9bd220 | 404 | |
nucleosam | 0:36024a9bd220 | 405 | /* Wait for motor 3 ends moving */ |
nucleosam | 0:36024a9bd220 | 406 | motor3->WaitWhileActive(); |
nucleosam | 0:36024a9bd220 | 407 | } |
nucleosam | 0:36024a9bd220 | 408 | |
nucleosam | 0:36024a9bd220 | 409 | /* Get current position of motor 3 and print to the console */ |
nucleosam | 0:36024a9bd220 | 410 | pos = motor3->GetPosition(); |
nucleosam | 0:36024a9bd220 | 411 | printf(" Motor3 position: %d.\r\n", pos); |
nucleosam | 0:36024a9bd220 | 412 | |
nucleosam | 0:36024a9bd220 | 413 | /* Wait for 1s */ |
nucleosam | 0:36024a9bd220 | 414 | wait_ms(1000); |
nucleosam | 0:36024a9bd220 | 415 | |
nucleosam | 0:36024a9bd220 | 416 | if (pos == 3200) |
nucleosam | 0:36024a9bd220 | 417 | { |
nucleosam | 0:36024a9bd220 | 418 | /* Request all motors to go home and print to the console */ |
nucleosam | 0:36024a9bd220 | 419 | printf(" Request all motors to go home.\r\n"); |
nucleosam | 0:36024a9bd220 | 420 | motor1->QueueCommands(POWERSTEP01_GO_HOME,0); |
nucleosam | 0:36024a9bd220 | 421 | motor2->QueueCommands(POWERSTEP01_GO_HOME,0); |
nucleosam | 0:36024a9bd220 | 422 | motor3->QueueCommands(POWERSTEP01_GO_HOME,0); |
nucleosam | 0:36024a9bd220 | 423 | motor1->SendQueuedCommands(); |
nucleosam | 0:36024a9bd220 | 424 | |
nucleosam | 0:36024a9bd220 | 425 | /* Wait for all motors ends moving */ |
nucleosam | 0:36024a9bd220 | 426 | WaitForAllDevicesNotBusy(); |
nucleosam | 0:36024a9bd220 | 427 | } |
nucleosam | 0:36024a9bd220 | 428 | |
nucleosam | 0:36024a9bd220 | 429 | /* Wait for 1s */ |
nucleosam | 0:36024a9bd220 | 430 | wait_ms(1000); |
nucleosam | 0:36024a9bd220 | 431 | |
nucleosam | 0:36024a9bd220 | 432 | /* Request motor 1 to Goto position -3200 and print to the console */ |
nucleosam | 0:36024a9bd220 | 433 | printf("--> Request motor1 to go to position -3200.\r\n"); |
nucleosam | 0:36024a9bd220 | 434 | motor1->GoTo(-3200); |
nucleosam | 0:36024a9bd220 | 435 | |
nucleosam | 0:36024a9bd220 | 436 | /* Wait for motor 1 ends moving */ |
nucleosam | 0:36024a9bd220 | 437 | motor1->WaitWhileActive(); |
nucleosam | 0:36024a9bd220 | 438 | |
nucleosam | 0:36024a9bd220 | 439 | /* Get current position of motor 1 and print to the console */ |
nucleosam | 0:36024a9bd220 | 440 | pos = motor1->GetPosition(); |
nucleosam | 0:36024a9bd220 | 441 | printf(" Motor1 position: %d.\r\n", pos); |
nucleosam | 0:36024a9bd220 | 442 | |
nucleosam | 0:36024a9bd220 | 443 | /* If the read position of motor 1 is -3200 */ |
nucleosam | 0:36024a9bd220 | 444 | /* Request motor 2 to go to the same position */ |
nucleosam | 0:36024a9bd220 | 445 | if (pos == -3200) |
nucleosam | 0:36024a9bd220 | 446 | { |
nucleosam | 0:36024a9bd220 | 447 | /* Request motor 2 to go to the same position and print to the console */ |
nucleosam | 0:36024a9bd220 | 448 | printf("--> Request motor2 to go to position -3200.\r\n"); |
nucleosam | 0:36024a9bd220 | 449 | motor2->GoTo(pos); |
nucleosam | 0:36024a9bd220 | 450 | |
nucleosam | 0:36024a9bd220 | 451 | /* Wait for motor 2 ends moving */ |
nucleosam | 0:36024a9bd220 | 452 | motor2->WaitWhileActive(); |
nucleosam | 0:36024a9bd220 | 453 | } |
nucleosam | 0:36024a9bd220 | 454 | |
nucleosam | 0:36024a9bd220 | 455 | /* Get current position of motor 2 and print to the console */ |
nucleosam | 0:36024a9bd220 | 456 | pos = motor2->GetPosition(); |
nucleosam | 0:36024a9bd220 | 457 | printf(" Motor2 position: %d.\r\n", pos); |
nucleosam | 0:36024a9bd220 | 458 | |
nucleosam | 0:36024a9bd220 | 459 | /* If the read position of motor 2 is -3200 */ |
nucleosam | 0:36024a9bd220 | 460 | /* Request motor 3 to go to the same position */ |
nucleosam | 0:36024a9bd220 | 461 | if (pos == -3200) |
nucleosam | 0:36024a9bd220 | 462 | { |
nucleosam | 0:36024a9bd220 | 463 | /* Request motor 3 to go to the same position and print to the console */ |
nucleosam | 0:36024a9bd220 | 464 | printf("--> Request motor3 to go to position -3200.\r\n"); |
nucleosam | 0:36024a9bd220 | 465 | motor3->GoTo(pos); |
nucleosam | 0:36024a9bd220 | 466 | |
nucleosam | 0:36024a9bd220 | 467 | /* Wait for motor 3 ends moving */ |
nucleosam | 0:36024a9bd220 | 468 | motor3->WaitWhileActive(); |
nucleosam | 0:36024a9bd220 | 469 | } |
nucleosam | 0:36024a9bd220 | 470 | |
nucleosam | 0:36024a9bd220 | 471 | /* Get current position of motor 3 and print to the console */ |
nucleosam | 0:36024a9bd220 | 472 | pos = motor3->GetPosition(); |
nucleosam | 0:36024a9bd220 | 473 | printf(" Motor3 position: %d.\r\n", pos); |
nucleosam | 0:36024a9bd220 | 474 | |
nucleosam | 0:36024a9bd220 | 475 | /* Wait for 1s */ |
nucleosam | 0:36024a9bd220 | 476 | wait_ms(1000); |
nucleosam | 0:36024a9bd220 | 477 | |
nucleosam | 0:36024a9bd220 | 478 | if (pos == -3200) |
nucleosam | 0:36024a9bd220 | 479 | { |
nucleosam | 0:36024a9bd220 | 480 | /* Set current position of motor 3 to be its mark position*/ |
nucleosam | 0:36024a9bd220 | 481 | printf(" Set mark to current position of motor3.\r\n"); |
nucleosam | 0:36024a9bd220 | 482 | motor3->SetMark(); |
nucleosam | 0:36024a9bd220 | 483 | |
nucleosam | 0:36024a9bd220 | 484 | /* Request all motors to go home and print to the console */ |
nucleosam | 0:36024a9bd220 | 485 | printf("--> Request all motors to go home.\r\n"); |
nucleosam | 0:36024a9bd220 | 486 | motor1->QueueCommands(POWERSTEP01_GO_HOME,0); |
nucleosam | 0:36024a9bd220 | 487 | motor2->QueueCommands(POWERSTEP01_GO_HOME,0); |
nucleosam | 0:36024a9bd220 | 488 | motor3->QueueCommands(POWERSTEP01_GO_HOME,0); |
nucleosam | 0:36024a9bd220 | 489 | motor1->SendQueuedCommands(); |
nucleosam | 0:36024a9bd220 | 490 | |
nucleosam | 0:36024a9bd220 | 491 | /* Wait for all device ends moving */ |
nucleosam | 0:36024a9bd220 | 492 | WaitForAllDevicesNotBusy(); |
nucleosam | 0:36024a9bd220 | 493 | } |
nucleosam | 0:36024a9bd220 | 494 | |
nucleosam | 0:36024a9bd220 | 495 | /* Wait for 1s */ |
nucleosam | 0:36024a9bd220 | 496 | wait_ms(1000); |
nucleosam | 0:36024a9bd220 | 497 | |
nucleosam | 0:36024a9bd220 | 498 | /* Request motor 1 and motor 3 to go their mark position */ |
nucleosam | 0:36024a9bd220 | 499 | printf("--> Request motor1 and motor3 to go to their marked position.\r\n"); |
nucleosam | 0:36024a9bd220 | 500 | motor1->QueueCommands(POWERSTEP01_GO_MARK,0); |
nucleosam | 0:36024a9bd220 | 501 | motor2->QueueCommands(POWERSTEP01_NOP,0); |
nucleosam | 0:36024a9bd220 | 502 | motor3->QueueCommands(POWERSTEP01_GO_MARK,0); |
nucleosam | 0:36024a9bd220 | 503 | motor1->SendQueuedCommands(); |
nucleosam | 0:36024a9bd220 | 504 | |
nucleosam | 0:36024a9bd220 | 505 | /* Wait for motor 1 and 2 ends moving */ |
nucleosam | 0:36024a9bd220 | 506 | WaitForAllDevicesNotBusy(); |
nucleosam | 0:36024a9bd220 | 507 | |
nucleosam | 0:36024a9bd220 | 508 | /* Wait for 1s */ |
nucleosam | 0:36024a9bd220 | 509 | wait_ms(1000); |
nucleosam | 0:36024a9bd220 | 510 | |
nucleosam | 0:36024a9bd220 | 511 | /* Request motor 1 to run in StepperMotor::FWD direction at 400 steps/s*/ |
nucleosam | 0:36024a9bd220 | 512 | printf("--> Request motor1 to run at 400 steps/s in forward direction.\r\n"); |
nucleosam | 0:36024a9bd220 | 513 | motor1->Run(StepperMotor::FWD, 400); |
nucleosam | 0:36024a9bd220 | 514 | |
nucleosam | 0:36024a9bd220 | 515 | /* Wait for device to reach the targeted speed */ |
nucleosam | 0:36024a9bd220 | 516 | while((motor1->ReadStatusRegister() & POWERSTEP01_STATUS_MOT_STATUS)!= |
nucleosam | 0:36024a9bd220 | 517 | POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD) |
nucleosam | 0:36024a9bd220 | 518 | { |
nucleosam | 0:36024a9bd220 | 519 | /* Record the reached speed in step/s rounded to integer */ |
nucleosam | 0:36024a9bd220 | 520 | unsignedIntegerValue = motor1->GetSpeed(); |
nucleosam | 0:36024a9bd220 | 521 | /* Print reached speed to the console in step/s */ |
nucleosam | 0:36024a9bd220 | 522 | printf(" motor1 reached Speed: %d step/s.\r\n", unsignedIntegerValue); |
nucleosam | 0:36024a9bd220 | 523 | wait_ms(50); |
nucleosam | 0:36024a9bd220 | 524 | } |
nucleosam | 0:36024a9bd220 | 525 | |
nucleosam | 0:36024a9bd220 | 526 | /* Record the reached speed in step/s */ |
nucleosam | 0:36024a9bd220 | 527 | floatValue = motor1->GetAnalogValue(POWERSTEP01_SPEED); |
nucleosam | 0:36024a9bd220 | 528 | /* Print reached speed to the console in step/s */ |
nucleosam | 0:36024a9bd220 | 529 | printf(" motor1 reached Speed: %f step/s.\r\n", floatValue); |
nucleosam | 0:36024a9bd220 | 530 | |
nucleosam | 0:36024a9bd220 | 531 | /* Request motor 2 to run in StepperMotor::FWD direction at 300 steps/s*/ |
nucleosam | 0:36024a9bd220 | 532 | /* Request motor 3 to run in StepperMotor::FWD direction at 200 steps/s*/ |
nucleosam | 0:36024a9bd220 | 533 | /* and start at same time. */ |
nucleosam | 0:36024a9bd220 | 534 | printf("--> Request motor2 and motor3 to run respectively in forward direction\r\n"); |
nucleosam | 0:36024a9bd220 | 535 | printf(" at 300 steps/s and 200 steps/s and start at same time.\r\n"); |
nucleosam | 0:36024a9bd220 | 536 | motor1->QueueCommands(POWERSTEP01_NOP,0); |
nucleosam | 0:36024a9bd220 | 537 | motor2->QueueCommands((uint8_t)POWERSTEP01_RUN|(uint8_t)StepperMotor::FWD,POWERSTEP01::Speed_Steps_s_to_RegVal(300)); |
nucleosam | 0:36024a9bd220 | 538 | motor3->QueueCommands((uint8_t)POWERSTEP01_RUN|(uint8_t)StepperMotor::FWD,POWERSTEP01::Speed_Steps_s_to_RegVal(200)); |
nucleosam | 0:36024a9bd220 | 539 | motor1->SendQueuedCommands(); |
nucleosam | 0:36024a9bd220 | 540 | |
nucleosam | 0:36024a9bd220 | 541 | /* Wait for device to reach the targeted speed */ |
nucleosam | 0:36024a9bd220 | 542 | while(((motor2->ReadStatusRegister() & POWERSTEP01_STATUS_MOT_STATUS)!= |
nucleosam | 0:36024a9bd220 | 543 | POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)|| |
nucleosam | 0:36024a9bd220 | 544 | ((motor3->ReadStatusRegister() & POWERSTEP01_STATUS_MOT_STATUS)!= |
nucleosam | 0:36024a9bd220 | 545 | POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)); |
nucleosam | 0:36024a9bd220 | 546 | |
nucleosam | 0:36024a9bd220 | 547 | /* Record the reached speed in step/s */ |
nucleosam | 0:36024a9bd220 | 548 | floatValue = motor2->GetAnalogValue(POWERSTEP01_SPEED); |
nucleosam | 0:36024a9bd220 | 549 | /* Print reached speed to the console in step/s */ |
nucleosam | 0:36024a9bd220 | 550 | printf(" motor2 reached Speed: %f step/s.\r\n", floatValue); |
nucleosam | 0:36024a9bd220 | 551 | |
nucleosam | 0:36024a9bd220 | 552 | /* Record the reached speed in step/s */ |
nucleosam | 0:36024a9bd220 | 553 | floatValue = motor3->GetAnalogValue(POWERSTEP01_SPEED); |
nucleosam | 0:36024a9bd220 | 554 | /* Print reached speed to the console in step/s */ |
nucleosam | 0:36024a9bd220 | 555 | printf(" motor3 reached Speed: %f step/s.\r\n", floatValue); |
nucleosam | 0:36024a9bd220 | 556 | |
nucleosam | 0:36024a9bd220 | 557 | /* Wait for 3s */ |
nucleosam | 0:36024a9bd220 | 558 | wait_ms(3000); |
nucleosam | 0:36024a9bd220 | 559 | |
nucleosam | 0:36024a9bd220 | 560 | /* Request motor 2 to make a soft stop */ |
nucleosam | 0:36024a9bd220 | 561 | printf("--> Request motor2 to stop softly\r\n"); |
nucleosam | 0:36024a9bd220 | 562 | motor2->SoftStop(); |
nucleosam | 0:36024a9bd220 | 563 | |
nucleosam | 0:36024a9bd220 | 564 | /* Wait for motor 2 end moving */ |
nucleosam | 0:36024a9bd220 | 565 | motor2->WaitWhileActive(); |
nucleosam | 0:36024a9bd220 | 566 | |
nucleosam | 0:36024a9bd220 | 567 | /* Request motor 1 and 3 to make a hard stop */ |
nucleosam | 0:36024a9bd220 | 568 | printf("--> Request motor1 and motor3 to stop immediately\r\n"); |
nucleosam | 0:36024a9bd220 | 569 | motor1->QueueCommands(POWERSTEP01_HARD_STOP,0); |
nucleosam | 0:36024a9bd220 | 570 | motor2->QueueCommands(POWERSTEP01_NOP,0); |
nucleosam | 0:36024a9bd220 | 571 | motor3->QueueCommands(POWERSTEP01_HARD_STOP,0); |
nucleosam | 0:36024a9bd220 | 572 | motor1->SendQueuedCommands(); |
nucleosam | 0:36024a9bd220 | 573 | |
nucleosam | 0:36024a9bd220 | 574 | /* Wait for both motors end moving */ |
nucleosam | 0:36024a9bd220 | 575 | WaitForAllDevicesNotBusy(); |
nucleosam | 0:36024a9bd220 | 576 | |
nucleosam | 0:36024a9bd220 | 577 | /* Request all motors to go home and print to the console */ |
nucleosam | 0:36024a9bd220 | 578 | printf("--> Request all motors to go home.\r\n"); |
nucleosam | 0:36024a9bd220 | 579 | motor1->QueueCommands(POWERSTEP01_GO_HOME,0); |
nucleosam | 0:36024a9bd220 | 580 | motor2->QueueCommands(POWERSTEP01_GO_HOME,0); |
nucleosam | 0:36024a9bd220 | 581 | motor3->QueueCommands(POWERSTEP01_GO_HOME,0); |
nucleosam | 0:36024a9bd220 | 582 | motor1->SendQueuedCommands(); |
nucleosam | 0:36024a9bd220 | 583 | |
nucleosam | 0:36024a9bd220 | 584 | /* Wait for all device ends moving */ |
nucleosam | 0:36024a9bd220 | 585 | WaitForAllDevicesNotBusy(); |
nucleosam | 0:36024a9bd220 | 586 | |
nucleosam | 0:36024a9bd220 | 587 | /* Get acceleration, deceleration, Maxspeed and MinSpeed of motor 1*/ |
nucleosam | 0:36024a9bd220 | 588 | myMaxSpeed= motor1->GetParameter(POWERSTEP01_MAX_SPEED); |
nucleosam | 0:36024a9bd220 | 589 | myAcceleration = motor1->GetParameter(POWERSTEP01_ACC); |
nucleosam | 0:36024a9bd220 | 590 | myDeceleration = motor1->GetParameter(POWERSTEP01_DEC); |
nucleosam | 0:36024a9bd220 | 591 | myMinSpeed = motor1->GetParameter(POWERSTEP01_MIN_SPEED); |
nucleosam | 0:36024a9bd220 | 592 | |
nucleosam | 0:36024a9bd220 | 593 | /* Select 1/16 microstepping mode for motor 1 */ |
nucleosam | 0:36024a9bd220 | 594 | printf(" Set 1/16 microstepping mode for motor1.\r\n"); |
nucleosam | 0:36024a9bd220 | 595 | motor1->SetStepMode(STEP_MODE_1_16); |
nucleosam | 0:36024a9bd220 | 596 | |
nucleosam | 0:36024a9bd220 | 597 | /* Select 1/8 microstepping mode for motor 2 */ |
nucleosam | 0:36024a9bd220 | 598 | printf(" Set 1/8 microstepping mode for motor2.\r\n"); |
nucleosam | 0:36024a9bd220 | 599 | motor2->SetStepMode(STEP_MODE_1_8); |
nucleosam | 0:36024a9bd220 | 600 | |
nucleosam | 0:36024a9bd220 | 601 | /* Set speed and acceleration of motor 2 */ |
nucleosam | 0:36024a9bd220 | 602 | /* Do not scale with microstepping mode */ |
nucleosam | 0:36024a9bd220 | 603 | motor2->SetParameter(POWERSTEP01_ACC, myAcceleration); |
nucleosam | 0:36024a9bd220 | 604 | motor2->SetParameter(POWERSTEP01_DEC, myDeceleration); |
nucleosam | 0:36024a9bd220 | 605 | motor2->SetParameter(POWERSTEP01_MIN_SPEED, myMinSpeed); |
nucleosam | 0:36024a9bd220 | 606 | motor2->SetParameter(POWERSTEP01_MAX_SPEED, myMaxSpeed); |
nucleosam | 0:36024a9bd220 | 607 | |
nucleosam | 0:36024a9bd220 | 608 | /* Select ful step mode for motor 3 */ |
nucleosam | 0:36024a9bd220 | 609 | printf(" Set ful step mode for motor3.\r\n"); |
nucleosam | 0:36024a9bd220 | 610 | motor3->SetStepMode(STEP_MODE_FULL); |
nucleosam | 0:36024a9bd220 | 611 | |
nucleosam | 0:36024a9bd220 | 612 | /* Set speed and acceleration of motor 3 */ |
nucleosam | 0:36024a9bd220 | 613 | /* Do not scale with microstepping mode */ |
nucleosam | 0:36024a9bd220 | 614 | motor3->SetParameter(POWERSTEP01_ACC, myAcceleration); |
nucleosam | 0:36024a9bd220 | 615 | motor3->SetParameter(POWERSTEP01_DEC, myDeceleration); |
nucleosam | 0:36024a9bd220 | 616 | motor3->SetParameter(POWERSTEP01_MIN_SPEED, myMinSpeed); |
nucleosam | 0:36024a9bd220 | 617 | motor3->SetParameter(POWERSTEP01_MAX_SPEED, myMaxSpeed); |
nucleosam | 0:36024a9bd220 | 618 | |
nucleosam | 0:36024a9bd220 | 619 | /* Printing to the console. */ |
nucleosam | 0:36024a9bd220 | 620 | printf("--> Infinite Loop...\r\n"); |
nucleosam | 0:36024a9bd220 | 621 | /* Infinite loop */ |
nucleosam | 0:36024a9bd220 | 622 | while(1) |
nucleosam | 0:36024a9bd220 | 623 | { |
nucleosam | 0:36024a9bd220 | 624 | /* motor 1 is using 1/16 microstepping mode */ |
nucleosam | 0:36024a9bd220 | 625 | /* motor 2 is using 1/8 microstepping mode */ |
nucleosam | 0:36024a9bd220 | 626 | /* motor 3 is using full step mode */ |
nucleosam | 0:36024a9bd220 | 627 | /* position is in microsteps */ |
nucleosam | 0:36024a9bd220 | 628 | motor1->QueueCommands(POWERSTEP01_GO_TO,-3200); |
nucleosam | 0:36024a9bd220 | 629 | motor2->QueueCommands(POWERSTEP01_GO_TO,1600); |
nucleosam | 0:36024a9bd220 | 630 | motor3->QueueCommands(POWERSTEP01_GO_TO,-200); |
nucleosam | 0:36024a9bd220 | 631 | motor1->SendQueuedCommands(); |
nucleosam | 0:36024a9bd220 | 632 | |
nucleosam | 0:36024a9bd220 | 633 | /* Wait for all device ends moving */ |
nucleosam | 0:36024a9bd220 | 634 | WaitForAllDevicesNotBusy(); |
nucleosam | 0:36024a9bd220 | 635 | |
nucleosam | 0:36024a9bd220 | 636 | motor1->QueueCommands(POWERSTEP01_GO_TO,3200); |
nucleosam | 0:36024a9bd220 | 637 | motor2->QueueCommands(POWERSTEP01_GO_TO,-1600); |
nucleosam | 0:36024a9bd220 | 638 | motor3->QueueCommands(POWERSTEP01_GO_TO,200); |
nucleosam | 0:36024a9bd220 | 639 | motor1->SendQueuedCommands(); |
nucleosam | 0:36024a9bd220 | 640 | |
nucleosam | 0:36024a9bd220 | 641 | /* Wait for all device ends moving */ |
nucleosam | 0:36024a9bd220 | 642 | WaitForAllDevicesNotBusy(); |
nucleosam | 0:36024a9bd220 | 643 | } |
nucleosam | 0:36024a9bd220 | 644 | } |
nucleosam | 0:36024a9bd220 | 645 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ |