svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
Diff: realtime.h
- Revision:
- 12:721a9ea55e91
- Parent:
- 10:5bdd3dfd5f59
- Child:
- 13:789b451cc27d
diff -r f7934de1d03a -r 721a9ea55e91 realtime.h --- a/realtime.h Sun Oct 14 07:53:07 2018 +0000 +++ b/realtime.h Sat Nov 03 11:43:03 2018 +0000 @@ -1,70 +1,35 @@ Ticker rt_ticker; -float rt_delta; 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 geometric_navigation(void){ - current_speed = (motor_speed[1]+motor_speed[0])/2.0; //MOVE TO SKOROST!!!!! - rt_delta = current_speed*t_step; - current_angle += (motor_speed[0]-motor_speed[1])*t_step/(half_axis*2)*57.3;//deg - current_path += rt_delta; - current_x += rt_delta*sin(current_angle/57.3); - current_y += rt_delta*cos(current_angle/57.3); - //pc.printf("%3.1f - %3.1f ; %3.1f - %3.1f ; %d\n",target_path,current_path,target_angle,current_angle,motor_busy); //be careful in interrupt - } +void balance() +{ + float a = x_prop * (current.x - target.x) + x_diff * current.speed; + if (a > max_accel) a = max_accel ; if (a < -max_accel) a = -max_accel; + float delta_v = (2*balance_prop*ax + balance_diff*5.73*gy - a) * t_step; + + set_motor_speed(motor_speed[0] - delta_v, motor_speed[1] - delta_v); + + wifi.printf("%.2f %.2f %.2f %.2f;",a,gy,ax,delta_v*100); +} + void realtime(){ //35uS - (3.5mS @ 115200) - (500uS @ 921600) (w/o sin,cos) - //test = 1; - geometric_navigation(); - //inertial_navigation(); - -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); -skorost_1(linear_acceleration,0); - - //wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,target_speed,current_speed); +/* 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);*/ + - /*if(motor_busy == 1) { - if(stop_flag == 1) {skorost(0,0);skorost(1,0);if(abs(motor_speed[0])<1) motor_busy = 0;} //Stop with accel - else { //motion - if(infinite_flag == 1){skorost(1,speed*(1-half_width/radius));skorost(0,speed*(1+half_width/radius));} - else{ - if(angle_task == 1) {//radial motion - if((angle_dir == 1) && (target_angle <= current_angle)) motor_busy = 0; - if((angle_dir == 0) && (target_angle >= current_angle)) motor_busy = 0; - if(motor_busy == 1) { - if(radius > 1) { - if((angle_dir == 1) && (speed>0) || (angle_dir == 0) && (speed<0)){ - skorost(1,speed*(1-half_width/radius));skorost(0,speed*(1+half_width/radius));} - else {skorost(1,speed*(1+half_width/radius));skorost(0,speed*(1-half_width/radius));}} - else {if(angle_dir == 1){skorost(1,-speed); skorost(0,speed);} else {skorost(1,speed); skorost(0,-speed);} //turn on the spot - } - } - } - if(angle_task == 0) {//linear motion - if((path_dir == 1) && (target_path <= current_path)) motor_busy = 0; - if((path_dir == 0) && (target_path >= current_path)) motor_busy = 0; - if(motor_busy == 1) {if(path_dir == 1) {skorost(1,speed);skorost(0,speed);} else{skorost(1,-speed);skorost(0,-speed);} } - } - } - } - } */ - //echo_step(1); - /*static int tmc=50; - static int tm_step = 1; - tmc += tm_step; - if (tmc > 100) tm_step = -1; - if (tmc < 0) tm_step = 1; - skorost_1(1 * tm_step,0);*/ - - realtime_flag = 1; - - - - // test = 0; - //pc.printf("%d \n", proc_counter); proc_counter = 0; - } + realtime_flag = 1; +} + +void realtime_init(){ + rt_ticker.attach(&realtime, t_step); } + + -void realtime_init(){ - rt_ticker.attach(&realtime, 0.01); - } \ No newline at end of file