TreppenRobo
first commit
Dependencies: PM2_Libary
Diff: main.cpp
- Branch:
- lupo
- Revision:
- 42:ec3a88a24666
- Parent:
- 41:a8557360c15e
--- a/main.cpp Thu May 19 12:25:41 2022 +0200
+++ b/main.cpp Sun May 22 10:01:40 2022 +0200
@@ -30,16 +30,17 @@
float ir_distance_mm_Lookdown_B;
float ir_distance_mm_Lookdown_F;
+// create AnalogIn object to read in infrared distance sensor, 0...3.3V are mapped to 0...1
AnalogIn ir_analog_in_Distance_L(PC_2);
-
AnalogIn ir_analog_in_Lookdown_B(PC_5);
AnalogIn ir_analog_in_Lookdown_F(PB_1);
-// create AnalogIn object to read in infrared distance sensor, 0...3.3V are mapped to 0...1
+// Digital Inputs
DigitalIn mechanical_button(PC_3);
-// 78:1, 100:1, ... Metal Gearmotor 20Dx44L mm 12V CB
+
DigitalOut enable_motors(PB_15); // create DigitalOut object to enable dc motors
+
float pwm_period_s = 0.00005f; // define pwm period time in seconds and create FastPWM objects to command dc motors
//motor pin declaration
@@ -54,9 +55,7 @@
//***********************************************************************************************************************************************************
// Hardware controll Setup and functions (motors and sensors)
-//these variables represent relative position NOT absolut
-float startPos = -0.555; //from last lift up position to start position
-float liftPos = -0.555; //from start position to lift up position
+
// create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
const float max_voltage = 12.0f; // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
@@ -75,24 +74,26 @@
// PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters
//PositionController positionController_M3(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M3, encoder_M3); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
//***********************************************************************************************************************************************************
-// calculations for basic movment and controll
+
-//placeholder variables for prototype testing
+//these variables represent relative position NOT absolut
+float startPos = -0.545; //from last lift up position to start position
+float liftPos = -0.555; //from start position to lift up position
-const float drive_straight_mm = 250.0; // placeholder for testing drives amount forward
-const float drive_back_mm = -20.0f; // placeholder for testing drives amount backwards
+const float drive_straight_mm = 200.0;
+const float drive_back_mm = -20.0f;
int ToNextFunction = 0; // current state of the system (which function is beeing executed)
-int state=0;
+int state=0; //return value of functions
float desired_pos;
+int nextStep=0;
-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.
const float start_deg_arm = -asin((dist_arm_ground - dist_grappleratt_grappler_uk) / arm_length) * 180.0/pi ; //calculates the starting degree of the arm (gripper has to touch ground in frotn of Wall-E)
// definition of rotation speeds for motors 0 = none 1.0 = max.
-const float max_speed_rps_wheel = 0.7f; // 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_wheel = 0.8f; // 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.9f; // 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)
@@ -110,7 +111,7 @@
//RETURN: deg_wheel = degree which the motor has to turn in order to cover distance(mm)
float wheel_dist_to_deg(float distance)
{
- float deg_wheel = (distance* 360.0f) / (wheel_diameter * pi) ;
+ float deg_wheel = (distance) / (wheel_diameter * pi) ;
return deg_wheel;
}
@@ -212,25 +213,27 @@
{
float diff;
- int gripper=StepDetection_down(ir_analog_in_Distance_L);
+ int gripper=nextStepDetection(ir_distance_mm_L, 2);
-
+ //first step to calculate desired position
if (desired_pos==0) {
desired_pos=turn_relative_deg(startPos, positionController_M_Arm.getRotation());
- positionController_M_Arm.setDesiredRotation(desired_pos, max_speed_rps_arm); // command to turn motor to desired deg.
+ positionController_M_Arm.setDesiredRotation(desired_pos, 0.5); // command to turn motor to desired deg.
}
+ // to check if the position controller is finished
+ diff =abs( desired_pos-(positionController_M_Arm.getRotation()));
- diff =abs( desired_pos-(positionController_M_Arm.getRotation()));
+ //prints for testing
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 (gripper){
- desired_pos=turn_relative_deg(0.0, positionController_M_Arm.getRotation() );
- positionController_M_Arm.setDesiredRotation(desired_pos, max_speed_rps_arm);
+ // stops the positioning, when the gripper is in proximity of the sensor
+ if (gripper==3){
+ desired_pos=turn_relative_deg(-0.01, positionController_M_Arm.getRotation() );
+ positionController_M_Arm.setDesiredRotation(desired_pos, 0.2);
}
- if((diff<0.009)&&gripper){
+ if((diff<0.008)&&gripper){
return 1;
}
else {
@@ -245,21 +248,27 @@
float diff_R;
float diff_L;
+// calculates the desired position
if (desired_pos==0) {
- desired_pos= distance/(wheel_diameter*pi);
+ desired_pos=wheel_dist_to_deg(distance);
float relativ_turns_rightmotor = turn_relative_deg(desired_pos, positionController_M_right.getRotation());
float relativ_turns_leftmotor = turn_relative_deg(desired_pos, positionController_M_left.getRotation());
}
-
+
+
positionController_M_right.setDesiredRotation(desired_pos, max_speed_rps_wheel);
positionController_M_left.setDesiredRotation(desired_pos, max_speed_rps_wheel);
-
+ // to check if the position controller are finished
diff_R= abs(desired_pos-(positionController_M_right.getRotation()));
diff_L= abs(desired_pos-(positionController_M_left.getRotation()));
+
+ //prints for testing
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))
+
+
+ if ((diff_R<=0.02) && (diff_L<=0.02))
{
return 1;
}
@@ -273,16 +282,20 @@
int lift_up()
{
float diff;
+
+ // calculates the desired position
if (desired_pos==0) {
desired_pos = turn_relative_deg(liftPos,positionController_M_Arm.getRotation());
}
-
-
positionController_M_Arm.setDesiredRotation(desired_pos, max_speed_rps_arm);
-
+
+ // to check if the position controller is finished
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);
+
+ //prints for testing
+ 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;
}
@@ -304,16 +317,14 @@
user_button.fall(&user_button_pressed_fcn);
user_button.rise(&user_button_released_fcn);
mechanical_button.mode(PullDown);
- printf("test");
- ToNextFunction = 0;
+
while (true)
{
ir_distance_mm_L= mapping(ir_analog_in_Distance_L.read()*1.0e3f * 3.3f);
- if (ToNextFunction>=1||(mechanical_button.read()!=1))
- {
+ if (ToNextFunction>=1||(mechanical_button.read()!=1)) {
enable_motors=1;
}
@@ -321,6 +332,7 @@
switch (ToNextFunction)
{
+ // case 0: For referencing the arm position
case 0: while (mechanical_button.read()!=1)
{
positionController_M_Arm.setDesiredRotation(-1,0.5);
@@ -330,29 +342,26 @@
positionController_M_Arm.setDesiredRotation(positionController_M_Arm.getRotation());
}
-
-
break;
+ // case 1:
case 1:
ToNextFunction +=1;
state=0;
-
-
break;
+ // case 2: drive too the stair
case 2: state=drive_straight(drive_straight_mm);
if (state==1){
ToNextFunction += 1;
state=0;
desired_pos=0;
-
}
-
break;
+ // case 3: lift the roboer up
case 3: state=lift_up();
if (state==1){
@@ -362,22 +371,22 @@
}
break;
+ // case 4: detect if there is a next step
case 4: state=nextStepDetection(ir_distance_mm_L,4);
-
-
-
+ // if there is a next step, variable nextStep=1
if (state==3){
nextStep=1;
}
- printf("distance:%3.3f Output:%d NextStep:%d\n ", ir_distance_mm_L, nextStepDetection(ir_distance_mm_L,4), nextStep);
-
+
+
ToNextFunction +=1;
state=0;
-
+
+ printf("distance:%3.3f Output:%d NextStep:%d\n ", ir_distance_mm_L, nextStepDetection(ir_distance_mm_L,4), nextStep);
break;
+ // case 5: Drive the roboter back until there is no step underneath the lookdown sensor
case 5:
-
state=drive_straight(drive_back_mm);
if (StepDetection_down(ir_analog_in_Lookdown_B) != 1)
@@ -391,15 +400,17 @@
}
break;
+ //case 6: bring arm back to starting positon
case 6:
state=set_arm_stair_height();
-
+ //if there is a next step, continue to climb up
if ((state==1) && (nextStep)){
ToNextFunction = 1;
state=0;
desired_pos=0;
nextStep=0;
}
+ //if there is no next step, stop
if ((state==1) && (nextStep!=1)) {
ToNextFunction=0;
state=0;