svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
common.h
- Committer:
- dima285
- Date:
- 2019-07-23
- Branch:
- svoe
- Revision:
- 22:14e85f2068c7
- Parent:
- 20:e73f49ba5001
- Child:
- 23:bc05a104be10
File content as of revision 22:14e85f2068c7:
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 = 4;//4 float balance_int = 5; float balance_integral; float x_prop = 45;//100; float x_diff = 45;//100; float x_int = 8;//3 float x_integral; float azimuth_prop = 40; float azimuth_diff = 10;//15; float coord_accuracy = 0.04; 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}}; float traj_1D [12] = {0,0.05,0,0.1,0,0.2,0,0.5,0,1,0,2}; bool motion_flag; int delay_counter;//задержка между шагами сервы int serva_counter; int delay2_counter; int fall_timer; //int motion_timeout; int motion_mode; int timeout_counter; #define STOP 0 #define GO 2 #define ROTATE 1