common.h

00001 Serial wifi(PA_9, PA_10);
00002 Serial sound_out(PB_10, PB_11);
00003 Serial sound_in(PA_2, PA_3);
00004 VL53L1X glaz(PB_9,PB_8);
00005 //USBSerial usb_serial(0x1f00, 0x2012, 0x0001,  false);
00006 DigitalOut myled(LED1);
00007 DigitalOut test(PB_15); //Test pin for diagnostics and debugging
00008 
00009 int volatile proc_counter; //(3e5 - 2e6 idle, 2e5 - 5e5 at motion) / per 50 mS
00010 
00011 float const t_step = 0.02;//SI realtime step
00012 float time_sec;
00013 
00014 float const g = 9.81;//SI
00015 float const pi = 3.1415926535;
00016 float const r_wheel = 0.038;//SI
00017 float const center_mas = 0.022;//SI (ot osi)
00018 float const mass = 1.3;//SI
00019 float const m_inert = 5e-3;//SI
00020 float const half_axis = 0.08;//SI
00021 float const ppr = 3200;//pulses per revolution
00022 float const deg5_rad = 0.08726646; //5/360*2*pi; //5 gradusov v radianah
00023 float const pi2_byte = 40.743665431; //256/2/pi;
00024 
00025 struct coord{
00026     float x; //m //vpered
00027     float y; //m //vpravo
00028     float azimuth; // rad [-pi;+pi]
00029     float speed; //m/s
00030     float path; //m
00031     float omega; //rad/s  
00032     float accel;  
00033     float eps;
00034     bool dir;
00035     int echo_cm;
00036     float echo_azimuth;
00037 };
00038 
00039 coord current;
00040 coord target;
00041 coord delta;//odin raz i vse (v analize obstacle)
00042 coord max;
00043 coord theor;
00044 
00045 //smotri da,max v system.h
00046 float da;
00047 
00048 float balance_prop = 1.2;
00049 float balance_diff = 4;//4
00050 float balance_int = 5;
00051 float balance_integral;
00052 float x_prop = 45;//100;
00053 float x_diff = 45;//100;
00054 float x_int = 8;//3
00055 float x_integral;
00056 
00057 float azimuth_prop = 40;
00058 float azimuth_diff = 10;//15;
00059 
00060 float coord_accuracy = 0.04;
00061 bool coord_ready;
00062 
00063 bool fall_flag;
00064 
00065 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}};
00066 float traj_1D [12] = {0,0.05,0,0.1,0,0.2,0,0.5,0,1,0,2};
00067 
00068 bool motion_flag;
00069 int delay_counter;//задержка между шагами сервы
00070 int serva_counter;
00071 int delay2_counter;
00072 int fall_timer;
00073 //int motion_timeout;
00074 int motion_mode;
00075 int timeout_counter;
00076 #define STOP 0
00077 #define GO 2
00078 #define ROTATE 1
00079 
00080 bool scan_360_flag;
00081 int scan_360_counter;
00082 int array_360_cm[100]; //full scan array
00083 float array_360_rad[100];
00084 int echo_delay_counter;
00085