ST / Mbed 2 deprecated IHM03A1_ExampleFor3Motors

Motion control example for 3 motors.

Dependencies:   X_NUCLEO_IHM03A1 mbed

Fork of IHM03A1_ExampleFor3Motors by ST Expansion SW Team

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?

UserRevisionLine numberNew 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>&copy; 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****/