svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
realtime.h@15:960b922433d1, 2018-12-01 (annotated)
- Committer:
- Stas285
- Date:
- Sat Dec 01 14:25:04 2018 +0000
- Revision:
- 15:960b922433d1
- Parent:
- 14:e12d0fdc3a48
- Child:
- 17:bd6b6ac89e0e
coord_ready
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Stas285 | 0:e9488589a8ee | 1 | Ticker rt_ticker; |
Stas285 | 4:904b737ef08a | 2 | bool realtime_flag; |
Stas285 | 0:e9488589a8ee | 3 | |
Stas285 | 15:960b922433d1 | 4 | void balance_coord(){ |
Stas285 | 15:960b922433d1 | 5 | float delta_x = (target.x - current.x); if (delta_x == 0) delta_x = 0.0001; |
dima285 | 13:789b451cc27d | 6 | float delta_y = (target.y - current.y); |
Stas285 | 15:960b922433d1 | 7 | float delta_s = sqrt(delta_x*delta_x + delta_y*delta_y); //always positive //if (delta_x < 0) {delta_s = -delta_s;} |
Stas285 | 15:960b922433d1 | 8 | float azimuth_to_target = atan(delta_y/delta_x); if (delta_x < 0){if (delta_y > 0) azimuth_to_target += pi; else azimuth_to_target -= pi;} |
dima285 | 13:789b451cc27d | 9 | |
Stas285 | 15:960b922433d1 | 10 | float delta_phi_0 = azimuth_to_target - current.azimuth; //azimuth: target error |
Stas285 | 15:960b922433d1 | 11 | if (delta_phi_0 < -pi) delta_phi_0 += 2*pi; if (delta_phi_0 > pi) delta_phi_0 -= 2*pi; |
Stas285 | 15:960b922433d1 | 12 | |
Stas285 | 15:960b922433d1 | 13 | float normal_error = delta_s * sin(delta_phi_0); |
Stas285 | 15:960b922433d1 | 14 | float tan_error = delta_s * cos(delta_phi_0); |
dima285 | 12:721a9ea55e91 | 15 | |
Stas285 | 15:960b922433d1 | 16 | float a = x_prop * tan_error - x_diff * current.speed; //Motion PID |
Stas285 | 15:960b922433d1 | 17 | if ((current.speed > max_speed) && (a > 0)) a = 0 ; if ((current.speed < -max_speed) && (a < 0)) a = 0 ; |
dima285 | 13:789b451cc27d | 18 | if (a > max_accel) a = max_accel ; if (a < -max_accel) a = -max_accel; |
Stas285 | 15:960b922433d1 | 19 | |
Stas285 | 15:960b922433d1 | 20 | float delta_v = (balance_prop*ax + balance_diff * gy + a) * t_step; //Balance PID |
dima285 | 13:789b451cc27d | 21 | |
Stas285 | 15:960b922433d1 | 22 | float delta_phi_1 = (target.azimuth - current.azimuth);//* (current.speed * t_step / delta_s); //azimuth: sever error |
Stas285 | 15:960b922433d1 | 23 | if (delta_phi_1 < -pi) delta_phi_1 += 2*pi; if (delta_phi_1 > pi) delta_phi_1 -= 2*pi; |
Stas285 | 15:960b922433d1 | 24 | if (delta_phi_1 < -pi/2) delta_phi_1 = -pi/2; if (delta_phi_1 > pi/2) delta_phi_1 = pi/2; |
Stas285 | 15:960b922433d1 | 25 | |
Stas285 | 15:960b922433d1 | 26 | if (delta_phi_0 < -pi/2) delta_phi_0 += pi; if (delta_phi_0 > pi/2) delta_phi_0 -= pi; //Normalization [-pi/2; pi/2] - (zadom/peredom) |
Stas285 | 15:960b922433d1 | 27 | // float delta_phi = cos(delta_s/coord_accuracy) * coord_accuracy/(delta_s + coord_accuracy) * 1.0 * delta_phi_1 + abs(tan_error) /*delta_s*/ /(delta_s + coord_accuracy) * delta_phi_0; |
Stas285 | 15:960b922433d1 | 28 | float delta_phi = cos(delta_s/coord_accuracy) * exp(-delta_s/coord_accuracy) * delta_phi_1 + (1-exp(-delta_s/coord_accuracy)) * abs(tan_error) /(coord_accuracy) * delta_phi_0; //fusion of delta_phi_0 and delta_phi_1 + uprezhdenie |
Stas285 | 15:960b922433d1 | 29 | if (delta_phi > pi/2) delta_phi -= pi; if (delta_phi < -pi/2) delta_phi += pi; |
Stas285 | 15:960b922433d1 | 30 | |
Stas285 | 15:960b922433d1 | 31 | float eps = azimuth_prop * delta_phi - azimuth_diff * current.omega; if (eps > max_eps) eps = max_eps; if (eps < -max_eps) eps = -max_eps; //Azimuth PID |
dima285 | 13:789b451cc27d | 32 | float delta_omega = eps * t_step; |
dima285 | 13:789b451cc27d | 33 | |
Stas285 | 15:960b922433d1 | 34 | //wifi.printf("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f;\r\n",current.x*100, current.y*100, current.azimuth, target.x*100, azimuth_to_target, eps, delta_omega, delta_phi_0, delta_phi_1, delta_phi); |
dima285 | 13:789b451cc27d | 35 | |
Stas285 | 15:960b922433d1 | 36 | set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis); //Set motor |
dima285 | 13:789b451cc27d | 37 | |
Stas285 | 15:960b922433d1 | 38 | if ((abs(normal_error) < coord_accuracy) && (abs(tan_error) < coord_accuracy/5)) coord_ready = 1; else coord_ready = 0; |
dima285 | 12:721a9ea55e91 | 39 | } |
dima285 | 12:721a9ea55e91 | 40 | |
Stas285 | 15:960b922433d1 | 41 | void balance_motion(){ |
Stas285 | 15:960b922433d1 | 42 | float delta_s, v, a, delta_v; |
Stas285 | 15:960b922433d1 | 43 | |
Stas285 | 15:960b922433d1 | 44 | delta_s = target.path - current.path; |
Stas285 | 15:960b922433d1 | 45 | if(delta_s>0){v=sqrt(2*max_accel*delta_s); if(v>max_speed) v=max_speed;} else {v=-sqrt(-2*max_accel*delta_s); if(v<-max_speed) v=-max_speed;} //Canonic trajectory with brake |
Stas285 | 15:960b922433d1 | 46 | a = x_prop * (v - current.speed); if (a > max_accel) a = max_accel ; if (a < -max_accel) a = -max_accel; |
Stas285 | 15:960b922433d1 | 47 | delta_v = (balance_prop*ax + balance_diff * gy + a) * t_step; //Balance PID |
Stas285 | 15:960b922433d1 | 48 | v = current.speed - delta_v; |
Stas285 | 15:960b922433d1 | 49 | set_motor_speed(v*(radius+half_axis)/radius, v*(radius-half_axis)/radius); //Set motor |
Stas285 | 15:960b922433d1 | 50 | } |
dima285 | 9:8f98b1c277a4 | 51 | |
Stas285 | 15:960b922433d1 | 52 | void realtime(){ |
Stas285 | 15:960b922433d1 | 53 | static int fall_timer; |
Stas285 | 0:e9488589a8ee | 54 | |
Stas285 | 15:960b922433d1 | 55 | time_sec += t_step; |
Stas285 | 15:960b922433d1 | 56 | fall_timer++; if(abs(ax)<1) fall_timer=0; if(fall_timer>50) fall_flag=1; else fall_flag=0; //fall check |
Stas285 | 15:960b922433d1 | 57 | realtime_flag = 1; } |
dima285 | 12:721a9ea55e91 | 58 | |
dima285 | 12:721a9ea55e91 | 59 | void realtime_init(){ |
dima285 | 12:721a9ea55e91 | 60 | rt_ticker.attach(&realtime, t_step); } |
dima285 | 12:721a9ea55e91 | 61 | |
dima285 | 12:721a9ea55e91 | 62 | |
Stas285 | 0:e9488589a8ee | 63 |