Diff: main.cpp

Revision:

10:21eecb227c05

Parent:

9:ef9b37e2464f

Child:

11:6deabd374c96

--- a/main.cpp	Sun Feb 04 06:55:45 2018 +0000
+++ b/main.cpp	Tue Feb 06 07:51:55 2018 +0000
@@ -17,17 +17,20 @@
  /* Includes ------------------------------------------------------------------*/
 
 
+
 /* Helper header files. */
 #include "DevSPI.h"
-
-
+void FWD();
+void BWD();
+void LEFT();
+void RIGHT();
 /* Expansion Board specific header files. */
 #include "XNucleoIHM02A1.h"
 
 #include "mbed.h"
 #include "ble/BLE.h"
 #include "LEDService.h"
-#include "BlueNRGDevice.h"
+
 
 /* Definitions ---------------------------------------------------------------*/
 
@@ -42,18 +45,15 @@
 #define DELAY_1 1000
 #define DELAY_2 2000
 #define DELAY_3 5000
-void call3();
 
 #ifdef TARGET_STM32F401
     DevSPI dev_spi(PB_15, PB_14, PB_13);
 #else
     DevSPI dev_spi(PB_15, PB_14, PB_13);
 #endif
-
+ 
 /* Variables -----------------------------------------------------------------*/
 
-/* Motor Control Expansion Board. */
-
 /* Initialization parameters of the motors connected to the expansion board. */
 L6470_init_t init[L6470DAISYCHAINSIZE] = {
     /* First Motor. */
@@ -113,7 +113,9 @@
     }
 };
 
-XNucleoIHM02A1 *x_nucleo_ihm02a1= new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
+
+/* Motor Control Expansion Board. */
+XNucleoIHM02A1* x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
 
 DigitalOut actuatedLED(LED2);
 const static char     DEVICE_NAME[] = "mydevice";    //   CHANGE NAME
@@ -127,7 +129,6 @@
     BLE::Instance().gap().startAdvertising(); // restart advertising
 }
 
-
 /**
  * This callback allows the LEDService to receive updates to the ledState Characteristic.
  *
@@ -136,20 +137,9 @@
  */
 void onDataWrittenCallback(const GattWriteCallbackParams *params) {
     if ((params->handle == ledServicePtr->getValueHandle()) && (params->len == 1)) {
- 
+        
         if (  *(params->data)== 0x00 ) 
         {    
-            actuatedLED=1     ;
-           call3();
-              
-            actuatedLED=0     ;wait(0.5)            ;
-            actuatedLED=1     ;wait(0.5)            ;
-            actuatedLED=0     ;wait(0.5)            ;
-
-              
-        }
-        else if (*(params->data)== 0x01)  
-        { 
             actuatedLED=0      ;
             wait(0.5)            ;
             actuatedLED=1      ;
@@ -159,26 +149,29 @@
             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)            ;
+        }
+        else if (*(params->data)== 0x01)  
+        { 
+            actuatedLED=1      ;
+            FWD();
+        }
+        else if (*(params->data)== 0x02)  {
             actuatedLED=1      ;
+            BWD();
         }
+        else if (*(params->data)== 0x03)  {
+            actuatedLED=1      ;
+            RIGHT();
+        }
+        else if (*(params->data)== 0x04)  {
+            actuatedLED=1      ;
+            LEFT();
+        }
+           
    
     }
  
@@ -203,7 +196,7 @@
     if (error != BLE_ERROR_NONE) {
         /* In case of error, forward the error handling to onBleInitError */
         onBleInitError(ble, error);
-       return;
+        return;
     }
 
     /* Ensure that it is the default instance of BLE */
@@ -238,24 +231,87 @@
     ble.init(bleInitComplete); 
  
 }
+  
+void FWD()
+{
 
-void call3()
-{
     /* Building a list of motor control components. */
     L6470 **motors = x_nucleo_ihm02a1->get_components();
-   motors[0]->set_home(); 
-    /* Waiting. */
-    wait_ms(DELAY_1);
+    motors[0]->set_home(); 
+
+    /* Getting the current position. */
+    int position = motors[0]->get_position();
+
+        /* Preparing each motor to perform a run at a specified speed. */
+    for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+        motors[m]->prepare_move(StepperMotor::FWD, 25600);
+    }
+
+    /* Performing the action on each motor at the same time. */
+    x_nucleo_ihm02a1->perform_prepared_actions();
+    
+    /* Waiting while active. */
+    motors[0]->wait_while_active();
+}
+
+void BWD()
+{
+
+    /* Building a list of motor control components. */
+    L6470 **motors = x_nucleo_ihm02a1->get_components();
+    motors[0]->set_home(); 
 
     /* Getting the current position. */
     int position = motors[0]->get_position();
 
-    /* Waiting. */
-    wait_ms(DELAY_1);
+        /* Preparing each motor to perform a run at a specified speed. */
+    for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+        motors[m]->prepare_move(StepperMotor::BWD, 25600);
+    }
+
+    /* Performing the action on each motor at the same time. */
+    x_nucleo_ihm02a1->perform_prepared_actions();
+    
+    /* Waiting while active. */
+    motors[0]->wait_while_active();
+}
+void LEFT()
+{
+
+    /* Building a list of motor control components. */
+    L6470 **motors = x_nucleo_ihm02a1->get_components();
+    motors[0]->set_home(); 
+
+    /* Getting the current position. */
+    int position = motors[0]->get_position();
+
+        /* Preparing each motor to perform a run at a specified speed. */
+    motors[0]->prepare_move(StepperMotor::FWD, 25600);
+    motors[1]->prepare_move(StepperMotor::BWD, 25600);
 
-    /* Moving. */
-    motors[0]->move(StepperMotor::FWD, STEPS_1);
+    /* Performing the action on each motor at the same time. */
+    x_nucleo_ihm02a1->perform_prepared_actions();
     
-    }
+    /* Waiting while active. */
+    motors[0]->wait_while_active();
+}
+void RIGHT()
+{
+
+    /* Building a list of motor control components. */
+    L6470 **motors = x_nucleo_ihm02a1->get_components();
+    motors[0]->set_home(); 
+
+    /* Getting the current position. */
+    int position = motors[0]->get_position();
+
+        /* Preparing each motor to perform a run at a specified speed. */
+    motors[1]->prepare_move(StepperMotor::FWD, 25600);
+    motors[0]->prepare_move(StepperMotor::BWD, 25600);
+
+    /* Performing the action on each motor at the same time. */
+    x_nucleo_ihm02a1->perform_prepared_actions();
     
-
+    /* Waiting while active. */
+    motors[0]->wait_while_active();
+}
\ No newline at end of file