ST / Mbed OS HelloWorld_IHM01A1_2Motors_mbedOS

Simple test application for the STMicroelectronics X-NUCLEO-IHM01A1 Stepper Motor Control Expansion Board, built against mbed OS.

Dependencies:   X_NUCLEO_IHM01A1

Fork of HelloWorld_IHM01A1_2Motors by ST

Motor Control with the X-NUCLEO-IHM01A1 Expansion Board

This application provides a simple example of usage of the X-NUCLEO-IHM01A1 Stepper Motor Control Expansion Board.
It shows how to use two stepper motors connected to two stacked boards in daisy chain configuration, moving the rotors to specific positions, with given speed values, directions of rotation, etc.

main.cpp@24:8cb3c4ad055f, 2016-09-08 (annotated)

Committer:
Davidroid
Date:
Thu Sep 08 12:52:04 2016 +0000
Revision:
24:8cb3c4ad055f
Parent:
17:aae1446c67f4
Child:
28:3f17a4152bcf
Comment about pps added.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Davidroid 0:e6a49a092e2a 1 /**
Davidroid 0:e6a49a092e2a 2 ******************************************************************************
Davidroid 0:e6a49a092e2a 3 * @file main.cpp
Davidroid 17:aae1446c67f4 4 * @author Davide Aliprandi, STMicroelectronics
Davidroid 0:e6a49a092e2a 5 * @version V1.0.0
Davidroid 0:e6a49a092e2a 6 * @date October 14th, 2015
Davidroid 17:aae1446c67f4 7 * @brief mbed test application for the STMicroelectronics X-NUCLEO-IHM01A1
Davidroid 3:02d9ec4f88b2 8 * Motor Control Expansion Board: control of 2 motors.
Davidroid 0:e6a49a092e2a 9 ******************************************************************************
Davidroid 0:e6a49a092e2a 10 * @attention
Davidroid 0:e6a49a092e2a 11 *
Davidroid 0:e6a49a092e2a 12 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
Davidroid 0:e6a49a092e2a 13 *
Davidroid 0:e6a49a092e2a 14 * Redistribution and use in source and binary forms, with or without modification,
Davidroid 0:e6a49a092e2a 15 * are permitted provided that the following conditions are met:
Davidroid 0:e6a49a092e2a 16 * 1. Redistributions of source code must retain the above copyright notice,
Davidroid 0:e6a49a092e2a 17 * this list of conditions and the following disclaimer.
Davidroid 0:e6a49a092e2a 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
Davidroid 0:e6a49a092e2a 19 * this list of conditions and the following disclaimer in the documentation
Davidroid 0:e6a49a092e2a 20 * and/or other materials provided with the distribution.
Davidroid 0:e6a49a092e2a 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
Davidroid 0:e6a49a092e2a 22 * may be used to endorse or promote products derived from this software
Davidroid 0:e6a49a092e2a 23 * without specific prior written permission.
Davidroid 0:e6a49a092e2a 24 *
Davidroid 0:e6a49a092e2a 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Davidroid 0:e6a49a092e2a 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Davidroid 0:e6a49a092e2a 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Davidroid 0:e6a49a092e2a 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Davidroid 0:e6a49a092e2a 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Davidroid 0:e6a49a092e2a 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Davidroid 0:e6a49a092e2a 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Davidroid 0:e6a49a092e2a 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Davidroid 0:e6a49a092e2a 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Davidroid 0:e6a49a092e2a 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Davidroid 0:e6a49a092e2a 35 *
Davidroid 0:e6a49a092e2a 36 ******************************************************************************
Davidroid 0:e6a49a092e2a 37 */
Davidroid 0:e6a49a092e2a 38
Davidroid 0:e6a49a092e2a 39
Davidroid 0:e6a49a092e2a 40 /* Includes ------------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 41
Davidroid 0:e6a49a092e2a 42 /* mbed specific header files. */
Davidroid 0:e6a49a092e2a 43 #include "mbed.h"
Davidroid 0:e6a49a092e2a 44
Davidroid 0:e6a49a092e2a 45 /* Helper header files. */
Davidroid 0:e6a49a092e2a 46 #include "DevSPI.h"
Davidroid 0:e6a49a092e2a 47
Davidroid 0:e6a49a092e2a 48 /* Component specific header files. */
Davidroid 0:e6a49a092e2a 49 #include "l6474_class.h"
Davidroid 0:e6a49a092e2a 50
Davidroid 0:e6a49a092e2a 51
Davidroid 0:e6a49a092e2a 52 /* Definitions ---------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 53
Davidroid 24:8cb3c4ad055f 54 /* Number of steps. */
Davidroid 9:a9e51320aee4 55 #define STEPS 3200
Davidroid 0:e6a49a092e2a 56
Davidroid 24:8cb3c4ad055f 57 /* Delay in milliseconds. */
Davidroid 24:8cb3c4ad055f 58 #define DELAY_1 2000
Davidroid 24:8cb3c4ad055f 59 #define DELAY_2 6000
Davidroid 24:8cb3c4ad055f 60 #define DELAY_3 8000
Davidroid 24:8cb3c4ad055f 61
Davidroid 24:8cb3c4ad055f 62 /* Speed in pps (Pulses Per Second).
Davidroid 24:8cb3c4ad055f 63 In Full Step mode: 1 pps = 1 step/s).
Davidroid 24:8cb3c4ad055f 64 In 1/N Step Mode: N pps = 1 step/s). */
Davidroid 24:8cb3c4ad055f 65 #define SPEED_1 2400
Davidroid 24:8cb3c4ad055f 66 #define SPEED_2 1200
Davidroid 24:8cb3c4ad055f 67
Davidroid 0:e6a49a092e2a 68
Davidroid 0:e6a49a092e2a 69 /* Variables -----------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 70
Davidroid 0:e6a49a092e2a 71 /* Motor Control Component. */
Davidroid 3:02d9ec4f88b2 72 L6474 *motor1;
Davidroid 3:02d9ec4f88b2 73 L6474 *motor2;
Davidroid 0:e6a49a092e2a 74
Davidroid 0:e6a49a092e2a 75
Davidroid 0:e6a49a092e2a 76 /* Main ----------------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 77
Davidroid 0:e6a49a092e2a 78 int main()
Davidroid 0:e6a49a092e2a 79 {
Davidroid 8:cec4c2c03a27 80 /*----- Initialization. -----*/
Davidroid 8:cec4c2c03a27 81
Davidroid 0:e6a49a092e2a 82 /* Initializing SPI bus. */
Davidroid 0:e6a49a092e2a 83 DevSPI dev_spi(D11, D12, D13);
Davidroid 0:e6a49a092e2a 84
Davidroid 9:a9e51320aee4 85 /* Initializing Motor Control Components. */
Davidroid 5:a0268a435bb1 86 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
Davidroid 16:810667a9f31f 87 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
Davidroid 14:fcd452b03d1a 88 if (motor1->Init() != COMPONENT_OK)
Davidroid 14:fcd452b03d1a 89 exit(EXIT_FAILURE);
Davidroid 14:fcd452b03d1a 90 if (motor2->Init() != COMPONENT_OK)
Davidroid 14:fcd452b03d1a 91 exit(EXIT_FAILURE);
Davidroid 0:e6a49a092e2a 92
Davidroid 0:e6a49a092e2a 93 /* Printing to the console. */
Davidroid 3:02d9ec4f88b2 94 printf("Motor Control Application Example for 2 Motors\r\n\n");
Davidroid 0:e6a49a092e2a 95
Davidroid 8:cec4c2c03a27 96
Davidroid 8:cec4c2c03a27 97 /*----- Moving. -----*/
Davidroid 8:cec4c2c03a27 98
Davidroid 8:cec4c2c03a27 99 /* Printing to the console. */
Davidroid 9:a9e51320aee4 100 printf("--> Moving forward: M1 %d steps, M2 %d steps.\r\n", STEPS >> 1, STEPS);
Davidroid 8:cec4c2c03a27 101
Davidroid 8:cec4c2c03a27 102 /* Moving N steps in the forward direction. */
Davidroid 9:a9e51320aee4 103 motor1->Move(StepperMotor::FWD, STEPS >> 1);
Davidroid 9:a9e51320aee4 104 motor2->Move(StepperMotor::FWD, STEPS);
Davidroid 8:cec4c2c03a27 105
Davidroid 8:cec4c2c03a27 106 /* Waiting while the motor is active. */
Davidroid 8:cec4c2c03a27 107 motor1->WaitWhileActive();
Davidroid 8:cec4c2c03a27 108 motor2->WaitWhileActive();
Davidroid 8:cec4c2c03a27 109
Davidroid 8:cec4c2c03a27 110 /* Getting current position. */
Davidroid 8:cec4c2c03a27 111 int position1 = motor1->GetPosition();
Davidroid 8:cec4c2c03a27 112 int position2 = motor2->GetPosition();
Davidroid 8:cec4c2c03a27 113
Davidroid 8:cec4c2c03a27 114 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 115 printf(" Position: M1 %d, M2 %d.\r\n", position1, position2);
Davidroid 8:cec4c2c03a27 116
Davidroid 8:cec4c2c03a27 117 /* Waiting 2 seconds. */
Davidroid 24:8cb3c4ad055f 118 wait_ms(DELAY_1);
Davidroid 8:cec4c2c03a27 119
Davidroid 8:cec4c2c03a27 120
Davidroid 8:cec4c2c03a27 121 /*----- Moving. -----*/
Davidroid 8:cec4c2c03a27 122
Davidroid 8:cec4c2c03a27 123 /* Printing to the console. */
Davidroid 9:a9e51320aee4 124 printf("--> Moving backward: M1 %d steps, M2 %d steps.\r\n", STEPS >> 1, STEPS);
Davidroid 8:cec4c2c03a27 125
Davidroid 8:cec4c2c03a27 126
Davidroid 8:cec4c2c03a27 127 /* Moving N steps in the backward direction. */
Davidroid 9:a9e51320aee4 128 motor1->Move(StepperMotor::BWD, STEPS >> 1);
Davidroid 9:a9e51320aee4 129 motor2->Move(StepperMotor::BWD, STEPS);
Davidroid 8:cec4c2c03a27 130
Davidroid 8:cec4c2c03a27 131 /* Waiting while the motor is active. */
Davidroid 8:cec4c2c03a27 132 motor1->WaitWhileActive();
Davidroid 8:cec4c2c03a27 133 motor2->WaitWhileActive();
Davidroid 8:cec4c2c03a27 134
Davidroid 8:cec4c2c03a27 135 /* Getting current position. */
Davidroid 8:cec4c2c03a27 136 position1 = motor1->GetPosition();
Davidroid 8:cec4c2c03a27 137 position2 = motor2->GetPosition();
Davidroid 8:cec4c2c03a27 138
Davidroid 8:cec4c2c03a27 139 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 140 printf(" Position: M1 %d, M2 %d.\r\n", position1, position2);
Davidroid 8:cec4c2c03a27 141 printf("--> Setting Home.\r\n");
Davidroid 8:cec4c2c03a27 142
Davidroid 8:cec4c2c03a27 143 /* Setting the current position to be the home position. */
Davidroid 8:cec4c2c03a27 144 motor1->SetHome();
Davidroid 8:cec4c2c03a27 145 motor2->SetHome();
Davidroid 8:cec4c2c03a27 146
Davidroid 8:cec4c2c03a27 147 /* Waiting 2 seconds. */
Davidroid 24:8cb3c4ad055f 148 wait_ms(DELAY_1);
Davidroid 8:cec4c2c03a27 149
Davidroid 8:cec4c2c03a27 150
Davidroid 8:cec4c2c03a27 151 /*----- Going to a specified position. -----*/
Davidroid 8:cec4c2c03a27 152
Davidroid 8:cec4c2c03a27 153 /* Printing to the console. */
Davidroid 9:a9e51320aee4 154 printf("--> Going to position: M1 %d, M2 %d.\r\n", STEPS, STEPS >> 1);
Davidroid 8:cec4c2c03a27 155
Davidroid 8:cec4c2c03a27 156 /* Requesting to go to a specified position. */
Davidroid 9:a9e51320aee4 157 motor1->GoTo(STEPS);
Davidroid 9:a9e51320aee4 158 motor2->GoTo(STEPS >> 1);
Davidroid 8:cec4c2c03a27 159
Davidroid 8:cec4c2c03a27 160 /* Waiting while the motor is active. */
Davidroid 8:cec4c2c03a27 161 motor1->WaitWhileActive();
Davidroid 8:cec4c2c03a27 162 motor2->WaitWhileActive();
Davidroid 8:cec4c2c03a27 163
Davidroid 8:cec4c2c03a27 164 /* Getting current position. */
Davidroid 8:cec4c2c03a27 165 position1 = motor1->GetPosition();
Davidroid 8:cec4c2c03a27 166 position2 = motor2->GetPosition();
Davidroid 8:cec4c2c03a27 167
Davidroid 8:cec4c2c03a27 168 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 169 printf(" Position: M1 %d, M2 %d.\r\n", position1, position2);
Davidroid 8:cec4c2c03a27 170
Davidroid 8:cec4c2c03a27 171 /* Waiting 2 seconds. */
Davidroid 24:8cb3c4ad055f 172 wait_ms(DELAY_1);
Davidroid 8:cec4c2c03a27 173
Davidroid 8:cec4c2c03a27 174
Davidroid 8:cec4c2c03a27 175 /*----- Going Home. -----*/
Davidroid 8:cec4c2c03a27 176
Davidroid 8:cec4c2c03a27 177 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 178 printf("--> Going Home.\r\n");
Davidroid 8:cec4c2c03a27 179
Davidroid 8:cec4c2c03a27 180 /* Requesting to go to home. */
Davidroid 8:cec4c2c03a27 181 motor1->GoHome();
Davidroid 8:cec4c2c03a27 182 motor2->GoHome();
Davidroid 8:cec4c2c03a27 183
Davidroid 8:cec4c2c03a27 184 /* Waiting while the motor is active. */
Davidroid 8:cec4c2c03a27 185 motor1->WaitWhileActive();
Davidroid 8:cec4c2c03a27 186 motor2->WaitWhileActive();
Davidroid 8:cec4c2c03a27 187
Davidroid 8:cec4c2c03a27 188 /* Getting current position. */
Davidroid 8:cec4c2c03a27 189 position1 = motor1->GetPosition();
Davidroid 8:cec4c2c03a27 190 position2 = motor2->GetPosition();
Davidroid 0:e6a49a092e2a 191
Davidroid 8:cec4c2c03a27 192 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 193 printf(" Position: M1 %d, M2 %d.\r\n", position1, position2);
Davidroid 8:cec4c2c03a27 194
Davidroid 8:cec4c2c03a27 195 /* Waiting 2 seconds. */
Davidroid 24:8cb3c4ad055f 196 wait_ms(DELAY_1);
Davidroid 8:cec4c2c03a27 197
Davidroid 8:cec4c2c03a27 198
Davidroid 8:cec4c2c03a27 199 /*----- Running. -----*/
Davidroid 8:cec4c2c03a27 200
Davidroid 8:cec4c2c03a27 201 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 202 printf("--> M1 running backward, M2 running forward.\r\n");
Davidroid 8:cec4c2c03a27 203
Davidroid 8:cec4c2c03a27 204 /* Requesting to run backward. */
Davidroid 8:cec4c2c03a27 205 motor1->Run(StepperMotor::BWD);
Davidroid 8:cec4c2c03a27 206 motor2->Run(StepperMotor::FWD);
Davidroid 8:cec4c2c03a27 207
Davidroid 8:cec4c2c03a27 208 /* Waiting until delay has expired. */
Davidroid 24:8cb3c4ad055f 209 wait_ms(DELAY_2);
Davidroid 8:cec4c2c03a27 210
Davidroid 8:cec4c2c03a27 211 /* Getting current speed. */
Davidroid 8:cec4c2c03a27 212 int speed1 = motor1->GetSpeed();
Davidroid 8:cec4c2c03a27 213 int speed2 = motor2->GetSpeed();
Davidroid 8:cec4c2c03a27 214
Davidroid 8:cec4c2c03a27 215 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 216 printf(" Speed: M1 %d, M2 %d.\r\n", speed1, speed2);
Davidroid 8:cec4c2c03a27 217
Davidroid 8:cec4c2c03a27 218 /*----- Increasing the speed while running. -----*/
Davidroid 8:cec4c2c03a27 219
Davidroid 8:cec4c2c03a27 220 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 221 printf("--> Increasing the speed while running.\r\n");
Davidroid 8:cec4c2c03a27 222
Davidroid 24:8cb3c4ad055f 223 /* Increasing the speed. */
Davidroid 24:8cb3c4ad055f 224 motor1->SetMaxSpeed(SPEED_1);
Davidroid 24:8cb3c4ad055f 225 motor2->SetMaxSpeed(SPEED_1);
Davidroid 8:cec4c2c03a27 226
Davidroid 8:cec4c2c03a27 227 /* Waiting until delay has expired. */
Davidroid 24:8cb3c4ad055f 228 wait_ms(DELAY_2);
Davidroid 8:cec4c2c03a27 229
Davidroid 8:cec4c2c03a27 230 /* Getting current speed. */
Davidroid 8:cec4c2c03a27 231 speed1 = motor1->GetSpeed();
Davidroid 8:cec4c2c03a27 232 speed2 = motor2->GetSpeed();
Davidroid 8:cec4c2c03a27 233
Davidroid 8:cec4c2c03a27 234 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 235 printf(" Speed: M1 %d, M2 %d.\r\n", speed1, speed2);
Davidroid 8:cec4c2c03a27 236
Davidroid 8:cec4c2c03a27 237
Davidroid 8:cec4c2c03a27 238 /*----- Decreasing the speed while running. -----*/
Davidroid 0:e6a49a092e2a 239
Davidroid 8:cec4c2c03a27 240 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 241 printf("--> Decreasing the speed while running.\r\n");
Davidroid 8:cec4c2c03a27 242
Davidroid 24:8cb3c4ad055f 243 /* Decreasing the speed. */
Davidroid 24:8cb3c4ad055f 244 motor1->SetMaxSpeed(SPEED_2);
Davidroid 24:8cb3c4ad055f 245 motor2->SetMaxSpeed(SPEED_2);
Davidroid 8:cec4c2c03a27 246
Davidroid 8:cec4c2c03a27 247 /* Waiting until delay has expired. */
Davidroid 24:8cb3c4ad055f 248 wait_ms(DELAY_3);
Davidroid 8:cec4c2c03a27 249
Davidroid 8:cec4c2c03a27 250 /* Getting current speed. */
Davidroid 8:cec4c2c03a27 251 speed1 = motor1->GetSpeed();
Davidroid 8:cec4c2c03a27 252 speed2 = motor2->GetSpeed();
Davidroid 8:cec4c2c03a27 253
Davidroid 8:cec4c2c03a27 254 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 255 printf(" Speed: M1 %d, M2 %d.\r\n", speed1, speed2);
Davidroid 8:cec4c2c03a27 256
Davidroid 8:cec4c2c03a27 257
Davidroid 8:cec4c2c03a27 258 /*----- Requiring hard-stop while running. -----*/
Davidroid 8:cec4c2c03a27 259
Davidroid 8:cec4c2c03a27 260 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 261 printf("--> Requiring hard-stop while running.\r\n");
Davidroid 8:cec4c2c03a27 262
Davidroid 8:cec4c2c03a27 263 /* Requesting to immediatly stop. */
Davidroid 8:cec4c2c03a27 264 motor1->HardStop();
Davidroid 8:cec4c2c03a27 265 motor2->HardStop();
Davidroid 8:cec4c2c03a27 266
Davidroid 8:cec4c2c03a27 267 /* Waiting while the motor is active. */
Davidroid 8:cec4c2c03a27 268 motor1->WaitWhileActive();
Davidroid 8:cec4c2c03a27 269 motor2->WaitWhileActive();
Davidroid 8:cec4c2c03a27 270
Davidroid 8:cec4c2c03a27 271 /* Waiting 2 seconds. */
Davidroid 24:8cb3c4ad055f 272 wait_ms(DELAY_1);
Davidroid 8:cec4c2c03a27 273
Davidroid 8:cec4c2c03a27 274
Davidroid 8:cec4c2c03a27 275 /*----- Infinite Loop. -----*/
Davidroid 8:cec4c2c03a27 276
Davidroid 8:cec4c2c03a27 277 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 278 printf("--> Infinite Loop...\r\n");
Davidroid 8:cec4c2c03a27 279
Davidroid 8:cec4c2c03a27 280 /* Setting the current position to be the home position. */
Davidroid 8:cec4c2c03a27 281 motor1->SetHome();
Davidroid 8:cec4c2c03a27 282 motor2->SetHome();
Davidroid 8:cec4c2c03a27 283
Davidroid 8:cec4c2c03a27 284 /* Infinite Loop. */
Davidroid 8:cec4c2c03a27 285 while(1)
Davidroid 8:cec4c2c03a27 286 {
Davidroid 8:cec4c2c03a27 287 /* Requesting to go to a specified position. */
Davidroid 9:a9e51320aee4 288 motor1->GoTo(STEPS >> 1);
Davidroid 9:a9e51320aee4 289 motor2->GoTo(- (STEPS >> 1));
Davidroid 3:02d9ec4f88b2 290
Davidroid 0:e6a49a092e2a 291 /* Waiting while the motor is active. */
Davidroid 3:02d9ec4f88b2 292 motor1->WaitWhileActive();
Davidroid 3:02d9ec4f88b2 293 motor2->WaitWhileActive();
Davidroid 0:e6a49a092e2a 294
Davidroid 0:e6a49a092e2a 295 /* Requesting to go to a specified position. */
Davidroid 9:a9e51320aee4 296 motor1->GoTo(- (STEPS >> 1));
Davidroid 9:a9e51320aee4 297 motor2->GoTo(STEPS >> 1);
Davidroid 0:e6a49a092e2a 298
Davidroid 0:e6a49a092e2a 299 /* Waiting while the motor is active. */
Davidroid 3:02d9ec4f88b2 300 motor1->WaitWhileActive();
Davidroid 3:02d9ec4f88b2 301 motor2->WaitWhileActive();
Davidroid 0:e6a49a092e2a 302 }
Davidroid 0:e6a49a092e2a 303 }