File content as of revision 2:ec30613f2b2b:

#include "mbed.h"

#include "RobotServo.h"
#include "SensorFusion.h"
#include "SPI_9dSensor.h"
#include "RobotBicycleConst.h"
/****************************
Wiring Map

CK3 PC_10 | PC_11   MI3||re  PC_9      | PC_8   rw
MO3 PC_12 | PD_2       ||   |-----------------|
    3.3V  | E5V        ||rs |PB_8  D15 | PC_6 |TX6
    BOOT0 | GND        ||ra |PB_9  D14 | PC_5 |
   |---------------|   ||   |AVDD      | U5V  |
   |NC    | NC     |   ||   |GND       | NC   |
   |NC    | IOREF  |   ||rk |PA_5  D13 | PA_12|RX6
   |PA_13 | NRST   |   ||   |PA_6  D12 | PA_11| rl
   |PA_14 | 3.3V   |   ||   |PA_7  D11 | PB_12|
   |PA_15 | 5.0V   |   ||rb |PB_6  D10 | NC   |
   |GND   | GND    |   ||   |PC_7  D9  | GND  |
   |PB_7  | GND    |   ||lb |PA_9  D8  | PB_2 |CSG
   |PC_13 | VIN    |   ||   |-----------------|
   |---------------|   ||   |-----------------|
    PC_14 | NC         ||ll |PA_8  D7  | PB_1 |CSX
   |---------------|   ||lk |PB_10 D6  | PB_15|MO2
   |PC_15 | PA_0 A0|   ||la |PB_4  D5  | PB_14|MI2
   |PH_0  | PA_1 A1|   ||ls |PB_5  D4  | PB_13|CK2
   |PH_1  | PA_4 A2|   ||le |PB_3  D3  | AGND |
   |VBAT  | PB_0 A3|   ||lw |PA_10 D2  | PC_4 |
   |PC_2  | PC_1 A4|   ||TX2|PA_2  D1  | NC   |
   |PC_3  | PC_0 A5|   ||RX2|PA_3  D0  | NC   |
   |---------------|   ||   |-----------------|
****************************/
AnalogIn analog_value(A5);//When using the ADC module, those pins connected to  AnalogIn can't be used to output.

Ticker timer1;
Ticker timer2;
Timeout timeout;

//Serial pc(D1, D0) declared in SPI_9dSensor.h
Serial BT(PC_6, PA_12);

SPI spi3(PC_12, PC_11, PC_10);

float meas, L_inver;
bool resetting = 0;

void timer1_interrupt(void);
void timer2_interrupt(void);
void reset_offset(void);
void attimeout(void);

int main() {
    setupServo();
    setup_spi();
    init_Sensors();
    BT.baud(115200);
    
    reset_offset();
    
    timer1.attach_us(&timer1_interrupt, 4000);//4.0ms interrupt period (250 Hz)
    timer2.attach_us(&timer2_interrupt, 4098);//4.098ms interrupt period (244 Hz)
    
    spi3.format(8, 3);
    pc.printf("System ready.\r\n");
    
//    pc.printf("\nAnalogIn example\r\n");
    
    while(1) {
//        pc.printf("measure: %f V,\t%f cm\r\n", meas, 1/L_inver);
//        wait(0.1); // 200 ms
    }
}

void timer1_interrupt(void)
{
    meas = analog_value.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
    meas = meas * 3.3f; // Change the value to be in the 0 to 3300 range
    L_inver = 0.0063f * meas - 0.005769f;
}

void timer2_interrupt(void)
{
    ;
}

void reset_offset(void)
{
    pc.printf("Reseting gyro and accel-X offset...\r\n");
    resetting = 1;
    timeout.attach(&attimeout, 2.0f);
    while(resetting)
    {
        reset_gyro_offset();
        reset_acceX_offset();
    }
    timeout.detach();
    pc.printf("Done reseting.\r\n");
//    pc.printf("%d\r\n", Acce_axis_zero[0]);
}

void attimeout(void)
{
    resetting = 0;
}