svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
common.h@19:2fe650d29823, 2019-02-24 (annotated)
- Committer:
- dima285
- Date:
- Sun Feb 24 11:02:56 2019 +0000
- Revision:
- 19:2fe650d29823
- Parent:
- 17:bd6b6ac89e0e
- Child:
- 20:e73f49ba5001
3 PID
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 |
Stas285 | 15:960b922433d1 | 29 | float omega; //rad/s |
dima285 | 12:721a9ea55e91 | 30 | }; |
dima285 | 12:721a9ea55e91 | 31 | |
dima285 | 12:721a9ea55e91 | 32 | coord current; |
dima285 | 12:721a9ea55e91 | 33 | coord target; |
dima285 | 12:721a9ea55e91 | 34 | |
dima285 | 19:2fe650d29823 | 35 | float balance_prop = 1.5; |
dima285 | 13:789b451cc27d | 36 | float balance_diff = 7; |
Stas285 | 15:960b922433d1 | 37 | float x_prop = 45;//45; |
dima285 | 19:2fe650d29823 | 38 | float x_diff = 45;//37; |
Stas285 | 15:960b922433d1 | 39 | float azimuth_prop = 40;//50; |
Stas285 | 15:960b922433d1 | 40 | float azimuth_diff = 10;//15; |
dima285 | 12:721a9ea55e91 | 41 | |
dima285 | 19:2fe650d29823 | 42 | float coord_accuracy = 0.15; |
Stas285 | 15:960b922433d1 | 43 | bool coord_ready; |
dima285 | 13:789b451cc27d | 44 | |
Stas285 | 15:960b922433d1 | 45 | bool fall_flag; |
dima285 | 13:789b451cc27d | 46 | |
Stas285 | 15:960b922433d1 | 47 | 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}}; |
Stas285 | 15:960b922433d1 | 48 | |
dima285 | 17:bd6b6ac89e0e | 49 | bool motion_flag; |
dima285 | 17:bd6b6ac89e0e | 50 | int delay_counter; |
dima285 | 17:bd6b6ac89e0e | 51 | int serva_counter; |
dima285 | 19:2fe650d29823 | 52 | int delay2_counter; |
dima285 | 19:2fe650d29823 | 53 | int fall_timer; |
dima285 | 19:2fe650d29823 | 54 | int motion_timeout; |
dima285 | 19:2fe650d29823 | 55 | int motion_mode; |
dima285 | 19:2fe650d29823 | 56 | #define stop 0; |
dima285 | 19:2fe650d29823 | 57 | #define go 1; |
dima285 | 19:2fe650d29823 | 58 | #define rotate 2; |