NEHSROBOT / Mbed 2 deprecated MOTOR_BLE_V2

Smartphone app control stepper motors via bluetooth low energy(BLE) on stm32F401RE x IHM02A1 x IDB05A1 V2 .

Dependencies:   BLE_API X_NUCLEO_IDB0XA1 X_NUCLEO_IHM02A1 mbed

Fork of BLE_LED_IDB0XA1_demo by NEHSROBOT

main.cpp@7:126b141a8c86, 2018-02-01 (annotated)

Committer:
barry210110
Date:
Thu Feb 01 07:55:42 2018 +0000
Revision:
7:126b141a8c86
Parent:
3:e0efdb741bd4
Child:
8:ef9b37e2464f
Child:
9:66938f196868
bluetooth control motor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
apalmieri 0:aae2d6c2a9eb 1 /* mbed Microcontroller Library
apalmieri 0:aae2d6c2a9eb 2 * Copyright (c) 2006-2013 ARM Limited
apalmieri 0:aae2d6c2a9eb 3 *
apalmieri 0:aae2d6c2a9eb 4 * Licensed under the Apache License, Version 2.0 (the "License");
apalmieri 0:aae2d6c2a9eb 5 * you may not use this file except in compliance with the License.
apalmieri 0:aae2d6c2a9eb 6 * You may obtain a copy of the License at
apalmieri 0:aae2d6c2a9eb 7 *
apalmieri 0:aae2d6c2a9eb 8 * http://www.apache.org/licenses/LICENSE-2.0
apalmieri 0:aae2d6c2a9eb 9 *
apalmieri 0:aae2d6c2a9eb 10 * Unless required by applicable law or agreed to in writing, software
apalmieri 0:aae2d6c2a9eb 11 * distributed under the License is distributed on an "AS IS" BASIS,
apalmieri 0:aae2d6c2a9eb 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
apalmieri 0:aae2d6c2a9eb 13 * See the License for the specific language governing permissions and
apalmieri 0:aae2d6c2a9eb 14 * limitations under the License.
apalmieri 0:aae2d6c2a9eb 15 */
barry210110 7:126b141a8c86 16
barry210110 7:126b141a8c86 17 /* Includes ------------------------------------------------------------------*/
barry210110 7:126b141a8c86 18
barry210110 7:126b141a8c86 19
barry210110 7:126b141a8c86 20
barry210110 7:126b141a8c86 21 /* Helper header files. */
barry210110 7:126b141a8c86 22 #include "DevSPI.h"
barry210110 7:126b141a8c86 23 void call();
barry210110 7:126b141a8c86 24 /* Expansion Board specific header files. */
barry210110 7:126b141a8c86 25 #include "XNucleoIHM02A1.h"
apalmieri 0:aae2d6c2a9eb 26
apalmieri 0:aae2d6c2a9eb 27 #include "mbed.h"
apalmieri 0:aae2d6c2a9eb 28 #include "ble/BLE.h"
apalmieri 0:aae2d6c2a9eb 29 #include "LEDService.h"
barry210110 7:126b141a8c86 30 #include "BlueNRGDevice.h"
barry210110 7:126b141a8c86 31 int temp,add ;
barry210110 7:126b141a8c86 32 /* Definitions ---------------------------------------------------------------*/
apalmieri 0:aae2d6c2a9eb 33
barry210110 7:126b141a8c86 34 /* Number of movements per revolution. */
barry210110 7:126b141a8c86 35 #define MPR_1 4
barry210110 7:126b141a8c86 36
barry210110 7:126b141a8c86 37 /* Number of steps. */
barry210110 7:126b141a8c86 38 #define STEPS_1 (400 * 128) /* 1 revolution given a 400 steps motor configured at 1/128 microstep mode. */
barry210110 7:126b141a8c86 39 #define STEPS_2 (STEPS_1 * 2)
barry210110 7:126b141a8c86 40
barry210110 7:126b141a8c86 41 /* Delay in milliseconds. */
barry210110 7:126b141a8c86 42 #define DELAY_1 1000
barry210110 7:126b141a8c86 43 #define DELAY_2 2000
barry210110 7:126b141a8c86 44 #define DELAY_3 5000
barry210110 7:126b141a8c86 45
barry210110 7:126b141a8c86 46
barry210110 7:126b141a8c86 47 /* Variables -----------------------------------------------------------------*/
barry210110 7:126b141a8c86 48
barry210110 7:126b141a8c86 49 /* Motor Control Expansion Board. */
barry210110 7:126b141a8c86 50 XNucleoIHM02A1 *x_nucleo_ihm02a1;
apalmieri 0:aae2d6c2a9eb 51
barry210110 7:126b141a8c86 52 /* Initialization parameters of the motors connected to the expansion board. */
barry210110 7:126b141a8c86 53 L6470_init_t init[L6470DAISYCHAINSIZE] = {
barry210110 7:126b141a8c86 54 /* First Motor. */
barry210110 7:126b141a8c86 55 {
barry210110 7:126b141a8c86 56 9.0, /* Motor supply voltage in V. */
barry210110 7:126b141a8c86 57 400, /* Min number of steps per revolution for the motor. */
barry210110 7:126b141a8c86 58 1.7, /* Max motor phase voltage in A. */
barry210110 7:126b141a8c86 59 3.06, /* Max motor phase voltage in V. */
barry210110 7:126b141a8c86 60 300.0, /* Motor initial speed [step/s]. */
barry210110 7:126b141a8c86 61 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
barry210110 7:126b141a8c86 62 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
barry210110 7:126b141a8c86 63 992.0, /* Motor maximum speed [step/s]. */
barry210110 7:126b141a8c86 64 0.0, /* Motor minimum speed [step/s]. */
barry210110 7:126b141a8c86 65 602.7, /* Motor full-step speed threshold [step/s]. */
barry210110 7:126b141a8c86 66 3.06, /* Holding kval [V]. */
barry210110 7:126b141a8c86 67 3.06, /* Constant speed kval [V]. */
barry210110 7:126b141a8c86 68 3.06, /* Acceleration starting kval [V]. */
barry210110 7:126b141a8c86 69 3.06, /* Deceleration starting kval [V]. */
barry210110 7:126b141a8c86 70 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
barry210110 7:126b141a8c86 71 392.1569e-6, /* Start slope [s/step]. */
barry210110 7:126b141a8c86 72 643.1372e-6, /* Acceleration final slope [s/step]. */
barry210110 7:126b141a8c86 73 643.1372e-6, /* Deceleration final slope [s/step]. */
barry210110 7:126b141a8c86 74 0, /* Thermal compensation factor (range [0, 15]). */
barry210110 7:126b141a8c86 75 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
barry210110 7:126b141a8c86 76 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
barry210110 7:126b141a8c86 77 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
barry210110 7:126b141a8c86 78 0xFF, /* Alarm conditions enable. */
barry210110 7:126b141a8c86 79 0x2E88 /* Ic configuration. */
barry210110 7:126b141a8c86 80 },
barry210110 7:126b141a8c86 81
barry210110 7:126b141a8c86 82 /* Second Motor. */
barry210110 7:126b141a8c86 83 {
barry210110 7:126b141a8c86 84 9.0, /* Motor supply voltage in V. */
barry210110 7:126b141a8c86 85 400, /* Min number of steps per revolution for the motor. */
barry210110 7:126b141a8c86 86 1.7, /* Max motor phase voltage in A. */
barry210110 7:126b141a8c86 87 3.06, /* Max motor phase voltage in V. */
barry210110 7:126b141a8c86 88 300.0, /* Motor initial speed [step/s]. */
barry210110 7:126b141a8c86 89 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
barry210110 7:126b141a8c86 90 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
barry210110 7:126b141a8c86 91 992.0, /* Motor maximum speed [step/s]. */
barry210110 7:126b141a8c86 92 0.0, /* Motor minimum speed [step/s]. */
barry210110 7:126b141a8c86 93 602.7, /* Motor full-step speed threshold [step/s]. */
barry210110 7:126b141a8c86 94 3.06, /* Holding kval [V]. */
barry210110 7:126b141a8c86 95 3.06, /* Constant speed kval [V]. */
barry210110 7:126b141a8c86 96 3.06, /* Acceleration starting kval [V]. */
barry210110 7:126b141a8c86 97 3.06, /* Deceleration starting kval [V]. */
barry210110 7:126b141a8c86 98 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
barry210110 7:126b141a8c86 99 392.1569e-6, /* Start slope [s/step]. */
barry210110 7:126b141a8c86 100 643.1372e-6, /* Acceleration final slope [s/step]. */
barry210110 7:126b141a8c86 101 643.1372e-6, /* Deceleration final slope [s/step]. */
barry210110 7:126b141a8c86 102 0, /* Thermal compensation factor (range [0, 15]). */
barry210110 7:126b141a8c86 103 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
barry210110 7:126b141a8c86 104 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
barry210110 7:126b141a8c86 105 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
barry210110 7:126b141a8c86 106 0xFF, /* Alarm conditions enable. */
barry210110 7:126b141a8c86 107 0x2E88 /* Ic configuration. */
barry210110 7:126b141a8c86 108 }
barry210110 7:126b141a8c86 109 };
barry210110 7:126b141a8c86 110
barry210110 7:126b141a8c86 111 DigitalOut actuatedLED(LED2);
barry210110 7:126b141a8c86 112 const static char DEVICE_NAME[] = "mydevice"; // CHANGE NAME
barry210110 7:126b141a8c86 113 static const uint16_t uuid16_list[] = {LEDService::LED_SERVICE_UUID}; // GATT ATTRIBUTE UUID
apalmieri 0:aae2d6c2a9eb 114
apalmieri 0:aae2d6c2a9eb 115 LEDService *ledServicePtr;
apalmieri 0:aae2d6c2a9eb 116
apalmieri 0:aae2d6c2a9eb 117 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
apalmieri 0:aae2d6c2a9eb 118 {
apalmieri 0:aae2d6c2a9eb 119 (void)params;
apalmieri 3:e0efdb741bd4 120 BLE::Instance().gap().startAdvertising(); // restart advertising
apalmieri 0:aae2d6c2a9eb 121 }
apalmieri 0:aae2d6c2a9eb 122
apalmieri 0:aae2d6c2a9eb 123 /**
apalmieri 0:aae2d6c2a9eb 124 * This callback allows the LEDService to receive updates to the ledState Characteristic.
apalmieri 0:aae2d6c2a9eb 125 *
apalmieri 0:aae2d6c2a9eb 126 * @param[in] params
apalmieri 0:aae2d6c2a9eb 127 * Information about the characterisitc being updated.
apalmieri 0:aae2d6c2a9eb 128 */
apalmieri 0:aae2d6c2a9eb 129 void onDataWrittenCallback(const GattWriteCallbackParams *params) {
apalmieri 0:aae2d6c2a9eb 130 if ((params->handle == ledServicePtr->getValueHandle()) && (params->len == 1)) {
barry210110 7:126b141a8c86 131
barry210110 7:126b141a8c86 132 /* switch( *(params->data)){
barry210110 7:126b141a8c86 133 case 0:
barry210110 7:126b141a8c86 134 actuatedLED=0 ;
barry210110 7:126b141a8c86 135 wait(1) ;
barry210110 7:126b141a8c86 136 actuatedLED=1 ;
barry210110 7:126b141a8c86 137 wait(1) ;
barry210110 7:126b141a8c86 138 actuatedLED=0 ;
barry210110 7:126b141a8c86 139 break ;
barry210110 7:126b141a8c86 140 case 1:
barry210110 7:126b141a8c86 141 actuatedLED=1 ;
barry210110 7:126b141a8c86 142 wait(1) ;
barry210110 7:126b141a8c86 143 actuatedLED=0 ;
barry210110 7:126b141a8c86 144 wait(1) ;
barry210110 7:126b141a8c86 145 actuatedLED=1 ;
barry210110 7:126b141a8c86 146 break ;
barry210110 7:126b141a8c86 147 } */
barry210110 7:126b141a8c86 148 if ( *(params->data)== 0x00 )
barry210110 7:126b141a8c86 149 {
barry210110 7:126b141a8c86 150 actuatedLED=1 ;
barry210110 7:126b141a8c86 151 call();
barry210110 7:126b141a8c86 152
barry210110 7:126b141a8c86 153 }
barry210110 7:126b141a8c86 154 else if (*(params->data)== 0x01)
barry210110 7:126b141a8c86 155 {
barry210110 7:126b141a8c86 156 actuatedLED=0 ;
barry210110 7:126b141a8c86 157 wait(0.5) ;
barry210110 7:126b141a8c86 158 actuatedLED=1 ;
barry210110 7:126b141a8c86 159 wait(0.5) ;
barry210110 7:126b141a8c86 160 actuatedLED=0 ;
barry210110 7:126b141a8c86 161 wait(0.5) ;
barry210110 7:126b141a8c86 162 actuatedLED=1 ;
barry210110 7:126b141a8c86 163 wait(0.5) ;
barry210110 7:126b141a8c86 164 actuatedLED=0 ;
barry210110 7:126b141a8c86 165
barry210110 7:126b141a8c86 166 }
barry210110 7:126b141a8c86 167 else if (*(params->data)== 0x02) {
barry210110 7:126b141a8c86 168 actuatedLED=1 ;
barry210110 7:126b141a8c86 169 wait(0.5) ;
barry210110 7:126b141a8c86 170 actuatedLED=0 ;
barry210110 7:126b141a8c86 171 wait(0.5) ;
barry210110 7:126b141a8c86 172 actuatedLED=1 ;
barry210110 7:126b141a8c86 173 }
barry210110 7:126b141a8c86 174 else if (*(params->data)== 0x03) {
barry210110 7:126b141a8c86 175 actuatedLED=1 ;
barry210110 7:126b141a8c86 176 wait(0.5) ;
barry210110 7:126b141a8c86 177 actuatedLED=0 ;
barry210110 7:126b141a8c86 178 wait(0.5) ;
barry210110 7:126b141a8c86 179 actuatedLED=1 ;
barry210110 7:126b141a8c86 180 wait(0.5) ;
barry210110 7:126b141a8c86 181 actuatedLED=0 ;
barry210110 7:126b141a8c86 182 wait(0.5) ;
barry210110 7:126b141a8c86 183 actuatedLED=1 ;
barry210110 7:126b141a8c86 184 }
barry210110 7:126b141a8c86 185
apalmieri 0:aae2d6c2a9eb 186 }
barry210110 7:126b141a8c86 187
apalmieri 0:aae2d6c2a9eb 188 }
apalmieri 0:aae2d6c2a9eb 189
apalmieri 0:aae2d6c2a9eb 190 /**
apalmieri 0:aae2d6c2a9eb 191 * This function is called when the ble initialization process has failled
apalmieri 0:aae2d6c2a9eb 192 */
barry210110 7:126b141a8c86 193 int onBleInitError(BLE &ble, ble_error_t error)
apalmieri 0:aae2d6c2a9eb 194 {
apalmieri 0:aae2d6c2a9eb 195 /* Initialization error handling should go here */
apalmieri 0:aae2d6c2a9eb 196 }
apalmieri 0:aae2d6c2a9eb 197
apalmieri 0:aae2d6c2a9eb 198 /**
apalmieri 0:aae2d6c2a9eb 199 * Callback triggered when the ble initialization process has finished
apalmieri 0:aae2d6c2a9eb 200 */
apalmieri 0:aae2d6c2a9eb 201 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
apalmieri 0:aae2d6c2a9eb 202 {
apalmieri 0:aae2d6c2a9eb 203 BLE& ble = params->ble;
apalmieri 0:aae2d6c2a9eb 204 ble_error_t error = params->error;
apalmieri 0:aae2d6c2a9eb 205
apalmieri 0:aae2d6c2a9eb 206 if (error != BLE_ERROR_NONE) {
apalmieri 0:aae2d6c2a9eb 207 /* In case of error, forward the error handling to onBleInitError */
apalmieri 0:aae2d6c2a9eb 208 onBleInitError(ble, error);
apalmieri 0:aae2d6c2a9eb 209 return;
apalmieri 0:aae2d6c2a9eb 210 }
apalmieri 0:aae2d6c2a9eb 211
apalmieri 0:aae2d6c2a9eb 212 /* Ensure that it is the default instance of BLE */
apalmieri 0:aae2d6c2a9eb 213 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
apalmieri 0:aae2d6c2a9eb 214 return;
apalmieri 0:aae2d6c2a9eb 215 }
apalmieri 0:aae2d6c2a9eb 216
apalmieri 0:aae2d6c2a9eb 217 ble.gap().onDisconnection(disconnectionCallback);
apalmieri 0:aae2d6c2a9eb 218 ble.gattServer().onDataWritten(onDataWrittenCallback);
apalmieri 0:aae2d6c2a9eb 219
apalmieri 0:aae2d6c2a9eb 220 bool initialValueForLEDCharacteristic = true;
apalmieri 3:e0efdb741bd4 221 ledServicePtr = new LEDService(ble, initialValueForLEDCharacteristic);
apalmieri 0:aae2d6c2a9eb 222
apalmieri 0:aae2d6c2a9eb 223 /* setup advertising */
apalmieri 0:aae2d6c2a9eb 224 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
apalmieri 0:aae2d6c2a9eb 225 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
apalmieri 0:aae2d6c2a9eb 226 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
apalmieri 0:aae2d6c2a9eb 227 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
apalmieri 0:aae2d6c2a9eb 228 ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
apalmieri 0:aae2d6c2a9eb 229 ble.gap().startAdvertising();
barry210110 7:126b141a8c86 230
apalmieri 0:aae2d6c2a9eb 231 while (true) {
apalmieri 0:aae2d6c2a9eb 232 ble.waitForEvent();
apalmieri 0:aae2d6c2a9eb 233 }
apalmieri 0:aae2d6c2a9eb 234 }
apalmieri 0:aae2d6c2a9eb 235
apalmieri 0:aae2d6c2a9eb 236 int main(void)
barry210110 7:126b141a8c86 237 {
barry210110 7:126b141a8c86 238
apalmieri 0:aae2d6c2a9eb 239 BLE &ble = BLE::Instance();
apalmieri 0:aae2d6c2a9eb 240
barry210110 7:126b141a8c86 241 ble.init(bleInitComplete);
barry210110 7:126b141a8c86 242
apalmieri 0:aae2d6c2a9eb 243 }
barry210110 7:126b141a8c86 244
barry210110 7:126b141a8c86 245 void call()
barry210110 7:126b141a8c86 246 {
barry210110 7:126b141a8c86 247 /*----- Initialization. -----*/
barry210110 7:126b141a8c86 248 /* Initializing SPI bus. */
barry210110 7:126b141a8c86 249
barry210110 7:126b141a8c86 250
barry210110 7:126b141a8c86 251 #ifdef TARGET_STM32F401
barry210110 7:126b141a8c86 252 DevSPI dev_spi(PB_15, PB_14, PB_13);
barry210110 7:126b141a8c86 253 #else
barry210110 7:126b141a8c86 254 DevSPI dev_spi(PB_15, PB_14, PB_13);
barry210110 7:126b141a8c86 255 #endif
apalmieri 0:aae2d6c2a9eb 256
barry210110 7:126b141a8c86 257 /* Initializing Motor Control Expansion Board. */
barry210110 7:126b141a8c86 258 x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
barry210110 7:126b141a8c86 259
barry210110 7:126b141a8c86 260 /* Building a list of motor control components. */
barry210110 7:126b141a8c86 261 L6470 **motors = x_nucleo_ihm02a1->get_components();
barry210110 7:126b141a8c86 262 motors[0]->set_home();
barry210110 7:126b141a8c86 263 /* Waiting. */
barry210110 7:126b141a8c86 264 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 265
barry210110 7:126b141a8c86 266 /* Getting the current position. */
barry210110 7:126b141a8c86 267 int position = motors[0]->get_position();
barry210110 7:126b141a8c86 268
barry210110 7:126b141a8c86 269 /* Printing to the console. */
barry210110 7:126b141a8c86 270 printf("--> Getting the current position: %d\r\n", position);
barry210110 7:126b141a8c86 271
barry210110 7:126b141a8c86 272 /* Waiting. */
barry210110 7:126b141a8c86 273 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 274
barry210110 7:126b141a8c86 275 /* Printing to the console. */
barry210110 7:126b141a8c86 276 printf("--> Moving forward %d steps.\r\n", STEPS_1);
barry210110 7:126b141a8c86 277
barry210110 7:126b141a8c86 278 /* Moving. */
barry210110 7:126b141a8c86 279 motors[0]->move(StepperMotor::FWD, STEPS_1);
barry210110 7:126b141a8c86 280
barry210110 7:126b141a8c86 281 /* Waiting while active. */
barry210110 7:126b141a8c86 282 motors[0]->wait_while_active();
barry210110 7:126b141a8c86 283
barry210110 7:126b141a8c86 284 /* Getting the current position. */
barry210110 7:126b141a8c86 285 position = motors[0]->get_position();
barry210110 7:126b141a8c86 286
barry210110 7:126b141a8c86 287 /* Printing to the console. */
barry210110 7:126b141a8c86 288 printf("--> Getting the current position: %d\r\n", position);
barry210110 7:126b141a8c86 289
barry210110 7:126b141a8c86 290 /* Printing to the console. */
barry210110 7:126b141a8c86 291 printf("--> Marking the current position.\r\n");
barry210110 7:126b141a8c86 292
barry210110 7:126b141a8c86 293 /* Marking the current position. */
barry210110 7:126b141a8c86 294 motors[0]->set_mark();
barry210110 7:126b141a8c86 295
barry210110 7:126b141a8c86 296 /* Waiting. */
barry210110 7:126b141a8c86 297 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 298
barry210110 7:126b141a8c86 299 /* Printing to the console. */
barry210110 7:126b141a8c86 300 printf("--> Moving backward %d steps.\r\n", STEPS_2);
barry210110 7:126b141a8c86 301
barry210110 7:126b141a8c86 302 /* Moving. */
barry210110 7:126b141a8c86 303 motors[0]->move(StepperMotor::BWD, STEPS_2);
barry210110 7:126b141a8c86 304
barry210110 7:126b141a8c86 305 /* Waiting while active. */
barry210110 7:126b141a8c86 306 motors[0]->wait_while_active();
barry210110 7:126b141a8c86 307
barry210110 7:126b141a8c86 308 /* Waiting. */
barry210110 7:126b141a8c86 309 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 310
barry210110 7:126b141a8c86 311 /* Getting the current position. */
barry210110 7:126b141a8c86 312 position = motors[0]->get_position();
barry210110 7:126b141a8c86 313
barry210110 7:126b141a8c86 314 /* Printing to the console. */
barry210110 7:126b141a8c86 315 printf("--> Getting the current position: %d\r\n", position);
barry210110 7:126b141a8c86 316
barry210110 7:126b141a8c86 317 /* Waiting. */
barry210110 7:126b141a8c86 318 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 319
barry210110 7:126b141a8c86 320 /* Printing to the console. */
barry210110 7:126b141a8c86 321 printf("--> Going to marked position.\r\n");
barry210110 7:126b141a8c86 322
barry210110 7:126b141a8c86 323 /* Going to marked position. */
barry210110 7:126b141a8c86 324 motors[0]->go_mark();
barry210110 7:126b141a8c86 325
barry210110 7:126b141a8c86 326 /* Waiting while active. */
barry210110 7:126b141a8c86 327 motors[0]->wait_while_active();
barry210110 7:126b141a8c86 328
barry210110 7:126b141a8c86 329 /* Waiting. */
barry210110 7:126b141a8c86 330 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 331
barry210110 7:126b141a8c86 332 /* Getting the current position. */
barry210110 7:126b141a8c86 333 position = motors[0]->get_position();
barry210110 7:126b141a8c86 334
barry210110 7:126b141a8c86 335 /* Printing to the console. */
barry210110 7:126b141a8c86 336 printf("--> Getting the current position: %d\r\n", position);
barry210110 7:126b141a8c86 337
barry210110 7:126b141a8c86 338 /* Waiting. */
barry210110 7:126b141a8c86 339 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 340
barry210110 7:126b141a8c86 341 /* Printing to the console. */
barry210110 7:126b141a8c86 342 printf("--> Going to home position.\r\n");
barry210110 7:126b141a8c86 343
barry210110 7:126b141a8c86 344 /* Going to home position. */
barry210110 7:126b141a8c86 345 motors[0]->go_home();
barry210110 7:126b141a8c86 346
barry210110 7:126b141a8c86 347 /* Waiting while active. */
barry210110 7:126b141a8c86 348 motors[0]->wait_while_active();
barry210110 7:126b141a8c86 349
barry210110 7:126b141a8c86 350 /* Waiting. */
barry210110 7:126b141a8c86 351 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 352
barry210110 7:126b141a8c86 353 /* Getting the current position. */
barry210110 7:126b141a8c86 354 position = motors[0]->get_position();
barry210110 7:126b141a8c86 355
barry210110 7:126b141a8c86 356 /* Printing to the console. */
barry210110 7:126b141a8c86 357 printf("--> Getting the current position: %d\r\n", position);
barry210110 7:126b141a8c86 358
barry210110 7:126b141a8c86 359 /* Waiting. */
barry210110 7:126b141a8c86 360 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 361
barry210110 7:126b141a8c86 362 /* Printing to the console. */
barry210110 7:126b141a8c86 363 printf("--> Halving the microsteps.\r\n");
barry210110 7:126b141a8c86 364
barry210110 7:126b141a8c86 365 /* Halving the microsteps. */
barry210110 7:126b141a8c86 366 init[0].step_sel = (init[0].step_sel > 0 ? init[0].step_sel - 1 : init[0].step_sel);
barry210110 7:126b141a8c86 367 if (!motors[0]->set_step_mode((StepperMotor::step_mode_t) init[0].step_sel)) {
barry210110 7:126b141a8c86 368 printf(" Step Mode not allowed.\r\n");
barry210110 7:126b141a8c86 369 }
barry210110 7:126b141a8c86 370
barry210110 7:126b141a8c86 371 /* Waiting. */
barry210110 7:126b141a8c86 372 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 373
barry210110 7:126b141a8c86 374 /* Printing to the console. */
barry210110 7:126b141a8c86 375 printf("--> Setting home position.\r\n");
barry210110 7:126b141a8c86 376
barry210110 7:126b141a8c86 377 /* Setting the home position. */
barry210110 7:126b141a8c86 378 motors[0]->set_home();
barry210110 7:126b141a8c86 379
barry210110 7:126b141a8c86 380 /* Waiting. */
barry210110 7:126b141a8c86 381 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 382
barry210110 7:126b141a8c86 383 /* Getting the current position. */
barry210110 7:126b141a8c86 384 position = motors[0]->get_position();
barry210110 7:126b141a8c86 385
barry210110 7:126b141a8c86 386 /* Printing to the console. */
barry210110 7:126b141a8c86 387 printf("--> Getting the current position: %d\r\n", position);
barry210110 7:126b141a8c86 388
barry210110 7:126b141a8c86 389 /* Waiting. */
barry210110 7:126b141a8c86 390 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 391
barry210110 7:126b141a8c86 392 /* Printing to the console. */
barry210110 7:126b141a8c86 393 printf("--> Moving forward %d steps.\r\n", STEPS_1);
barry210110 7:126b141a8c86 394
barry210110 7:126b141a8c86 395 /* Moving. */
barry210110 7:126b141a8c86 396 motors[0]->move(StepperMotor::FWD, STEPS_1);
barry210110 7:126b141a8c86 397
barry210110 7:126b141a8c86 398 /* Waiting while active. */
barry210110 7:126b141a8c86 399 motors[0]->wait_while_active();
barry210110 7:126b141a8c86 400
barry210110 7:126b141a8c86 401 /* Getting the current position. */
barry210110 7:126b141a8c86 402 position = motors[0]->get_position();
barry210110 7:126b141a8c86 403
barry210110 7:126b141a8c86 404 /* Printing to the console. */
barry210110 7:126b141a8c86 405 printf("--> Getting the current position: %d\r\n", position);
barry210110 7:126b141a8c86 406
barry210110 7:126b141a8c86 407 /* Printing to the console. */
barry210110 7:126b141a8c86 408 printf("--> Marking the current position.\r\n");
barry210110 7:126b141a8c86 409
barry210110 7:126b141a8c86 410 /* Marking the current position. */
barry210110 7:126b141a8c86 411 motors[0]->set_mark();
barry210110 7:126b141a8c86 412
barry210110 7:126b141a8c86 413 /* Waiting. */
barry210110 7:126b141a8c86 414 wait_ms(DELAY_2);
barry210110 7:126b141a8c86 415
barry210110 7:126b141a8c86 416
barry210110 7:126b141a8c86 417 /*----- Running together for a certain amount of time. -----*/
barry210110 7:126b141a8c86 418
barry210110 7:126b141a8c86 419 /* Printing to the console. */
barry210110 7:126b141a8c86 420 printf("--> Running together for %d seconds.\r\n", DELAY_3 / 1000);
barry210110 7:126b141a8c86 421
barry210110 7:126b141a8c86 422 /* Preparing each motor to perform a run at a specified speed. */
barry210110 7:126b141a8c86 423 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
barry210110 7:126b141a8c86 424 motors[m]->prepare_run(StepperMotor::BWD, 400);
barry210110 7:126b141a8c86 425 }
barry210110 7:126b141a8c86 426
barry210110 7:126b141a8c86 427 /* Performing the action on each motor at the same time. */
barry210110 7:126b141a8c86 428 x_nucleo_ihm02a1->perform_prepared_actions();
barry210110 7:126b141a8c86 429
barry210110 7:126b141a8c86 430 /* Waiting. */
barry210110 7:126b141a8c86 431 wait_ms(DELAY_3);
barry210110 7:126b141a8c86 432
barry210110 7:126b141a8c86 433
barry210110 7:126b141a8c86 434 /*----- Increasing the speed while running. -----*/
barry210110 7:126b141a8c86 435
barry210110 7:126b141a8c86 436 /* Preparing each motor to perform a run at a specified speed. */
barry210110 7:126b141a8c86 437 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
barry210110 7:126b141a8c86 438 motors[m]->prepare_get_speed();
barry210110 7:126b141a8c86 439 }
barry210110 7:126b141a8c86 440
barry210110 7:126b141a8c86 441 /* Performing the action on each motor at the same time. */
barry210110 7:126b141a8c86 442 uint32_t* results = x_nucleo_ihm02a1->perform_prepared_actions();
barry210110 7:126b141a8c86 443
barry210110 7:126b141a8c86 444 /* Printing to the console. */
barry210110 7:126b141a8c86 445 printf(" Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
barry210110 7:126b141a8c86 446
barry210110 7:126b141a8c86 447 /* Printing to the console. */
barry210110 7:126b141a8c86 448 printf("--> Doublig the speed while running again for %d seconds.\r\n", DELAY_3 / 1000);
barry210110 7:126b141a8c86 449
barry210110 7:126b141a8c86 450 /* Preparing each motor to perform a run at a specified speed. */
barry210110 7:126b141a8c86 451 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
barry210110 7:126b141a8c86 452 motors[m]->prepare_run(StepperMotor::BWD, results[m] << 1);
barry210110 7:126b141a8c86 453 }
barry210110 7:126b141a8c86 454
barry210110 7:126b141a8c86 455 /* Performing the action on each motor at the same time. */
barry210110 7:126b141a8c86 456 results = x_nucleo_ihm02a1->perform_prepared_actions();
barry210110 7:126b141a8c86 457
barry210110 7:126b141a8c86 458 /* Waiting. */
barry210110 7:126b141a8c86 459 wait_ms(DELAY_3);
barry210110 7:126b141a8c86 460
barry210110 7:126b141a8c86 461 /* Preparing each motor to perform a run at a specified speed. */
barry210110 7:126b141a8c86 462 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
barry210110 7:126b141a8c86 463 motors[m]->prepare_get_speed();
barry210110 7:126b141a8c86 464 }
barry210110 7:126b141a8c86 465
barry210110 7:126b141a8c86 466 /* Performing the action on each motor at the same time. */
barry210110 7:126b141a8c86 467 results = x_nucleo_ihm02a1->perform_prepared_actions();
barry210110 7:126b141a8c86 468
barry210110 7:126b141a8c86 469 /* Printing to the console. */
barry210110 7:126b141a8c86 470 printf(" Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
barry210110 7:126b141a8c86 471
barry210110 7:126b141a8c86 472 /* Waiting. */
barry210110 7:126b141a8c86 473 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 474
barry210110 7:126b141a8c86 475
barry210110 7:126b141a8c86 476 /*----- Hard Stop. -----*/
barry210110 7:126b141a8c86 477
barry210110 7:126b141a8c86 478 /* Printing to the console. */
barry210110 7:126b141a8c86 479 printf("--> Hard Stop.\r\n");
barry210110 7:126b141a8c86 480
barry210110 7:126b141a8c86 481 /* Preparing each motor to perform a hard stop. */
barry210110 7:126b141a8c86 482 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
barry210110 7:126b141a8c86 483 motors[m]->prepare_hard_stop();
barry210110 7:126b141a8c86 484 }
barry210110 7:126b141a8c86 485
barry210110 7:126b141a8c86 486 /* Performing the action on each motor at the same time. */
barry210110 7:126b141a8c86 487 x_nucleo_ihm02a1->perform_prepared_actions();
barry210110 7:126b141a8c86 488
barry210110 7:126b141a8c86 489 /* Waiting. */
barry210110 7:126b141a8c86 490 wait_ms(DELAY_2);
barry210110 7:126b141a8c86 491
barry210110 7:126b141a8c86 492
barry210110 7:126b141a8c86 493 /*----- Doing a full revolution on each motor, one after the other. -----*/
barry210110 7:126b141a8c86 494
barry210110 7:126b141a8c86 495 /* Printing to the console. */
barry210110 7:126b141a8c86 496 printf("--> Doing a full revolution on each motor, one after the other.\r\n");
barry210110 7:126b141a8c86 497
barry210110 7:126b141a8c86 498 /* Doing a full revolution on each motor, one after the other. */
barry210110 7:126b141a8c86 499 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
barry210110 7:126b141a8c86 500 for (int i = 0; i < MPR_1; i++) {
barry210110 7:126b141a8c86 501 /* Computing the number of steps. */
barry210110 7:126b141a8c86 502 int steps = (int) (((int) init[m].fullstepsperrevolution * pow(2.0f, init[m].step_sel)) / MPR_1);
barry210110 7:126b141a8c86 503
barry210110 7:126b141a8c86 504 /* Moving. */
barry210110 7:126b141a8c86 505 motors[m]->move(StepperMotor::FWD, steps);
barry210110 7:126b141a8c86 506
barry210110 7:126b141a8c86 507 /* Waiting while active. */
barry210110 7:126b141a8c86 508 motors[m]->wait_while_active();
barry210110 7:126b141a8c86 509
barry210110 7:126b141a8c86 510 /* Waiting. */
barry210110 7:126b141a8c86 511 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 512 }
barry210110 7:126b141a8c86 513 }
barry210110 7:126b141a8c86 514
barry210110 7:126b141a8c86 515 /* Waiting. */
barry210110 7:126b141a8c86 516 wait_ms(DELAY_2);
barry210110 7:126b141a8c86 517
barry210110 7:126b141a8c86 518
barry210110 7:126b141a8c86 519 /*----- High Impedance State. -----*/
barry210110 7:126b141a8c86 520
barry210110 7:126b141a8c86 521 /* Printing to the console. */
barry210110 7:126b141a8c86 522 printf("--> High Impedance State.\r\n");
barry210110 7:126b141a8c86 523
barry210110 7:126b141a8c86 524 /* Preparing each motor to set High Impedance State. */
barry210110 7:126b141a8c86 525 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
barry210110 7:126b141a8c86 526 motors[m]->prepare_hard_hiz();
barry210110 7:126b141a8c86 527 }
barry210110 7:126b141a8c86 528
barry210110 7:126b141a8c86 529 /* Performing the action on each motor at the same time. */
barry210110 7:126b141a8c86 530 x_nucleo_ihm02a1->perform_prepared_actions();
barry210110 7:126b141a8c86 531
barry210110 7:126b141a8c86 532 /* Waiting. */
barry210110 7:126b141a8c86 533 wait_ms(DELAY_2);
barry210110 7:126b141a8c86 534
barry210110 7:126b141a8c86 535 nCS_ = 0 ;
barry210110 7:126b141a8c86 536 }
barry210110 7:126b141a8c86 537