Diff: realtime.h

Revision:

12:721a9ea55e91

Parent:

10:5bdd3dfd5f59

Child:

13:789b451cc27d

--- 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