TreppenRobo / Mbed OS Prototyp

Workshop 2

Dependencies:   PM2_Libary

main.cpp@38:cbad84e4c714, 2022-04-05 (annotated)

Committer:
lupomic
Date:
Tue Apr 05 08:52:46 2022 +0200
Revision:
38:cbad84e4c714
Parent:
37:6ac4db3cc57b 
merge corrected

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lupomic 34:4c04c6c04c55 1 /*this Program acts as a prototype. it dosn't include any PWM modulation nor cours-correcten.
lupomic 34:4c04c6c04c55 2 It is built as Step after Step programm, if any step fails the Program feils it shoud proof the concept of the pland roboter
lupomic 34:4c04c6c04c55 3 */
lupomic 34:4c04c6c04c55 4 #include "AnalogIn.h"
lupomic 34:4c04c6c04c55 5 #include "DigitalIn.h"
lupomic 34:4c04c6c04c55 6 #include "DigitalOut.h"
lupomic 34:4c04c6c04c55 7 #include "PinNames.h"
pmic 1:93d997d6b232 8 #include "mbed.h"
lupomic 34:4c04c6c04c55 9 #include "pinmap.h"
lupomic 34:4c04c6c04c55 10 #include <cstdint>
lupomic 34:4c04c6c04c55 11 #include <cwchar>
pmic 17:c19b471f05cb 12 #include "PM2_Libary.h"
pmic 6:e1fa1a2d7483 13
lupomic 34:4c04c6c04c55 14 //------------------------------------general IO and Variables----------------------------------------------------------
lupomic 38:cbad84e4c714 15
lupomic 37:6ac4db3cc57b 16 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)
lupomic 37:6ac4db3cc57b 17 InterruptIn user_button(PC_13); // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
lupomic 37:6ac4db3cc57b 18 void user_button_pressed_fcn(); // custom functions which gets executed when user button gets pressed and released, definition below
lupomic 37:6ac4db3cc57b 19 void user_button_released_fcn();
pmic 17:c19b471f05cb 20
lupomic 38:cbad84e4c714 21 int ToNextFunction = 0 ;
lupomic 34:4c04c6c04c55 22 float trigerValueRearSensor = 0.0f;
lupomic 34:4c04c6c04c55 23 float trigerValueFrontSensor = 0.0f;
pmic 17:c19b471f05cb 24
lupomic 38:cbad84e4c714 25 //-----------------------------------------actors Definition----------------------------------------------------
lupomic 38:cbad84e4c714 26 // Infrarot sensors pin declaration
lupomic 38:cbad84e4c714 27 AnalogIn FrontSensor(PC_6); //IR TOF sensor at the front facing down
lupomic 38:cbad84e4c714 28 AnalogIn RearSensor(PC_8); //IR TOF sensor at the back facing down
lupomic 38:cbad84e4c714 29 AnalogIn LeftEyeSensor(PC_2); // Infrared sensor on top in "head" part left eye
lupomic 38:cbad84e4c714 30 AnalogIn RightEyeSensor(PC_3); // Infrared sensor on top in "head" part right eye
pmic 6:e1fa1a2d7483 31
lupomic 38:cbad84e4c714 32 //Encoder pin declaration
lupomic 38:cbad84e4c714 33 EncoderCounter encoder_M_right(PA_6, PC_7); //encoder pin decalaration for wheels right side
lupomic 38:cbad84e4c714 34 EncoderCounter encoder_M_left(PB_6, PB_7); //encoder pin decalaration for wheels left side
lupomic 38:cbad84e4c714 35 EncoderCounter encoder_M_arm(PA_0, PA_1); //encoder pin decalaration for arm
pmic 17:c19b471f05cb 36
lupomic 38:cbad84e4c714 37 //motor pin declaration
lupomic 38:cbad84e4c714 38 FastPWM pwm_M_right(PA_9);
lupomic 38:cbad84e4c714 39 FastPWM pwm_M_left(PB_13);
lupomic 38:cbad84e4c714 40 FastPWM pwm_M_arm(PA_10);
pmic 30:1e8295770bc1 41 // create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
pmic 24:86f1a63e35a0 42 float max_voltage = 12.0f; // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
lupomic 38:cbad84e4c714 43 float counts_per_turn_wheels = 2000.0f * 100.0f; // define counts per turn at gearbox end (counts/turn * gearratio) for wheels
lupomic 38:cbad84e4c714 44 float counts_per_turn_arm = 40000.0f * 100.0f; // define counts per turn at gearbox end (counts/turn * gearratio) for arm
pmic 25:ea1d6e27c895 45 float kn = 180.0f / 12.0f; // define motor constant in rpm per V
pmic 30:1e8295770bc1 46 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
pmic 30:1e8295770bc1 47 float kp = 0.1f; // define custom kp, this is the default speed controller gain for gear box 78.125:1
pmic 6:e1fa1a2d7483 48
lupomic 34:4c04c6c04c55 49 //motors for tracks
lupomic 38:cbad84e4c714 50 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 38:cbad84e4c714 51 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 34:4c04c6c04c55 52
lupomic 34:4c04c6c04c55 53 //Arm Motor
lupomic 38:cbad84e4c714 54 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
lupomic 37:6ac4db3cc57b 55
lupomic 37:6ac4db3cc57b 56
lupomic 37:6ac4db3cc57b 57 //Platzhalter Variabeln für die Positionierung
lupomic 37:6ac4db3cc57b 58 int16_t PositionStair = 20;
lupomic 37:6ac4db3cc57b 59 int16_t PositionBackOff = 100;
lupomic 37:6ac4db3cc57b 60 int16_t degArmStart = 40;
lupomic 37:6ac4db3cc57b 61 int16_t degArmLift = 18;
lupomic 37:6ac4db3cc57b 62
lupomic 37:6ac4db3cc57b 63
pmic 17:c19b471f05cb 64
lupomic 34:4c04c6c04c55 65 //-----------------------------------------Functions----------------------------------------------------------
lupomic 34:4c04c6c04c55 66 //only moves the arm in to the starting position
lupomic 34:4c04c6c04c55 67 int StartPosition(void){
lupomic 34:4c04c6c04c55 68 if(true){
lupomic 34:4c04c6c04c55 69 return 1;
lupomic 34:4c04c6c04c55 70 }
lupomic 34:4c04c6c04c55 71 return NULL;
lupomic 34:4c04c6c04c55 72 }
lupomic 34:4c04c6c04c55 73
lupomic 34:4c04c6c04c55 74 //Drives forward into the next step
lupomic 37:6ac4db3cc57b 75 int Drive(int16_t dist){
lupomic 34:4c04c6c04c55 76 int8_t i = 0; //prov condition variable
lupomic 34:4c04c6c04c55 77
pmic 17:c19b471f05cb 78
lupomic 34:4c04c6c04c55 79 int8_t distance = dist; //distance which will be driven in [mm]
lupomic 34:4c04c6c04c55 80 float factor = 1.0f; //factor for calculating the value in to the float which will be given to the setDesiredRotation function
lupomic 34:4c04c6c04c55 81 float distanceValue = float(distance)*factor;
lupomic 37:6ac4db3cc57b 82
lupomic 34:4c04c6c04c55 83 positionController_M_right.setDesiredRotation(distanceValue);
lupomic 34:4c04c6c04c55 84 positionController_M_left.setDesiredRotation(distanceValue);
lupomic 37:6ac4db3cc57b 85
lupomic 34:4c04c6c04c55 86 return 0;
lupomic 34:4c04c6c04c55 87
lupomic 34:4c04c6c04c55 88 }
pmic 17:c19b471f05cb 89
lupomic 34:4c04c6c04c55 90 //only turns the arm until the robot is on the next step
lupomic 34:4c04c6c04c55 91 //not yet clear if the motor controler function drives to a absolute poition or if it drives the given distance relative to the current position
lupomic 37:6ac4db3cc57b 92 int LiftUp(int16_t deg){
lupomic 34:4c04c6c04c55 93 int8_t rotation = deg;
lupomic 34:4c04c6c04c55 94 int8_t i = 0; //prov condition variable
lupomic 34:4c04c6c04c55 95 do{
lupomic 34:4c04c6c04c55 96 positionController_M_Arm.setDesiredRotation(deg);
lupomic 34:4c04c6c04c55 97
lupomic 34:4c04c6c04c55 98 i++;
lupomic 34:4c04c6c04c55 99 }while(i < 1);
lupomic 34:4c04c6c04c55 100 return 0;
lupomic 34:4c04c6c04c55 101 }
pmic 1:93d997d6b232 102
lupomic 34:4c04c6c04c55 103
pmic 17:c19b471f05cb 104
lupomic 34:4c04c6c04c55 105
pmic 1:93d997d6b232 106 int main()
pmic 23:26b3a25fc637 107 {
lupomic 37:6ac4db3cc57b 108 // attach button fall and rise functions to user button object
lupomic 37:6ac4db3cc57b 109 user_button.fall(&user_button_pressed_fcn);
lupomic 37:6ac4db3cc57b 110 user_button.rise(&user_button_released_fcn);
lupomic 37:6ac4db3cc57b 111
lupomic 37:6ac4db3cc57b 112
lupomic 34:4c04c6c04c55 113 while (true) {
lupomic 37:6ac4db3cc57b 114
pmic 6:e1fa1a2d7483 115
lupomic 37:6ac4db3cc57b 116
lupomic 37:6ac4db3cc57b 117 switch (ToNextFunction) {
lupomic 37:6ac4db3cc57b 118 case 1: StartPosition();
lupomic 37:6ac4db3cc57b 119 ToNextFunction+=1;
lupomic 37:6ac4db3cc57b 120 break;
lupomic 37:6ac4db3cc57b 121 case 2: Drive(PositionStair);
lupomic 37:6ac4db3cc57b 122 ToNextFunction+=1;
lupomic 37:6ac4db3cc57b 123 break;
lupomic 37:6ac4db3cc57b 124 case 3: LiftUp(degArmLift);
lupomic 37:6ac4db3cc57b 125 ToNextFunction+=1;
lupomic 37:6ac4db3cc57b 126 break;
lupomic 37:6ac4db3cc57b 127 case 4: Drive(PositionBackOff);
lupomic 37:6ac4db3cc57b 128 ToNextFunction+=1;
lupomic 37:6ac4db3cc57b 129 break;
lupomic 37:6ac4db3cc57b 130 case 5: LiftUp(degArmStart);
lupomic 37:6ac4db3cc57b 131 ToNextFunction = 0;
lupomic 37:6ac4db3cc57b 132 break;
lupomic 37:6ac4db3cc57b 133 default: ToNextFunction = 0;
pmic 1:93d997d6b232 134 }
lupomic 37:6ac4db3cc57b 135
lupomic 34:4c04c6c04c55 136
lupomic 37:6ac4db3cc57b 137 thread_sleep_for(10);
pmic 1:93d997d6b232 138 }
lupomic 37:6ac4db3cc57b 139
lupomic 37:6ac4db3cc57b 140 }
lupomic 37:6ac4db3cc57b 141 void user_button_pressed_fcn()
lupomic 37:6ac4db3cc57b 142 {
lupomic 37:6ac4db3cc57b 143 user_button_timer.start();
lupomic 37:6ac4db3cc57b 144 user_button_timer.reset();
pmic 1:93d997d6b232 145 }
pmic 6:e1fa1a2d7483 146
lupomic 37:6ac4db3cc57b 147 void user_button_released_fcn()
lupomic 37:6ac4db3cc57b 148 {
lupomic 37:6ac4db3cc57b 149 // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
lupomic 37:6ac4db3cc57b 150 int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
lupomic 37:6ac4db3cc57b 151 user_button_timer.stop();
lupomic 37:6ac4db3cc57b 152 if (user_button_elapsed_time_ms > 200) {
lupomic 37:6ac4db3cc57b 153 ToNextFunction = 1;
lupomic 37:6ac4db3cc57b 154 }
lupomic 37:6ac4db3cc57b 155
lupomic 37:6ac4db3cc57b 156
lupomic 37:6ac4db3cc57b 157
lupomic 37:6ac4db3cc57b 158
lupomic 37:6ac4db3cc57b 159 }