first commit
Dependencies: PM2_Libary
Diff: main.cpp
- Branch:
- lupo
- Revision:
- 40:04b032b01dd5
- Parent:
- 39:4c5e4ff386da
- Child:
- 41:a8557360c15e
diff -r 4c5e4ff386da -r 04b032b01dd5 main.cpp --- 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