Diff: main.cpp

Revision:

9:569c0f55dd9b

Parent:

8:3d6b1f78f89c

Child:

10:ad8fced7d6b6

--- a/main.cpp	Sat Jan 29 14:47:48 2022 +0000
+++ b/main.cpp	Thu Feb 03 13:07:37 2022 +0000
@@ -46,13 +46,15 @@
 double auto_y_component;
 bool position_arrival = false;
 bool angle_arrival = true;
+double regulation = 0.5;
 
 //for odometry
 double theta2;
 double pltheta;
 
-double x_pos_target[] = {18.0};
-double y_pos_target[] = {10};
+//target
+double x_pos_target[] = {0};
+double y_pos_target[] = {30};
 double angle_target[] = {10};
 
 double max_mini(double a){
@@ -74,7 +76,7 @@
     scrp.addCMD(2,getLstick_y);
     scrp.addCMD(3,getL2);
     scrp.addCMD(4,getR2);
-    scrp.addCMD(5,change_mode);
+    scrp.addCMD(50,change_mode);
 
     
     while(1){
@@ -82,6 +84,7 @@
         bno.setmode(OPERATION_MODE_IMUPLUS);
         bno.get_angles();
         
+        
         theta = bno.euler.yaw * (PI / 180);
         theta2 = (90 - bno.euler.yaw) * (PI /180);
         
@@ -99,7 +102,12 @@
         }
         
         get_position(pltheta);
-        DS.pass_val(x_component,y_component,r2_num,l2_num);
+        if(l1 == 0){
+            DS.pass_val(x_component,y_component,r2_num,l2_num);
+        }else{
+            DS.pass_val(x_component * regulation,y_component * regulation,r2_num * regulation,l2_num * regulation);
+        }
+        //printf("%d\n",y_component);
         
         if(auto_mode == false){
             if(DS.cal_input() == true){
@@ -109,26 +117,27 @@
             }
         }else{
             //printf("%lf\n",bno.euler.yaw);
-            //if(position_arrival == false){
+            if(position_arrival == false){
                 auto_x_component = max_mini((x_pos_target[0] - x_position) * 150);
                 auto_y_component = max_mini((y_pos_target[0] - y_position) * 150);
                 TG.pass_val(auto_y_component,auto_x_component,theta);
-                /*if((x_pos_target[0] - x_position) <= 0.001 || (x_pos_target[0] - x_position) >= -0.001 && (y_pos_target[0] - y_position) <= 0.001 || (y_pos_target[0] - y_position) >= -0.001){
+                //printf("go to target\n");
+                if((x_pos_target[0] - x_position) <= 0.5 && (x_pos_target[0] - x_position) >= -0.5 && (y_pos_target[0] - y_position) <= 0.5 && (y_pos_target[0] - y_position) >= -0.5){
                     position_arrival = true;
                     TG.pass_val(0,0,theta);
                     printf("arrival_position\n");
                 }
             }else{
                 if(angle_arrival == false){
-                    //pid_angle.angle_ctl(bno.get_angles(),angle_target,0.004,0.0007,0);
-                    //TG.pass_target(output);
-                    if(angle_target[0] - bno.euler.yaw <= 0.1 || angle_target[0] - bno.euler.yaw >= -0.1){
+                    pid_angle.angle_ctl(bno.euler.yaw,angle_target[0],0.004,0.0007,0);
+                    TG.pass_target(pid_angle.output);
+                    if(angle_target[0] - bno.euler.yaw <= 0.1 && angle_target[0] - bno.euler.yaw >= -0.1){
                         angle_arrival = true;
                         TG.pass_val(0,0,theta);
                         printf("arrival_angle\n");
                     }
                 }
-            }*/
+            }
         }
         
         pid_1.pass_val(data_1.get(),TG.obt_target1(),0.00007,0.000001,0.00000014);
@@ -162,7 +171,7 @@
             led4 = 0;
         }
         
-        printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",TG.obt_target1(),TG.obt_target2(),TG.obt_target3(),TG.obt_target4(),pid_1.obt_spd(),pid_2.obt_spd(),pid_3.obt_spd(),pid_4.obt_spd());
+        //printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",TG.obt_target1(),TG.obt_target2(),TG.obt_target3(),TG.obt_target4(),pid_1.obt_spd(),pid_2.obt_spd(),pid_3.obt_spd(),pid_4.obt_spd());
         printf("%lf,%lf\n",x_position,y_position);
         /*if(auto_mode == false){
             printf("manual\n");