File content as of revision 0:1faa9570d725:

#include "mbed.h"
#include "Motor.h"
#include "MPU9250.h"
#include "stepperMotor.h"

#include "PID.h"

#define INTERVAL 0.1
#define Lx 0.0
#define Ku 26.2//0.922000
#define Pu 20.8
#define Kp Ku*0.182
#define Ti Pu*0.55
#define Td Pu*0.35
#define P Kp
#define I Kp/Ti
#define D Kp*Td
#include<esp8266.cpp>

//DigitalOut myled(LED1);
Serial esp(dp16,dp15);

Ticker tick;
SPI spi(dp2,dp1,dp6);//(A6, A5, A4);//(p5,p6,p7)
mpu9250_spi imu(spi,dp26);//,A3);//,p8)   //define the mpu9250 object
Timer pi;
PID pid1(P,I,D,&pi);
PID pid2(P,I,D,&pi);
DigitalOut led1(dp28);
void init();

esp8266 ESP;
Stepper wheel1(dp13,dp14);//(D11,D12);//dp10,11

Stepper wheel2(dp10,dp9);//(D11,D12);//dp10,11
#define k1 3.8
#define k2 1.6
#define k3 0.1
#define k4 0.1

float st1()
{
    float s1 = wheel1.pulse * 7.5;
    
    static float prev_s2 = 0;
    float s2 = (s1 - prev_s2)/INTERVAL;
    static float prev = 0;
    float s5 = (s2 - prev)/INTERVAL;
    static float prev_s3 =  0;
    float s3 =  imu.kalman_angle[2];
    float s4 = (s3 - prev_s3)/INTERVAL;
    prev_s2=s1;
    prev_s3=s3;
    prev = s2;
    //printf("%f,%f,%f,%f,%f",k1*s5,k2*s2,k3*s3,k4*s4,k1*s1+k2*s2+k3*s3+k4*s4);
    printf(end);
    return k1*s3+k2*s4+k3*s2+k4*s1;
    
}
#define k12 k1
#define k22 k2
#define k32 k3
#define k42 k4
float st2()
{
    float s1 = wheel2.pulse * 7.5;
    
    static float prev_s2 = 0;
    float s2 = (s1 - prev_s2)/INTERVAL;
    static float prev = 0;
    float s5 = (s2 - prev)/INTERVAL;
    static float prev_s3 =  0;
    float s3 =  imu.kalman_angle[2];
    float s4 = (s3 - prev_s3)/INTERVAL;
    prev_s2=s1;
    prev = s2;
    prev_s3=s3;
    //printf("%f,%f,%f,%f,%f",k1*s5,k2*s2,k3*s3,k4*s4,k1*s1+k2*s2+k3*s3+k4*s4);
    printf(end);
    return k1*s3+k2*s4+k3*s2+k4*s1;
    
}
int main() {
    //while(1);
    //printf("st");
    /*DigitalOut a(dp9);
    DigitalOut b(dp10);
    while(1)
    {
        a=1;
        b=0;
        wait(0.5);
        a=1;
        b=1;
        wait(0.5);
        a=0;
        b=1;
        wait(0.5);
        a=0;
        b=0;
        wait(0.5);
        printf("rotate");
        printf(end);
    }*/
    
    init();
    
    led1 = 1;
    wheel1.start();
    wheel2.start();
    while(1) {
        double data[3];
        imu.read_all();
        imu.set_angle();
        imu.get_angle_acc();
        imu.get_angle(&data[0],&data[1],&data[2]);
        //printf("%10.3lf, %10.3lf, %10.3f\n",
        //          imu.kalman_angle[2],imu.angle[2],imu.angle_acc[2]);
        if(abs(imu.kalman_angle[2]) < 50)
        {
            
            pid1.dPoint = imu.kalman_angle[2];
            pid2.dPoint = imu.kalman_angle[2];
            pid1.PIDctrl();
            pid2.PIDctrl();
            float d1 = pid1.data , d2 = pid2.data;
            printf("%10.3f , %10.3f \n" , d1,d2);
            wheel1 = -st1() / 255; 
            wheel2 = -st2() / 255;        
        }
        else
        {
            pid1.pid_reset();
            pid2.pid_reset();
            wheel1 = 0;
            wheel2 = 0;
            wheel1.pulse = 0;
            wheel2.pulse = 0;
        }/*
        if(imu.kalman_angle[1]<0)
        {
            wheel1 = 1;
            wait(0.5);
            wheel1 = -1;
                        wait(0.1);
        }
        else if(imu.kalman_angle[1]>0)
        {
            wheel1 = -1;
            wait(0.5);
            wheel1 = 1;
            wait(0.1);            
        }*/
        wait(INTERVAL);
    }
}

void init()
{
    //pc.baud(230400);
    
    esp.baud(115200);
    ESP.setSerial(&esp);
    led1=0;
    wait(1);
    for(int i= 0 ; i<6;i++){led1=1;wait(0.05);led1=0;wait(0.05);}
    wait(1);
    /*while(1)
    {
        int i = esp.getc();
        if(i == 'A')
        {
            led1 = 1;
            wait(0.5);
            esp.putc(i);
        }
        if(i == 'B')
        {
            led1 = 0;
            wait(0.5);
            esp.putc(i);
        }
    }*/
    //printf("sart!!");
    /*while(ESP.ATcommand(""))
    {
        wait(2);
        printf("esp8266 is no connection\n");
    }*/
    //ESP.wifiConnectionMode(2);
    //ESP.wifiSoftAPSetting("ESP8266","ta",5,3);
    
    //ESP.WifiFindAccesspoint();
    //myled = 1;
    /*
    do
    {
        wait(1);
        //printf("Finding ESP8266");
    }while(ESP.WifiFindAccesspoint("ESP8366"));
    while(ESP.wifiConnect("ESP8366","ryuutaka"))
    {
        wait(1);
//        printf("connecting to ESP8266\n");
    }*/
    //printf("esp8266 is acyivity\n");
    double g[3];
    //Thread thread2(GET);
    if(imu.init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250
        //printf("\nCouldn't initialize MPU9250 via SPI!");
    }
    char x = imu.whoami();
    do {
        x = imu.whoami();
        printf("WHOAMI=0x%2x\n",x); //output the I2C address to know if SPI is working, it should be 104
        wait(0.3);
    } while(x!=0x71);
    //printf("Gyro_scale=%u\n",);    //Set full scale range for gyros
    imu.set_gyro_scale(BITS_FS_2000DPS);
    wait(0.3);
    //printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
    imu.set_acc_scale(BITS_FS_16G);
    wait(0.3);
    do {
        x=imu.AK8963_whoami();
        printf("AK8963 WHIAM=0x%2x\n",x);
        wait(0.3);
    } while(x!=0x48);
    imu.MPU_setup();
    wait(0.1);
    pid1.OutputLimits(255,-255);
    pid1.start();
    pid1.dTarget = 0;
    pid2.OutputLimits(255,-255);
    pid2.start();
    pid2.dTarget = 0;
}