Diff: main.cpp

Branch:

lupo

Revision:

39:4c5e4ff386da

Parent:

38:8121e7a79c0b

Child:

40:04b032b01dd5

--- a/main.cpp	Mon May 16 16:37:07 2022 +0200
+++ b/main.cpp	Wed May 18 11:28:25 2022 +0200
@@ -84,6 +84,8 @@
 int         ToNextFunction = 0;      // current state of the system (which function is beeing executed)
 int         state=0;
 float       desired_pos;
+
+bool        nextStep=false;
 // definition variables for calculations
 const float   pi = 2 * acos(0.0); // definiton of pi
 const float   end_pos_lift_deg = 180 + asin((dist_arm_ground-(dist_grappleratt_grappler_uk))/arm_length) * 180 / pi;  // calculates the degree which the arm has to have when lift_up has been executed.
@@ -222,8 +224,8 @@
 
 
     diff =abs( desired_pos-(positionController_M_Arm.getRotation()));
-    printf("Set arm      Position ARM (rot): %3.3f Desired:%3.3f    State:%d     ToNextfunction:%d Diff:%3.3f\n",
-    positionController_M_Arm.getRotation(), desired_pos, state, ToNextFunction, diff);
+   // printf("Set arm      Position ARM (rot): %3.3f Desired:%3.3f    State:%d     ToNextfunction:%d Diff:%3.3f\n",
+   // positionController_M_Arm.getRotation(), desired_pos, state, ToNextFunction, diff);
     if (diff<=0.009){
         return 1;
     }
@@ -251,8 +253,8 @@
 
     diff_R= abs(desired_pos-(positionController_M_right.getRotation()));
     diff_L= abs(desired_pos-(positionController_M_left.getRotation()));
-    printf("Drive Straight Position Right(rot): %3.3f; Position Left (rot): %3.3f Desired: %3.3f Diff:%3.3f State:%d ToNextfunction:%d\n",
-            positionController_M_right.getRotation(),positionController_M_left.getRotation(),desired_pos,diff_L, state, ToNextFunction);
+  //  printf("Drive Straight Position Right(rot): %3.3f; Position Left (rot): %3.3f Desired: %3.3f Diff:%3.3f State:%d ToNextfunction:%d\n",
+//         positionController_M_right.getRotation(),positionController_M_left.getRotation(),desired_pos,diff_L, state, ToNextFunction);
     if ((diff_R<=0.03) && (diff_L<=0.03))
     {
         return 1;
@@ -276,7 +278,7 @@
     positionController_M_Arm.setDesiredRotation(desired_pos, max_speed_rps_arm);
  
     diff=abs(desired_pos-positionController_M_Arm.getRotation());
-    printf("Lift Up:       Position ARM (rot): %3.3f Desired:%3.3f     State:%d  ToNextfunction:%d\n",positionController_M_Arm.getRotation(),desired_pos, state, ToNextFunction);
+   // printf("Lift Up:       Position ARM (rot): %3.3f Desired:%3.3f     State:%d  ToNextfunction:%d\n",positionController_M_Arm.getRotation(),desired_pos, state, ToNextFunction);
     if(diff<=0.03)
     { return 1;
     }
@@ -298,7 +300,7 @@
     user_button.fall(&user_button_pressed_fcn);
     user_button.rise(&user_button_released_fcn);
     mechanical_button.mode(PullDown);
-  
+  printf("test");
 
     while (true)
     {
@@ -329,24 +331,14 @@
             break;
 
             case 1:  
-              
-                    ToNextFunction += 1;
-                    state=0;
-            
+        
+                ToNextFunction +=1;
+                state=0;
+       
 
             break;
 
-            case 2:
-            state=nextStepDetection(ir_distance_mm_L,10);
-            printf("distance:%3.3f  Output:%d\n", ir_distance_mm_L, nextStepDetection(ir_distance_mm_L,10));
-             if (state==3){
-                    ToNextFunction +=1;
-                    state=0;
-             }
-            break;
-
-            case 3: 
-            state=drive_straight(drive_straight_mm);
+            case 2:  state=drive_straight(drive_straight_mm);
            
             if (state==1){
                     ToNextFunction += 1;
@@ -354,25 +346,43 @@
                     desired_pos=0;
 
             }
+        
             break;
 
-            case 4: 
-            state=lift_up();
+            case 3:    state=lift_up();
             
             if (state==1){
                     ToNextFunction += 1;
                 	   state=0;
                     desired_pos=0;
             }
+            break;
+
+            case 4:          state=nextStepDetection(ir_distance_mm_L,10);
+            printf("distance:%3.3f  Output:%d\n", ir_distance_mm_L, nextStepDetection(ir_distance_mm_L,10));
+            nextStep=false;
+
+            if (state==3){
+                 nextStep=true;
+               }
+              
+         
+         //  ToNextFunction +=1;
+         //   state=0;
+            
              break;
 
             case 5: 
+           
             state=drive_straight(drive_back_mm);
             
-            if  ((state == 1) && (StepDetection_down(ir_analog_in_Lookdown_B) != 1)){
+            if  (StepDetection_down(ir_analog_in_Lookdown_B) != 1)
+            {
                     ToNextFunction += 1;
                     state=0;
                     desired_pos=0;
+                    positionController_M_left.setDesiredRotation(positionController_M_left.getRotation());
+                    positionController_M_right.setDesiredRotation(positionController_M_right.getRotation());
 
             }
             break;
@@ -380,10 +390,17 @@
             case 6: 
             state=set_arm_stair_height();
            
-             if (state==1){
-                    ToNextFunction = 0;
+             if ((state==1)&&(nextStep)){
+                    ToNextFunction = 1;
                     state=0;
                     desired_pos=0;
+                    nextStep=false;
+            }
+            if ((state==1)&&(nextStep!=1)) {
+                ToNextFunction=0;
+                state=0;
+                desired_pos=0;
+                nextStep=false;
             }
             break;  
 
@@ -411,6 +428,6 @@
     user_button_timer.stop();
     if (user_button_elapsed_time_ms > 200) 
     {
-       ToNextFunction =1;
+       ToNextFunction =4;
     }
 }
\ No newline at end of file