File content as of revision 20:e73f49ba5001:

Serial wifi(PA_9, PA_10);
Serial sound_out(PB_10, PB_11);
Serial sound_in(PA_2, PA_3);
VL53L1X glaz(PB_9,PB_8);
//USBSerial usb_serial(0x1f00, 0x2012, 0x0001,  false);
DigitalOut myled(LED1);
DigitalOut test(PB_15); //Test pin for diagnostics and debugging

int volatile proc_counter; //(3e5 - 2e6 idle, 2e5 - 5e5 at motion) / per 50 mS

float const t_step = 0.02;//SI realtime step
float time_sec;

float const g = 9.81;//SI
float const pi = 3.1415926535;
float const r_wheel = 0.038;//SI
float const center_mas = 0.022;//SI (ot osi)
float const mass = 1.3;//SI
float const m_inert = 5e-3;//SI
float const half_axis = 0.08;//SI
float const ppr = 3200;//pulses per revolution

struct coord{
    float x; //m //vpered
    float y; //m //vpravo
    float azimuth; // rad [-pi;+pi]
    float speed; //m/s
    float path; //m
    float omega; //rad/s  
    float accel;  
    float eps;
    bool dir;
};

coord current;
coord target;
coord delta;//odin raz i vse (v analize obstacle)
coord max;
coord theor;

//smotri da,max v system.h
float da;

float balance_prop = 1.2;
float balance_diff = 7;
float x_prop = 55;//45;
float x_diff = 55;//50;
float azimuth_prop = 40;
float azimuth_diff = 10;//15;

float coord_accuracy = 0.15;
bool coord_ready;

bool fall_flag;

float trajectory [10][3] = {{0,0,0},{0.2,0.2,pi/2},{0,0.4,pi},{-0.2,0.2,-pi/2},{0,0},{0.2,-0.2,-pi/2},{0,-0.4,-pi},{-0.2,-0.2,pi/2},{0,0,0}};

bool motion_flag;
int delay_counter;
int serva_counter;
int delay2_counter;
int fall_timer;
int motion_timeout;
int motion_mode;
#define STOP 0
#define GO 2
#define ROTATE 1