File content as of revision 0:6bde2b4e342e:

 /**
 ******************************************************************************
 * @file    main.cpp
 * @author  Giulio Vespoli, Unina Fablab
 * @version 1.0
 * @date    04-April-2017
 * @brief   Simple Example application for using the X_NUCLEO_IKS01A1 
 *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
 ******************************************************************************
*/


/* Includes */
#include "mbed.h"
#include "rtos.h"
#include "x_nucleo_iks01a2.h"

#define RAD_TO_DEG 57.295779513082320876798154814105f



/* Instantiate the expansion board */
static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D10, D11); //d4 diventa d11 e d5 diventa d10

static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
static LSM303AGR_ACC_Sensor *accelerometer = mems_expansion_board->accelerometer;


int32_t mdps[3];
int16_t mdpsRaw[3];
int32_t acc[3];


bool useCalibrate = false;
float dpsPerDigit = .07f; //70 mdps diventeranno 0.07dps
float gyrosensitivity; 


float offset=0;         
float correct_speed=0;         
float acquired_speed=0;
float yaw = 0;



void readRaw()
{
  acc_gyro->Get_G_AxesRaw(mdpsRaw);
  acquired_speed = mdpsRaw[2];           //acquisisco velocità angolare dal giroscopio
}



float readNormalize()
{
    readRaw();
    correct_speed = (acquired_speed - offset) * dpsPerDigit;       //tolgo al valore acquisito l'offset
    return correct_speed;
}


  
void calibrate(int samples)
{
    printf("CALIBRATING\r\n");
 
    float sumZ = 0;   
    for (uint8_t i = 0; i < samples; ++i)
    {   
        acc_gyro->Get_G_AxesRaw(mdpsRaw);                                               //Acquisisco dal giroscopio 
       printf("SAMPLE - LSM6DSL [gyro/mdps]:   %6ld, %6ld,\r\n", i, mdpsRaw[2]);        //Stampo il numero del campione acquisito e la velocità angolare
        sumZ += mdpsRaw[2];                                                             //Faccio la somma delle velocità angolari acquisite
        wait(0.1);
    }
    
    offset = sumZ / samples;                                                             //calcolo l'offset come valor medio di 100 acquisizioni
    
    printf("CALIBRATED\r\n");    
    printf("\r\n");
}



void calcRotation2(void const *args)
{
    
    Timer t;
    double dt=0;                           //in dt memorizzerò l'intervallo di tempo tra due acquisizioni 
    
    while(1) 
    {
        t.stop();
        dt= t.read();
        
        correct_speed = readNormalize();
        t.reset();
        t.start();
        
        // Calcolo lo yaw 
        yaw = yaw + correct_speed * dt;    //lo spostamento complessivo al passo k sarà uguale allo spostamento complessivo all'istante k-1 + la nuova rotazione
    }
}
      


 

/* Component specific header files. */
#include "stspin240_250_class.h"
STSPIN240_250 *motor;


int main()
{ 
  uint8_t id;
  acc_gyro->Enable_X();   //ABILITO ACCELLEROMETRO
  acc_gyro->Enable_G();   //ABILITO GIROSCOPIO
  
  accelerometer->Enable();  //ABILITO UN ALTRO ACCELLEROMETRO
  
  printf("\r\n--- Starting new run ---\r\n");
 
  wait(1);
  
  acc_gyro->ReadID(&id);                                          //RICAVO L'INDIRIZZO DEL GIROSCOPIO E ACCELLEROMETRO
  printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
 
  accelerometer->ReadID(&id);                                       //RICAVO L'INDIRIZZO DELL'ACCELLEROMETRO
  printf("LSM303AGR accelerometer           = 0x%X\r\n", id);
  
  
   acc_gyro->Get_G_Sensitivity(&gyrosensitivity);                  //RICAVO LA SENSIBILITA' DEL GIRO E ACC, 70
   printf("Sensitivity mV/o/ms:  %f\r\n", gyrosensitivity);
   
   float fullScale;
   acc_gyro->Get_G_FS(&fullScale);                                
   printf("Full Scale:  %f\r\n", fullScale);
   
   float gODR;
   acc_gyro->Get_G_ODR(&gODR);
   printf("ODR:  %f\r\n", gODR);
    
   calibrate(100);                                                   //Uso 100 campioni per calcolare l'offset
   
   Thread rotTrigger(calcRotation2);                                 //Chiamo la funzione calcRotation2 che aggiorna il valore dello yaw
    
 
 
 /* Initializing Motor Control Component. */
    motor = new STSPIN240_250(D2, D9, D6, D7, D5, D4, A0 );         //Chiamo il costruttore per inizializzare l'oggetto motor

    /* Set dual bridge enabled as two motors are used*/
    motor->SetDualFullBridgeConfig(1);

    /* Set PWM Frequency of Ref to 20000 Hz */
    motor->SetRefPwmFreq(0, 20000);       
   motor->SetRefPwmFreq(1, 20000);

    /* Set PWM duty cycle of Ref to 100% */
    motor->SetRefPwmDc(0, 100);       

    motor->SetRefPwmDc(1, 100);
    
    /* Set PWM Frequency of bridge A inputs to 20000 Hz */
    motor->SetBridgeInputPwmFreq(0,20000);

    /* Set PWM Frequency of bridge B inputs to 20000 Hz */
    motor->SetBridgeInputPwmFreq(1,20000);
               
                

                int speed_sx=35;
                int speed_dx=35;
                
                
                motor->SetSpeed(0,speed_sx);                       
                motor->SetSpeed(1,speed_dx);                         
                motor->Run(1, BDCMotor::FWD);                   
                motor->Run(0, BDCMotor::FWD);    

        while(1)
        {
            if(yaw>0.0001f)  //90° corrisponde ad uno yaw di 0.08, quindi 0.0001 corrisponde ad un decimo di grado (0.1°)
            {
                speed_sx=45;
                speed_dx=35;
            }
            
            if(yaw<-0.0001f)
            {
                speed_dx=45;
                speed_sx=35;
            }
            
               printf( "yaw = %2.4f \r\n", yaw);
               motor->SetSpeed(0,speed_sx);   
               motor->SetSpeed(1,speed_dx); 
               printf( "VELOCITA SX = %6ld\r\n", speed_sx);
               printf( "VELOCITA DX= %6ld\r\n", speed_dx);
               printf("\r\n");
               
        }
}