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