![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Smartphone app control stepper motors via bluetooth low energy(BLE) on stm32F401RE x IHM02A1 x IDB05A1.
Dependencies: BLE_API X_NUCLEO_IDB0XA1 X_NUCLEO_IHM02A1 mbed
Fork of BLE_LED_IDB0XA1_demo by
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:66938f196868
- Child:
- 9:ef9b37e2464f
bluetooth control motor
Who changed what in which revision?
User | Revision | Line number | New 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 |