svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
common.h@22:14e85f2068c7, 2019-07-23 (annotated)
- Committer:
- dima285
- Date:
- Tue Jul 23 12:55:23 2019 +0000
- Branch:
- svoe
- Revision:
- 22:14e85f2068c7
- Parent:
- 20:e73f49ba5001
- Child:
- 23:bc05a104be10
23.07.2019
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Stas285 | 1:e2a6e523bf1f | 1 | Serial wifi(PA_9, PA_10); |
Stas285 | 15:960b922433d1 | 2 | Serial sound_out(PB_10, PB_11); |
Stas285 | 15:960b922433d1 | 3 | Serial sound_in(PA_2, PA_3); |
dima285 | 17:bd6b6ac89e0e | 4 | VL53L1X glaz(PB_9,PB_8); |
Stas285 | 6:6e89cdc3db92 | 5 | //USBSerial usb_serial(0x1f00, 0x2012, 0x0001, false); |
dima285 | 19:2fe650d29823 | 6 | DigitalOut myled(LED1); |
Stas285 | 1:e2a6e523bf1f | 7 | DigitalOut test(PB_15); //Test pin for diagnostics and debugging |
dima285 | 19:2fe650d29823 | 8 | |
Stas285 | 1:e2a6e523bf1f | 9 | int volatile proc_counter; //(3e5 - 2e6 idle, 2e5 - 5e5 at motion) / per 50 mS |
dima285 | 9:8f98b1c277a4 | 10 | |
dima285 | 12:721a9ea55e91 | 11 | float const t_step = 0.02;//SI realtime step |
Stas285 | 15:960b922433d1 | 12 | float time_sec; |
dima285 | 12:721a9ea55e91 | 13 | |
Stas285 | 15:960b922433d1 | 14 | float const g = 9.81;//SI |
Stas285 | 15:960b922433d1 | 15 | float const pi = 3.1415926535; |
dima285 | 12:721a9ea55e91 | 16 | float const r_wheel = 0.038;//SI |
dima285 | 12:721a9ea55e91 | 17 | float const center_mas = 0.022;//SI (ot osi) |
dima285 | 12:721a9ea55e91 | 18 | float const mass = 1.3;//SI |
dima285 | 12:721a9ea55e91 | 19 | float const m_inert = 5e-3;//SI |
dima285 | 12:721a9ea55e91 | 20 | float const half_axis = 0.08;//SI |
dima285 | 12:721a9ea55e91 | 21 | float const ppr = 3200;//pulses per revolution |
dima285 | 12:721a9ea55e91 | 22 | |
dima285 | 12:721a9ea55e91 | 23 | struct coord{ |
Stas285 | 15:960b922433d1 | 24 | float x; //m //vpered |
Stas285 | 15:960b922433d1 | 25 | float y; //m //vpravo |
Stas285 | 15:960b922433d1 | 26 | float azimuth; // rad [-pi;+pi] |
Stas285 | 15:960b922433d1 | 27 | float speed; //m/s |
Stas285 | 15:960b922433d1 | 28 | float path; //m |
dima285 | 20:e73f49ba5001 | 29 | float omega; //rad/s |
dima285 | 20:e73f49ba5001 | 30 | float accel; |
dima285 | 20:e73f49ba5001 | 31 | float eps; |
dima285 | 20:e73f49ba5001 | 32 | bool dir; |
dima285 | 12:721a9ea55e91 | 33 | }; |
dima285 | 12:721a9ea55e91 | 34 | |
dima285 | 12:721a9ea55e91 | 35 | coord current; |
dima285 | 12:721a9ea55e91 | 36 | coord target; |
dima285 | 20:e73f49ba5001 | 37 | coord delta;//odin raz i vse (v analize obstacle) |
dima285 | 20:e73f49ba5001 | 38 | coord max; |
dima285 | 20:e73f49ba5001 | 39 | coord theor; |
dima285 | 12:721a9ea55e91 | 40 | |
dima285 | 20:e73f49ba5001 | 41 | //smotri da,max v system.h |
dima285 | 20:e73f49ba5001 | 42 | float da; |
dima285 | 20:e73f49ba5001 | 43 | |
dima285 | 20:e73f49ba5001 | 44 | float balance_prop = 1.2; |
dima285 | 22:14e85f2068c7 | 45 | float balance_diff = 4;//4 |
dima285 | 22:14e85f2068c7 | 46 | float balance_int = 5; |
dima285 | 22:14e85f2068c7 | 47 | float balance_integral; |
dima285 | 22:14e85f2068c7 | 48 | float x_prop = 45;//100; |
dima285 | 22:14e85f2068c7 | 49 | float x_diff = 45;//100; |
dima285 | 22:14e85f2068c7 | 50 | float x_int = 8;//3 |
dima285 | 22:14e85f2068c7 | 51 | float x_integral; |
dima285 | 22:14e85f2068c7 | 52 | |
dima285 | 20:e73f49ba5001 | 53 | float azimuth_prop = 40; |
Stas285 | 15:960b922433d1 | 54 | float azimuth_diff = 10;//15; |
dima285 | 12:721a9ea55e91 | 55 | |
dima285 | 22:14e85f2068c7 | 56 | float coord_accuracy = 0.04; |
Stas285 | 15:960b922433d1 | 57 | bool coord_ready; |
dima285 | 13:789b451cc27d | 58 | |
Stas285 | 15:960b922433d1 | 59 | bool fall_flag; |
dima285 | 13:789b451cc27d | 60 | |
Stas285 | 15:960b922433d1 | 61 | 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}}; |
dima285 | 22:14e85f2068c7 | 62 | float traj_1D [12] = {0,0.05,0,0.1,0,0.2,0,0.5,0,1,0,2}; |
Stas285 | 15:960b922433d1 | 63 | |
dima285 | 17:bd6b6ac89e0e | 64 | bool motion_flag; |
dima285 | 22:14e85f2068c7 | 65 | int delay_counter;//задержка между шагами сервы |
dima285 | 17:bd6b6ac89e0e | 66 | int serva_counter; |
dima285 | 19:2fe650d29823 | 67 | int delay2_counter; |
dima285 | 19:2fe650d29823 | 68 | int fall_timer; |
dima285 | 22:14e85f2068c7 | 69 | //int motion_timeout; |
dima285 | 19:2fe650d29823 | 70 | int motion_mode; |
dima285 | 22:14e85f2068c7 | 71 | int timeout_counter; |
dima285 | 20:e73f49ba5001 | 72 | #define STOP 0 |
dima285 | 20:e73f49ba5001 | 73 | #define GO 2 |
dima285 | 20:e73f49ba5001 | 74 | #define ROTATE 1 |
dima285 | 20:e73f49ba5001 | 75 | |
dima285 | 20:e73f49ba5001 | 76 |