main.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    main.cpp
00004  * @author  CLab
00005  * @version V1.0.0
00006  * @date    2-December-2016
00007  * @brief   Simple Example application for using the X_NUCLEO_IKS01A1 
00008  *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
00009  ******************************************************************************
00010  * @attention
00011  *
00012  * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
00013  *
00014  * Redistribution and use in source and binary forms, with or without modification,
00015  * are permitted provided that the following conditions are met:
00016  *   1. Redistributions of source code must retain the above copyright notice,
00017  *      this list of conditions and the following disclaimer.
00018  *   2. Redistributions in binary form must reproduce the above copyright notice,
00019  *      this list of conditions and the following disclaimer in the documentation
00020  *      and/or other materials provided with the distribution.
00021  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00022  *      may be used to endorse or promote products derived from this software
00023  *      without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00028  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00029  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00030  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00031  *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00033  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00034  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  ******************************************************************************
00037 */ 
00038 
00039 /* Includes */
00040 #include "DevSPI.h"
00041 #include "XNucleoIHM02A1.h"
00042 #include "mbed.h"
00043 #include "XNucleoIKS01A2.h"
00044 #include "rtos.h"
00045 #include "iostream"
00046 using namespace std;
00047 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00048 DigitalOut actuatedLED(LED2);
00049 void move_thread();
00050 
00051 
00052 /* Initialization parameters of the motors connected to the expansion board. */
00053 //L6470_init_t init[L6470DAISYCHAINSIZE] = {
00054 L6470_init_t init[2] = {
00055     /* First Motor. */
00056     {
00057         12.0,                           /* Motor supply voltage in V. */
00058         200,                           /* Min number of steps per revolution for the motor. */
00059         1.7,                           /* Max motor phase voltage in A. */
00060         3.06,                          /* Max motor phase voltage in V. */
00061         30000.0,                         /* Motor initial speed [step/s]. */
00062   //      5000,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00063   //      5000,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00064         2500.0,                         /* Motor maximum speed [step/s]. */
00065         0.0,                           /* Motor minimum speed [step/s]. */
00066         602.7,                         /* Motor full-step speed threshold [step/s]. */
00067         3.06,                          /* Holding kval [V]. */
00068         3.06,                          /* Constant speed kval [V]. */
00069         3.06,                          /* Acceleration starting kval [V]. */
00070         3.06,                          /* Deceleration starting kval [V]. */
00071         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00072         392.1569e-6,                   /* Start slope [s/step]. */
00073         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00074         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00075         0,                             /* Thermal compensation factor (range [0, 15]). */
00076         3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00077         3.06 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00078         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00079         0xFF,                          /* Alarm conditions enable. */
00080         0x2E88                         /* Ic configuration. */
00081     },
00082 
00083     /* Second Motor. */
00084     {
00085         12.0,                           /* Motor supply voltage in V. */
00086         200,                           /* Min number of steps per revolution for the motor. */
00087         1.7,                           /* Max motor phase voltage in A. */
00088         3.06,                          /* Max motor phase voltage in V. */
00089         300.0,                         /* Motor initial speed [step/s]. */
00090     //    10000,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00091     //    10000,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00092         2300.0,                         /* Motor maximum speed [step/s]. */
00093         0.0,                           /* Motor minimum speed [step/s]. */
00094         602.7,                         /* Motor full-step speed threshold [step/s]. */
00095         3.06,                          /* Holding kval [V]. */
00096         3.06,                          /* Constant speed kval [V]. */
00097         3.06,                          /* Acceleration starting kval [V]. */
00098         3.06,                          /* Deceleration starting kval [V]. */
00099         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00100         392.1569e-6,                   /* Start slope [s/step]. */
00101         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00102         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00103         0,                             /* Thermal compensation factor (range [0, 15]). */
00104         3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00105         3.06 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00106         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00107         0xFF,                          /* Alarm conditions enable. */
00108         0x2E88                         /* Ic configuration. */
00109     }
00110 };
00111 
00112 
00113 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00114 
00115 #define sampleTime  0.003
00116 #define targetAngle 6
00117 #define Kp 15
00118 #define Ki 0
00119 #define Kd 0
00120 #define pi 3.1415926
00121 
00122   Timer timer;
00123   int begin, end;
00124   Thread thread;
00125 //  int32_t accpaxes[3];
00126 //  double accpdaxes[3];
00127   int32_t accaxes[3];
00128   double accdaxes[3];
00129   int32_t gyroaxes[3];
00130   double gyrodaxes[3];
00131   int32_t motorspeed;
00132   double currentAngle;
00133   double accpAngle;
00134   double accAngle;
00135   double errSum;
00136   double err;
00137   double prevAngle;
00138   double theta;  
00139   double gyroAngle;
00140   
00141   
00142 #ifdef TARGET_STM32F429
00143     DevSPI dev_spi(D11, D12, D13);
00144 #else
00145     DevSPI dev_spi(D11, D12, D3);
00146 #endif
00147   XNucleoIHM02A1 *x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
00148 /* Instantiate the expansion board */
00149 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
00150 /* Retrieve the composing elements of the expansion board */
00151 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
00152 //static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
00153 /* Simple main function */
00154 
00155  
00156   
00157 int main() {
00158   uint8_t id;
00159   /* Enable sensors */
00160   //accelerometer->enable();
00161   acc_gyro->enable_x();
00162   //acc_gyro->enable_g();
00163   //printf("\r\n--- Starting new run ---\r\n");
00164   
00165   //accelerometer->read_id(&id);
00166   //printf("LSM303AGR accelerometer           = 0x%X\r\n", id);
00167   acc_gyro->read_id(&id);
00168   printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
00169 
00170   thread.start(move_thread);
00171 }
00172 
00173  
00174 void move_thread() {
00175         
00176     while (true) {
00177         
00178         L6470 **motors = x_nucleo_ihm02a1->get_components();
00179         timer.start();
00180         actuatedLED=0;
00181         begin = timer.read_us();
00182         //accelerometer->get_x_axes(accpaxes);
00183         //accpdaxes[0] = accpaxes[0] +40;
00184         //accpdaxes[2] = accpaxes[2] +40;
00185         //theta = accpdaxes[0]/accpdaxes[2];
00186        acc_gyro->get_x_axes(accaxes);
00187         accdaxes[0] = accaxes[0];
00188         accdaxes[2] = accaxes[2];       
00189         theta = accdaxes[0]/accdaxes[2];
00190         //accpAngle = atan(theta)*180/pi;
00191         accAngle = atan(theta)*180/pi;
00192         //acc_gyro->get_g_axes(gyroaxes);
00193         //gyrodaxes[1] = gyroaxes[1];
00194         //gyroAngle = (gyrodaxes[1])*sampleTime*0.001;
00195         //currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);
00196         currentAngle = accAngle;
00197         err = currentAngle - targetAngle;
00198         errSum = errSum + err;  
00199         motorspeed = Kp*err + Ki*errSum*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime;   
00200         
00201  //       if (motorspeed > 0){
00202             motors[0]->run(StepperMotor::FWD, 1000);
00203             motors[1]->run(StepperMotor::BWD, 3000);
00204  //       }else if(motorspeed <= 0) {
00205  //           motors[0]->run(StepperMotor::BWD, -motorspeed);
00206   //          motors[1]->run(StepperMotor::FWD, -motorspeed);
00207  //       }
00208   uint32_t r,r2;
00209  //  for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00210        r =  motors[0]->get_acceleration();
00211        r2 =  motors[1]->get_acceleration();
00212  //   }
00213    // uint32_t* results = x_nucleo_ihm02a1->perform_prepared_actions();
00214 
00215     printf("    Speed: M1 %d, M2 %d.\r\n", r, r2);  
00216         //x_nucleo_ihm02a1->perform_prepared_actions();
00217         //prevAngle = currentAngle;
00218 
00219        // printf("LSM303AGR [acc/mg]:  %6ld, %6ld, %6ld\r\n", accpaxes[0], accpaxes[1], accpaxes[2]);
00220        // printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", gyroaxes[0], gyroaxes[1], gyroaxes[2]);
00221        // printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", accaxes[0]-22, accaxes[1]+50, accaxes[2]-45);
00222        // printf("accAngle = %6lf\r\n", accAngle );
00223        // printf("accpAngle = %6lf\r\n", accpAngle );
00224        // printf("theta = %6lf\r\n", theta );
00225        // printf("currentAngle = %6lf\r\n", currentAngle );
00226        // printf("gyroAngle = %6lf\r\n", gyroAngle );
00227        Thread::wait(2);
00228         end = timer.read_us();
00229         actuatedLED=1;
00230       //  printf("Toggle the led takes %d us \r\n", end - begin);
00231        
00232     }
00233 }