first commit
Dependencies: PM2_Libary
Diff: main.cpp
- Branch:
- lupo
- Revision:
- 38:8121e7a79c0b
- Parent:
- 37:05252c4a2d4e
- Child:
- 39:4c5e4ff386da
diff -r 05252c4a2d4e -r 8121e7a79c0b main.cpp --- a/main.cpp Mon May 02 14:35:00 2022 +0200 +++ b/main.cpp Mon May 16 16:37:07 2022 +0200 @@ -10,8 +10,8 @@ const float dist_arm_attach_distsensor = 20; // distance between pivot point arm on body to start distancesensor on top in horizontal (6) const float dist_distsensors = 200; // distance between the two distancesensors on top of Wall-E (9) const float dist_arm_ground = 51; // distance between pivotpoint arm and ground (5) -const float gripper_area_height = 16 ; // Height of Grappler cutout to grapple Stair (8) -const float dist_grappleratt_grappler_uk = 33; // distance between pivotpoint Grappler and bottom edge (?) +const float dist_arm_attach_OK_griparea = 10.5 ; // Height of Grappler cutout to grapple Stair (8) (maybe add 1mm so gripper is a bit over the plate) +const float dist_grappleratt_grappler_uk = 36.5; // distance between pivotpoint Grappler and bottom edge (?) const float height_stairs = 100; // height to top of next stairstep in mm //*********************************************************************************************************************************************************** @@ -31,11 +31,13 @@ float ir_distance_mm_Lookdown_F; AnalogIn ir_analog_in_Distance_L(PC_2); -AnalogIn ir_analog_in_Distance_R(PC_3); + 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 +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 @@ -52,10 +54,14 @@ //*********************************************************************************************************************************************************** // Hardware controll Setup and functions (motors and sensors) +//these variables represent relative position NOT absolut +float startPos = -0.525; //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 const float counts_per_turn_wheels = 20.0f * 78.125f; // define counts per turn at gearbox end (counts/turn * gearratio) for wheels -const float counts_per_turn_arm = 20.0f * 78.125f * 20.0f; // define counts per turn at gearbox end (counts/turn * gearratio) for arm +const float counts_per_turn_arm = 20.0f * 78.125f * 19.0f; // define counts per turn at gearbox end (counts/turn * gearratio) for arm const float kn = 180.0f / 12.0f; // define motor constant in rpm per V const float k_gear = 100.0f / 78.125f; // define additional ratio in case you are using a dc motor with a different gear box, e.g. 100:1 (DC with 100:1 has 2'000 turns for 360°) const float kp = 0.1f; // define custom kp, this is the default speed controller gain for gear box 78.125:1 @@ -69,39 +75,75 @@ // 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 //*********************************************************************************************************************************************************** -// logic functions for basic movement +// calculations for basic movment and controll //placeholder variables for prototype testing -const int drive_stright_mm = 500; // placeholder for testing drives amount forward -const int drive_back_mm = -100; // placeholder for testing drives amount backwards -int ToNextFunction = 0; // current state of the system (which function is beeing executed) +const int drive_straight_mm = 2; // placeholder for testing drives amount forward +const int drive_back_mm = -2; // placeholder for testing drives amount backwards +int ToNextFunction = 0; // current state of the system (which function is beeing executed) int state=0; +float desired_pos; +// 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_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) +// 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 deg_arm = asin(height_arm / arm_length) * 180.0/pi; // calculates the absolute degrees which the arm has to reach + return deg_arm; +} + +//calculates the deg which the wheels have to turn in order to cover specified distance in mm +//PARAM: distance = distance to drive in milimeter +//RETURN: deg_wheel = degree which the motor has to turn in order to cover distance(mm) +float wheel_dist_to_deg(int distance) +{ + float deg_wheel = distance / (wheel_diameter * pi) * 360; + return deg_wheel; +} -// definition important variables -const float pi = 2 * acos(0.0); // definiton of pi -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 -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) -// import functions from file mapping -extern double powerx(double base, double pow2); -extern double mapping (float adc_value_mV); +// increments the Motor for defined degree from the current one +// PARAM: deg_to_turn = degree to turn the Motor +// PARAM: current_rotation = the current rotation of the Motor (Motor.getRotation()) +// RETURN: new_turn_rotation = new Rotation value in rotations +float turn_relative_deg(float deg_to_turn, float current_rotation) +{ + float new_turn_rotation = current_rotation + deg_to_turn; + return new_turn_rotation; +} -// 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) -float calc_arm_deg_for_height(int height_mm) +// sets the Motor to a specified degree in one rotation +// PARAM: end_deg = new position of the arm in degree 0 <= value >=360 +// PARAM: current_rotation = the current rotation of the Motor (Motor.getRotation()) +// RETURN: new_partial_rotation = new deg value in rotations +float turn_absolut_deg(float end_deg, float current_rotations) { - float deg_arm; - if ((height_mm - dist_arm_ground - (dist_grappleratt_grappler_uk - gripper_area_height)) > arm_length) //check if height is reachable - { - 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. - } - else - { - float height_arm = height_mm - dist_arm_ground - (dist_grappleratt_grappler_uk - gripper_area_height); // calculates the height which only the arm has to cover (- attachement height (arm to robot) etc.) - deg_arm = asin(height_arm / arm_length) * 180.0/pi; // calculates the absolute degrees which the arm has to reach - } - return deg_arm; + int full_rotations; + if(current_rotations > 0) + { + full_rotations = round(current_rotations - 0.5); + } + else if(current_rotations < 0) + { + full_rotations = round(current_rotations + 0.5); + } + else + { + full_rotations = 0; + } + float new_partial_rotation = full_rotations - start_deg_arm/360 + end_deg/360; + return new_partial_rotation; } //calculates position of arm when lift up has ended. @@ -109,66 +151,85 @@ float calc_pos_end_lift() { float end_deg; - end_deg = asin((dist_arm_ground-(dist_grappleratt_grappler_uk-gripper_area_height))/arm_length) + start_deg_arm; + end_deg = asin((dist_arm_ground-(dist_grappleratt_grappler_uk-dist_grappleratt_grappler_uk))/arm_length) + start_deg_arm; end_deg = end_deg * 180 / pi; return end_deg; } -//calculates the deg which the wheels have to turn in order to cover specified distance in mm -//RETURN: deg_wheel = degree which the motor has to turn in order to cover distance(mm) -float wheel_dist_to_deg(int distance) // distance has to be in mm. -{ - float deg_wheel = distance * 360 /(wheel_diameter * pi); - return deg_wheel; +//*********************************************************************************************************************************************************** +// important calculatet constant for Wall-E +const double deg_up_from_horizon_to_stair = calc_arm_deg_for_height(height_stairs); + +// import functions from file mapping +extern double powerx(double base, double pow2); +extern double mapping (float adc_value_mV); + +// +//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 +//returns 2 if the distance isn't in the expected range + +uint8_t nextStepDetection(double distanceCm,double setpointDistance){ + double distance = distanceCm; + double setpoint = setpointDistance; + if(distance == 0){ + return 10; //sensor value is outside the expected range + } + if((distance <= (setpoint + 1)) && (distance >= (setpoint - 1))){ + return 3; //the distance to the next step is in ±1cm of the setpoint + } + if(distance < setpoint){ + return 0; //the robot is to close to the step to rotate the arm unhindered + } + if(distance > setpoint){ + return 1; //the robot is too far away from the next step + } + else{ + return 2; + } + } +//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 +//returns 2 if the distance isn't in the expected range +uint8_t StepDetection_down(float sensor) -// increments the Motor for defined degree from the current one -// PARAM: deg_to_turn = degree to turn the Motor -// PARAM: current_full_rotation = the current rotation of the Motor (Motor.getRotation()) -// RETURN: new Rotation value in rotations -float turn_relative_deg(float deg_to_turn, float current_full_rotation) { - float current_rotations = current_full_rotation; - float new_turn_rotation = current_rotations + deg_to_turn/360.0; - return new_turn_rotation; -} - -// sets the Motor to a specified degree in one rotation -// PARAM: end_deg = new position of the arm in degree 0 <= value >=360 -// PARAM: current_rotations = the current rotation of the Motor (Motor.getRotation()) -// RETURN: new_partial_rotation = new deg value in rotations -float turn_absolut_deg(float end_deg, float current_rotations) -{ - int full_rotations = current_rotations; - float new_partial_rotation = current_rotations - start_deg_arm/360; - return new_partial_rotation; + double d_valueMM = mapping(sensor*1.0e3f*3.3f); + if(d_valueMM >= 4) return 0; + else if( d_valueMM > 100 ) return 2; + else if((d_valueMM < 4)||(d_valueMM==0)) return 1; + + else return 5; } // bring arm in starting position. Height of stairs. int set_arm_stair_height() + { 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 - 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. - return 2; + 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. - 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){ + 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); + if (diff<=0.009){ return 1; } else { - return NULL;} - - - enable_motors = 0; - + return NULL; +} } //Drives forward into the next step @@ -177,21 +238,22 @@ { float diff_R; float diff_L; - float deg_to_turn = wheel_dist_to_deg(distance); - 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()); - ; - 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; +if (desired_pos==0) { + 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); - diff_R= fabs(relativ_turns_rightmotor-positionController_M_right.getRotation()); - diff_L= fabs(relativ_turns_leftmotor-positionController_M_left.getRotation()); - printf("Case 2: Position Left(rot): %3.3f Position Right (rot): %3.3f Desired Rotation Left:%3.3f Desired Rotation Right;%3.3f Diff L:%3.3f Diff R:%3.3f \n", - positionController_M_left.getRotation(),positionController_M_right.getRotation(),relativ_turns_leftmotor, relativ_turns_rightmotor, diff_L, diff_R ); - if ((diff_R<=0.01) && (diff_L<=0.01)) + 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); + if ((diff_R<=0.03) && (diff_L<=0.03)) { return 1; } @@ -201,53 +263,30 @@ } } -//only turns the arm until the robot is on the next step +//turns the arm until the robot is on the next step int lift_up() { float diff; - float position_lift_end_deg = asin((-dist_arm_ground - (dist_grappleratt_grappler_uk-gripper_area_height)) / 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()); + if (desired_pos==0) { + desired_pos = turn_relative_deg(liftPos,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) + 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); + if(diff<=0.03) { return 1; } else { return 0; } - } //*********************************************************************************************************************************************************** -int NextStep (float){ - return 1; -} - - -//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 -//returns 2 if the distance isn't in the expected range - -uint8_t StepDetection(double distance){ - double d_valueMM = distance; - if(d_valueMM >= 4) return 0; - if(d_valueMM < 4) return 1; - if(d_valueMM <= 0 || d_valueMM > 100 ) return 2; - else return 2; - -} -//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() -{ - -} - // while loop gets executed every main_task_period_ms milliseconds int main_task_period_ms = 30; // define main task period time in ms e.g. 30 ms -> main task runns ~33,33 times per second Timer main_task_timer; // create Timer object which we use to run the main task every main task period time in ms @@ -258,74 +297,103 @@ // attach button fall and rise functions to user button object user_button.fall(&user_button_pressed_fcn); user_button.rise(&user_button_released_fcn); - + mechanical_button.mode(PullDown); + while (true) { - enable_motors = 1; + ir_distance_mm_L= mapping(ir_analog_in_Distance_L.read()*1.0e3f * 3.3f); - ir_distance_mm_R= mapping(ir_analog_in_Distance_R.read()*1.0e3f * 3.3f); - ir_distance_mm_Lookdown_B= mapping(ir_analog_in_Lookdown_B.read()*1.0e3f * 3.3f); - ir_distance_mm_Lookdown_F= mapping(ir_analog_in_Lookdown_F.read()*1.0e3f * 3.3f); + + + if (ToNextFunction>=1||(mechanical_button.read()!=1)) + { + enable_motors=1; + } switch (ToNextFunction) { + + case 0: while (mechanical_button.read()!=1) + { + positionController_M_Arm.setDesiredRotation(-1,0.5); + + } + if (mechanical_button){ + positionController_M_Arm.setDesiredRotation(positionController_M_Arm.getRotation()); - case 1: - set_arm_stair_height(); - printf("Case 1: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation()); - if (state==1){ + } + + + break; + + case 1: + ToNextFunction += 1; - } + state=0; + + break; case 2: - state=NextStep(ir_analog_in_Distance_L); - if (state==1){ - ToNextFunction = 0; - } + 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_stright_mm); - printf("Case 2: Position Right(rot): %3.3f; Position Left (rot): %3.3f\n", - positionController_M_right.getRotation(),positionController_M_left.getRotation()); + state=drive_straight(drive_straight_mm); + if (state==1){ - ToNextFunction = 0; + ToNextFunction += 1; + state=0; + desired_pos=0; + } break; case 4: state=lift_up(); - printf("Case 3: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation()); - if ((state==1)&&(StepDetection(ir_distance_mm_Lookdown_B))&&StepDetection(ir_distance_mm_Lookdown_F)){ + + if (state==1){ ToNextFunction += 1; + state=0; + desired_pos=0; } - + 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)){ + + if ((state == 1) && (StepDetection_down(ir_analog_in_Lookdown_B) != 1)){ ToNextFunction += 1; + state=0; + desired_pos=0; + } break; case 6: - state=lift_up(); - printf("Case 5: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation()); + state=set_arm_stair_height(); + if (state==1){ - ToNextFunction = 1; + ToNextFunction = 0; + state=0; + desired_pos=0; } break; + default: ; } } - // read timer and make the main thread sleep for the remaining time span (non blocking) - int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count(); - thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms); - return 0; + // read timer and make the main thread sleep for the remaining time span (non blocking) + int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count(); + thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms); + return 0; } @@ -343,6 +411,6 @@ user_button_timer.stop(); if (user_button_elapsed_time_ms > 200) { - ToNextFunction = 3; + ToNextFunction =1; } } \ No newline at end of file