File content as of revision 10:3b952ea7fad4:

#include "mbed.h"
#include "SDFileSystem.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 |
//out|PC_2  | PC_1 A4|   ||TX2|PA_2  D1  | NC   |
//out|PC_3  | PC_0 A5|IR ||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;

DigitalOut pin_pc2(PC_2);
DigitalOut pin_pc3(PC_3);

Serial pc(D1, D0);
Serial BT(PC_6, PA_12);

SDFileSystem SD_card(PC_12, PC_11, PC_10, PD_2, "sd");  //mosi, miso, sck, cs, name

//**** 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 *****//
bool interrupt = 0;
char BTCharR = 0;

float Vx = 0.0;
float Vx_old = 0.0;

unsigned char fall_down = 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;

FILE *dataPtr;
//********************************//

bool resetting = 0;

void timer1_interrupt(void);
void timer2_interrupt(void);
void fprintf_data(unsigned char mode);
void reset_offset(void);
void ready_to_go(void);
void attimeout(void);

int main() {
    pin_pc2 = pin_pc3 = 0;
    
    setupServo();
    pc.baud(115200);
    BT.baud(115200);
    if(setup_spi_sensor())
    {
        interrupt = 0;
        pc.printf("SPI sensor ready.\r\n");
    }
    else interrupt = 1;
    init_Sensors();
    
    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)
    
//    mkdir("/sd/20160718", 0777);
//    dataPtr = fopen("/sd/20160718/data.dat", "w");
    dataPtr = fopen("/sd/data.dat", "w");
    if(dataPtr == NULL) {
        error("Could not open file for write\r\n");
        interrupt = 1;
    }
    fprintf_data(0);
    pc.printf("System ready.\r\n");
    
    reset_pos();
    lookuptable_steering(HANDLE_START);
//    lookuptable_steering(0);
    
    while(!interrupt)
    {
        if(BT.readable())
        {
            BTCharR = BT.getc();
            switch(BTCharR)
            {
                case 's': //reset_offset();
                          ready_to_go();
                          pc.printf("go\r\n");
                          pedal_state = 1; sys_state = L_PD; gamma_ref = 0.0;
                          s = 1; break;        ///start
                case 'a': pc.printf("stop\r\n");
                          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 '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;   ///calibrate
                case 'h': fclose(dataPtr); interrupt = 1; break;     ///halt
                default: break;
            }
            BTCharR = 0;
        }
        
        sensorG_read_3axis();
        sensorX_read_3axis();
        
        get_9axis_scale();
        get_9axis_data(pedal_state);
        
        L_inver = 0.0063f * V_meas - 0.005769f;
    }
}

void timer1_interrupt(void)
{
//    pin_pc2 = !pin_pc2;
    V_meas = (analog_value.read() * 3.3f) * IR_FILT_CONST + V_meas * (1.0f - IR_FILT_CONST);
    
//    pc.printf("%d\t%d\t%d\r\n", filted_sensor_data(INDEX_ACCE_X, 1.8), filted_sensor_data(INDEX_ACCE_Y, 1.8), filted_sensor_data(INDEX_ACCE_Z, 1.8));
    
    roll_fusion(axm,aym,azm,u3,u1,1);
    x1_hat_estimat(axm,aym,azm,u3,u2,Alpha);
    m_x1_hat(mx,my,mz,u3,u2,Alpha);
    m_x2_hat(mx,my,mz,u3,u1,Alpha);
    
    if(cosroll != 0)
    {
        if(roll_angle >= 0.1745f || roll_angle <= -0.1745f)fall_down = 1;
        else fall_down = 0;
        
        droll_angle = lpf(u1, droll_angle_old, 62.8);
        droll_angle_old = droll_angle;
        dyaw_angle = lpf(u3/cosroll, dyaw_angle_old, 3.1416);
        dyaw_angle_old = dyaw_angle;
//        m_yaw_fusion();
        
        ///cut off
        if(m_sinyaw <= -1.0f)m_sinyaw = -1.0f;
        else if(m_sinyaw >= 1.0f)m_sinyaw = 1.0f;
        else ;
        if(m_cosyaw <= -1.0f)m_cosyaw = -1.0f;
        else if(m_cosyaw >= 1.0f)m_cosyaw = 1.0f;
        else ;
        
        ///Controller
        if(sys_state == L_PD)
        {
            calc_PD(K_l, 0.0);
            calc_Gamma(u,15,b_h);
            gamma_rad = gamma_rad + L_PD_OFFSET;
            
            dphi_hat = 0.0;
            phi_hat = 0.0;
//            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
//            yaw_angle_old = yaw_angle;
        }
        else if(sys_state == M_PD)
        {
            calc_PD(K_m, 0.0);
            calc_Gamma(u,15,b_h);

            dphi_hat = 0.0;
            phi_hat = 0.0;
//            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
//            yaw_angle_old = yaw_angle;

            count2ztc_m++;
            if(count2ztc_m >= CNT2ZTCm){sys_state = M_ZTC;}
        }
        else if(sys_state == M_ZTC)
        {
            if(count2straight < CNT2STRT){count2straight++;}
            else{gamma_ref = 0.0; roll_ref = 0.0;}
            calc_PD(K_m, phi_hat);
            calc_Phi(K_mphi);
            calc_Gamma(u,15,b_h);

//            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
//            yaw_angle_old = yaw_angle;
        }
        else if(sys_state == H_PD)
        {
            calc_PD(K_h,0.0);
            calc_Gamma(u,15,b_h);

            dphi_hat = 0.0;
            phi_hat = 0.0;
//            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
//            yaw_angle_old = yaw_angle;

//            count2ztc_h++;
            if(count2ztc_h >= CNT2ZTCh){sys_state = H_ZTC;}
        }
        else if(sys_state == H_ZTC)
        {
            if(count2straight < CNT2STRT){count2straight++;}
            else{gamma_ref = 0.0; roll_ref = 0.0;}
            calc_PD(K_h, phi_hat);
            calc_Phi(K_hphi);
            calc_Gamma(u,15,b_h);

//            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
//            yaw_angle_old = yaw_angle;
        }
        else
        {
            u = 0.0;
//            calc_PD(K_l, 0.0);
            calc_Gamma(u,15,b_h);
            dphi_hat = 0.0;
            phi_hat = 0.0;
            yaw_angle = 0.0;
            yaw_angle_old = 0.0;
        }
        
        ///Anti-Windup
        anti_wdup();

//        calc_Gamma(u,15,b_h);
        if(gamma_rad > 0.349f)gamma_rad = 0.349f;
        else if(gamma_rad < -0.349f)gamma_rad = -0.349f;
        else ;
//        printf("%d   Roll:%9.6f   dRoll:%9.6f   u:%9.6f   phi_hat:%9.6f   gamma:%9.6f\r\n", sys_state, roll_angle, droll_angle, u, phi_hat, gamma_rad);

        ///Show results or Send datas
        gamma_degree = (short int)(gamma_rad*114.59f);
        if(gamma_degree > 40)gamma_degree = 40;
        else if(gamma_degree < -40)gamma_degree = -40;
        else ;
        
        ///Steering
        if(s == 1)
        {
            if(state_count > COUN2_HANDLE_START)
            {
                lookuptable_steering( gamma_degree );
            }
            else
            {
                lookuptable_steering( gamma_degree + HANDLE_START_BAL);
            }
        }
        else
        {
            if(c == FIRST_POS)
            {
                if(state_change > 0)
                {
                    lookuptable_steering(HANDLE_STOP);
                }
                else
                {
                    if(!resetting)
                    {
                        lookuptable_steering(HANDLE_START);
//                        lookuptable_steering(HANDLE_STRAIGHT);
//                        lookuptable_steering( gamma_degree );
                    }
                    else ;
                }
            }
            else  lookuptable_steering( gamma_degree + HANDLE_START_BAL);
        }
        
        ///print file
        if(sys_state >= 1)
        {
            count_isr++;
            T_total = (float)sample_time*count_isr;

            ///Integrate Ax to get Vx
//            Vx = Vx_old - axm_f*sample_time;
//            Vx_old = Vx;
//            if(Vx >= v_low && sys_state < M_PD){pedal_state = 2; sys_state = M_PD; gamma_ref = 0.0;}

            fprintf_data(1);
        }
    }
    else
    {
        roll_fusion(0,0,0,0,0,1);
        gamma_degree = 0;
    }
}

void timer2_interrupt(void)
{
//    pin_pc3 = !pin_pc3;
    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("m\r\n");
//                BT.printf("m\r\n");
                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 fprintf_data(unsigned char mode)
{
    if(mode == 0)
    {
        fprintf(dataPtr, "(not weighted)\nK_l: %f, %f\nK_m: %f, %f\nK_phi: %f, %f, %f\n", K_l[0], K_l[1], K_m[0], K_m[1], K_mphi[0], K_mphi[1], K_mphi[2]);
        fprintf(dataPtr, "Gyro offset: %d %d %d\n", Gyro_axis_zero[0], Gyro_axis_zero[1], Gyro_axis_zero[2]);
        fprintf(dataPtr, "Acce offset: %d %d %d\n", Acce_axis_zero[0], Acce_axis_zero[1], Acce_axis_zero[2]);
        fprintf(dataPtr, "Magn offset: %d %d %d\n", Magn_axis_zero[0], Magn_axis_zero[1], Magn_axis_zero[2]);
        fprintf(dataPtr, "sys_state\t");
        fprintf(dataPtr, "time\t");
        fprintf(dataPtr, "fall\t");
        fprintf(dataPtr, "Roll\t");
//        fprintf(dataPtr, "dRoll\t");
//        fprintf(dataPtr, "dPhi_hat\t");
        fprintf(dataPtr, "Phi_hat\t");
        fprintf(dataPtr, "dYaw\t");
//        fprintf(dataPtr, "Yaw\t");
//        fprintf(dataPtr, "Pedal_step\t");
//        fprintf(dataPtr, "Ctrl_out\t");
        fprintf(dataPtr, "Gamma_ref\t");
        fprintf(dataPtr, "Gamma_rad\n");
//        fprintf(dataPtr, "Speed-X\n");
    }
    else
    {
        fprintf(dataPtr, "%d\t", sys_state);
        fprintf(dataPtr, "%f\t", T_total);
        fprintf(dataPtr, "%d\t", fall_down);
        fprintf(dataPtr, "%f\t", roll_angle);
//        fprintf(dataPtr, "%f\t", droll_angle);
//        fprintf(dataPtr, "%f\t", dphi_hat);
        fprintf(dataPtr, "%f\t", phi_hat);
        fprintf(dataPtr, "%f\t", dyaw_angle);
//        fprintf(dataPtr, "%f\t", yaw_angle);
//        fprintf(dataPtr, "%d\t", pedal_step);
//        fprintf(dataPtr, "%f\t", u);
        fprintf(dataPtr, "%f\t", gamma_ref);
        fprintf(dataPtr, "%f\n", gamma_rad);
//        fprintf(dataPtr, "%f\n", Vx);
    }
}

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 ready_to_go(void)
{
    resetting = 1;
    lookuptable_pedaling(FIRST_POS);
    lookuptable_steering(HANDLE_STRAIGHT);
    timeout.attach(&attimeout, 1.0f);
    while(resetting)
    {
        wait_ms(100);
    }
    timeout.detach();
}

void attimeout(void)
{
    resetting = 0;
}
//void attimeout2(void)
//{
//    preparing = 0;
//}