TreppenRobo / Mbed OS actualProgramm

first commit

Dependencies:   PM2_Libary

main.cpp@38:8121e7a79c0b, 2022-05-16 (annotated)

Committer:
lupomic
Date:
Mon May 16 16:37:07 2022 +0200
Branch:
lupo
Revision:
38:8121e7a79c0b
Parent:
37:05252c4a2d4e
Child:
39:4c5e4ff386da
commit after testing

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 37:05252c4a2d4e 8 const float wheel_diameter = 30; // 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 37:05252c4a2d4e 33 AnalogIn ir_analog_in_Distance_L(PC_2);
lupomic 38:8121e7a79c0b 34
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 37:05252c4a2d4e 37 // create AnalogIn object to read in infrared distance sensor, 0...3.3V are mapped to 0...1
lupomic 33:70ea029a69e8 38
lupomic 38:8121e7a79c0b 39 DigitalIn mechanical_button(PC_3);
lupomic 38:8121e7a79c0b 40
pmic 24:86f1a63e35a0 41 // 78:1, 100:1, ... Metal Gearmotor 20Dx44L mm 12V CB
pmic 24:86f1a63e35a0 42 DigitalOut enable_motors(PB_15); // create DigitalOut object to enable dc motors
lupomic 37:05252c4a2d4e 43 float pwm_period_s = 0.00005f; // define pwm period time in seconds and create FastPWM objects to command dc motors
pmic 17:c19b471f05cb 44
lupomic 33:70ea029a69e8 45 //motor pin declaration
lupomic 37:05252c4a2d4e 46 FastPWM pwm_M_right (PB_13); //motor pin decalaration for wheels right side
lupomic 37:05252c4a2d4e 47 FastPWM pwm_M_left (PA_9); //motor pin decalaration for wheels left side
lupomic 37:05252c4a2d4e 48 FastPWM pwm_M_arm (PA_10); //motor pin decalaration for arm
pmic 17:c19b471f05cb 49
lupomic 33:70ea029a69e8 50 //Encoder pin declaration
lupomic 37:05252c4a2d4e 51 EncoderCounter encoder_M_right (PA_6, PC_7); //encoder pin decalaration for wheels right side
lupomic 37:05252c4a2d4e 52 EncoderCounter encoder_M_left (PB_6, PB_7); //encoder pin decalaration for wheels left side
lupomic 37:05252c4a2d4e 53 EncoderCounter encoder_M_arm (PA_0, PA_1); //encoder pin decalaration for arm
lupomic 37:05252c4a2d4e 54 //***********************************************************************************************************************************************************
lupomic 37:05252c4a2d4e 55 // Hardware controll Setup and functions (motors and sensors)
pmic 17:c19b471f05cb 56
lupomic 38:8121e7a79c0b 57 //these variables represent relative position NOT absolut
lupomic 38:8121e7a79c0b 58 float startPos = -0.525; //from last lift up position to start position
lupomic 38:8121e7a79c0b 59 float liftPos = -0.555; //from start position to lift up position
lupomic 38:8121e7a79c0b 60
pmic 30:1e8295770bc1 61 // create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
lupomic 37:05252c4a2d4e 62 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 37:05252c4a2d4e 63 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 64 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 65 const float kn = 180.0f / 12.0f; // define motor constant in rpm per V
lupomic 37:05252c4a2d4e 66 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 67 const float kp = 0.1f; // define custom kp, this is the default speed controller gain for gear box 78.125:1
pmic 6:e1fa1a2d7483 68
lupomic 33:70ea029a69e8 69 //motors for tracks
lupomic 33:70ea029a69e8 70 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 71 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 72 //Arm Motor
lupomic 33:70ea029a69e8 73 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 74
lupomic 33:70ea029a69e8 75 // 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 76 //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 77 //***********************************************************************************************************************************************************
lupomic 38:8121e7a79c0b 78 // calculations for basic movment and controll
pmic 17:c19b471f05cb 79
lupomic 37:05252c4a2d4e 80 //placeholder variables for prototype testing
pmic 20:7e7325edcf5c 81
lupomic 38:8121e7a79c0b 82 const int drive_straight_mm = 2; // placeholder for testing drives amount forward
lupomic 38:8121e7a79c0b 83 const int drive_back_mm = -2; // placeholder for testing drives amount backwards
lupomic 38:8121e7a79c0b 84 int ToNextFunction = 0; // current state of the system (which function is beeing executed)
lupomic 37:05252c4a2d4e 85 int state=0;
lupomic 38:8121e7a79c0b 86 float desired_pos;
lupomic 38:8121e7a79c0b 87 // definition variables for calculations
lupomic 38:8121e7a79c0b 88 const float pi = 2 * acos(0.0); // definiton of pi
lupomic 38:8121e7a79c0b 89 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 90 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 91
lupomic 38:8121e7a79c0b 92 // definition of rotation speeds for motors 0 = none 1.0 = max.
lupomic 38:8121e7a79c0b 93 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
lupomic 38:8121e7a79c0b 94 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 95
lupomic 38:8121e7a79c0b 96 // 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 97 // PARAM: height_mm = height which OK Gripperarea has to reach.
lupomic 38:8121e7a79c0b 98 // RETURN: deg_arm = absolut Position in deg that the arm has to take.
lupomic 38:8121e7a79c0b 99 float calc_arm_deg_for_height(int height_mm)
lupomic 38:8121e7a79c0b 100 {
lupomic 38:8121e7a79c0b 101 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 102 float deg_arm = asin(height_arm / arm_length) * 180.0/pi; // calculates the absolute degrees which the arm has to reach
lupomic 38:8121e7a79c0b 103 return deg_arm;
lupomic 38:8121e7a79c0b 104 }
lupomic 38:8121e7a79c0b 105
lupomic 38:8121e7a79c0b 106 //calculates the deg which the wheels have to turn in order to cover specified distance in mm
lupomic 38:8121e7a79c0b 107 //PARAM: distance = distance to drive in milimeter
lupomic 38:8121e7a79c0b 108 //RETURN: deg_wheel = degree which the motor has to turn in order to cover distance(mm)
lupomic 38:8121e7a79c0b 109 float wheel_dist_to_deg(int distance)
lupomic 38:8121e7a79c0b 110 {
lupomic 38:8121e7a79c0b 111 float deg_wheel = distance / (wheel_diameter * pi) * 360;
lupomic 38:8121e7a79c0b 112 return deg_wheel;
lupomic 38:8121e7a79c0b 113 }
lupomic 33:70ea029a69e8 114
lupomic 36:a48b21a9635c 115
lupomic 38:8121e7a79c0b 116 // increments the Motor for defined degree from the current one
lupomic 38:8121e7a79c0b 117 // PARAM: deg_to_turn = degree to turn the Motor
lupomic 38:8121e7a79c0b 118 // PARAM: current_rotation = the current rotation of the Motor (Motor.getRotation())
lupomic 38:8121e7a79c0b 119 // RETURN: new_turn_rotation = new Rotation value in rotations
lupomic 38:8121e7a79c0b 120 float turn_relative_deg(float deg_to_turn, float current_rotation)
lupomic 38:8121e7a79c0b 121 {
lupomic 38:8121e7a79c0b 122 float new_turn_rotation = current_rotation + deg_to_turn;
lupomic 38:8121e7a79c0b 123 return new_turn_rotation;
lupomic 38:8121e7a79c0b 124 }
lupomic 33:70ea029a69e8 125
lupomic 38:8121e7a79c0b 126 // sets the Motor to a specified degree in one rotation
lupomic 38:8121e7a79c0b 127 // PARAM: end_deg = new position of the arm in degree 0 <= value >=360
lupomic 38:8121e7a79c0b 128 // PARAM: current_rotation = the current rotation of the Motor (Motor.getRotation())
lupomic 38:8121e7a79c0b 129 // RETURN: new_partial_rotation = new deg value in rotations
lupomic 38:8121e7a79c0b 130 float turn_absolut_deg(float end_deg, float current_rotations)
lupomic 37:05252c4a2d4e 131 {
lupomic 38:8121e7a79c0b 132 int full_rotations;
lupomic 38:8121e7a79c0b 133 if(current_rotations > 0)
lupomic 38:8121e7a79c0b 134 {
lupomic 38:8121e7a79c0b 135 full_rotations = round(current_rotations - 0.5);
lupomic 38:8121e7a79c0b 136 }
lupomic 38:8121e7a79c0b 137 else if(current_rotations < 0)
lupomic 38:8121e7a79c0b 138 {
lupomic 38:8121e7a79c0b 139 full_rotations = round(current_rotations + 0.5);
lupomic 38:8121e7a79c0b 140 }
lupomic 38:8121e7a79c0b 141 else
lupomic 38:8121e7a79c0b 142 {
lupomic 38:8121e7a79c0b 143 full_rotations = 0;
lupomic 38:8121e7a79c0b 144 }
lupomic 38:8121e7a79c0b 145 float new_partial_rotation = full_rotations - start_deg_arm/360 + end_deg/360;
lupomic 38:8121e7a79c0b 146 return new_partial_rotation;
lupomic 37:05252c4a2d4e 147 }
lupomic 33:70ea029a69e8 148
lupomic 37:05252c4a2d4e 149 //calculates position of arm when lift up has ended.
lupomic 37:05252c4a2d4e 150 //RETURN: end_deg = degree which the motor has to turn in order to reach end lift position.
lupomic 37:05252c4a2d4e 151 float calc_pos_end_lift()
lupomic 37:05252c4a2d4e 152 {
lupomic 37:05252c4a2d4e 153 float end_deg;
lupomic 38:8121e7a79c0b 154 end_deg = asin((dist_arm_ground-(dist_grappleratt_grappler_uk-dist_grappleratt_grappler_uk))/arm_length) + start_deg_arm;
lupomic 37:05252c4a2d4e 155 end_deg = end_deg * 180 / pi;
lupomic 37:05252c4a2d4e 156 return end_deg;
lupomic 37:05252c4a2d4e 157 }
lupomic 37:05252c4a2d4e 158
lupomic 38:8121e7a79c0b 159 //***********************************************************************************************************************************************************
lupomic 38:8121e7a79c0b 160 // important calculatet constant for Wall-E
lupomic 38:8121e7a79c0b 161 const double deg_up_from_horizon_to_stair = calc_arm_deg_for_height(height_stairs);
lupomic 38:8121e7a79c0b 162
lupomic 38:8121e7a79c0b 163 // import functions from file mapping
lupomic 38:8121e7a79c0b 164 extern double powerx(double base, double pow2);
lupomic 38:8121e7a79c0b 165 extern double mapping (float adc_value_mV);
lupomic 38:8121e7a79c0b 166
lupomic 38:8121e7a79c0b 167 //
lupomic 38:8121e7a79c0b 168 //simple check if there is an object in proximity
lupomic 38:8121e7a79c0b 169 //returns 0 if there is NO object present
lupomic 38:8121e7a79c0b 170 //returns 1 if there is an object present
lupomic 38:8121e7a79c0b 171 //returns 2 if the distance isn't in the expected range
lupomic 38:8121e7a79c0b 172
lupomic 38:8121e7a79c0b 173 uint8_t nextStepDetection(double distanceCm,double setpointDistance){
lupomic 38:8121e7a79c0b 174 double distance = distanceCm;
lupomic 38:8121e7a79c0b 175 double setpoint = setpointDistance;
lupomic 38:8121e7a79c0b 176 if(distance == 0){
lupomic 38:8121e7a79c0b 177 return 10; //sensor value is outside the expected range
lupomic 38:8121e7a79c0b 178 }
lupomic 38:8121e7a79c0b 179 if((distance <= (setpoint + 1)) && (distance >= (setpoint - 1))){
lupomic 38:8121e7a79c0b 180 return 3; //the distance to the next step is in ±1cm of the setpoint
lupomic 38:8121e7a79c0b 181 }
lupomic 38:8121e7a79c0b 182 if(distance < setpoint){
lupomic 38:8121e7a79c0b 183 return 0; //the robot is to close to the step to rotate the arm unhindered
lupomic 38:8121e7a79c0b 184 }
lupomic 38:8121e7a79c0b 185 if(distance > setpoint){
lupomic 38:8121e7a79c0b 186 return 1; //the robot is too far away from the next step
lupomic 38:8121e7a79c0b 187 }
lupomic 38:8121e7a79c0b 188 else{
lupomic 38:8121e7a79c0b 189 return 2;
lupomic 38:8121e7a79c0b 190 }
lupomic 38:8121e7a79c0b 191
lupomic 33:70ea029a69e8 192 }
lupomic 38:8121e7a79c0b 193 //simple check if there is an object in proximity
lupomic 38:8121e7a79c0b 194 //returns 0 if there is NO object present
lupomic 38:8121e7a79c0b 195 //returns 1 if there is an object present
lupomic 38:8121e7a79c0b 196 //returns 2 if the distance isn't in the expected range
lupomic 38:8121e7a79c0b 197 uint8_t StepDetection_down(float sensor)
lupomic 37:05252c4a2d4e 198
lupomic 37:05252c4a2d4e 199 {
lupomic 38:8121e7a79c0b 200 double d_valueMM = mapping(sensor*1.0e3f*3.3f);
lupomic 38:8121e7a79c0b 201 if(d_valueMM >= 4) return 0;
lupomic 38:8121e7a79c0b 202 else if( d_valueMM > 100 ) return 2;
lupomic 38:8121e7a79c0b 203 else if((d_valueMM < 4)||(d_valueMM==0)) return 1;
lupomic 38:8121e7a79c0b 204
lupomic 38:8121e7a79c0b 205 else return 5;
lupomic 37:05252c4a2d4e 206 }
lupomic 33:70ea029a69e8 207
lupomic 37:05252c4a2d4e 208 // bring arm in starting position. Height of stairs.
lupomic 37:05252c4a2d4e 209 int set_arm_stair_height()
lupomic 38:8121e7a79c0b 210
lupomic 37:05252c4a2d4e 211 {
lupomic 37:05252c4a2d4e 212 float diff;
lupomic 38:8121e7a79c0b 213 if (desired_pos==0) {
lupomic 38:8121e7a79c0b 214 desired_pos=turn_relative_deg(startPos, positionController_M_Arm.getRotation());
lupomic 37:05252c4a2d4e 215 }
lupomic 38:8121e7a79c0b 216
lupomic 38:8121e7a79c0b 217
lupomic 38:8121e7a79c0b 218 positionController_M_Arm.setDesiredRotation(desired_pos, max_speed_rps_arm); // command to turn motor to desired deg.
lupomic 37:05252c4a2d4e 219
lupomic 38:8121e7a79c0b 220
lupomic 38:8121e7a79c0b 221
lupomic 38:8121e7a79c0b 222
lupomic 37:05252c4a2d4e 223
lupomic 38:8121e7a79c0b 224 diff =abs( desired_pos-(positionController_M_Arm.getRotation()));
lupomic 38:8121e7a79c0b 225 printf("Set arm Position ARM (rot): %3.3f Desired:%3.3f State:%d ToNextfunction:%d Diff:%3.3f\n",
lupomic 38:8121e7a79c0b 226 positionController_M_Arm.getRotation(), desired_pos, state, ToNextFunction, diff);
lupomic 38:8121e7a79c0b 227 if (diff<=0.009){
lupomic 37:05252c4a2d4e 228 return 1;
lupomic 37:05252c4a2d4e 229 }
lupomic 37:05252c4a2d4e 230 else {
lupomic 38:8121e7a79c0b 231 return NULL;
lupomic 38:8121e7a79c0b 232 }
lupomic 37:05252c4a2d4e 233 }
lupomic 37:05252c4a2d4e 234
lupomic 37:05252c4a2d4e 235 //Drives forward into the next step
lupomic 37:05252c4a2d4e 236 //Prameter:distance in milimeter
lupomic 37:05252c4a2d4e 237 int drive_straight(float distance)
lupomic 37:05252c4a2d4e 238 {
lupomic 37:05252c4a2d4e 239 float diff_R;
lupomic 37:05252c4a2d4e 240 float diff_L;
lupomic 34:9f779e91168e 241
lupomic 38:8121e7a79c0b 242 if (desired_pos==0) {
lupomic 38:8121e7a79c0b 243 desired_pos= wheel_dist_to_deg(distance);
lupomic 38:8121e7a79c0b 244 float relativ_turns_rightmotor = turn_relative_deg(desired_pos, positionController_M_right.getRotation());
lupomic 38:8121e7a79c0b 245 float relativ_turns_leftmotor = turn_relative_deg(desired_pos, positionController_M_left.getRotation());
lupomic 38:8121e7a79c0b 246 }
lupomic 38:8121e7a79c0b 247
lupomic 38:8121e7a79c0b 248 positionController_M_right.setDesiredRotation(desired_pos, max_speed_rps_wheel);
lupomic 38:8121e7a79c0b 249 positionController_M_left.setDesiredRotation(desired_pos, max_speed_rps_wheel);
lupomic 33:70ea029a69e8 250
lupomic 37:05252c4a2d4e 251
lupomic 38:8121e7a79c0b 252 diff_R= abs(desired_pos-(positionController_M_right.getRotation()));
lupomic 38:8121e7a79c0b 253 diff_L= abs(desired_pos-(positionController_M_left.getRotation()));
lupomic 38:8121e7a79c0b 254 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 38:8121e7a79c0b 255 positionController_M_right.getRotation(),positionController_M_left.getRotation(),desired_pos,diff_L, state, ToNextFunction);
lupomic 38:8121e7a79c0b 256 if ((diff_R<=0.03) && (diff_L<=0.03))
lupomic 37:05252c4a2d4e 257 {
lupomic 37:05252c4a2d4e 258 return 1;
lupomic 37:05252c4a2d4e 259 }
lupomic 37:05252c4a2d4e 260 else
lupomic 37:05252c4a2d4e 261 {
lupomic 37:05252c4a2d4e 262 return 0;
lupomic 37:05252c4a2d4e 263 }
lupomic 33:70ea029a69e8 264 }
lupomic 33:70ea029a69e8 265
lupomic 38:8121e7a79c0b 266 //turns the arm until the robot is on the next step
lupomic 37:05252c4a2d4e 267 int lift_up()
lupomic 37:05252c4a2d4e 268 {
lupomic 37:05252c4a2d4e 269 float diff;
lupomic 38:8121e7a79c0b 270 if (desired_pos==0) {
lupomic 38:8121e7a79c0b 271 desired_pos = turn_relative_deg(liftPos,positionController_M_Arm.getRotation());
lupomic 38:8121e7a79c0b 272 }
lupomic 38:8121e7a79c0b 273
lupomic 38:8121e7a79c0b 274
lupomic 37:05252c4a2d4e 275
lupomic 38:8121e7a79c0b 276 positionController_M_Arm.setDesiredRotation(desired_pos, max_speed_rps_arm);
lupomic 38:8121e7a79c0b 277
lupomic 38:8121e7a79c0b 278 diff=abs(desired_pos-positionController_M_Arm.getRotation());
lupomic 38:8121e7a79c0b 279 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 38:8121e7a79c0b 280 if(diff<=0.03)
lupomic 37:05252c4a2d4e 281 { return 1;
lupomic 37:05252c4a2d4e 282 }
lupomic 37:05252c4a2d4e 283 else
lupomic 37:05252c4a2d4e 284 { return 0;
lupomic 37:05252c4a2d4e 285 }
lupomic 33:70ea029a69e8 286
lupomic 37:05252c4a2d4e 287 }
lupomic 37:05252c4a2d4e 288 //***********************************************************************************************************************************************************
lupomic 37:05252c4a2d4e 289
lupomic 37:05252c4a2d4e 290 // while loop gets executed every main_task_period_ms milliseconds
lupomic 37:05252c4a2d4e 291 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 292 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 293 //***********************************************************************************************************************************************************
lupomic 37:05252c4a2d4e 294
lupomic 33:70ea029a69e8 295 int main(void)
pmic 23:26b3a25fc637 296 {
pmic 24:86f1a63e35a0 297 // attach button fall and rise functions to user button object
lupomic 37:05252c4a2d4e 298 user_button.fall(&user_button_pressed_fcn);
lupomic 37:05252c4a2d4e 299 user_button.rise(&user_button_released_fcn);
lupomic 38:8121e7a79c0b 300 mechanical_button.mode(PullDown);
lupomic 38:8121e7a79c0b 301
lupomic 37:05252c4a2d4e 302
lupomic 37:05252c4a2d4e 303 while (true)
lupomic 37:05252c4a2d4e 304 {
lupomic 38:8121e7a79c0b 305
lupomic 37:05252c4a2d4e 306 ir_distance_mm_L= mapping(ir_analog_in_Distance_L.read()*1.0e3f * 3.3f);
lupomic 38:8121e7a79c0b 307
lupomic 38:8121e7a79c0b 308
lupomic 38:8121e7a79c0b 309 if (ToNextFunction>=1||(mechanical_button.read()!=1))
lupomic 38:8121e7a79c0b 310 {
lupomic 38:8121e7a79c0b 311 enable_motors=1;
lupomic 38:8121e7a79c0b 312 }
pmic 24:86f1a63e35a0 313
pmic 6:e1fa1a2d7483 314
lupomic 37:05252c4a2d4e 315 switch (ToNextFunction)
lupomic 37:05252c4a2d4e 316 {
lupomic 38:8121e7a79c0b 317
lupomic 38:8121e7a79c0b 318 case 0: while (mechanical_button.read()!=1)
lupomic 38:8121e7a79c0b 319 {
lupomic 38:8121e7a79c0b 320 positionController_M_Arm.setDesiredRotation(-1,0.5);
lupomic 38:8121e7a79c0b 321
lupomic 38:8121e7a79c0b 322 }
lupomic 38:8121e7a79c0b 323 if (mechanical_button){
lupomic 38:8121e7a79c0b 324 positionController_M_Arm.setDesiredRotation(positionController_M_Arm.getRotation());
lupomic 34:9f779e91168e 325
lupomic 38:8121e7a79c0b 326 }
lupomic 38:8121e7a79c0b 327
lupomic 38:8121e7a79c0b 328
lupomic 38:8121e7a79c0b 329 break;
lupomic 38:8121e7a79c0b 330
lupomic 38:8121e7a79c0b 331 case 1:
lupomic 38:8121e7a79c0b 332
lupomic 37:05252c4a2d4e 333 ToNextFunction += 1;
lupomic 38:8121e7a79c0b 334 state=0;
lupomic 38:8121e7a79c0b 335
lupomic 38:8121e7a79c0b 336
lupomic 37:05252c4a2d4e 337 break;
pmic 6:e1fa1a2d7483 338
lupomic 37:05252c4a2d4e 339 case 2:
lupomic 38:8121e7a79c0b 340 state=nextStepDetection(ir_distance_mm_L,10);
lupomic 38:8121e7a79c0b 341 printf("distance:%3.3f Output:%d\n", ir_distance_mm_L, nextStepDetection(ir_distance_mm_L,10));
lupomic 38:8121e7a79c0b 342 if (state==3){
lupomic 38:8121e7a79c0b 343 ToNextFunction +=1;
lupomic 38:8121e7a79c0b 344 state=0;
lupomic 38:8121e7a79c0b 345 }
lupomic 38:8121e7a79c0b 346 break;
pmic 6:e1fa1a2d7483 347
lupomic 37:05252c4a2d4e 348 case 3:
lupomic 38:8121e7a79c0b 349 state=drive_straight(drive_straight_mm);
lupomic 38:8121e7a79c0b 350
lupomic 37:05252c4a2d4e 351 if (state==1){
lupomic 38:8121e7a79c0b 352 ToNextFunction += 1;
lupomic 38:8121e7a79c0b 353 state=0;
lupomic 38:8121e7a79c0b 354 desired_pos=0;
lupomic 38:8121e7a79c0b 355
lupomic 37:05252c4a2d4e 356 }
lupomic 33:70ea029a69e8 357 break;
lupomic 37:05252c4a2d4e 358
lupomic 37:05252c4a2d4e 359 case 4:
lupomic 37:05252c4a2d4e 360 state=lift_up();
lupomic 38:8121e7a79c0b 361
lupomic 38:8121e7a79c0b 362 if (state==1){
lupomic 37:05252c4a2d4e 363 ToNextFunction += 1;
lupomic 38:8121e7a79c0b 364 state=0;
lupomic 38:8121e7a79c0b 365 desired_pos=0;
lupomic 37:05252c4a2d4e 366 }
lupomic 38:8121e7a79c0b 367 break;
lupomic 37:05252c4a2d4e 368
lupomic 37:05252c4a2d4e 369 case 5:
lupomic 37:05252c4a2d4e 370 state=drive_straight(drive_back_mm);
lupomic 38:8121e7a79c0b 371
lupomic 38:8121e7a79c0b 372 if ((state == 1) && (StepDetection_down(ir_analog_in_Lookdown_B) != 1)){
lupomic 37:05252c4a2d4e 373 ToNextFunction += 1;
lupomic 38:8121e7a79c0b 374 state=0;
lupomic 38:8121e7a79c0b 375 desired_pos=0;
lupomic 38:8121e7a79c0b 376
lupomic 37:05252c4a2d4e 377 }
lupomic 33:70ea029a69e8 378 break;
lupomic 37:05252c4a2d4e 379
lupomic 37:05252c4a2d4e 380 case 6:
lupomic 38:8121e7a79c0b 381 state=set_arm_stair_height();
lupomic 38:8121e7a79c0b 382
lupomic 37:05252c4a2d4e 383 if (state==1){
lupomic 38:8121e7a79c0b 384 ToNextFunction = 0;
lupomic 38:8121e7a79c0b 385 state=0;
lupomic 38:8121e7a79c0b 386 desired_pos=0;
lupomic 37:05252c4a2d4e 387 }
lupomic 33:70ea029a69e8 388 break;
lupomic 38:8121e7a79c0b 389
lupomic 34:9f779e91168e 390 default: ;
lupomic 33:70ea029a69e8 391 }
lupomic 33:70ea029a69e8 392 }
lupomic 38:8121e7a79c0b 393 // read timer and make the main thread sleep for the remaining time span (non blocking)
lupomic 38:8121e7a79c0b 394 int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
lupomic 38:8121e7a79c0b 395 thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
lupomic 38:8121e7a79c0b 396 return 0;
pmic 1:93d997d6b232 397 }
pmic 6:e1fa1a2d7483 398
lupomic 33:70ea029a69e8 399
lupomic 37:05252c4a2d4e 400
pmic 24:86f1a63e35a0 401 void user_button_pressed_fcn()
pmic 25:ea1d6e27c895 402 {
pmic 26:28693b369945 403 user_button_timer.start();
pmic 6:e1fa1a2d7483 404 user_button_timer.reset();
pmic 6:e1fa1a2d7483 405 }
pmic 6:e1fa1a2d7483 406
lupomic 37:05252c4a2d4e 407 void user_button_released_fcn()
lupomic 37:05252c4a2d4e 408 {
pmic 24:86f1a63e35a0 409 // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
pmic 24:86f1a63e35a0 410 int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 411 user_button_timer.stop();
lupomic 37:05252c4a2d4e 412 if (user_button_elapsed_time_ms > 200)
lupomic 37:05252c4a2d4e 413 {
lupomic 38:8121e7a79c0b 414 ToNextFunction =1;
lupomic 37:05252c4a2d4e 415 }
lupomic 37:05252c4a2d4e 416 }