svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
realtime.h
- Committer:
- dima285
- Date:
- 2018-11-18
- Revision:
- 14:e12d0fdc3a48
- Parent:
- 13:789b451cc27d
- Child:
- 15:960b922433d1
File content as of revision 14:e12d0fdc3a48:
Ticker rt_ticker; bool realtime_flag; /*float balance_prop = 0.8; float balance_diff = 1.2; float x_prop = 40; float x_diff = 25; float azimuth_prop = 0.8; float azimuth_diff = 0.65;*/ void balance() { float a; float eps; float azimuth_to_target; float normal_error; float tan_error; float delta_x = (target.x - current.x); if (delta_x == 0) delta_x = 0.0001; float delta_y = (target.y - current.y); float delta_s = sqrt(delta_x*delta_x + delta_y*delta_y); if (delta_x < 0) {delta_s = -delta_s;} 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;} float delta_phi_0 = azimuth_to_target - current.azimuth; //target error float delta_phi_1 = (target.azimuth - current.azimuth);//* (current.speed * t_step / delta_s); //sever error float delta_phi = coord_accuracy/(delta_s + coord_accuracy) * 0.3 * delta_phi_1 + delta_s/(delta_s + coord_accuracy) * delta_phi_0; //ZNAK normal_error = delta_s * sin(delta_phi); tan_error = delta_s * cos(delta_phi); if (abs(current.speed) > max_speed) a = 0 ; else a = x_prop * tan_error - x_diff * current.speed; if (a > max_accel) a = max_accel ; if (a < -max_accel) a = -max_accel; float delta_v = (balance_prop*ax + balance_diff * gy + a) * t_step; //if (abs(normal_error) < coord_accuracy) {eps = 0;current.omega = 0;} else eps = azimuth_prop * delta_phi - azimuth_diff * current.omega; //gz * azimuth_diff; float delta_omega = eps * t_step; 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); set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis); if ((abs(normal_error) < coord_accuracy) && (abs(tan_error) < coord_accuracy)) ready = 1; else ready = 0; // wifi.printf("%.2f %.2f %.2f %.2f;",a,gy,ax,delta_v*100); } void realtime(){ //35uS - (3.5mS @ 115200) - (500uS @ 921600) (w/o sin,cos) /* float path_error = target_path-current_path; float target_speed = 0.1*path_error;//1m/s at 10cm float linear_acceleration = 1*(target_speed-current_speed);*/ realtime_flag = 1; } void realtime_init(){ rt_ticker.attach(&realtime, t_step); }