Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

realtime.h@14:e12d0fdc3a48, 2018-11-18 (annotated)

Committer:
dima285
Date:
Sun Nov 18 13:33:28 2018 +0000
Revision:
14:e12d0fdc3a48
Parent:
13:789b451cc27d
Child:
15:960b922433d1
12

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;
dima285 12:721a9ea55e91 3 /*float balance_prop = 0.8;
dima285 12:721a9ea55e91 4 float balance_diff = 1.2;
dima285 12:721a9ea55e91 5 float x_prop = 40;
dima285 12:721a9ea55e91 6 float x_diff = 25;
dima285 12:721a9ea55e91 7 float azimuth_prop = 0.8;
dima285 12:721a9ea55e91 8 float azimuth_diff = 0.65;*/
Stas285 0:e9488589a8ee 9
dima285 12:721a9ea55e91 10 void balance()
dima285 12:721a9ea55e91 11 {
dima285 13:789b451cc27d 12 float a;
dima285 13:789b451cc27d 13 float eps;
dima285 13:789b451cc27d 14 float azimuth_to_target;
dima285 13:789b451cc27d 15 float normal_error;
dima285 13:789b451cc27d 16 float tan_error;
dima285 13:789b451cc27d 17
dima285 13:789b451cc27d 18 float delta_x = (target.x - current.x);
dima285 13:789b451cc27d 19 if (delta_x == 0) delta_x = 0.0001;
dima285 13:789b451cc27d 20 float delta_y = (target.y - current.y);
dima285 13:789b451cc27d 21 float delta_s = sqrt(delta_x*delta_x + delta_y*delta_y);
dima285 13:789b451cc27d 22 if (delta_x < 0) {delta_s = -delta_s;}
dima285 13:789b451cc27d 23
dima285 13:789b451cc27d 24 azimuth_to_target = atan(delta_y/delta_x);
dima285 14:e12d0fdc3a48 25 if (delta_x < 0){if (delta_y > 0) azimuth_to_target += pi; else azimuth_to_target -= pi;}
dima285 14:e12d0fdc3a48 26 float delta_phi_0 = azimuth_to_target - current.azimuth; //target error
dima285 14:e12d0fdc3a48 27 float delta_phi_1 = (target.azimuth - current.azimuth);//* (current.speed * t_step / delta_s); //sever error
dima285 14:e12d0fdc3a48 28 float delta_phi = coord_accuracy/(delta_s + coord_accuracy) * 0.3 * delta_phi_1 + delta_s/(delta_s + coord_accuracy) * delta_phi_0; //ZNAK
dima285 12:721a9ea55e91 29
dima285 13:789b451cc27d 30 normal_error = delta_s * sin(delta_phi);
dima285 13:789b451cc27d 31 tan_error = delta_s * cos(delta_phi);
dima285 13:789b451cc27d 32
dima285 12:721a9ea55e91 33
dima285 13:789b451cc27d 34 if (abs(current.speed) > max_speed) a = 0 ; else a = x_prop * tan_error - x_diff * current.speed;
dima285 13:789b451cc27d 35 if (a > max_accel) a = max_accel ; if (a < -max_accel) a = -max_accel;
dima285 13:789b451cc27d 36 float delta_v = (balance_prop*ax + balance_diff * gy + a) * t_step;
dima285 13:789b451cc27d 37
dima285 14:e12d0fdc3a48 38 //if (abs(normal_error) < coord_accuracy) {eps = 0;current.omega = 0;} else
dima285 14:e12d0fdc3a48 39 eps = azimuth_prop * delta_phi - azimuth_diff * current.omega; //gz * azimuth_diff;
dima285 13:789b451cc27d 40 float delta_omega = eps * t_step;
dima285 13:789b451cc27d 41
dima285 14:e12d0fdc3a48 42 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 14:e12d0fdc3a48 43
dima285 13:789b451cc27d 44 set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis);
dima285 13:789b451cc27d 45
dima285 13:789b451cc27d 46 if ((abs(normal_error) < coord_accuracy) && (abs(tan_error) < coord_accuracy)) ready = 1; else ready = 0;
dima285 13:789b451cc27d 47
dima285 13:789b451cc27d 48 // wifi.printf("%.2f %.2f %.2f %.2f;",a,gy,ax,delta_v*100);
dima285 14:e12d0fdc3a48 49
dima285 12:721a9ea55e91 50 }
dima285 12:721a9ea55e91 51
dima285 9:8f98b1c277a4 52
Stas285 4:904b737ef08a 53 void realtime(){ //35uS - (3.5mS @ 115200) - (500uS @ 921600) (w/o sin,cos)
dima285 12:721a9ea55e91 54 /* float path_error = target_path-current_path;
dima285 12:721a9ea55e91 55 float target_speed = 0.1*path_error;//1m/s at 10cm
dima285 12:721a9ea55e91 56 float linear_acceleration = 1*(target_speed-current_speed);*/
dima285 12:721a9ea55e91 57
Stas285 0:e9488589a8ee 58
dima285 12:721a9ea55e91 59 realtime_flag = 1;
dima285 12:721a9ea55e91 60 }
dima285 12:721a9ea55e91 61
dima285 12:721a9ea55e91 62 void realtime_init(){
dima285 12:721a9ea55e91 63 rt_ticker.attach(&realtime, t_step); }
dima285 12:721a9ea55e91 64
dima285 12:721a9ea55e91 65
Stas285 0:e9488589a8ee 66