NEHSROBOT
20180223 ULTIMATE FINAL
Dependencies: BLE_API X_NUCLEO_IDB0XA1 X_NUCLEO_IHM02A1 mbed
Fork of
Motor_Ble_v1201820223FINAL
by 
NEHSROBOT
Diff: main.cpp
- Revision:
- 7:126b141a8c86
- Parent:
- 3:e0efdb741bd4
- Child:
- 8:66938f196868
- Child:
- 9:ef9b37e2464f
--- a/main.cpp Fri Sep 16 10:36:43 2016 +0000
+++ b/main.cpp Thu Feb 01 07:55:42 2018 +0000
@@ -13,15 +13,104 @@
* See the License for the specific language governing permissions and
* limitations under the License.
*/
+
+ /* Includes ------------------------------------------------------------------*/
+
+
+
+/* Helper header files. */
+#include "DevSPI.h"
+void call();
+/* Expansion Board specific header files. */
+#include "XNucleoIHM02A1.h"
#include "mbed.h"
#include "ble/BLE.h"
#include "LEDService.h"
+#include "BlueNRGDevice.h"
+int temp,add ;
+/* Definitions ---------------------------------------------------------------*/
-DigitalOut actuatedLED(LED1, 0);
+/* Number of movements per revolution. */
+#define MPR_1 4
+
+/* Number of steps. */
+#define STEPS_1 (400 * 128) /* 1 revolution given a 400 steps motor configured at 1/128 microstep mode. */
+#define STEPS_2 (STEPS_1 * 2)
+
+/* Delay in milliseconds. */
+#define DELAY_1 1000
+#define DELAY_2 2000
+#define DELAY_3 5000
+
+
+/* Variables -----------------------------------------------------------------*/
+
+/* Motor Control Expansion Board. */
+XNucleoIHM02A1 *x_nucleo_ihm02a1;
-const static char DEVICE_NAME[] = "LED";
-static const uint16_t uuid16_list[] = {LEDService::LED_SERVICE_UUID};
+/* Initialization parameters of the motors connected to the expansion board. */
+L6470_init_t init[L6470DAISYCHAINSIZE] = {
+ /* First Motor. */
+ {
+ 9.0, /* Motor supply voltage in V. */
+ 400, /* Min number of steps per revolution for the motor. */
+ 1.7, /* Max motor phase voltage in A. */
+ 3.06, /* Max motor phase voltage in V. */
+ 300.0, /* Motor initial speed [step/s]. */
+ 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+ 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
+ 992.0, /* Motor maximum speed [step/s]. */
+ 0.0, /* Motor minimum speed [step/s]. */
+ 602.7, /* Motor full-step speed threshold [step/s]. */
+ 3.06, /* Holding kval [V]. */
+ 3.06, /* Constant speed kval [V]. */
+ 3.06, /* Acceleration starting kval [V]. */
+ 3.06, /* Deceleration starting kval [V]. */
+ 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
+ 392.1569e-6, /* Start slope [s/step]. */
+ 643.1372e-6, /* Acceleration final slope [s/step]. */
+ 643.1372e-6, /* Deceleration final slope [s/step]. */
+ 0, /* Thermal compensation factor (range [0, 15]). */
+ 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
+ 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
+ StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
+ 0xFF, /* Alarm conditions enable. */
+ 0x2E88 /* Ic configuration. */
+ },
+
+ /* Second Motor. */
+ {
+ 9.0, /* Motor supply voltage in V. */
+ 400, /* Min number of steps per revolution for the motor. */
+ 1.7, /* Max motor phase voltage in A. */
+ 3.06, /* Max motor phase voltage in V. */
+ 300.0, /* Motor initial speed [step/s]. */
+ 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+ 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
+ 992.0, /* Motor maximum speed [step/s]. */
+ 0.0, /* Motor minimum speed [step/s]. */
+ 602.7, /* Motor full-step speed threshold [step/s]. */
+ 3.06, /* Holding kval [V]. */
+ 3.06, /* Constant speed kval [V]. */
+ 3.06, /* Acceleration starting kval [V]. */
+ 3.06, /* Deceleration starting kval [V]. */
+ 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
+ 392.1569e-6, /* Start slope [s/step]. */
+ 643.1372e-6, /* Acceleration final slope [s/step]. */
+ 643.1372e-6, /* Deceleration final slope [s/step]. */
+ 0, /* Thermal compensation factor (range [0, 15]). */
+ 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
+ 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
+ StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
+ 0xFF, /* Alarm conditions enable. */
+ 0x2E88 /* Ic configuration. */
+ }
+};
+
+DigitalOut actuatedLED(LED2);
+const static char DEVICE_NAME[] = "mydevice"; // CHANGE NAME
+static const uint16_t uuid16_list[] = {LEDService::LED_SERVICE_UUID}; // GATT ATTRIBUTE UUID
LEDService *ledServicePtr;
@@ -39,14 +128,69 @@
*/
void onDataWrittenCallback(const GattWriteCallbackParams *params) {
if ((params->handle == ledServicePtr->getValueHandle()) && (params->len == 1)) {
- actuatedLED = *(params->data);
+
+ /* switch( *(params->data)){
+ case 0:
+ actuatedLED=0 ;
+ wait(1) ;
+ actuatedLED=1 ;
+ wait(1) ;
+ actuatedLED=0 ;
+ break ;
+ case 1:
+ actuatedLED=1 ;
+ wait(1) ;
+ actuatedLED=0 ;
+ wait(1) ;
+ actuatedLED=1 ;
+ break ;
+ } */
+ if ( *(params->data)== 0x00 )
+ {
+ actuatedLED=1 ;
+ call();
+
+ }
+ else if (*(params->data)== 0x01)
+ {
+ actuatedLED=0 ;
+ wait(0.5) ;
+ actuatedLED=1 ;
+ wait(0.5) ;
+ actuatedLED=0 ;
+ wait(0.5) ;
+ actuatedLED=1 ;
+ wait(0.5) ;
+ actuatedLED=0 ;
+
+ }
+ else if (*(params->data)== 0x02) {
+ actuatedLED=1 ;
+ wait(0.5) ;
+ actuatedLED=0 ;
+ wait(0.5) ;
+ actuatedLED=1 ;
+ }
+ else if (*(params->data)== 0x03) {
+ actuatedLED=1 ;
+ wait(0.5) ;
+ actuatedLED=0 ;
+ wait(0.5) ;
+ actuatedLED=1 ;
+ wait(0.5) ;
+ actuatedLED=0 ;
+ wait(0.5) ;
+ actuatedLED=1 ;
+ }
+
}
+
}
/**
* This function is called when the ble initialization process has failled
*/
-void onBleInitError(BLE &ble, ble_error_t error)
+int onBleInitError(BLE &ble, ble_error_t error)
{
/* Initialization error handling should go here */
}
@@ -83,16 +227,311 @@
ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
ble.gap().startAdvertising();
-
+
while (true) {
ble.waitForEvent();
}
}
int main(void)
-{
+{
+
BLE &ble = BLE::Instance();
- ble.init(bleInitComplete);
+ ble.init(bleInitComplete);
+
}
+
+void call()
+{
+/*----- Initialization. -----*/
+/* Initializing SPI bus. */
+
+
+#ifdef TARGET_STM32F401
+ DevSPI dev_spi(PB_15, PB_14, PB_13);
+#else
+ DevSPI dev_spi(PB_15, PB_14, PB_13);
+#endif
+ /* Initializing Motor Control Expansion Board. */
+ x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
+
+ /* Building a list of motor control components. */
+ L6470 **motors = x_nucleo_ihm02a1->get_components();
+ motors[0]->set_home();
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+ /* Getting the current position. */
+ int position = motors[0]->get_position();
+
+ /* Printing to the console. */
+ printf("--> Getting the current position: %d\r\n", position);
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+ /* Printing to the console. */
+ printf("--> Moving forward %d steps.\r\n", STEPS_1);
+
+ /* Moving. */
+ motors[0]->move(StepperMotor::FWD, STEPS_1);
+
+ /* Waiting while active. */
+ motors[0]->wait_while_active();
+
+ /* Getting the current position. */
+ position = motors[0]->get_position();
+
+ /* Printing to the console. */
+ printf("--> Getting the current position: %d\r\n", position);
+
+ /* Printing to the console. */
+ printf("--> Marking the current position.\r\n");
+
+ /* Marking the current position. */
+ motors[0]->set_mark();
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+ /* Printing to the console. */
+ printf("--> Moving backward %d steps.\r\n", STEPS_2);
+
+ /* Moving. */
+ motors[0]->move(StepperMotor::BWD, STEPS_2);
+
+ /* Waiting while active. */
+ motors[0]->wait_while_active();
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+ /* Getting the current position. */
+ position = motors[0]->get_position();
+
+ /* Printing to the console. */
+ printf("--> Getting the current position: %d\r\n", position);
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+ /* Printing to the console. */
+ printf("--> Going to marked position.\r\n");
+
+ /* Going to marked position. */
+ motors[0]->go_mark();
+
+ /* Waiting while active. */
+ motors[0]->wait_while_active();
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+ /* Getting the current position. */
+ position = motors[0]->get_position();
+
+ /* Printing to the console. */
+ printf("--> Getting the current position: %d\r\n", position);
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+ /* Printing to the console. */
+ printf("--> Going to home position.\r\n");
+
+ /* Going to home position. */
+ motors[0]->go_home();
+
+ /* Waiting while active. */
+ motors[0]->wait_while_active();
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+ /* Getting the current position. */
+ position = motors[0]->get_position();
+
+ /* Printing to the console. */
+ printf("--> Getting the current position: %d\r\n", position);
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+ /* Printing to the console. */
+ printf("--> Halving the microsteps.\r\n");
+
+ /* Halving the microsteps. */
+ init[0].step_sel = (init[0].step_sel > 0 ? init[0].step_sel - 1 : init[0].step_sel);
+ if (!motors[0]->set_step_mode((StepperMotor::step_mode_t) init[0].step_sel)) {
+ printf(" Step Mode not allowed.\r\n");
+ }
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+ /* Printing to the console. */
+ printf("--> Setting home position.\r\n");
+
+ /* Setting the home position. */
+ motors[0]->set_home();
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+ /* Getting the current position. */
+ position = motors[0]->get_position();
+
+ /* Printing to the console. */
+ printf("--> Getting the current position: %d\r\n", position);
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+ /* Printing to the console. */
+ printf("--> Moving forward %d steps.\r\n", STEPS_1);
+
+ /* Moving. */
+ motors[0]->move(StepperMotor::FWD, STEPS_1);
+
+ /* Waiting while active. */
+ motors[0]->wait_while_active();
+
+ /* Getting the current position. */
+ position = motors[0]->get_position();
+
+ /* Printing to the console. */
+ printf("--> Getting the current position: %d\r\n", position);
+
+ /* Printing to the console. */
+ printf("--> Marking the current position.\r\n");
+
+ /* Marking the current position. */
+ motors[0]->set_mark();
+
+ /* Waiting. */
+ wait_ms(DELAY_2);
+
+
+ /*----- Running together for a certain amount of time. -----*/
+
+ /* Printing to the console. */
+ printf("--> Running together for %d seconds.\r\n", DELAY_3 / 1000);
+
+ /* Preparing each motor to perform a run at a specified speed. */
+ for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+ motors[m]->prepare_run(StepperMotor::BWD, 400);
+ }
+
+ /* Performing the action on each motor at the same time. */
+ x_nucleo_ihm02a1->perform_prepared_actions();
+
+ /* Waiting. */
+ wait_ms(DELAY_3);
+
+
+ /*----- Increasing the speed while running. -----*/
+
+ /* Preparing each motor to perform a run at a specified speed. */
+ for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+ motors[m]->prepare_get_speed();
+ }
+
+ /* Performing the action on each motor at the same time. */
+ uint32_t* results = x_nucleo_ihm02a1->perform_prepared_actions();
+
+ /* Printing to the console. */
+ printf(" Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
+
+ /* Printing to the console. */
+ printf("--> Doublig the speed while running again for %d seconds.\r\n", DELAY_3 / 1000);
+
+ /* Preparing each motor to perform a run at a specified speed. */
+ for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+ motors[m]->prepare_run(StepperMotor::BWD, results[m] << 1);
+ }
+
+ /* Performing the action on each motor at the same time. */
+ results = x_nucleo_ihm02a1->perform_prepared_actions();
+
+ /* Waiting. */
+ wait_ms(DELAY_3);
+
+ /* Preparing each motor to perform a run at a specified speed. */
+ for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+ motors[m]->prepare_get_speed();
+ }
+
+ /* Performing the action on each motor at the same time. */
+ results = x_nucleo_ihm02a1->perform_prepared_actions();
+
+ /* Printing to the console. */
+ printf(" Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+
+
+ /*----- Hard Stop. -----*/
+
+ /* Printing to the console. */
+ printf("--> Hard Stop.\r\n");
+
+ /* Preparing each motor to perform a hard stop. */
+ for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+ motors[m]->prepare_hard_stop();
+ }
+
+ /* Performing the action on each motor at the same time. */
+ x_nucleo_ihm02a1->perform_prepared_actions();
+
+ /* Waiting. */
+ wait_ms(DELAY_2);
+
+
+ /*----- Doing a full revolution on each motor, one after the other. -----*/
+
+ /* Printing to the console. */
+ printf("--> Doing a full revolution on each motor, one after the other.\r\n");
+
+ /* Doing a full revolution on each motor, one after the other. */
+ for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+ for (int i = 0; i < MPR_1; i++) {
+ /* Computing the number of steps. */
+ int steps = (int) (((int) init[m].fullstepsperrevolution * pow(2.0f, init[m].step_sel)) / MPR_1);
+
+ /* Moving. */
+ motors[m]->move(StepperMotor::FWD, steps);
+
+ /* Waiting while active. */
+ motors[m]->wait_while_active();
+
+ /* Waiting. */
+ wait_ms(DELAY_1);
+ }
+ }
+
+ /* Waiting. */
+ wait_ms(DELAY_2);
+
+
+ /*----- High Impedance State. -----*/
+
+ /* Printing to the console. */
+ printf("--> High Impedance State.\r\n");
+
+ /* Preparing each motor to set High Impedance State. */
+ for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+ motors[m]->prepare_hard_hiz();
+ }
+
+ /* Performing the action on each motor at the same time. */
+ x_nucleo_ihm02a1->perform_prepared_actions();
+
+ /* Waiting. */
+ wait_ms(DELAY_2);
+
+ nCS_ = 0 ;
+ }
+