File content as of revision 0:a1ef0a01d09f:

/**
 ******************************************************************************
 * @file    main.cpp
 * @author  CLab
 * @version V1.0.0
 * @date    2-December-2016
 * @brief   Simple Example application for using the X_NUCLEO_IKS01A1 
 *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *   1. Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *   2. Redistributions in binary form must reproduce the above copyright notice,
 *      this list of conditions and the following disclaimer in the documentation
 *      and/or other materials provided with the distribution.
 *   3. Neither the name of STMicroelectronics nor the names of its contributors
 *      may be used to endorse or promote products derived from this software
 *      without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 ******************************************************************************
*/ 

/* Includes */
#include "DevSPI.h"
#include "XNucleoIHM02A1.h"
#include "mbed.h"
#include "XNucleoIKS01A2.h"
#include "rtos.h"
#include "iostream"
using namespace std;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
DigitalOut actuatedLED(LED2);
void move_thread();


/* Initialization parameters of the motors connected to the expansion board. */
//L6470_init_t init[L6470DAISYCHAINSIZE] = {
L6470_init_t init[2] = {
    /* First Motor. */
    {
        12.0,                           /* Motor supply voltage in V. */
        200,                           /* 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. */
        30000.0,                         /* Motor initial speed [step/s]. */
  //      5000,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
  //      5000,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
        2500.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. */
    {
        12.0,                           /* Motor supply voltage in V. */
        200,                           /* 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]. */
    //    10000,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
    //    10000,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
        2300.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. */
    }
};


/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#define sampleTime  0.003
#define targetAngle 6
#define Kp 15
#define Ki 0
#define Kd 0
#define pi 3.1415926

  Timer timer;
  int begin, end;
  Thread thread;
//  int32_t accpaxes[3];
//  double accpdaxes[3];
  int32_t accaxes[3];
  double accdaxes[3];
  int32_t gyroaxes[3];
  double gyrodaxes[3];
  int32_t motorspeed;
  double currentAngle;
  double accpAngle;
  double accAngle;
  double errSum;
  double err;
  double prevAngle;
  double theta;  
  double gyroAngle;
  
  
#ifdef TARGET_STM32F429
    DevSPI dev_spi(D11, D12, D13);
#else
    DevSPI dev_spi(D11, D12, D3);
#endif
  XNucleoIHM02A1 *x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
/* Instantiate the expansion board */
static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
/* Retrieve the composing elements of the expansion board */
static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
//static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
/* Simple main function */

 
  
int main() {
  uint8_t id;
  /* Enable sensors */
  //accelerometer->enable();
  acc_gyro->enable_x();
  //acc_gyro->enable_g();
  //printf("\r\n--- Starting new run ---\r\n");
  
  //accelerometer->read_id(&id);
  //printf("LSM303AGR accelerometer           = 0x%X\r\n", id);
  acc_gyro->read_id(&id);
  printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);

  thread.start(move_thread);
}

 
void move_thread() {
        
    while (true) {
        
        L6470 **motors = x_nucleo_ihm02a1->get_components();
        timer.start();
        actuatedLED=0;
        begin = timer.read_us();
        //accelerometer->get_x_axes(accpaxes);
        //accpdaxes[0] = accpaxes[0] +40;
        //accpdaxes[2] = accpaxes[2] +40;
        //theta = accpdaxes[0]/accpdaxes[2];
       acc_gyro->get_x_axes(accaxes);
        accdaxes[0] = accaxes[0];
        accdaxes[2] = accaxes[2];       
        theta = accdaxes[0]/accdaxes[2];
        //accpAngle = atan(theta)*180/pi;
        accAngle = atan(theta)*180/pi;
        //acc_gyro->get_g_axes(gyroaxes);
        //gyrodaxes[1] = gyroaxes[1];
        //gyroAngle = (gyrodaxes[1])*sampleTime*0.001;
        //currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);
        currentAngle = accAngle;
        err = currentAngle - targetAngle;
        errSum = errSum + err;  
        motorspeed = Kp*err + Ki*errSum*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime;   
        
 //       if (motorspeed > 0){
            motors[0]->run(StepperMotor::FWD, 1000);
            motors[1]->run(StepperMotor::BWD, 3000);
 //       }else if(motorspeed <= 0) {
 //           motors[0]->run(StepperMotor::BWD, -motorspeed);
  //          motors[1]->run(StepperMotor::FWD, -motorspeed);
 //       }
  uint32_t r,r2;
 //  for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
       r =  motors[0]->get_acceleration();
       r2 =  motors[1]->get_acceleration();
 //   }
   // uint32_t* results = x_nucleo_ihm02a1->perform_prepared_actions();

    printf("    Speed: M1 %d, M2 %d.\r\n", r, r2);  
        //x_nucleo_ihm02a1->perform_prepared_actions();
        //prevAngle = currentAngle;

       // printf("LSM303AGR [acc/mg]:  %6ld, %6ld, %6ld\r\n", accpaxes[0], accpaxes[1], accpaxes[2]);
       // printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", gyroaxes[0], gyroaxes[1], gyroaxes[2]);
       // printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", accaxes[0]-22, accaxes[1]+50, accaxes[2]-45);
       // printf("accAngle = %6lf\r\n", accAngle );
       // printf("accpAngle = %6lf\r\n", accpAngle );
       // printf("theta = %6lf\r\n", theta );
       // printf("currentAngle = %6lf\r\n", currentAngle );
       // printf("gyroAngle = %6lf\r\n", gyroAngle );
       Thread::wait(2);
        end = timer.read_us();
        actuatedLED=1;
      //  printf("Toggle the led takes %d us \r\n", end - begin);
       
    }
}