File content as of revision 13:789b451cc27d:

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);
    float delta_phi_0 = azimuth_to_target - current.azimuth;
    float delta_phi_1 = (target.azimuth - azimuth_to_target) ;//* (current.speed * t_step / delta_s);
    float delta_phi = coord_accuracy/(delta_s + coord_accuracy) * 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;
    
    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);
   wifi.printf("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f;",current.x,target.x,delta_x,delta_s,current.speed,a,delta_v,ax,gy);
}


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);    }