Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

realtime.h@20:e73f49ba5001, 2019-03-23 (annotated)

Committer:
dima285
Date:
Sat Mar 23 16:38:44 2019 +0000
Revision:
20:e73f49ba5001
Parent:
19:2fe650d29823
Child:
22:14e85f2068c7
prost

Who changed what in which revision?

UserRevisionLine numberNew 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 20:e73f49ba5001 82 float delta_v = (balance_prop*ax + balance_diff * gy + theor.accel) * t_step; //Balance PID
dima285 20:e73f49ba5001 83 float delta_omega = max.eps * sign_de * t_step;
dima285 20:e73f49ba5001 84 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 85 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 86 }
dima285 9:8f98b1c277a4 87
dima285 20:e73f49ba5001 88 void balance_motion_PID(){
dima285 20:e73f49ba5001 89 float tma;
dima285 20:e73f49ba5001 90 int sign_de; //eps
dima285 20:e73f49ba5001 91
dima285 20:e73f49ba5001 92 switch(motion_mode){
dima285 20:e73f49ba5001 93 case GO:{
dima285 20:e73f49ba5001 94 sign_de = 0;
dima285 20:e73f49ba5001 95 float path_error = (target.path - current.path);
dima285 20:e73f49ba5001 96 if (abs(path_error) < coord_accuracy) {coord_ready = 1;tma = 0;}
dima285 20:e73f49ba5001 97 else {
dima285 20:e73f49ba5001 98 //if (path_error > 0.04) path_error = 0.04; if (path_error < -0.04) path_error = -0.04;
dima285 20:e73f49ba5001 99 tma = x_prop * path_error - x_diff * current.speed;
dima285 20:e73f49ba5001 100 tma = atan(tma) ;//* max.accel; //Motion PID
dima285 20:e73f49ba5001 101 coord_ready = 0;}
dima285 20:e73f49ba5001 102 break;}
dima285 20:e73f49ba5001 103 case STOP:{
dima285 20:e73f49ba5001 104 tma = x_prop * (target.path - current.path) - x_diff * current.speed;
dima285 20:e73f49ba5001 105 tma = atan(tma); //Motion PID
dima285 20:e73f49ba5001 106 sign_de = 0;
dima285 20:e73f49ba5001 107 break;
dima285 20:e73f49ba5001 108 }
dima285 20:e73f49ba5001 109 case ROTATE:{
dima285 20:e73f49ba5001 110 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 111 if (tm_az < delta.azimuth/2) sign_de = -1; else sign_de = 1;
dima285 20:e73f49ba5001 112 if (abs(tm_az) < 0.1) {coord_ready = 1;sign_de = 0;
dima285 20:e73f49ba5001 113 motor_speed[0] = (motor_speed[0] + motor_speed[1])/2;
dima285 20:e73f49ba5001 114 motor_speed[1] = motor_speed[0];
dima285 20:e73f49ba5001 115 }
dima285 20:e73f49ba5001 116 else coord_ready = 0;
dima285 20:e73f49ba5001 117 break;
dima285 20:e73f49ba5001 118 }
dima285 20:e73f49ba5001 119 }
dima285 20:e73f49ba5001 120 //wifi.printf("%d, %f\r\n",sign_da,da);
dima285 20:e73f49ba5001 121 float delta_v = (balance_prop*ax + balance_diff * gy + tma) * t_step; //Balance PID
dima285 20:e73f49ba5001 122 float delta_omega = max.eps * sign_de * t_step;
dima285 20:e73f49ba5001 123 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 124 wifi.printf("%f, %f, %f, %f %f, %f\r\n",current.path,theor.path,theor.speed,theor.accel,target.path,motor_speed[0]);
dima285 20:e73f49ba5001 125 }
dima285 20:e73f49ba5001 126
dima285 20:e73f49ba5001 127
dima285 20:e73f49ba5001 128
Stas285 15:960b922433d1 129 void realtime(){
Stas285 15:960b922433d1 130 time_sec += t_step;
dima285 19:2fe650d29823 131 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 132 realtime_flag = 1;
dima285 19:2fe650d29823 133 }
dima285 12:721a9ea55e91 134
dima285 12:721a9ea55e91 135 void realtime_init(){
dima285 12:721a9ea55e91 136 rt_ticker.attach(&realtime, t_step); }
dima285 12:721a9ea55e91 137
dima285 12:721a9ea55e91 138
Stas285 0:e9488589a8ee 139