/*L10 - experiment control motoare elicopter  cu viteza reglabila
prin sensorul de aceeleratie
Motoarele sint interfate cu optocuploare si se alimenteaza la
12V
*/
#include "mbed.h"
#include "TSISensor.h"
#include "MMA8451Q.h"
#define MMA8451_I2C_ADDRESS (0x1d<<1)

    TSISensor tsi ; 
    PwmOut M1(PTD4); // motor 1
    PwmOut M2(PTA12);// motor 2
    PwmOut M3(PTA4); // motor 3
    PwmOut M4(PTA5); // motor 4
    MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS); // sensor acceleratie
   float x; 
    
int main(void) 
{
    
    M1.write(1);  // opresc motor 1
//  wait(0.1);  
     M2.write(1); // opresc motor 2
 //    wait(0.1);  
      M3.write(1); // opresc motor 3
  //    wait(0.1);  
       M4.write(1); // opresc motor 4
  //     wait(0.1);
       
   M1.period(0.020);  // perioada motor 1
   wait(0.1);
    M2.period(0.020);
    wait(0.1);
     M3.period(0.020);
     wait(0.1);
      M4.period(0.020);
      wait(0.1);
    
    while (1) 
    {
      //  x=abs(acc.getAccX()/4); // lectura sensor  acceleratie X
       x=abs(acc.getAccY()/4);
      //  x=abs(acc.getAccZ()/4;
        wait(0.1);
    
      M1.write(1-x);
      wait(0.1);   
       M2.write(1-x);
       wait(0.1);     
      M3.write(1-x);
       wait(0.1);
       M4.write(1-x);
        wait(0.1);
    }
}