TreppenRobo
Prototyp V2
Dependencies: PM2_Libary
Diff: main.cpp
- Branch:
- michi
- Revision:
- 45:8050724fe19b
- Parent:
- 44:c2d4bc4be5f2
- Child:
- 46:eba2263eb626
--- a/main.cpp Mon Apr 18 12:54:10 2022 +0200
+++ b/main.cpp Mon Apr 18 16:02:10 2022 +0200
@@ -63,12 +63,15 @@
// logic functions for basic movement
//Platzhalter Variabeln für die Positionierung
-float PositionStair = 0.2;
-float PositionBackOff = -0.5;
+int drive_stright_mm = 100;
+int PositionBackOff = -100;
float degArmStart = 0.0;
float degArmLift = -0.5;
int ToNextFunction = 0; // current state of the system (which function is beeing executed)
-float max_speed_rps = 0.5f; // define maximum speed that the position controller is changig the speed, has to be smaller or equal to kn * max_voltage
+float max_speed_rps_wheel = 0.6f; // define maximum speed that the position controller is changig the speed for the wheels, has to be smaller or equal to kn * max_voltage
+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
+double start_deg_arm = asin((dist_arm_ground - dist_grappleratt_grappler_uk) / arm_length); //calculates the starting degree of the arm (gripper has to touch ground in frotn of Wall-E)
+double current_deg_arm = start_deg_arm; // saves the current degree the arm has.
// calculates the deg which the arm has to take to reach a certain height (the input height will be the height of OK Gripper area)
double calc_arm_deg_for_height(int height_mm)
@@ -77,41 +80,43 @@
{
printf("Error in calc_arm_deg_for_height: desired height is bigger than Wall-E arm lenght."); // error message when desired height is not reachable.
}
- double height_arm = height_mm - dist_arm_ground - (dist_grappleratt_grappler_uk - gripper_area_height);
- double deg_arm_rad = asin(height_mm / arm_length); // deg in radians
- double pi = 2 * acos(0.0); // definiton of pi
- double deg_arm = deg_arm_rad * 180.0/pi; // deg in degrees
+ float height_arm = height_mm - dist_arm_ground - (dist_grappleratt_grappler_uk - gripper_area_height);
+ float deg_arm_rad = asin(height_arm / arm_length); // deg in radians
+ float pi = 2 * acos(0.0); // definiton of pi
+ float deg_arm = deg_arm_rad * 180.0/pi; // deg in degrees
return deg_arm;
}
+//calculates the deg which the wheels have to turn in order to cover specified distnace in mm
+float wheel_dist_to_deg(int distance) // distance has to be in mm.
+{
+ float pi = 2 * acos(0.0); // definiton of pi
+ float deg_wheel = distance * 360 /(wheel_diameter * pi);
+ return deg_wheel;
+}
+
// bring arme in starting position height of stairs.
int start_position()
{
- float deg = calc_arm_deg_for_height(height_stairs); //deg which arm motor has to turn to in order to grab stair
+ float 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
+ float deg = deg_up_from_horizon + start_deg_arm;
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.
+ printf("**************Error in start_position: degree is out of bound for Start Position.***************"); // error when desired reaching point is out of reach.
}
- positionController_M_Arm.setDesiredRotation(deg); // command to turn motor to desired deg.
+ positionController_M_Arm.setDesiredRotation(deg / 360.0, max_speed_rps_arm); // command to turn motor to desired deg.
+ current_deg_arm = positionController_M_Arm.getRotation() / 360.0;
return NULL;
}
-//calculates the deg which the wheels have to turn in order to cover specified distnace in mm
-int wheel_dist_to_deg(int distance) // distance has to be in mm.
-{
- double pi = 2 * acos(0.0); // definiton of pi
- int deg_wheel = distance * 360 /(wheel_diameter * pi);
- return deg_wheel;
-}
-
//Drives forward into the next step
// calculatioin of acctual distance with wheels is needed
int drive_straight(float distance)
{
- int deg_to_turn = wheel_dist_to_deg(distance);
- positionController_M_right.setDesiredRotation(deg_to_turn,max_speed_rps);
- positionController_M_left.setDesiredRotation(deg_to_turn,max_speed_rps);
- return 0;
+ double deg_to_turn = wheel_dist_to_deg(distance);
+ positionController_M_right.setDesiredRotation(deg_to_turn / 360.0, max_speed_rps_wheel);
+ positionController_M_left.setDesiredRotation(deg_to_turn / 360.0, max_speed_rps_wheel);
+ return NULL;
}
//only turns the arm until the robot is on the next step
@@ -119,8 +124,8 @@
int lift_up(float deg)
{
int8_t i = 0; //prov condition variable
- positionController_M_Arm.setDesiredRotation(deg);
- return 0;
+ positionController_M_Arm.setDesiredRotation(deg / 360.0, max_speed_rps_arm);
+ return NULL;
}
//***********************************************************************************************************************************************************
@@ -165,7 +170,7 @@
double infY =360 , supY = 2360; //Window for sensor values
double voltage_mV = adc_value_mV;
double p1 = -1.127*powerx(10,-14), p2 = 8.881*powerx(10,-11), p3 = -2.76*powerx(10,-7), p4 = 0.0004262, p5 = -0.3363, p6 = 120.1 ; //faktoren für polynomkurve -> von matlab exportiert
- if(voltage_mV > infY && voltage_mV < supY)
+ if((voltage_mV > infY) && (voltage_mV < supY))
{
distance = p1*powerx(voltage_mV,5) + p2*powerx(voltage_mV,4) + p3*powerx(voltage_mV,3) + p4*powerx(voltage_mV,2) + p5*voltage_mV + p6;
}
@@ -188,34 +193,41 @@
while (true)
{
- enable_motors = 1;
- ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f;
+ enable_motors = 1;
+ ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f;
switch (ToNextFunction)
{
- case 1: start_position();
- printf("Case 1: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
- // ToNextFunction+=1;
- break;
- case 2: drive_straight(PositionStair);
- printf("Case 2: Position Right(rot): %3.3f; Position Left (rot): %3.3f\n",
- positionController_M_right.getRotation(),positionController_M_left.getRotation());
- // ToNextFunction+=1;
+ case 0:
break;
- case 3: lift_up(degArmLift);
- // ToNextFunction+=1;
- printf("Case 3: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
- break;
- case 4: drive_straight(PositionBackOff);
+ case 1:
+ start_position();
+ printf("Case 1: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
+ // ToNextFunction+=1;
+ break;
+ case 2:
+ drive_straight(drive_stright_mm);
+ printf("Case 2: Position Right(rot): %3.3f; Position Left (rot): %3.3f\n",
+ positionController_M_right.getRotation(),positionController_M_left.getRotation());
+ // ToNextFunction+=1;
+ break;
+ case 3:
+ lift_up(degArmLift);
+ // ToNextFunction+=1;
+ printf("Case 3: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
+ break;
+ case 4:
+ drive_straight(PositionBackOff);
printf("Case 4: Position Right(rot): %3.3f; Position Left (rot): %3.3f\n",
- positionController_M_right.getRotation(),positionController_M_left.getRotation());
- // ToNextFunction+=1;
- break;
- case 5: lift_up(degArmStart);
+ positionController_M_right.getRotation(),positionController_M_left.getRotation());
+ // ToNextFunction+=1;
+ break;
+ case 5:
+ lift_up(degArmStart);
printf("Case 5: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
- // ToNextFunction = 0;
- break;
- default: ;
+ // ToNextFunction = 0;
+ break;
+ default: ;
}
}
// read timer and make the main thread sleep for the remaining time span (non blocking)