File content as of revision 3:197b748a397a:

#include "mbed.h"

#include "RobotServo.h"
#include "SensorFusion.h"
#include "SPI_9dSensor.h"
#include "RobotBicycleConst.h"
#include "ZTC.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   |
//   |---------------|   ||   |-----------------|
//************************************************//

#define IR_FILT_CONST   1.0f * 2.0f * 3.14159f / 250.0f

#define CNT2ZTCm        250
#define CNT2ZTCh        5
#define CNT2STRT        1250

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 BT(PC_6, PA_12);

SPI spi3(PC_12, PC_11, PC_10);

//**** Variables from Arduino ****//
int counter = 0;
int i = 0;
int8_t c = FIRST_POS;
bool s = 0;
int fusion_init = 0;
int state_count = 0;
uint8_t state_change = 0;
uint16_t brake_count = 0;
float V_meas = 0;
float L_inver = 0;
bool toggle = 0;
//********************************//

//***** Variables from RasPi *****//
char BTCharR = 0;

float Vx = 0.0;
float Vx_old = 0.0;

unsigned char pedal_state = 0;
unsigned char sys_state = S_S;
unsigned int count2ztc_h = 0;
unsigned int count2ztc_m = 0;
unsigned int count2straight = 0;
unsigned int count_isr = 0;
float T_total = 0.0;
//********************************//

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");
    
    reset_pos();
    lookuptable_steering(HANDLE_START);
    
    while(!interrupt)
    {
        if(BT.readable())
        {
            BTCharR = BT.getc();
            switch(BTCharR)
            {
                case 's': lookuptable_pedaling(FIRST_POS);
                          lookuptable_steering(HANDLE_STRAIGHT);
                          wait(1.5f);
                          pedal_state = 1; sys_state = L_PD; gamma_ref = 0.0;
                          s = 1; break;        ///start
                case 'a': pedal_state = 0; sys_state = S_S; Vx = 0.0; Vx_old = 0.0; gamma_ref = 0.0; count2ztc_h = 0; count2ztc_m = 0;
                          s = 0; state_count = 0; break;   ///stop
//                case 'm': pedal_state = 2; sys_state = M_PD; gamma_ref = 0.0; if(count2ztc_m >= CNT2ZTCm){sys_state = M_ZTC;} break;
//                case 'h': pedal_state = 3; sys_state = H_PD; gamma_ref = 0.0; if(count2ztc_h >= CNT2ZTCh){sys_state = H_ZTC;} break;
                case 'l': gamma_ref = DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break;     ///left
                case 'r': gamma_ref = -DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break;    ///right
                case 'f': gamma_ref = 0.0; roll_ref = 0.0; break;   ///forward
                case 'c': gamma_ref = 0.0; reset_offset(); break;   ///clear
                default: break;
            }
            BTCharR = 0;
        }
        
        L_inver = 0.0063f * V_meas - 0.005769f;
    }
}

void timer1_interrupt(void)
{
    V_meas = (analog_value.read() * 3.3f) * IR_FILT_CONST + V_meas * (1.0f - IR_FILT_CONST);
}

void timer2_interrupt(void)
{
    if(s == 1)           // bluetooth start 
    {
        i ++;
        if(i == Z_PEDAL_DIVIDER && state_change == 0) //start
        {
            i = 0;
            lookuptable_pedaling(c);
            c++;
            if(c == (FIRST_POS + TOT_STOP_POS)){c = FIRST_POS + TOT_STOP_POS + TOT_STOP_POS - 1;}
            else if(c >= TOT_FOOT_POS){c = 0;}
            state_count++;
        }
        else if(i == L_PEDAL_DIVIDER && state_change == 1)  //low_v
        {
            i = 0;
            lookuptable_pedaling(c);
            c++;
            if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
            else if(c >= TOT_FOOT_POS){c = 0;}
            state_count++;
        }
        else if(i == M_PEDAL_DIVIDER && state_change == 2)  //mid_v
        {
            i = 0;
            lookuptable_pedaling(c);
            c++;
            if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
            else if(c >= TOT_FOOT_POS){c = 0;}
            state_count++;
        }
        else if(i == M_PEDAL_DIVIDER && state_change == 3)  //high_v
        {
            i = 0;
            lookuptable_pedaling(c);
            c++;
            if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
            else if(c >= TOT_FOOT_POS){c = 0;}
        }
    
        if(state_count == COUN2_LOW_V)
        {
            state_change = 1;
        }
        else if(state_count == COUN2_MID_V)
        {
            state_change = 2;
        }
        else if(state_count == COUN2_HIGH_V)
        {
            if(state_change == 2)
            {
//                pc.printf("\nm");
                pedal_state = 2;
                sys_state = M_PD;
                gamma_ref = 0.0;
                if(count2ztc_m >= CNT2ZTCm)
                {
                    sys_state = M_ZTC;
                }
            }
            state_change = 3;
        }
    }
    else  //s is 0
    {
        if(c == FIRST_POS)
        {
            if(state_change > 0)
            {
                stop_pos();
                brake_count++;
                if(brake_count >= 3*244)
                {
                  state_change = 0;
                  brake_count = 0;
                }
            }
            else
            {
                reset_pos();
            }
        }
        else
        {
            i++;
            if(i == M_PEDAL_DIVIDER)
            {
                i = 0;
                lookuptable_pedaling(c);
                c++;
                if(c > FIRST_POS && c < (FIRST_POS + TOT_STOP_POS)){c = FIRST_POS;}
                else if(c >= TOT_FOOT_POS){c = 0;}
            }
        }
    }
}

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;
}