svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
realtime.h@23:bc05a104be10, 2019-08-06 (annotated)
- Committer:
- dima285
- Date:
- Tue Aug 06 14:13:55 2019 +0000
- Branch:
- svoe
- Revision:
- 23:bc05a104be10
- Parent:
- 22:14e85f2068c7
06.08.19
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 | |
dima285 | 19:2fe650d29823 | 4 | |
Stas285 | 15:960b922433d1 | 5 | void balance_coord(){ |
Stas285 | 15:960b922433d1 | 6 | float delta_x = (target.x - current.x); if (delta_x == 0) delta_x = 0.0001; |
dima285 | 13:789b451cc27d | 7 | float delta_y = (target.y - current.y); |
Stas285 | 15:960b922433d1 | 8 | 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 | 9 | 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 | 10 | |
Stas285 | 15:960b922433d1 | 11 | float delta_phi_0 = azimuth_to_target - current.azimuth; //azimuth: target error |
Stas285 | 15:960b922433d1 | 12 | if (delta_phi_0 < -pi) delta_phi_0 += 2*pi; if (delta_phi_0 > pi) delta_phi_0 -= 2*pi; |
Stas285 | 15:960b922433d1 | 13 | |
Stas285 | 15:960b922433d1 | 14 | float normal_error = delta_s * sin(delta_phi_0); |
Stas285 | 15:960b922433d1 | 15 | float tan_error = delta_s * cos(delta_phi_0); |
dima285 | 12:721a9ea55e91 | 16 | |
Stas285 | 15:960b922433d1 | 17 | float a = x_prop * tan_error - x_diff * current.speed; //Motion PID |
dima285 | 20:e73f49ba5001 | 18 | if ((current.speed > max.speed) && (a > 0)) a = max.accel * (2 - current.speed/max.speed); if ((current.speed < -max.speed) && (a < 0)) a = -max.accel * (2 + current.speed/max.speed); |
dima285 | 20:e73f49ba5001 | 19 | if (a > max.accel) a = max.accel ; if (a < -max.accel) a = -max.accel; |
Stas285 | 15:960b922433d1 | 20 | |
Stas285 | 15:960b922433d1 | 21 | float delta_v = (balance_prop*ax + balance_diff * gy + a) * t_step; //Balance PID |
dima285 | 13:789b451cc27d | 22 | |
Stas285 | 15:960b922433d1 | 23 | float delta_phi_1 = (target.azimuth - current.azimuth);//* (current.speed * t_step / delta_s); //azimuth: sever error |
Stas285 | 15:960b922433d1 | 24 | if (delta_phi_1 < -pi) delta_phi_1 += 2*pi; if (delta_phi_1 > pi) delta_phi_1 -= 2*pi; |
Stas285 | 15:960b922433d1 | 25 | 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 | 26 | |
Stas285 | 15:960b922433d1 | 27 | 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 | 28 | // 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 | 29 | 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 | 30 | if (delta_phi > pi/2) delta_phi -= pi; if (delta_phi < -pi/2) delta_phi += pi; |
Stas285 | 15:960b922433d1 | 31 | |
dima285 | 20:e73f49ba5001 | 32 | 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 | 33 | float delta_omega = eps * t_step; |
dima285 | 13:789b451cc27d | 34 | |
Stas285 | 15:960b922433d1 | 35 | //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 | 36 | |
Stas285 | 15:960b922433d1 | 37 | 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 | 38 | |
dima285 | 17:bd6b6ac89e0e | 39 | if ((abs(normal_error) < coord_accuracy) && (abs(tan_error) < coord_accuracy/2) && (abs(target.azimuth - current.azimuth) < 0.1)) coord_ready = 1; else coord_ready = 0; |
dima285 | 12:721a9ea55e91 | 40 | } |
dima285 | 12:721a9ea55e91 | 41 | |
Stas285 | 15:960b922433d1 | 42 | void balance_motion(){ |
Stas285 | 15:960b922433d1 | 43 | |
dima285 | 20:e73f49ba5001 | 44 | int sign_da; //signum of da |
dima285 | 20:e73f49ba5001 | 45 | int sign_de; //eps |
dima285 | 20:e73f49ba5001 | 46 | switch(motion_mode){ |
dima285 | 20:e73f49ba5001 | 47 | case GO:{ |
dima285 | 20:e73f49ba5001 | 48 | sign_de = 0; |
dima285 | 20:e73f49ba5001 | 49 | if ((abs(theor.path) < max.speed*max.speed/max.accel) || (abs(target.path - theor.path) < max.speed*max.speed/max.accel)){ |
dima285 | 20:e73f49ba5001 | 50 | //if (abs(theor.speed) < max.speed/2) sign_da = 1; else sign_da = -1;} //Razgon/Tormoz |
dima285 | 20:e73f49ba5001 | 51 | if (abs(theor.path) < max.speed*max.speed/max.accel/6) sign_da = 1; else sign_da = -1; } |
dima285 | 20:e73f49ba5001 | 52 | else sign_da = 0; //ezda |
dima285 | 20:e73f49ba5001 | 53 | if (target.dir == 0) sign_da = -sign_da; |
dima285 | 20:e73f49ba5001 | 54 | |
dima285 | 20:e73f49ba5001 | 55 | theor.speed += (theor.accel + sign_da * da) * t_step; //for next step |
dima285 | 20:e73f49ba5001 | 56 | theor.path += theor.speed * t_step; |
dima285 | 20:e73f49ba5001 | 57 | if (((target.path < theor.path) && (target.dir == 1)) || ((target.path > theor.path) && (target.dir == 0))) {coord_ready = 1;sign_da = 0;} |
dima285 | 20:e73f49ba5001 | 58 | else coord_ready = 0; |
dima285 | 20:e73f49ba5001 | 59 | break;} |
dima285 | 20:e73f49ba5001 | 60 | case STOP:{ |
dima285 | 20:e73f49ba5001 | 61 | //float tma = x_prop * (target.path - current.path) - x_diff * current.speed; //Motion PID |
dima285 | 20:e73f49ba5001 | 62 | sign_da = 0;//tma/da; |
dima285 | 20:e73f49ba5001 | 63 | sign_de = 0; |
dima285 | 20:e73f49ba5001 | 64 | theor.speed = 0; |
dima285 | 20:e73f49ba5001 | 65 | theor.accel = 0; |
dima285 | 20:e73f49ba5001 | 66 | break; |
dima285 | 20:e73f49ba5001 | 67 | } |
dima285 | 20:e73f49ba5001 | 68 | case ROTATE:{ |
dima285 | 20:e73f49ba5001 | 69 | sign_da = 0; |
dima285 | 20:e73f49ba5001 | 70 | float tm_az = target.azimuth - current.azimuth; if (tm_az < - pi) tm_az += 2*pi; if (tm_az > pi) tm_az -= 2*pi; |
dima285 | 20:e73f49ba5001 | 71 | if (tm_az < delta.azimuth/2) sign_de = -1; else sign_de = 1; |
dima285 | 20:e73f49ba5001 | 72 | if (abs(tm_az) < 0.1) {coord_ready = 1;sign_de = 0; |
dima285 | 20:e73f49ba5001 | 73 | motor_speed[0] = (motor_speed[0] + motor_speed[1])/2; |
dima285 | 20:e73f49ba5001 | 74 | motor_speed[1] = motor_speed[0]; |
dima285 | 20:e73f49ba5001 | 75 | } |
dima285 | 20:e73f49ba5001 | 76 | else coord_ready = 0; |
dima285 | 20:e73f49ba5001 | 77 | break; |
dima285 | 20:e73f49ba5001 | 78 | } |
dima285 | 20:e73f49ba5001 | 79 | } |
dima285 | 20:e73f49ba5001 | 80 | theor.accel += sign_da * da; |
dima285 | 20:e73f49ba5001 | 81 | //wifi.printf("%d, %f\r\n",sign_da,da); |
dima285 | 22:14e85f2068c7 | 82 | balance_integral += ax; |
dima285 | 20:e73f49ba5001 | 83 | float delta_v = (balance_prop*ax + balance_diff * gy + theor.accel) * t_step; //Balance PID |
dima285 | 20:e73f49ba5001 | 84 | float delta_omega = max.eps * sign_de * t_step; |
dima285 | 20:e73f49ba5001 | 85 | set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis); //Set motor |
dima285 | 20:e73f49ba5001 | 86 | wifi.printf("%f, %f, %f, %f %f, %f\r\n",current.path,theor.path,theor.speed,theor.accel,target.path,motor_speed[0]); |
Stas285 | 15:960b922433d1 | 87 | } |
dima285 | 9:8f98b1c277a4 | 88 | |
dima285 | 20:e73f49ba5001 | 89 | void balance_motion_PID(){ |
dima285 | 20:e73f49ba5001 | 90 | float tma; |
dima285 | 20:e73f49ba5001 | 91 | int sign_de; //eps |
dima285 | 22:14e85f2068c7 | 92 | float tm_lim;//ogran skorosti (uskor ot skorosti) |
dima285 | 22:14e85f2068c7 | 93 | |
dima285 | 22:14e85f2068c7 | 94 | x_integral += (target.path - current.path); |
dima285 | 22:14e85f2068c7 | 95 | if (abs(current.speed) > 0.05) x_integral = 0; |
dima285 | 20:e73f49ba5001 | 96 | switch(motion_mode){ |
dima285 | 20:e73f49ba5001 | 97 | case GO:{ |
dima285 | 20:e73f49ba5001 | 98 | sign_de = 0; |
dima285 | 20:e73f49ba5001 | 99 | float path_error = (target.path - current.path); |
dima285 | 20:e73f49ba5001 | 100 | if (abs(path_error) < coord_accuracy) {coord_ready = 1;tma = 0;} |
dima285 | 20:e73f49ba5001 | 101 | else { |
dima285 | 20:e73f49ba5001 | 102 | //if (path_error > 0.04) path_error = 0.04; if (path_error < -0.04) path_error = -0.04; |
dima285 | 22:14e85f2068c7 | 103 | |
dima285 | 22:14e85f2068c7 | 104 | tma = x_prop * path_error - x_diff * current.speed + x_int * x_integral; |
dima285 | 22:14e85f2068c7 | 105 | if (current.speed * tma > 0) tm_lim = max.accel*(max.speed - abs(current.speed))/max.speed; else tm_lim = max.accel; //ogran skor |
dima285 | 22:14e85f2068c7 | 106 | tma = atan(tma/tm_lim*pi/2) * tm_lim * 2 / pi; //Motion PID |
dima285 | 20:e73f49ba5001 | 107 | coord_ready = 0;} |
dima285 | 20:e73f49ba5001 | 108 | break;} |
dima285 | 20:e73f49ba5001 | 109 | case STOP:{ |
dima285 | 22:14e85f2068c7 | 110 | tma = x_prop * (target.path - current.path) - x_diff * current.speed ;//+ x_int * x_integral; |
dima285 | 22:14e85f2068c7 | 111 | if (current.speed * tma > 0) tm_lim = max.accel*(max.speed - abs(current.speed))/max.speed; else tm_lim = max.accel; //ogran skor |
dima285 | 22:14e85f2068c7 | 112 | tma = atan(tma/tm_lim*pi/2) * tm_lim * 2 / pi; //ogran uskor //Motion PID |
dima285 | 20:e73f49ba5001 | 113 | sign_de = 0; |
dima285 | 20:e73f49ba5001 | 114 | break; |
dima285 | 20:e73f49ba5001 | 115 | } |
dima285 | 20:e73f49ba5001 | 116 | case ROTATE:{ |
dima285 | 20:e73f49ba5001 | 117 | float tm_az = target.azimuth - current.azimuth; if (tm_az < - pi) tm_az += 2*pi; if (tm_az > pi) tm_az -= 2*pi; |
dima285 | 20:e73f49ba5001 | 118 | if (tm_az < delta.azimuth/2) sign_de = -1; else sign_de = 1; |
dima285 | 20:e73f49ba5001 | 119 | if (abs(tm_az) < 0.1) {coord_ready = 1;sign_de = 0; |
dima285 | 20:e73f49ba5001 | 120 | motor_speed[0] = (motor_speed[0] + motor_speed[1])/2; |
dima285 | 20:e73f49ba5001 | 121 | motor_speed[1] = motor_speed[0]; |
dima285 | 20:e73f49ba5001 | 122 | } |
dima285 | 20:e73f49ba5001 | 123 | else coord_ready = 0; |
dima285 | 20:e73f49ba5001 | 124 | break; |
dima285 | 20:e73f49ba5001 | 125 | } |
dima285 | 20:e73f49ba5001 | 126 | } |
dima285 | 20:e73f49ba5001 | 127 | //wifi.printf("%d, %f\r\n",sign_da,da); |
dima285 | 22:14e85f2068c7 | 128 | float delta_v = (balance_prop * ax + balance_diff * gy + tma) * t_step; //Balance PID |
dima285 | 20:e73f49ba5001 | 129 | float delta_omega = max.eps * sign_de * t_step; |
dima285 | 20:e73f49ba5001 | 130 | set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis); //Set motor |
dima285 | 22:14e85f2068c7 | 131 | //wifi.printf("%f, %f, %f, %f %f, %f\r\n",current.path,theor.path,theor.speed,theor.accel,target.path,motor_speed[0]); |
dima285 | 22:14e85f2068c7 | 132 | wifi.printf("%f %f %f %f;",current.speed,tma,ax,gy); |
dima285 | 20:e73f49ba5001 | 133 | } |
dima285 | 20:e73f49ba5001 | 134 | |
dima285 | 20:e73f49ba5001 | 135 | |
dima285 | 20:e73f49ba5001 | 136 | |
Stas285 | 15:960b922433d1 | 137 | void realtime(){ |
Stas285 | 15:960b922433d1 | 138 | time_sec += t_step; |
dima285 | 19:2fe650d29823 | 139 | fall_timer++; if(abs(ax)<3) fall_timer=0; if(fall_timer>50) fall_flag=1; else fall_flag=0; //fall check |
dima285 | 19:2fe650d29823 | 140 | realtime_flag = 1; |
dima285 | 19:2fe650d29823 | 141 | } |
dima285 | 12:721a9ea55e91 | 142 | |
dima285 | 12:721a9ea55e91 | 143 | void realtime_init(){ |
dima285 | 12:721a9ea55e91 | 144 | rt_ticker.attach(&realtime, t_step); } |
dima285 | 12:721a9ea55e91 | 145 | |
dima285 | 12:721a9ea55e91 | 146 | |
Stas285 | 0:e9488589a8ee | 147 |