Diff: main.cpp

Revision:

7:126b141a8c86

Parent:

3:e0efdb741bd4

Child:

8:66938f196868

Child:

9:ef9b37e2464f

diff -r e696cac5ef17 -r 126b141a8c86 main.cpp
--- 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        ;
+    }
+