TreppenRobo / Mbed OS actualProgramm

first commit

Dependencies:   PM2_Libary

main.cpp@43:057640e99f8e, 2022-05-22 (annotated)

Committer:
lupomic
Date:
Sun May 22 10:10:17 2022 +0200
Revision:
43:057640e99f8e
Parent:
42:ec3a88a24666 
merge

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 1:93d997d6b232 1 #include "mbed.h"
pmic 17:c19b471f05cb 2 #include "PM2_Libary.h"
lupomic 33:70ea029a69e8 3 #include <cstdint>
lupomic 37:05252c4a2d4e 4 #include <cstdio>
lupomic 37:05252c4a2d4e 5 #include "math.h"
lupomic 37:05252c4a2d4e 6 //*******************************************************************************************************************************************************************
lupomic 37:05252c4a2d4e 7 // Defined Variables in mm coming from Hardware-team. Need to be updated
lupomic 41:a8557360c15e 8 const float wheel_diameter = 30.0f; // diameter of wheel with caterpillar to calculate mm per wheel turn (4)
lupomic 37:05252c4a2d4e 9 const float arm_length = 118.5; // lenght of arm from pivotpoint to pivotpoint (3)
lupomic 37:05252c4a2d4e 10 const float dist_arm_attach_distsensor = 20; // distance between pivot point arm on body to start distancesensor on top in horizontal (6)
lupomic 37:05252c4a2d4e 11 const float dist_distsensors = 200; // distance between the two distancesensors on top of Wall-E (9)
lupomic 37:05252c4a2d4e 12 const float dist_arm_ground = 51; // distance between pivotpoint arm and ground (5)
lupomic 38:8121e7a79c0b 13 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)
lupomic 38:8121e7a79c0b 14 const float dist_grappleratt_grappler_uk = 36.5; // distance between pivotpoint Grappler and bottom edge (?)
lupomic 35:96ed18b1af94 15
lupomic 37:05252c4a2d4e 16 const float height_stairs = 100; // height to top of next stairstep in mm
lupomic 37:05252c4a2d4e 17 //***********************************************************************************************************************************************************
lupomic 37:05252c4a2d4e 18 // declaration of Input - Output pins
pmic 17:c19b471f05cb 19
pmic 24:86f1a63e35a0 20 // user button on nucleo board
pmic 24:86f1a63e35a0 21 Timer user_button_timer; // create Timer object which we use to check if user button was pressed for a certain time (robust against signal bouncing)
pmic 24:86f1a63e35a0 22 InterruptIn user_button(PC_13); // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
pmic 24:86f1a63e35a0 23 void user_button_pressed_fcn(); // custom functions which gets executed when user button gets pressed and released, definition below
pmic 24:86f1a63e35a0 24 void user_button_released_fcn();
pmic 6:e1fa1a2d7483 25
lupomic 37:05252c4a2d4e 26 // Sharp GP2Y0A41SK0F, 4-40 cm IR Sensor
lupomic 37:05252c4a2d4e 27 // define variable to store measurement from infrared distancesensor in mm
lupomic 37:05252c4a2d4e 28 float ir_distance_mm_L;
lupomic 37:05252c4a2d4e 29 float ir_distance_mm_R;
lupomic 37:05252c4a2d4e 30 float ir_distance_mm_Lookdown_B;
lupomic 37:05252c4a2d4e 31 float ir_distance_mm_Lookdown_F;
pmic 6:e1fa1a2d7483 32
lupomic 42:ec3a88a24666 33 // create AnalogIn object to read in infrared distance sensor, 0...3.3V are mapped to 0...1
lupomic 37:05252c4a2d4e 34 AnalogIn ir_analog_in_Distance_L(PC_2);
lupomic 37:05252c4a2d4e 35 AnalogIn ir_analog_in_Lookdown_B(PC_5);
lupomic 37:05252c4a2d4e 36 AnalogIn ir_analog_in_Lookdown_F(PB_1);
lupomic 33:70ea029a69e8 37
lupomic 42:ec3a88a24666 38 // Digital Inputs
lupomic 38:8121e7a79c0b 39 DigitalIn mechanical_button(PC_3);
lupomic 38:8121e7a79c0b 40
lupomic 42:ec3a88a24666 41
pmic 24:86f1a63e35a0 42 DigitalOut enable_motors(PB_15); // create DigitalOut object to enable dc motors
lupomic 42:ec3a88a24666 43
lupomic 37:05252c4a2d4e 44 float pwm_period_s = 0.00005f; // define pwm period time in seconds and create FastPWM objects to command dc motors
pmic 17:c19b471f05cb 45
lupomic 33:70ea029a69e8 46 //motor pin declaration
lupomic 37:05252c4a2d4e 47 FastPWM pwm_M_right (PB_13); //motor pin decalaration for wheels right side
lupomic 37:05252c4a2d4e 48 FastPWM pwm_M_left (PA_9); //motor pin decalaration for wheels left side
lupomic 37:05252c4a2d4e 49 FastPWM pwm_M_arm (PA_10); //motor pin decalaration for arm
pmic 17:c19b471f05cb 50
lupomic 33:70ea029a69e8 51 //Encoder pin declaration
lupomic 37:05252c4a2d4e 52 EncoderCounter encoder_M_right (PA_6, PC_7); //encoder pin decalaration for wheels right side
lupomic 37:05252c4a2d4e 53 EncoderCounter encoder_M_left (PB_6, PB_7); //encoder pin decalaration for wheels left side
lupomic 37:05252c4a2d4e 54 EncoderCounter encoder_M_arm (PA_0, PA_1); //encoder pin decalaration for arm
lupomic 37:05252c4a2d4e 55 //***********************************************************************************************************************************************************
lupomic 37:05252c4a2d4e 56 // Hardware controll Setup and functions (motors and sensors)
pmic 17:c19b471f05cb 57
lupomic 42:ec3a88a24666 58
lupomic 38:8121e7a79c0b 59
pmic 30:1e8295770bc1 60 // create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
lupomic 37:05252c4a2d4e 61 const float max_voltage = 12.0f; // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
lupomic 41:a8557360c15e 62 const float counts_per_turn_wheels = 20.0f * 78.125f;// define counts per turn at gearbox end (counts/turn * gearratio) for wheels
lupomic 38:8121e7a79c0b 63 const float counts_per_turn_arm = 20.0f * 78.125f * 19.0f; // define counts per turn at gearbox end (counts/turn * gearratio) for arm
lupomic 37:05252c4a2d4e 64 const float kn = 180.0f / 12.0f; // define motor constant in rpm per V
lupomic 37:05252c4a2d4e 65 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°)
lupomic 37:05252c4a2d4e 66 const float kp = 0.1f; // define custom kp, this is the default speed controller gain for gear box 78.125:1
pmic 6:e1fa1a2d7483 67
lupomic 33:70ea029a69e8 68 //motors for tracks
lupomic 33:70ea029a69e8 69 PositionController positionController_M_right(counts_per_turn_wheels * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_right, encoder_M_right); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
lupomic 33:70ea029a69e8 70 PositionController positionController_M_left(counts_per_turn_wheels * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_left, encoder_M_left); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
lupomic 33:70ea029a69e8 71 //Arm Motor
lupomic 33:70ea029a69e8 72 PositionController positionController_M_Arm(counts_per_turn_arm * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_arm, encoder_M_arm); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
pmic 17:c19b471f05cb 73
lupomic 33:70ea029a69e8 74 // PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters
lupomic 33:70ea029a69e8 75 //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
lupomic 37:05252c4a2d4e 76 //***********************************************************************************************************************************************************
lupomic 42:ec3a88a24666 77
pmic 17:c19b471f05cb 78
lupomic 42:ec3a88a24666 79 //these variables represent relative position NOT absolut
lupomic 42:ec3a88a24666 80 float startPos = -0.545; //from last lift up position to start position
lupomic 42:ec3a88a24666 81 float liftPos = -0.555; //from start position to lift up position
pmic 20:7e7325edcf5c 82
lupomic 42:ec3a88a24666 83 const float drive_straight_mm = 200.0;
lupomic 42:ec3a88a24666 84 const float drive_back_mm = -20.0f;
lupomic 38:8121e7a79c0b 85 int ToNextFunction = 0; // current state of the system (which function is beeing executed)
lupomic 42:ec3a88a24666 86 int state=0; //return value of functions
lupomic 38:8121e7a79c0b 87 float desired_pos;
lupomic 42:ec3a88a24666 88 int nextStep=0;
lupomic 39:4c5e4ff386da 89
lupomic 38:8121e7a79c0b 90 // definition variables for calculations
lupomic 38:8121e7a79c0b 91 const float pi = 2 * acos(0.0); // definiton of pi
lupomic 38:8121e7a79c0b 92 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.
lupomic 38:8121e7a79c0b 93 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)
lupomic 38:8121e7a79c0b 94
lupomic 38:8121e7a79c0b 95 // definition of rotation speeds for motors 0 = none 1.0 = max.
lupomic 42:ec3a88a24666 96 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
lupomic 38:8121e7a79c0b 97 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
lupomic 38:8121e7a79c0b 98
lupomic 38:8121e7a79c0b 99 // 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)
lupomic 38:8121e7a79c0b 100 // PARAM: height_mm = height which OK Gripperarea has to reach.
lupomic 38:8121e7a79c0b 101 // RETURN: deg_arm = absolut Position in deg that the arm has to take.
lupomic 38:8121e7a79c0b 102 float calc_arm_deg_for_height(int height_mm)
lupomic 38:8121e7a79c0b 103 {
lupomic 38:8121e7a79c0b 104 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.)
lupomic 38:8121e7a79c0b 105 float deg_arm = asin(height_arm / arm_length) * 180.0/pi; // calculates the absolute degrees which the arm has to reach
lupomic 38:8121e7a79c0b 106 return deg_arm;
lupomic 38:8121e7a79c0b 107 }
lupomic 38:8121e7a79c0b 108
lupomic 38:8121e7a79c0b 109 //calculates the deg which the wheels have to turn in order to cover specified distance in mm
lupomic 38:8121e7a79c0b 110 //PARAM: distance = distance to drive in milimeter
lupomic 38:8121e7a79c0b 111 //RETURN: deg_wheel = degree which the motor has to turn in order to cover distance(mm)
lupomic 41:a8557360c15e 112 float wheel_dist_to_deg(float distance)
lupomic 38:8121e7a79c0b 113 {
lupomic 42:ec3a88a24666 114 float deg_wheel = (distance) / (wheel_diameter * pi) ;
lupomic 38:8121e7a79c0b 115 return deg_wheel;
lupomic 38:8121e7a79c0b 116 }
lupomic 33:70ea029a69e8 117
lupomic 36:a48b21a9635c 118
lupomic 38:8121e7a79c0b 119 // increments the Motor for defined degree from the current one
lupomic 38:8121e7a79c0b 120 // PARAM: deg_to_turn = degree to turn the Motor
lupomic 38:8121e7a79c0b 121 // PARAM: current_rotation = the current rotation of the Motor (Motor.getRotation())
lupomic 38:8121e7a79c0b 122 // RETURN: new_turn_rotation = new Rotation value in rotations
lupomic 38:8121e7a79c0b 123 float turn_relative_deg(float deg_to_turn, float current_rotation)
lupomic 38:8121e7a79c0b 124 {
lupomic 38:8121e7a79c0b 125 float new_turn_rotation = current_rotation + deg_to_turn;
lupomic 38:8121e7a79c0b 126 return new_turn_rotation;
lupomic 38:8121e7a79c0b 127 }
lupomic 33:70ea029a69e8 128
lupomic 38:8121e7a79c0b 129 // sets the Motor to a specified degree in one rotation
lupomic 38:8121e7a79c0b 130 // PARAM: end_deg = new position of the arm in degree 0 <= value >=360
lupomic 38:8121e7a79c0b 131 // PARAM: current_rotation = the current rotation of the Motor (Motor.getRotation())
lupomic 38:8121e7a79c0b 132 // RETURN: new_partial_rotation = new deg value in rotations
lupomic 38:8121e7a79c0b 133 float turn_absolut_deg(float end_deg, float current_rotations)
lupomic 37:05252c4a2d4e 134 {
lupomic 38:8121e7a79c0b 135 int full_rotations;
lupomic 38:8121e7a79c0b 136 if(current_rotations > 0)
lupomic 38:8121e7a79c0b 137 {
lupomic 38:8121e7a79c0b 138 full_rotations = round(current_rotations - 0.5);
lupomic 38:8121e7a79c0b 139 }
lupomic 38:8121e7a79c0b 140 else if(current_rotations < 0)
lupomic 38:8121e7a79c0b 141 {
lupomic 38:8121e7a79c0b 142 full_rotations = round(current_rotations + 0.5);
lupomic 38:8121e7a79c0b 143 }
lupomic 38:8121e7a79c0b 144 else
lupomic 38:8121e7a79c0b 145 {
lupomic 38:8121e7a79c0b 146 full_rotations = 0;
lupomic 38:8121e7a79c0b 147 }
lupomic 38:8121e7a79c0b 148 float new_partial_rotation = full_rotations - start_deg_arm/360 + end_deg/360;
lupomic 38:8121e7a79c0b 149 return new_partial_rotation;
lupomic 37:05252c4a2d4e 150 }
lupomic 33:70ea029a69e8 151
lupomic 37:05252c4a2d4e 152 //calculates position of arm when lift up has ended.
lupomic 37:05252c4a2d4e 153 //RETURN: end_deg = degree which the motor has to turn in order to reach end lift position.
lupomic 37:05252c4a2d4e 154 float calc_pos_end_lift()
lupomic 37:05252c4a2d4e 155 {
lupomic 37:05252c4a2d4e 156 float end_deg;
lupomic 38:8121e7a79c0b 157 end_deg = asin((dist_arm_ground-(dist_grappleratt_grappler_uk-dist_grappleratt_grappler_uk))/arm_length) + start_deg_arm;
lupomic 37:05252c4a2d4e 158 end_deg = end_deg * 180 / pi;
lupomic 37:05252c4a2d4e 159 return end_deg;
lupomic 37:05252c4a2d4e 160 }
lupomic 37:05252c4a2d4e 161
lupomic 38:8121e7a79c0b 162 //***********************************************************************************************************************************************************
lupomic 38:8121e7a79c0b 163 // important calculatet constant for Wall-E
lupomic 38:8121e7a79c0b 164 const double deg_up_from_horizon_to_stair = calc_arm_deg_for_height(height_stairs);
lupomic 38:8121e7a79c0b 165
lupomic 38:8121e7a79c0b 166 // import functions from file mapping
lupomic 38:8121e7a79c0b 167 extern double powerx(double base, double pow2);
lupomic 38:8121e7a79c0b 168 extern double mapping (float adc_value_mV);
lupomic 38:8121e7a79c0b 169
lupomic 38:8121e7a79c0b 170 //
lupomic 38:8121e7a79c0b 171 //simple check if there is an object in proximity
lupomic 38:8121e7a79c0b 172 //returns 0 if there is NO object present
lupomic 38:8121e7a79c0b 173 //returns 1 if there is an object present
lupomic 38:8121e7a79c0b 174 //returns 2 if the distance isn't in the expected range
lupomic 38:8121e7a79c0b 175
lupomic 38:8121e7a79c0b 176 uint8_t nextStepDetection(double distanceCm,double setpointDistance){
lupomic 38:8121e7a79c0b 177 double distance = distanceCm;
lupomic 38:8121e7a79c0b 178 double setpoint = setpointDistance;
lupomic 38:8121e7a79c0b 179 if(distance == 0){
lupomic 38:8121e7a79c0b 180 return 10; //sensor value is outside the expected range
lupomic 38:8121e7a79c0b 181 }
lupomic 41:a8557360c15e 182 if((distance <= (setpoint + 2)) && (distance >= (setpoint - 2))){
lupomic 38:8121e7a79c0b 183 return 3; //the distance to the next step is in ±1cm of the setpoint
lupomic 38:8121e7a79c0b 184 }
lupomic 38:8121e7a79c0b 185 if(distance < setpoint){
lupomic 38:8121e7a79c0b 186 return 0; //the robot is to close to the step to rotate the arm unhindered
lupomic 38:8121e7a79c0b 187 }
lupomic 38:8121e7a79c0b 188 if(distance > setpoint){
lupomic 38:8121e7a79c0b 189 return 1; //the robot is too far away from the next step
lupomic 38:8121e7a79c0b 190 }
lupomic 38:8121e7a79c0b 191 else{
lupomic 38:8121e7a79c0b 192 return 2;
lupomic 38:8121e7a79c0b 193 }
lupomic 38:8121e7a79c0b 194
lupomic 33:70ea029a69e8 195 }
lupomic 38:8121e7a79c0b 196 //simple check if there is an object in proximity
lupomic 38:8121e7a79c0b 197 //returns 0 if there is NO object present
lupomic 38:8121e7a79c0b 198 //returns 1 if there is an object present
lupomic 38:8121e7a79c0b 199 //returns 2 if the distance isn't in the expected range
lupomic 38:8121e7a79c0b 200 uint8_t StepDetection_down(float sensor)
lupomic 37:05252c4a2d4e 201
lupomic 37:05252c4a2d4e 202 {
lupomic 38:8121e7a79c0b 203 double d_valueMM = mapping(sensor*1.0e3f*3.3f);
lupomic 38:8121e7a79c0b 204 if(d_valueMM >= 4) return 0;
lupomic 38:8121e7a79c0b 205 else if( d_valueMM > 100 ) return 2;
lupomic 38:8121e7a79c0b 206 else if((d_valueMM < 4)||(d_valueMM==0)) return 1;
lupomic 38:8121e7a79c0b 207
lupomic 38:8121e7a79c0b 208 else return 5;
lupomic 37:05252c4a2d4e 209 }
lupomic 33:70ea029a69e8 210
lupomic 37:05252c4a2d4e 211 // bring arm in starting position. Height of stairs.
lupomic 37:05252c4a2d4e 212 int set_arm_stair_height()
lupomic 38:8121e7a79c0b 213
lupomic 37:05252c4a2d4e 214 {
lupomic 37:05252c4a2d4e 215 float diff;
lupomic 42:ec3a88a24666 216 int gripper=nextStepDetection(ir_distance_mm_L, 2);
lupomic 38:8121e7a79c0b 217
lupomic 42:ec3a88a24666 218 //first step to calculate desired position
lupomic 41:a8557360c15e 219 if (desired_pos==0) {
lupomic 41:a8557360c15e 220 desired_pos=turn_relative_deg(startPos, positionController_M_Arm.getRotation());
lupomic 42:ec3a88a24666 221 positionController_M_Arm.setDesiredRotation(desired_pos, 0.5); // command to turn motor to desired deg.
lupomic 41:a8557360c15e 222 }
lupomic 42:ec3a88a24666 223 // to check if the position controller is finished
lupomic 42:ec3a88a24666 224 diff =abs( desired_pos-(positionController_M_Arm.getRotation()));
lupomic 37:05252c4a2d4e 225
lupomic 42:ec3a88a24666 226 //prints for testing
lupomic 40:04b032b01dd5 227 printf("Set arm Position ARM (rot): %3.3f Desired:%3.3f State:%d ToNextfunction:%d Diff:%3.3f\n",
lupomic 40:04b032b01dd5 228 positionController_M_Arm.getRotation(), desired_pos, state, ToNextFunction, diff);
lupomic 41:a8557360c15e 229
lupomic 42:ec3a88a24666 230 // stops the positioning, when the gripper is in proximity of the sensor
lupomic 42:ec3a88a24666 231 if (gripper==3){
lupomic 42:ec3a88a24666 232 desired_pos=turn_relative_deg(-0.01, positionController_M_Arm.getRotation() );
lupomic 42:ec3a88a24666 233 positionController_M_Arm.setDesiredRotation(desired_pos, 0.2);
lupomic 41:a8557360c15e 234 }
lupomic 41:a8557360c15e 235
lupomic 42:ec3a88a24666 236 if((diff<0.008)&&gripper){
lupomic 37:05252c4a2d4e 237 return 1;
lupomic 37:05252c4a2d4e 238 }
lupomic 37:05252c4a2d4e 239 else {
lupomic 38:8121e7a79c0b 240 return NULL;
lupomic 38:8121e7a79c0b 241 }
lupomic 37:05252c4a2d4e 242 }
lupomic 37:05252c4a2d4e 243
lupomic 37:05252c4a2d4e 244 //Drives forward into the next step
lupomic 37:05252c4a2d4e 245 //Prameter:distance in milimeter
lupomic 37:05252c4a2d4e 246 int drive_straight(float distance)
lupomic 37:05252c4a2d4e 247 {
lupomic 37:05252c4a2d4e 248 float diff_R;
lupomic 37:05252c4a2d4e 249 float diff_L;
lupomic 34:9f779e91168e 250
lupomic 42:ec3a88a24666 251 // calculates the desired position
lupomic 38:8121e7a79c0b 252 if (desired_pos==0) {
lupomic 42:ec3a88a24666 253 desired_pos=wheel_dist_to_deg(distance);
lupomic 38:8121e7a79c0b 254 float relativ_turns_rightmotor = turn_relative_deg(desired_pos, positionController_M_right.getRotation());
lupomic 38:8121e7a79c0b 255 float relativ_turns_leftmotor = turn_relative_deg(desired_pos, positionController_M_left.getRotation());
lupomic 38:8121e7a79c0b 256 }
lupomic 42:ec3a88a24666 257
lupomic 42:ec3a88a24666 258
lupomic 38:8121e7a79c0b 259 positionController_M_right.setDesiredRotation(desired_pos, max_speed_rps_wheel);
lupomic 38:8121e7a79c0b 260 positionController_M_left.setDesiredRotation(desired_pos, max_speed_rps_wheel);
lupomic 33:70ea029a69e8 261
lupomic 42:ec3a88a24666 262 // to check if the position controller are finished
lupomic 38:8121e7a79c0b 263 diff_R= abs(desired_pos-(positionController_M_right.getRotation()));
lupomic 38:8121e7a79c0b 264 diff_L= abs(desired_pos-(positionController_M_left.getRotation()));
lupomic 42:ec3a88a24666 265
lupomic 42:ec3a88a24666 266 //prints for testing
lupomic 41:a8557360c15e 267 printf("Drive Straight Position Right(rot): %3.3f; Position Left (rot): %3.3f Desired: %3.3f Diff:%3.3f State:%d ToNextfunction:%d\n",
lupomic 41:a8557360c15e 268 positionController_M_right.getRotation(),positionController_M_left.getRotation(),desired_pos,diff_L, state, ToNextFunction);
lupomic 42:ec3a88a24666 269
lupomic 42:ec3a88a24666 270
lupomic 42:ec3a88a24666 271 if ((diff_R<=0.02) && (diff_L<=0.02))
lupomic 37:05252c4a2d4e 272 {
lupomic 37:05252c4a2d4e 273 return 1;
lupomic 37:05252c4a2d4e 274 }
lupomic 37:05252c4a2d4e 275 else
lupomic 37:05252c4a2d4e 276 {
lupomic 37:05252c4a2d4e 277 return 0;
lupomic 37:05252c4a2d4e 278 }
lupomic 33:70ea029a69e8 279 }
lupomic 33:70ea029a69e8 280
lupomic 38:8121e7a79c0b 281 //turns the arm until the robot is on the next step
lupomic 37:05252c4a2d4e 282 int lift_up()
lupomic 37:05252c4a2d4e 283 {
lupomic 37:05252c4a2d4e 284 float diff;
lupomic 42:ec3a88a24666 285
lupomic 42:ec3a88a24666 286 // calculates the desired position
lupomic 38:8121e7a79c0b 287 if (desired_pos==0) {
lupomic 38:8121e7a79c0b 288 desired_pos = turn_relative_deg(liftPos,positionController_M_Arm.getRotation());
lupomic 38:8121e7a79c0b 289 }
lupomic 38:8121e7a79c0b 290
lupomic 38:8121e7a79c0b 291 positionController_M_Arm.setDesiredRotation(desired_pos, max_speed_rps_arm);
lupomic 42:ec3a88a24666 292
lupomic 42:ec3a88a24666 293 // to check if the position controller is finished
lupomic 38:8121e7a79c0b 294 diff=abs(desired_pos-positionController_M_Arm.getRotation());
lupomic 42:ec3a88a24666 295
lupomic 42:ec3a88a24666 296 //prints for testing
lupomic 42:ec3a88a24666 297 printf("Lift Up: Position ARM (rot): %3.3f Desired:%3.3f State:%d ToNextfunction:%d\n",positionController_M_Arm.getRotation(),desired_pos, state, ToNextFunction);
lupomic 42:ec3a88a24666 298
lupomic 38:8121e7a79c0b 299 if(diff<=0.03)
lupomic 37:05252c4a2d4e 300 { return 1;
lupomic 37:05252c4a2d4e 301 }
lupomic 37:05252c4a2d4e 302 else
lupomic 37:05252c4a2d4e 303 { return 0;
lupomic 37:05252c4a2d4e 304 }
lupomic 33:70ea029a69e8 305
lupomic 37:05252c4a2d4e 306 }
lupomic 37:05252c4a2d4e 307 //***********************************************************************************************************************************************************
lupomic 37:05252c4a2d4e 308
lupomic 37:05252c4a2d4e 309 // while loop gets executed every main_task_period_ms milliseconds
lupomic 37:05252c4a2d4e 310 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
lupomic 37:05252c4a2d4e 311 Timer main_task_timer; // create Timer object which we use to run the main task every main task period time in ms
lupomic 37:05252c4a2d4e 312 //***********************************************************************************************************************************************************
lupomic 37:05252c4a2d4e 313
lupomic 33:70ea029a69e8 314 int main(void)
pmic 23:26b3a25fc637 315 {
pmic 24:86f1a63e35a0 316 // attach button fall and rise functions to user button object
lupomic 37:05252c4a2d4e 317 user_button.fall(&user_button_pressed_fcn);
lupomic 37:05252c4a2d4e 318 user_button.rise(&user_button_released_fcn);
lupomic 38:8121e7a79c0b 319 mechanical_button.mode(PullDown);
lupomic 42:ec3a88a24666 320
lupomic 37:05252c4a2d4e 321 while (true)
lupomic 37:05252c4a2d4e 322 {
lupomic 38:8121e7a79c0b 323
lupomic 37:05252c4a2d4e 324 ir_distance_mm_L= mapping(ir_analog_in_Distance_L.read()*1.0e3f * 3.3f);
lupomic 38:8121e7a79c0b 325
lupomic 38:8121e7a79c0b 326
lupomic 42:ec3a88a24666 327 if (ToNextFunction>=1||(mechanical_button.read()!=1)) {
lupomic 38:8121e7a79c0b 328 enable_motors=1;
lupomic 38:8121e7a79c0b 329 }
lupomic 41:a8557360c15e 330
lupomic 41:a8557360c15e 331
lupomic 37:05252c4a2d4e 332 switch (ToNextFunction)
lupomic 37:05252c4a2d4e 333 {
lupomic 41:a8557360c15e 334
lupomic 42:ec3a88a24666 335 // case 0: For referencing the arm position
lupomic 38:8121e7a79c0b 336 case 0: while (mechanical_button.read()!=1)
lupomic 38:8121e7a79c0b 337 {
lupomic 38:8121e7a79c0b 338 positionController_M_Arm.setDesiredRotation(-1,0.5);
lupomic 38:8121e7a79c0b 339
lupomic 38:8121e7a79c0b 340 }
lupomic 38:8121e7a79c0b 341 if (mechanical_button){
lupomic 38:8121e7a79c0b 342 positionController_M_Arm.setDesiredRotation(positionController_M_Arm.getRotation());
lupomic 34:9f779e91168e 343
lupomic 38:8121e7a79c0b 344 }
lupomic 38:8121e7a79c0b 345 break;
lupomic 38:8121e7a79c0b 346
lupomic 42:ec3a88a24666 347 // case 1:
lupomic 38:8121e7a79c0b 348 case 1:
lupomic 39:4c5e4ff386da 349
lupomic 39:4c5e4ff386da 350 ToNextFunction +=1;
lupomic 39:4c5e4ff386da 351 state=0;
lupomic 37:05252c4a2d4e 352 break;
pmic 6:e1fa1a2d7483 353
lupomic 42:ec3a88a24666 354 // case 2: drive too the stair
lupomic 39:4c5e4ff386da 355 case 2: state=drive_straight(drive_straight_mm);
lupomic 38:8121e7a79c0b 356
lupomic 37:05252c4a2d4e 357 if (state==1){
lupomic 38:8121e7a79c0b 358 ToNextFunction += 1;
lupomic 38:8121e7a79c0b 359 state=0;
lupomic 38:8121e7a79c0b 360 desired_pos=0;
lupomic 37:05252c4a2d4e 361 }
lupomic 33:70ea029a69e8 362 break;
lupomic 37:05252c4a2d4e 363
lupomic 42:ec3a88a24666 364 // case 3: lift the roboer up
lupomic 39:4c5e4ff386da 365 case 3: state=lift_up();
lupomic 38:8121e7a79c0b 366
lupomic 38:8121e7a79c0b 367 if (state==1){
lupomic 37:05252c4a2d4e 368 ToNextFunction += 1;
lupomic 38:8121e7a79c0b 369 state=0;
lupomic 38:8121e7a79c0b 370 desired_pos=0;
lupomic 37:05252c4a2d4e 371 }
lupomic 39:4c5e4ff386da 372 break;
lupomic 39:4c5e4ff386da 373
lupomic 42:ec3a88a24666 374 // case 4: detect if there is a next step
lupomic 41:a8557360c15e 375 case 4: state=nextStepDetection(ir_distance_mm_L,4);
lupomic 42:ec3a88a24666 376 // if there is a next step, variable nextStep=1
lupomic 39:4c5e4ff386da 377 if (state==3){
lupomic 40:04b032b01dd5 378 nextStep=1;
lupomic 39:4c5e4ff386da 379 }
lupomic 42:ec3a88a24666 380
lupomic 42:ec3a88a24666 381
lupomic 40:04b032b01dd5 382 ToNextFunction +=1;
lupomic 40:04b032b01dd5 383 state=0;
lupomic 42:ec3a88a24666 384
lupomic 42:ec3a88a24666 385 printf("distance:%3.3f Output:%d NextStep:%d\n ", ir_distance_mm_L, nextStepDetection(ir_distance_mm_L,4), nextStep);
lupomic 38:8121e7a79c0b 386 break;
lupomic 37:05252c4a2d4e 387
lupomic 42:ec3a88a24666 388 // case 5: Drive the roboter back until there is no step underneath the lookdown sensor
lupomic 37:05252c4a2d4e 389 case 5:
lupomic 37:05252c4a2d4e 390 state=drive_straight(drive_back_mm);
lupomic 38:8121e7a79c0b 391
lupomic 39:4c5e4ff386da 392 if (StepDetection_down(ir_analog_in_Lookdown_B) != 1)
lupomic 39:4c5e4ff386da 393 {
lupomic 37:05252c4a2d4e 394 ToNextFunction += 1;
lupomic 38:8121e7a79c0b 395 state=0;
lupomic 38:8121e7a79c0b 396 desired_pos=0;
lupomic 39:4c5e4ff386da 397 positionController_M_left.setDesiredRotation(positionController_M_left.getRotation());
lupomic 39:4c5e4ff386da 398 positionController_M_right.setDesiredRotation(positionController_M_right.getRotation());
lupomic 38:8121e7a79c0b 399
lupomic 37:05252c4a2d4e 400 }
lupomic 33:70ea029a69e8 401 break;
lupomic 37:05252c4a2d4e 402
lupomic 42:ec3a88a24666 403 //case 6: bring arm back to starting positon
lupomic 37:05252c4a2d4e 404 case 6:
lupomic 38:8121e7a79c0b 405 state=set_arm_stair_height();
lupomic 42:ec3a88a24666 406 //if there is a next step, continue to climb up
lupomic 40:04b032b01dd5 407 if ((state==1) && (nextStep)){
lupomic 39:4c5e4ff386da 408 ToNextFunction = 1;
lupomic 38:8121e7a79c0b 409 state=0;
lupomic 38:8121e7a79c0b 410 desired_pos=0;
lupomic 40:04b032b01dd5 411 nextStep=0;
lupomic 39:4c5e4ff386da 412 }
lupomic 42:ec3a88a24666 413 //if there is no next step, stop
lupomic 40:04b032b01dd5 414 if ((state==1) && (nextStep!=1)) {
lupomic 39:4c5e4ff386da 415 ToNextFunction=0;
lupomic 39:4c5e4ff386da 416 state=0;
lupomic 39:4c5e4ff386da 417 desired_pos=0;
lupomic 40:04b032b01dd5 418 nextStep=0;
lupomic 37:05252c4a2d4e 419 }
lupomic 33:70ea029a69e8 420 break;
lupomic 38:8121e7a79c0b 421
lupomic 34:9f779e91168e 422 default: ;
lupomic 33:70ea029a69e8 423 }
lupomic 33:70ea029a69e8 424 }
lupomic 38:8121e7a79c0b 425 // read timer and make the main thread sleep for the remaining time span (non blocking)
lupomic 38:8121e7a79c0b 426 int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
lupomic 38:8121e7a79c0b 427 thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
lupomic 38:8121e7a79c0b 428 return 0;
pmic 1:93d997d6b232 429 }
pmic 6:e1fa1a2d7483 430
lupomic 33:70ea029a69e8 431
lupomic 37:05252c4a2d4e 432
pmic 24:86f1a63e35a0 433 void user_button_pressed_fcn()
pmic 25:ea1d6e27c895 434 {
pmic 26:28693b369945 435 user_button_timer.start();
pmic 6:e1fa1a2d7483 436 user_button_timer.reset();
pmic 6:e1fa1a2d7483 437 }
pmic 6:e1fa1a2d7483 438
lupomic 37:05252c4a2d4e 439 void user_button_released_fcn()
lupomic 37:05252c4a2d4e 440 {
pmic 24:86f1a63e35a0 441 // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
pmic 24:86f1a63e35a0 442 int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 443 user_button_timer.stop();
lupomic 37:05252c4a2d4e 444 if (user_button_elapsed_time_ms > 200)
lupomic 37:05252c4a2d4e 445 {
lupomic 40:04b032b01dd5 446 ToNextFunction =1;
lupomic 37:05252c4a2d4e 447 }
lupomic 37:05252c4a2d4e 448 }