File content as of revision 10:5bdd3dfd5f59:

Ticker rt_ticker;
float rt_delta;
bool realtime_flag;

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 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);   
    
    /*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;
  }
  
void realtime_init(){
    rt_ticker.attach(&realtime, 0.01);
    }