Diff: main.cpp

Revision:

9:66938f196868

Parent:

7:126b141a8c86

--- a/main.cpp	Thu Feb 01 07:55:42 2018 +0000
+++ b/main.cpp	Sun Feb 04 08:49:24 2018 +0000
@@ -20,15 +20,18 @@
 
 /* Helper header files. */
 #include "DevSPI.h"
-void call();
+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"
-int       temp,add         ;
+
+
 /* Definitions ---------------------------------------------------------------*/
 
 /* Number of movements per revolution. */
@@ -43,12 +46,14 @@
 #define DELAY_2 2000
 #define DELAY_3 5000
 
-
+#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. */
-XNucleoIHM02A1 *x_nucleo_ihm02a1;
-
 /* Initialization parameters of the motors connected to the expansion board. */
 L6470_init_t init[L6470DAISYCHAINSIZE] = {
     /* First Motor. */
@@ -108,6 +113,10 @@
     }
 };
 
+
+/* 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
 static const uint16_t uuid16_list[] = {LEDService::LED_SERVICE_UUID};   // GATT ATTRIBUTE UUID
@@ -129,30 +138,12 @@
 void onDataWrittenCallback(const GattWriteCallbackParams *params) {
     if ((params->handle == ledServicePtr->getValueHandle()) && (params->len == 1)) {
         
- /*       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      ;
@@ -162,26 +153,25 @@
             actuatedLED=1      ;
             wait(0.5)            ;
             actuatedLED=0      ;
-            
+        }
+        else if (*(params->data)== 0x01)  
+        { 
+            actuatedLED=1      ;
+            FWD();
         }
         else if (*(params->data)== 0x02)  {
             actuatedLED=1      ;
-            wait(0.5)            ;
-            actuatedLED=0      ;
-            wait(0.5)            ;
+            BWD();
+        }
+        else if (*(params->data)== 0x03)  {
             actuatedLED=1      ;
+            RIGHT();
         }
-          else if (*(params->data)== 0x03)  {
-           actuatedLED=1      ;
-            wait(0.5)            ;
-            actuatedLED=0      ;
-            wait(0.5)            ;
+        else if (*(params->data)== 0x04)  {
             actuatedLED=1      ;
-            wait(0.5)            ;
-            actuatedLED=0      ;
-            wait(0.5)            ;
-            actuatedLED=1      ;
+            LEFT();
         }
+           
    
     }
  
@@ -242,296 +232,86 @@
  
 }
   
-void call()
+void FWD()
 {
-/*----- 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);
+    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();
 
-    /* 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. */
+        /* 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);
+        motors[m]->prepare_move(StepperMotor::BWD, 25600);
     }
 
     /* 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);
-    }
+    
+    /* Waiting while active. */
+    motors[0]->wait_while_active();
+}
+void LEFT()
+{
 
-    /* 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();
+    /* Building a list of motor control components. */
+    L6470 **motors = x_nucleo_ihm02a1->get_components();
+    motors[0]->set_home(); 
 
-    /* Printing to the console. */
-    printf("    Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
-
-    /* Waiting. */
-    wait_ms(DELAY_1);
-
+    /* Getting the current position. */
+    int position = motors[0]->get_position();
 
-    /*----- 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();
-    }
+        /* 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);
 
     /* 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);
+    
+    /* Waiting while active. */
+    motors[0]->wait_while_active();
+}
+void RIGHT()
+{
 
-            /* Moving. */
-            motors[m]->move(StepperMotor::FWD, steps);
-            
-            /* Waiting while active. */
-            motors[m]->wait_while_active();
-
-            /* Waiting. */
-            wait_ms(DELAY_1);
-        }
-    }
+    /* Building a list of motor control components. */
+    L6470 **motors = x_nucleo_ihm02a1->get_components();
+    motors[0]->set_home(); 
 
-    /* Waiting. */
-    wait_ms(DELAY_2);
-
-
-    /*----- High Impedance State. -----*/
+    /* Getting the current position. */
+    int position = motors[0]->get_position();
 
-    /* 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();
-    }
+        /* 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. */
-    wait_ms(DELAY_2);  
-
-   nCS_ = 0        ;
-    }
-
+    
+    /* Waiting while active. */
+    motors[0]->wait_while_active();
+}
\ No newline at end of file