#include "mbed.

h"
#include "MMA8451Q.h"
#include"esc.h"
void liftup();
void get_acc();
Ticker tick;
float x=0, y=0, z=0;
#if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
PinName const SDA = PTE25;
PinName const SCL = PTE24;
#elif defined (TARGET_KL05Z)
PinName const SDA = PTB4;
PinName const SCL = PTB3;
#elif defined (TARGET_K20D50M)
PinName const SDA = PTB1;
PinName const SCL = PTB0;
#else
#error TARGET NOT DEFINED
#endif
#define MMA8451_I2C_ADDRESS (0x1d<<1)
MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
ESC esc1(PTA12); //declare the ESC as connected to pin p26
Timer t;
int main(void)
{
t.start();
tick.attach(&get_acc,0.1);
PwmOut rled(LED1);
PwmOut gled(LED2);
PwmOut bled(LED3);
printf("MMA8451 ID: %d\n", acc.getWhoAmI());
while (true) {
rled = 1.0f - x;
gled = 1.0f - y;
bled = 1.0f - z;
wait(1);
printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z);
liftup();
}
}

void liftup()
{
t.stop();
printf("time taken=%f\n",t.read());
float throttle_var=0;//that means 50%.
printf("Lift up");
while(x<0.2 && y<0.2 && z>0.8) {
printf("motor run");

throttle_var=throttle_var+0.01;
esc1 = throttle_var; //memorize the throttle value (it doesn't send it t
o the ESC).
esc1.setThrottle(throttle_var);
esc1(); //actually sets the throttle to the ESC.
printf("\nthrottle_valu=%f",throttle_var);
wait_ms(200); //20ms is the default period of the ESC pwm; the ESC may
not run faster.
}
}
void get_acc()
{
x = abs(acc.getAccX());
y = abs(acc.getAccY());
z = abs(acc.getAccZ());
}