NEHSROBOT / Mbed 2 deprecated Motor_Ble_v1201820223FINAL

20180223FINAL

Dependencies:   BLE_API X_NUCLEO_IDB0XA1 X_NUCLEO_IHM02A1 mbed

Fork of Motor_Ble_v10223 by NEHSROBOT

main.cpp@9:ef9b37e2464f, 2018-02-04 (annotated)

Committer:
barry210110
Date:
Sun Feb 04 06:55:45 2018 +0000
Revision:
9:ef9b37e2464f
Parent:
7:126b141a8c86
Child:
10:21eecb227c05
final;

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 /* Helper header files. */
barry210110 7:126b141a8c86 21 #include "DevSPI.h"
barry210110 9:ef9b37e2464f 22
barry210110 9:ef9b37e2464f 23
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 9:ef9b37e2464f 31
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 9:ef9b37e2464f 45 void call3();
barry210110 7:126b141a8c86 46
barry210110 9:ef9b37e2464f 47 #ifdef TARGET_STM32F401
barry210110 9:ef9b37e2464f 48 DevSPI dev_spi(PB_15, PB_14, PB_13);
barry210110 9:ef9b37e2464f 49 #else
barry210110 9:ef9b37e2464f 50 DevSPI dev_spi(PB_15, PB_14, PB_13);
barry210110 9:ef9b37e2464f 51 #endif
barry210110 7:126b141a8c86 52
barry210110 7:126b141a8c86 53 /* Variables -----------------------------------------------------------------*/
barry210110 7:126b141a8c86 54
barry210110 7:126b141a8c86 55 /* Motor Control Expansion Board. */
apalmieri 0:aae2d6c2a9eb 56
barry210110 7:126b141a8c86 57 /* Initialization parameters of the motors connected to the expansion board. */
barry210110 7:126b141a8c86 58 L6470_init_t init[L6470DAISYCHAINSIZE] = {
barry210110 7:126b141a8c86 59 /* First Motor. */
barry210110 7:126b141a8c86 60 {
barry210110 7:126b141a8c86 61 9.0, /* Motor supply voltage in V. */
barry210110 7:126b141a8c86 62 400, /* Min number of steps per revolution for the motor. */
barry210110 7:126b141a8c86 63 1.7, /* Max motor phase voltage in A. */
barry210110 7:126b141a8c86 64 3.06, /* Max motor phase voltage in V. */
barry210110 7:126b141a8c86 65 300.0, /* Motor initial speed [step/s]. */
barry210110 7:126b141a8c86 66 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
barry210110 7:126b141a8c86 67 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
barry210110 7:126b141a8c86 68 992.0, /* Motor maximum speed [step/s]. */
barry210110 7:126b141a8c86 69 0.0, /* Motor minimum speed [step/s]. */
barry210110 7:126b141a8c86 70 602.7, /* Motor full-step speed threshold [step/s]. */
barry210110 7:126b141a8c86 71 3.06, /* Holding kval [V]. */
barry210110 7:126b141a8c86 72 3.06, /* Constant speed kval [V]. */
barry210110 7:126b141a8c86 73 3.06, /* Acceleration starting kval [V]. */
barry210110 7:126b141a8c86 74 3.06, /* Deceleration starting kval [V]. */
barry210110 7:126b141a8c86 75 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
barry210110 7:126b141a8c86 76 392.1569e-6, /* Start slope [s/step]. */
barry210110 7:126b141a8c86 77 643.1372e-6, /* Acceleration final slope [s/step]. */
barry210110 7:126b141a8c86 78 643.1372e-6, /* Deceleration final slope [s/step]. */
barry210110 7:126b141a8c86 79 0, /* Thermal compensation factor (range [0, 15]). */
barry210110 7:126b141a8c86 80 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
barry210110 7:126b141a8c86 81 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
barry210110 7:126b141a8c86 82 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
barry210110 7:126b141a8c86 83 0xFF, /* Alarm conditions enable. */
barry210110 7:126b141a8c86 84 0x2E88 /* Ic configuration. */
barry210110 7:126b141a8c86 85 },
barry210110 7:126b141a8c86 86
barry210110 7:126b141a8c86 87 /* Second Motor. */
barry210110 7:126b141a8c86 88 {
barry210110 7:126b141a8c86 89 9.0, /* Motor supply voltage in V. */
barry210110 7:126b141a8c86 90 400, /* Min number of steps per revolution for the motor. */
barry210110 7:126b141a8c86 91 1.7, /* Max motor phase voltage in A. */
barry210110 7:126b141a8c86 92 3.06, /* Max motor phase voltage in V. */
barry210110 7:126b141a8c86 93 300.0, /* Motor initial speed [step/s]. */
barry210110 7:126b141a8c86 94 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
barry210110 7:126b141a8c86 95 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
barry210110 7:126b141a8c86 96 992.0, /* Motor maximum speed [step/s]. */
barry210110 7:126b141a8c86 97 0.0, /* Motor minimum speed [step/s]. */
barry210110 7:126b141a8c86 98 602.7, /* Motor full-step speed threshold [step/s]. */
barry210110 7:126b141a8c86 99 3.06, /* Holding kval [V]. */
barry210110 7:126b141a8c86 100 3.06, /* Constant speed kval [V]. */
barry210110 7:126b141a8c86 101 3.06, /* Acceleration starting kval [V]. */
barry210110 7:126b141a8c86 102 3.06, /* Deceleration starting kval [V]. */
barry210110 7:126b141a8c86 103 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
barry210110 7:126b141a8c86 104 392.1569e-6, /* Start slope [s/step]. */
barry210110 7:126b141a8c86 105 643.1372e-6, /* Acceleration final slope [s/step]. */
barry210110 7:126b141a8c86 106 643.1372e-6, /* Deceleration final slope [s/step]. */
barry210110 7:126b141a8c86 107 0, /* Thermal compensation factor (range [0, 15]). */
barry210110 7:126b141a8c86 108 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
barry210110 7:126b141a8c86 109 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
barry210110 7:126b141a8c86 110 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
barry210110 7:126b141a8c86 111 0xFF, /* Alarm conditions enable. */
barry210110 7:126b141a8c86 112 0x2E88 /* Ic configuration. */
barry210110 7:126b141a8c86 113 }
barry210110 7:126b141a8c86 114 };
barry210110 7:126b141a8c86 115
barry210110 9:ef9b37e2464f 116 XNucleoIHM02A1 *x_nucleo_ihm02a1= new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
barry210110 9:ef9b37e2464f 117
barry210110 7:126b141a8c86 118 DigitalOut actuatedLED(LED2);
barry210110 7:126b141a8c86 119 const static char DEVICE_NAME[] = "mydevice"; // CHANGE NAME
barry210110 7:126b141a8c86 120 static const uint16_t uuid16_list[] = {LEDService::LED_SERVICE_UUID}; // GATT ATTRIBUTE UUID
apalmieri 0:aae2d6c2a9eb 121
apalmieri 0:aae2d6c2a9eb 122 LEDService *ledServicePtr;
apalmieri 0:aae2d6c2a9eb 123
apalmieri 0:aae2d6c2a9eb 124 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
apalmieri 0:aae2d6c2a9eb 125 {
apalmieri 0:aae2d6c2a9eb 126 (void)params;
apalmieri 3:e0efdb741bd4 127 BLE::Instance().gap().startAdvertising(); // restart advertising
apalmieri 0:aae2d6c2a9eb 128 }
apalmieri 0:aae2d6c2a9eb 129
barry210110 9:ef9b37e2464f 130
apalmieri 0:aae2d6c2a9eb 131 /**
apalmieri 0:aae2d6c2a9eb 132 * This callback allows the LEDService to receive updates to the ledState Characteristic.
apalmieri 0:aae2d6c2a9eb 133 *
apalmieri 0:aae2d6c2a9eb 134 * @param[in] params
apalmieri 0:aae2d6c2a9eb 135 * Information about the characterisitc being updated.
apalmieri 0:aae2d6c2a9eb 136 */
apalmieri 0:aae2d6c2a9eb 137 void onDataWrittenCallback(const GattWriteCallbackParams *params) {
apalmieri 0:aae2d6c2a9eb 138 if ((params->handle == ledServicePtr->getValueHandle()) && (params->len == 1)) {
barry210110 9:ef9b37e2464f 139
barry210110 7:126b141a8c86 140 if ( *(params->data)== 0x00 )
barry210110 7:126b141a8c86 141 {
barry210110 7:126b141a8c86 142 actuatedLED=1 ;
barry210110 9:ef9b37e2464f 143 call3();
barry210110 9:ef9b37e2464f 144
barry210110 9:ef9b37e2464f 145 actuatedLED=0 ;wait(0.5) ;
barry210110 9:ef9b37e2464f 146 actuatedLED=1 ;wait(0.5) ;
barry210110 9:ef9b37e2464f 147 actuatedLED=0 ;wait(0.5) ;
barry210110 9:ef9b37e2464f 148
barry210110 7:126b141a8c86 149
barry210110 7:126b141a8c86 150 }
barry210110 7:126b141a8c86 151 else if (*(params->data)== 0x01)
barry210110 7:126b141a8c86 152 {
barry210110 7:126b141a8c86 153 actuatedLED=0 ;
barry210110 7:126b141a8c86 154 wait(0.5) ;
barry210110 7:126b141a8c86 155 actuatedLED=1 ;
barry210110 7:126b141a8c86 156 wait(0.5) ;
barry210110 7:126b141a8c86 157 actuatedLED=0 ;
barry210110 7:126b141a8c86 158 wait(0.5) ;
barry210110 7:126b141a8c86 159 actuatedLED=1 ;
barry210110 7:126b141a8c86 160 wait(0.5) ;
barry210110 7:126b141a8c86 161 actuatedLED=0 ;
barry210110 7:126b141a8c86 162
barry210110 7:126b141a8c86 163 }
barry210110 7:126b141a8c86 164 else if (*(params->data)== 0x02) {
barry210110 7:126b141a8c86 165 actuatedLED=1 ;
barry210110 7:126b141a8c86 166 wait(0.5) ;
barry210110 7:126b141a8c86 167 actuatedLED=0 ;
barry210110 7:126b141a8c86 168 wait(0.5) ;
barry210110 7:126b141a8c86 169 actuatedLED=1 ;
barry210110 7:126b141a8c86 170 }
barry210110 7:126b141a8c86 171 else if (*(params->data)== 0x03) {
barry210110 7:126b141a8c86 172 actuatedLED=1 ;
barry210110 7:126b141a8c86 173 wait(0.5) ;
barry210110 7:126b141a8c86 174 actuatedLED=0 ;
barry210110 7:126b141a8c86 175 wait(0.5) ;
barry210110 7:126b141a8c86 176 actuatedLED=1 ;
barry210110 7:126b141a8c86 177 wait(0.5) ;
barry210110 7:126b141a8c86 178 actuatedLED=0 ;
barry210110 7:126b141a8c86 179 wait(0.5) ;
barry210110 7:126b141a8c86 180 actuatedLED=1 ;
barry210110 7:126b141a8c86 181 }
barry210110 7:126b141a8c86 182
apalmieri 0:aae2d6c2a9eb 183 }
barry210110 7:126b141a8c86 184
apalmieri 0:aae2d6c2a9eb 185 }
apalmieri 0:aae2d6c2a9eb 186
apalmieri 0:aae2d6c2a9eb 187 /**
apalmieri 0:aae2d6c2a9eb 188 * This function is called when the ble initialization process has failled
apalmieri 0:aae2d6c2a9eb 189 */
barry210110 7:126b141a8c86 190 int onBleInitError(BLE &ble, ble_error_t error)
apalmieri 0:aae2d6c2a9eb 191 {
apalmieri 0:aae2d6c2a9eb 192 /* Initialization error handling should go here */
apalmieri 0:aae2d6c2a9eb 193 }
apalmieri 0:aae2d6c2a9eb 194
apalmieri 0:aae2d6c2a9eb 195 /**
apalmieri 0:aae2d6c2a9eb 196 * Callback triggered when the ble initialization process has finished
apalmieri 0:aae2d6c2a9eb 197 */
apalmieri 0:aae2d6c2a9eb 198 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
apalmieri 0:aae2d6c2a9eb 199 {
apalmieri 0:aae2d6c2a9eb 200 BLE& ble = params->ble;
apalmieri 0:aae2d6c2a9eb 201 ble_error_t error = params->error;
apalmieri 0:aae2d6c2a9eb 202
apalmieri 0:aae2d6c2a9eb 203 if (error != BLE_ERROR_NONE) {
apalmieri 0:aae2d6c2a9eb 204 /* In case of error, forward the error handling to onBleInitError */
apalmieri 0:aae2d6c2a9eb 205 onBleInitError(ble, error);
barry210110 9:ef9b37e2464f 206 return;
apalmieri 0:aae2d6c2a9eb 207 }
apalmieri 0:aae2d6c2a9eb 208
apalmieri 0:aae2d6c2a9eb 209 /* Ensure that it is the default instance of BLE */
apalmieri 0:aae2d6c2a9eb 210 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
apalmieri 0:aae2d6c2a9eb 211 return;
apalmieri 0:aae2d6c2a9eb 212 }
apalmieri 0:aae2d6c2a9eb 213
apalmieri 0:aae2d6c2a9eb 214 ble.gap().onDisconnection(disconnectionCallback);
apalmieri 0:aae2d6c2a9eb 215 ble.gattServer().onDataWritten(onDataWrittenCallback);
apalmieri 0:aae2d6c2a9eb 216
apalmieri 0:aae2d6c2a9eb 217 bool initialValueForLEDCharacteristic = true;
apalmieri 3:e0efdb741bd4 218 ledServicePtr = new LEDService(ble, initialValueForLEDCharacteristic);
apalmieri 0:aae2d6c2a9eb 219
apalmieri 0:aae2d6c2a9eb 220 /* setup advertising */
apalmieri 0:aae2d6c2a9eb 221 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
apalmieri 0:aae2d6c2a9eb 222 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
apalmieri 0:aae2d6c2a9eb 223 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
apalmieri 0:aae2d6c2a9eb 224 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
apalmieri 0:aae2d6c2a9eb 225 ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
apalmieri 0:aae2d6c2a9eb 226 ble.gap().startAdvertising();
barry210110 7:126b141a8c86 227
apalmieri 0:aae2d6c2a9eb 228 while (true) {
apalmieri 0:aae2d6c2a9eb 229 ble.waitForEvent();
apalmieri 0:aae2d6c2a9eb 230 }
apalmieri 0:aae2d6c2a9eb 231 }
apalmieri 0:aae2d6c2a9eb 232
apalmieri 0:aae2d6c2a9eb 233 int main(void)
barry210110 7:126b141a8c86 234 {
barry210110 7:126b141a8c86 235
apalmieri 0:aae2d6c2a9eb 236 BLE &ble = BLE::Instance();
apalmieri 0:aae2d6c2a9eb 237
barry210110 7:126b141a8c86 238 ble.init(bleInitComplete);
barry210110 7:126b141a8c86 239
apalmieri 0:aae2d6c2a9eb 240 }
barry210110 9:ef9b37e2464f 241
barry210110 9:ef9b37e2464f 242 void call3()
barry210110 7:126b141a8c86 243 {
barry210110 7:126b141a8c86 244 /* Building a list of motor control components. */
barry210110 7:126b141a8c86 245 L6470 **motors = x_nucleo_ihm02a1->get_components();
barry210110 7:126b141a8c86 246 motors[0]->set_home();
barry210110 7:126b141a8c86 247 /* Waiting. */
barry210110 7:126b141a8c86 248 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 249
barry210110 7:126b141a8c86 250 /* Getting the current position. */
barry210110 7:126b141a8c86 251 int position = motors[0]->get_position();
barry210110 7:126b141a8c86 252
barry210110 7:126b141a8c86 253 /* Waiting. */
barry210110 7:126b141a8c86 254 wait_ms(DELAY_1);
barry210110 7:126b141a8c86 255
barry210110 7:126b141a8c86 256 /* Moving. */
barry210110 7:126b141a8c86 257 motors[0]->move(StepperMotor::FWD, STEPS_1);
barry210110 7:126b141a8c86 258
barry210110 7:126b141a8c86 259 }
barry210110 9:ef9b37e2464f 260
barry210110 7:126b141a8c86 261