Diff: main.cpp

Branch:

lupo

Revision:

40:04b032b01dd5

Parent:

39:4c5e4ff386da

Child:

41:a8557360c15e

--- a/main.cpp	Wed May 18 11:28:25 2022 +0200
+++ b/main.cpp	Wed May 18 13:50:44 2022 +0200
@@ -85,7 +85,7 @@
 int         state=0;
 float       desired_pos;
 
-bool        nextStep=false;
+int        nextStep=0;
 // 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.
@@ -178,7 +178,7 @@
     if(distance == 0){
         return 10; //sensor value is outside the expected range
     }
-    if((distance <= (setpoint + 1)) && (distance >= (setpoint - 1))){
+    if((distance <= (setpoint + 5)) && (distance >= (setpoint - 5))){
         return 3; //the distance to the next step is in ±1cm of the setpoint
     }
     if(distance < setpoint){
@@ -224,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;
     }
@@ -278,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;
     }
@@ -359,16 +359,16 @@
             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;
+                 nextStep=1;
                }
-              
+              printf("distance:%3.3f  Output:%d\n NextStep:%d", ir_distance_mm_L, nextStepDetection(ir_distance_mm_L,10), nextStep); 
          
-         //  ToNextFunction +=1;
-         //   state=0;
+            ToNextFunction +=1;
+            state=0;
             
              break;
 
@@ -390,17 +390,17 @@
             case 6: 
             state=set_arm_stair_height();
            
-             if ((state==1)&&(nextStep)){
+             if ((state==1) && (nextStep)){
                     ToNextFunction = 1;
                     state=0;
                     desired_pos=0;
-                    nextStep=false;
+                    nextStep=0;
             }
-            if ((state==1)&&(nextStep!=1)) {
+            if ((state==1) && (nextStep!=1)) {
                 ToNextFunction=0;
                 state=0;
                 desired_pos=0;
-                nextStep=false;
+                nextStep=0;
             }
             break;  
 
@@ -428,6 +428,6 @@
     user_button_timer.stop();
     if (user_button_elapsed_time_ms > 200) 
     {
-       ToNextFunction =4;
+       ToNextFunction =1;
     }
 }
\ No newline at end of file