TreppenRobo / Mbed OS Prototyp

Workshop 2

Dependencies:   PM2_Libary

main.cpp@37:6ac4db3cc57b, 2022-04-05 (annotated)

Committer:
lupomic
Date:
Tue Apr 05 08:27:12 2022 +0200
Revision:
37:6ac4db3cc57b
Parent:
36:c6961428c1e1
Child:
38:cbad84e4c714
Main Task

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 37:6ac4db3cc57b 15 // user button on nucleo board
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 37:6ac4db3cc57b 21
lupomic 37:6ac4db3cc57b 22 //arbitrary allocation of pins -> for testing this has to be adjusted
lupomic 37:6ac4db3cc57b 23 AnalogIn ch0(PA_15); //IR TOF sensor at the front
lupomic 37:6ac4db3cc57b 24 AnalogIn RearSensor(PC_9); //IR TOF sensor at the back facing down
lupomic 37:6ac4db3cc57b 25
lupomic 37:6ac4db3cc57b 26 DigitalOut armMotor(PB_15);
lupomic 37:6ac4db3cc57b 27 // main() runs in its own thread in the OS
lupomic 37:6ac4db3cc57b 28 // Enable the ADC, 12MHz, the final term should be (2**BURST_CHANNELS - 1)
lupomic 34:4c04c6c04c55 29 float trigerValueRearSensor = 0.0f;
lupomic 34:4c04c6c04c55 30 float trigerValueFrontSensor = 0.0f;
pmic 17:c19b471f05cb 31
lupomic 37:6ac4db3cc57b 32 int ToNextFunction = 0 ;
lupomic 37:6ac4db3cc57b 33
pmic 6:e1fa1a2d7483 34
lupomic 34:4c04c6c04c55 35 //-----------------------------------------actors Definition----------------------------------------------------
lupomic 37:6ac4db3cc57b 36 //Encoder pin
lupomic 37:6ac4db3cc57b 37 EncoderCounter encoder_M_right(PA_0, PA_1);
lupomic 37:6ac4db3cc57b 38 EncoderCounter encoder_M_left(PA_2, PA_3); //Pin zuweisung Provisorisch!!!
pmic 17:c19b471f05cb 39
lupomic 37:6ac4db3cc57b 40 //motor pin
lupomic 37:6ac4db3cc57b 41 FastPWM pwm_M_right(PA_10);
lupomic 37:6ac4db3cc57b 42 FastPWM pwm_M_left(PA_9);
pmic 30:1e8295770bc1 43 // create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
pmic 24:86f1a63e35a0 44 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:6ac4db3cc57b 45 float counts_per_turn = 20.0f * 78.125f; // define counts per turn at gearbox end: counts/turn * gearratio
pmic 25:ea1d6e27c895 46 float kn = 180.0f / 12.0f; // define motor constant in rpm per V
pmic 30:1e8295770bc1 47 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 48 float kp = 0.1f; // define custom kp, this is the default speed controller gain for gear box 78.125:1
pmic 6:e1fa1a2d7483 49
lupomic 34:4c04c6c04c55 50 //motors for tracks
lupomic 37:6ac4db3cc57b 51 PositionController positionController_M_right(counts_per_turn * 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 37:6ac4db3cc57b 52 PositionController positionController_M_left(counts_per_turn * 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 53
lupomic 34:4c04c6c04c55 54 //Arm Motor
lupomic 37:6ac4db3cc57b 55 PositionController positionController_M_Arm(counts_per_turn * 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 37:6ac4db3cc57b 56
lupomic 37:6ac4db3cc57b 57
lupomic 37:6ac4db3cc57b 58 //Platzhalter Variabeln für die Positionierung
lupomic 37:6ac4db3cc57b 59 int16_t PositionStair = 20;
lupomic 37:6ac4db3cc57b 60 int16_t PositionBackOff = 100;
lupomic 37:6ac4db3cc57b 61 int16_t degArmStart = 40;
lupomic 37:6ac4db3cc57b 62 int16_t degArmLift = 18;
lupomic 37:6ac4db3cc57b 63
lupomic 37:6ac4db3cc57b 64
pmic 17:c19b471f05cb 65
lupomic 34:4c04c6c04c55 66 //-----------------------------------------Functions----------------------------------------------------------
lupomic 34:4c04c6c04c55 67 //only moves the arm in to the starting position
lupomic 34:4c04c6c04c55 68 int StartPosition(void){
lupomic 34:4c04c6c04c55 69 if(true){
lupomic 34:4c04c6c04c55 70 return 1;
lupomic 34:4c04c6c04c55 71 }
lupomic 34:4c04c6c04c55 72 return NULL;
lupomic 34:4c04c6c04c55 73 }
lupomic 34:4c04c6c04c55 74
lupomic 34:4c04c6c04c55 75 //Drives forward into the next step
lupomic 37:6ac4db3cc57b 76 int Drive(int16_t dist){
lupomic 34:4c04c6c04c55 77 int8_t i = 0; //prov condition variable
lupomic 34:4c04c6c04c55 78
pmic 17:c19b471f05cb 79
lupomic 34:4c04c6c04c55 80 int8_t distance = dist; //distance which will be driven in [mm]
lupomic 34:4c04c6c04c55 81 float factor = 1.0f; //factor for calculating the value in to the float which will be given to the setDesiredRotation function
lupomic 34:4c04c6c04c55 82 float distanceValue = float(distance)*factor;
lupomic 37:6ac4db3cc57b 83
lupomic 34:4c04c6c04c55 84 positionController_M_right.setDesiredRotation(distanceValue);
lupomic 34:4c04c6c04c55 85 positionController_M_left.setDesiredRotation(distanceValue);
lupomic 37:6ac4db3cc57b 86
lupomic 34:4c04c6c04c55 87 return 0;
lupomic 34:4c04c6c04c55 88
lupomic 34:4c04c6c04c55 89 }
pmic 17:c19b471f05cb 90
lupomic 34:4c04c6c04c55 91 //only turns the arm until the robot is on the next step
lupomic 34:4c04c6c04c55 92 //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 93 int LiftUp(int16_t deg){
lupomic 34:4c04c6c04c55 94 int8_t rotation = deg;
lupomic 34:4c04c6c04c55 95 int8_t i = 0; //prov condition variable
lupomic 34:4c04c6c04c55 96 do{
lupomic 34:4c04c6c04c55 97 positionController_M_Arm.setDesiredRotation(deg);
lupomic 34:4c04c6c04c55 98
lupomic 34:4c04c6c04c55 99 i++;
lupomic 34:4c04c6c04c55 100 }while(i < 1);
lupomic 34:4c04c6c04c55 101 return 0;
lupomic 34:4c04c6c04c55 102 }
pmic 1:93d997d6b232 103
lupomic 34:4c04c6c04c55 104
pmic 17:c19b471f05cb 105
lupomic 34:4c04c6c04c55 106
pmic 1:93d997d6b232 107 int main()
pmic 23:26b3a25fc637 108 {
lupomic 37:6ac4db3cc57b 109 // attach button fall and rise functions to user button object
lupomic 37:6ac4db3cc57b 110 user_button.fall(&user_button_pressed_fcn);
lupomic 37:6ac4db3cc57b 111 user_button.rise(&user_button_released_fcn);
lupomic 37:6ac4db3cc57b 112
lupomic 37:6ac4db3cc57b 113
lupomic 34:4c04c6c04c55 114 while (true) {
lupomic 37:6ac4db3cc57b 115
pmic 6:e1fa1a2d7483 116
lupomic 37:6ac4db3cc57b 117
lupomic 37:6ac4db3cc57b 118 switch (ToNextFunction) {
lupomic 37:6ac4db3cc57b 119 case 1: StartPosition();
lupomic 37:6ac4db3cc57b 120 ToNextFunction+=1;
lupomic 37:6ac4db3cc57b 121 break;
lupomic 37:6ac4db3cc57b 122 case 2: Drive(PositionStair);
lupomic 37:6ac4db3cc57b 123 ToNextFunction+=1;
lupomic 37:6ac4db3cc57b 124 break;
lupomic 37:6ac4db3cc57b 125 case 3: LiftUp(degArmLift);
lupomic 37:6ac4db3cc57b 126 ToNextFunction+=1;
lupomic 37:6ac4db3cc57b 127 break;
lupomic 37:6ac4db3cc57b 128 case 4: Drive(PositionBackOff);
lupomic 37:6ac4db3cc57b 129 ToNextFunction+=1;
lupomic 37:6ac4db3cc57b 130 break;
lupomic 37:6ac4db3cc57b 131 case 5: LiftUp(degArmStart);
lupomic 37:6ac4db3cc57b 132 ToNextFunction = 0;
lupomic 37:6ac4db3cc57b 133 break;
lupomic 37:6ac4db3cc57b 134 default: ToNextFunction = 0;
pmic 1:93d997d6b232 135 }
lupomic 37:6ac4db3cc57b 136
lupomic 34:4c04c6c04c55 137
lupomic 37:6ac4db3cc57b 138 thread_sleep_for(10);
pmic 1:93d997d6b232 139 }
lupomic 37:6ac4db3cc57b 140
lupomic 37:6ac4db3cc57b 141 }
lupomic 37:6ac4db3cc57b 142 void user_button_pressed_fcn()
lupomic 37:6ac4db3cc57b 143 {
lupomic 37:6ac4db3cc57b 144 user_button_timer.start();
lupomic 37:6ac4db3cc57b 145 user_button_timer.reset();
pmic 1:93d997d6b232 146 }
pmic 6:e1fa1a2d7483 147
lupomic 37:6ac4db3cc57b 148 void user_button_released_fcn()
lupomic 37:6ac4db3cc57b 149 {
lupomic 37:6ac4db3cc57b 150 // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
lupomic 37:6ac4db3cc57b 151 int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
lupomic 37:6ac4db3cc57b 152 user_button_timer.stop();
lupomic 37:6ac4db3cc57b 153 if (user_button_elapsed_time_ms > 200) {
lupomic 37:6ac4db3cc57b 154 ToNextFunction = 1;
lupomic 37:6ac4db3cc57b 155 }
lupomic 37:6ac4db3cc57b 156
lupomic 37:6ac4db3cc57b 157
lupomic 37:6ac4db3cc57b 158
lupomic 37:6ac4db3cc57b 159
lupomic 37:6ac4db3cc57b 160 }