TreppenRobo
Prototyp V2
Dependencies: PM2_Libary
Diff: main.cpp
- Branch:
- michi
- Revision:
- 94:7cf17f2eaa28
- Parent:
- 93:4661e91fdd50
- Child:
- 95:af4821333ddb
--- a/main.cpp Tue May 03 18:39:55 2022 +0200
+++ b/main.cpp Wed May 04 08:38:43 2022 +0200
@@ -85,12 +85,13 @@
const float max_speed_rps_wheel = 0.5f; // define maximum speed that the position controller is changig the speed for the wheels, has to be smaller or equal to kn * max_voltage
const float max_speed_rps_arm = 0.3f; // define maximum speed that the position controller is changig the speed for the arm, has to be smaller or equal to kn * max_voltage
+
// calculates the deg which the arm has to take to reach a certain height (the input height has to be the height of OK Gripper area)
// PARAM: height_mm = height which OK Gripperarea has to reach.
// RETURN: deg_arm = absolut Position in deg that the arm has to take.
float calc_arm_deg_for_height(int height_mm)
{
- float height_arm = height_mm - (dist_arm_ground - dist_arm_attach_OK_griparea); // calculates the height which only the arm has to cover (- attachement height (arm to robot) etc.)
+ float height_arm = height_mm - (dist_arm_ground - dist_arm_attach_OK_griparea + dist_grappleratt_grappler_uk); // calculates the height which only the arm has to cover (- attachement height (arm to robot) etc.)
float deg_arm = asin(height_arm / arm_length) * 180.0/pi; // calculates the absolute degrees which the arm has to reach
return deg_arm;
}
@@ -176,21 +177,14 @@
enable_motors = 1;
float diff;
double deg_up_from_horizon = calc_arm_deg_for_height(height_stairs); //deg which arm motor has to turn to in order to grab stair. starting from horizontal position
- if ((0.0 > deg) || (deg > 360.0))
- {
- printf("Error in start_position: degree is out of bound for Start Position."); // error when desired reaching point is out of reach.
- return 2;
- }
- enable_motors = 1;
positionController_M_Arm.setDesiredRotation(deg / 360.0, max_speed_rps_arm); // command to turn motor to desired deg.
diff = deg-(positionController_M_Arm.getRotation() * 360.0);
- if (diff<=0.3){
+ if (diff <= 0.3){
return 1;
}
else {
- return NULL;}
- enable_motors = 0;
+ return 0;}
}
//Drives forward into the next step
@@ -204,15 +198,12 @@
float relativ_turns_rightmotor = turn_relative_deg(deg_to_turn, positionController_M_right.getRotation());
float relativ_turns_leftmotor = turn_relative_deg(deg_to_turn, positionController_M_left.getRotation());
- enable_motors = 1;
positionController_M_right.setDesiredRotation(relativ_turns_rightmotor, max_speed_rps_wheel);
positionController_M_left.setDesiredRotation(relativ_turns_leftmotor, max_speed_rps_wheel);
- enable_motors = 0;
-
- diff_R= relativ_turns_rightmotor-positionController_M_right.getRotation();
- diff_L= relativ_turns_leftmotor-positionController_M_left.getRotation();
- if ((diff_R<=0.3) && (diff_L<=0.3))
+ diff_R = relativ_turns_rightmotor-positionController_M_right.getRotation();
+ diff_L = relativ_turns_leftmotor-positionController_M_left.getRotation();
+ if ((diff_R <= 0.3) && (diff_L <= 0.3))
{
return 1;
}
@@ -224,23 +215,30 @@
}
//turns the arm until the robot is on the next step
-void lift_up()
+int lift_up()
{
float diff;
float absolut_pos_arm = turn_absolut_deg(end_pos_lift_deg, positionController_M_Arm.getRotation()-1);
float position_lift_end_deg = asin((-dist_arm_ground - (dist_grappleratt_grappler_uk )) / arm_length) - 90; // calculates the degree which has to be reached in order to get on top of next step
float relativ_turns_arm = turn_absolut_deg(position_lift_end_deg, positionController_M_Arm.getRotation());
-
- enable_motors = 1;
+
positionController_M_Arm.setDesiredRotation(relativ_turns_arm , max_speed_rps_arm);
- enable_motors = 0;
+ diff = relativ_turns_arm - positionController_M_Arm.getRotation();
+ if (diff <= 0.01)
+ {
+ return 1;
+ }
+ else
+ {
+ return 0;
+ }
}
//function to climbe one step up and get in to position to drive in to the next step
//PARAM: None
//RETURN: None
-int lift_sequenze()
+void lift_sequenze()
{
lift_up();
if ((StepDetection_down(ir_analog_in_Lookdown_B) == 1) && (StepDetection_down(ir_analog_in_Lookdown_F) == 1))
@@ -251,26 +249,13 @@
set_arm_stair_height();
positionController_M_Arm.setDesiredRotation(0, max_speed_rps_arm);
-
- float diff=relativ_turns_arm-positionController_M_Arm.getRotation();
- if(diff<=0.3)
- { return 1;
- }
- else
- { return 0;
- }
-
-
}
//***********************************************************************************************************************************************************
+//Function which checks if sensors and motors have been wired correctly and the expectet results will happen. otherwise Wall-E will show with armmovement.
+void check_start();
-//Function which checks if sensors and motors have been wired correctly and the expectet results will happen. otherwise Wall-E will show with armmovement.
-int check_start();
-int NextStep (float){
- return 1;
-}
+int NextStep(float);
-//
//simple check if there is an object in proximity
//returns 0 if there is NO object present
//returns 1 if there is an object present
@@ -305,42 +290,60 @@
float ir_distance_mm_Lookdown_B= mapping(ir_analog_in_Lookdown_B.read()*1.0e3f * 3.3f);
float ir_distance_mm_Lookdown_F= mapping(ir_analog_in_Lookdown_F.read()*1.0e3f * 3.3f);
-
+ if (ToNextFunction >= 1)
+ {
+ enable_motors =1;
+ }
switch (ToNextFunction)
{
-
- set_arm_stair_height();
-
+ case 1:
+ state = set_arm_stair_height();
printf("Case 1: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
if (state==1){
ToNextFunction += 1;
}
break;
- case 2:
- drive_straight(drive_straight_mm);
+ case 2:
+ state = NextStep(ir_analog_in_Distance_L);
+ if (state==1){
+ ToNextFunction += 1;
+ }
+
+ case 3:
+ state = drive_straight(drive_straight_mm);
printf("Case 2: Position Right(rot): %3.3f; Position Left (rot): %3.3f\n",
positionController_M_right.getRotation(),positionController_M_left.getRotation());
- break;
-
- case 3:
- lift_sequenze();
- printf("Case 3: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
+ if (state==1){
+ ToNextFunction += 1;
+ }
break;
case 4:
- drive_straight(drive_back_mm);
- ToNextFunction = 0;
+ state = lift_up();
+ printf("Case 3: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
+ if (((state==1) && (StepDetection_down(ir_distance_mm_Lookdown_B))) && (StepDetection_down(ir_distance_mm_Lookdown_F)))
+ {
+ ToNextFunction += 1;
+ }
break;
case 5:
- state=drive_straight(drive_back_mm);
- printf("Case 4: Position Right(rot): %3.3f; Position Left (rot): %3.3f\n",
- positionController_M_right.getRotation(),positionController_M_left.getRotation());
- if ((state==1)&&(StepDetection(ir_distance_mm_Lookdown_B)!=1)){
- ToNextFunction += 1;
+ state = drive_straight(drive_back_mm);
+ if ((state == 1) && (StepDetection_down(ir_distance_mm_Lookdown_B) != 1))
+ {
+ ToNextFunction += 1;
+ }
+ break;
+
+ case 6:
+ state=lift_up();
+ printf("Case 5: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
+ if (state==1)
+ {
+ ToNextFunction = 1;
}
break;