File content as of revision 1:ac613232f130:

#include "mbed.h"
#include "math.h"
#include "Stepper.h"
#include "MPU6050.h"
#include "PID.h"
#include "ESP8266Interface.h"
#include "TCPSocketConnection.h"
#include "TCPSocketServer.h"
#include "Websocket.h"

//------------------------------------
// Terminal configuration
// 9600 bauds, 8-bit data, no parity
//------------------------------------
//      PINOUT DESCRIPTION
// MOTORS OUTPUT 
// LEFT: EN-A1 STEP-A0 DIR-D13
// RIGHT: EN-D12 STEP-D11 DIR-D10
//
// IMU MPU MPU6050
// SDA-D4 SCL-D5
//
// DISTANCE SENSORS
// D6, D3
// D6 - temporary ESP8266 RESET
//
// ESP8266 WIFI MODULE
// TX-D1 RX-D2 EN-D7
//
// WS2812B LED 
// D2 - digital output
// -----------------------------------

//To Do
//2 REGULATORS 


Serial pc(SERIAL_TX, SERIAL_RX);
stepper left(A1,A0,D13); 
stepper right(D12,D11,D10); 
DigitalOut led1(LED1);
DigitalOut wifiEnable(D7,1);
AnalogIn shfront(D3); 
MPU6050 imu; 
Ticker loop;
//PID controll(120.0,0.004,0.0007,0.01);
PID controll(0.01);
ESP8266Interface wifi(D1,D0,D6,"WLAN_Staszic","st@szicwifi5",115200);

int i; 
float pitch;
float roll;
float frontdistance; 
double speed; 
double Stepper_speed;
double Target_speed=0;//6.0(it is a good paraments with 6.0!! very good! nearly to be static !);//0.0;
char str[10];


void pid_thread() {
    
        imu.complementaryFilter(&pitch, &roll);
        controll.setProcessValue(pitch); 
        speed = controll.compute(); 
      //  Stepper_speed=controll.Speedcompute(speed,Target_speed);        
        left.step(speed); 
        right.step(-speed);
        
        Stepper_speed=controll.Speedcompute(speed,Target_speed); 
        left.step(Stepper_speed); 
        right.step(-Stepper_speed);
}


int main() {

    left.disable(); 
    right.disable();

    imu.init();   
    imu.calibrate(&pitch,&roll);
    
    //wifi.init(); //Reset
    //wifi.connect(); //Use DHCP
    
    //Websocket ws("ws://192.168.0.103:1234/ws");
    
    //Check for successful socket connection
    //if(!ws.connect())ws.close();
    //else{
        
    left.enable(); 
    right.enable(); //wstawaine
    
    //left.step(3000); 
    //right.step(-3000);
    wait(1);
    wait(1); 
    wait(1);  
    left.step(-3000); 
    right.step(3000);
    wait(0.7); 

    loop.attach_us(pid_thread,10000); 
    controll.setSetPoint(-14.1);
        while(1) {
            // string with a message
            //sprintf(str,"%f",pitch);
            //ws.send(str);
        
            //memset(str,0,10);
            //wait(0.5);
            //if (ws.read(str)) {
            //    printf("rcv'd: %s\n\r", str);
            //}
        }
        
    //}
    //ws.close();

}