TreppenRobo / Mbed OS Prototyp

Workshop 2

Dependencies:   PM2_Libary

main.cpp@34:4c04c6c04c55, 2022-03-25 (annotated)

Committer:
lupomic
Date:
Fri Mar 25 10:19:06 2022 +0000
Revision:
34:4c04c6c04c55
Parent:
33:68077b39d421
Child:
36:c6961428c1e1
V0.0

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 34:4c04c6c04c55 15 //arbitrary allocation of pins -> for testing this has to be adjusted
lupomic 34:4c04c6c04c55 16 AnalogIn ch0(PA_15); //IR TOF sensor at the front
lupomic 34:4c04c6c04c55 17 AnalogIn RearSensor(PC_9); //IR TOF sensor at the back facing down
pmic 17:c19b471f05cb 18
lupomic 34:4c04c6c04c55 19 DigitalOut armMotor(PC_12);
lupomic 34:4c04c6c04c55 20 // main() runs in its own thread in the OS
lupomic 34:4c04c6c04c55 21 // Enable the ADC, 12MHz, the final term should be (2**BURST_CHANNELS - 1)
lupomic 34:4c04c6c04c55 22 int ToNextFunction;
lupomic 34:4c04c6c04c55 23 float trigerValueRearSensor = 0.0f;
lupomic 34:4c04c6c04c55 24 float trigerValueFrontSensor = 0.0f;
pmic 17:c19b471f05cb 25
pmic 6:e1fa1a2d7483 26
lupomic 34:4c04c6c04c55 27 //-----------------------------------------actors Definition----------------------------------------------------
pmic 17:c19b471f05cb 28
lupomic 34:4c04c6c04c55 29 //Encoder pin
lupomic 34:4c04c6c04c55 30 EncoderCounter encoder_M_right(PA_0, PA_1);
lupomic 34:4c04c6c04c55 31 EncoderCounter encoder_M_left(PA_2, PA_3); //Pin zuweisung Provisorisch!!!
pmic 17:c19b471f05cb 32
lupomic 34:4c04c6c04c55 33 //motor pin
lupomic 34:4c04c6c04c55 34 FastPWM pwm_M_right(PA_10);
lupomic 34:4c04c6c04c55 35 FastPWM pwm_M_left(PA_9);
pmic 17:c19b471f05cb 36
pmic 30:1e8295770bc1 37 // create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
pmic 24:86f1a63e35a0 38 float max_voltage = 12.0f; // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
pmic 24:86f1a63e35a0 39 float counts_per_turn = 20.0f * 78.125f; // define counts per turn at gearbox end: counts/turn * gearratio
pmic 25:ea1d6e27c895 40 float kn = 180.0f / 12.0f; // define motor constant in rpm per V
pmic 30:1e8295770bc1 41 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 42 float kp = 0.1f; // define custom kp, this is the default speed controller gain for gear box 78.125:1
pmic 6:e1fa1a2d7483 43
lupomic 34:4c04c6c04c55 44 //motors for tracks
lupomic 34:4c04c6c04c55 45 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 34:4c04c6c04c55 46 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 47
lupomic 34:4c04c6c04c55 48 //Arm Motor
lupomic 34:4c04c6c04c55 49 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
pmic 17:c19b471f05cb 50
lupomic 34:4c04c6c04c55 51 //-----------------------------------------Functions----------------------------------------------------------
lupomic 34:4c04c6c04c55 52 //only moves the arm in to the starting position
lupomic 34:4c04c6c04c55 53 int StartPosition(void){
lupomic 34:4c04c6c04c55 54 if(true){
lupomic 34:4c04c6c04c55 55 return 1;
lupomic 34:4c04c6c04c55 56 }
lupomic 34:4c04c6c04c55 57 return NULL;
lupomic 34:4c04c6c04c55 58 }
lupomic 34:4c04c6c04c55 59
lupomic 34:4c04c6c04c55 60 //Drives forward into the next step
lupomic 34:4c04c6c04c55 61 int Drive(int8_t dist){
lupomic 34:4c04c6c04c55 62 int8_t i = 0; //prov condition variable
lupomic 34:4c04c6c04c55 63
pmic 17:c19b471f05cb 64
lupomic 34:4c04c6c04c55 65 int8_t distance = dist; //distance which will be driven in [mm]
lupomic 34:4c04c6c04c55 66 float factor = 1.0f; //factor for calculating the value in to the float which will be given to the setDesiredRotation function
lupomic 34:4c04c6c04c55 67 float distanceValue = float(distance)*factor;
lupomic 34:4c04c6c04c55 68 do{
lupomic 34:4c04c6c04c55 69 positionController_M_right.setDesiredRotation(distanceValue);
lupomic 34:4c04c6c04c55 70 positionController_M_left.setDesiredRotation(distanceValue);
lupomic 34:4c04c6c04c55 71 i++;
lupomic 34:4c04c6c04c55 72 }while(i < 1);
lupomic 34:4c04c6c04c55 73 return 0;
lupomic 34:4c04c6c04c55 74
lupomic 34:4c04c6c04c55 75 }
pmic 17:c19b471f05cb 76
lupomic 34:4c04c6c04c55 77 //only turns the arm until the robot is on the next step
lupomic 34:4c04c6c04c55 78 //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 34:4c04c6c04c55 79 int LiftUp(int8_t deg){
lupomic 34:4c04c6c04c55 80 int8_t rotation = deg;
lupomic 34:4c04c6c04c55 81 int8_t i = 0; //prov condition variable
lupomic 34:4c04c6c04c55 82 do{
lupomic 34:4c04c6c04c55 83 positionController_M_Arm.setDesiredRotation(deg);
lupomic 34:4c04c6c04c55 84
lupomic 34:4c04c6c04c55 85 i++;
lupomic 34:4c04c6c04c55 86 }while(i < 1);
lupomic 34:4c04c6c04c55 87 return 0;
lupomic 34:4c04c6c04c55 88 }
pmic 1:93d997d6b232 89
lupomic 34:4c04c6c04c55 90
pmic 17:c19b471f05cb 91
lupomic 34:4c04c6c04c55 92
pmic 1:93d997d6b232 93 int main()
pmic 23:26b3a25fc637 94 {
lupomic 34:4c04c6c04c55 95 while (true) {
lupomic 34:4c04c6c04c55 96 ToNextFunction = StartPosition();
lupomic 34:4c04c6c04c55 97 if(NULL != ToNextFunction){
lupomic 34:4c04c6c04c55 98 ToNextFunction = Drive(3);
lupomic 34:4c04c6c04c55 99 if(NULL != ToNextFunction){
lupomic 34:4c04c6c04c55 100 ToNextFunction = LiftUp(5);
lupomic 34:4c04c6c04c55 101 if(NULL != ToNextFunction){
lupomic 34:4c04c6c04c55 102 ToNextFunction = Drive(-1);
pmic 6:e1fa1a2d7483 103
lupomic 34:4c04c6c04c55 104 }
pmic 8:9bb806a7f585 105 }
pmic 1:93d997d6b232 106 }
lupomic 34:4c04c6c04c55 107
lupomic 34:4c04c6c04c55 108 thread_sleep_for(10);
pmic 1:93d997d6b232 109 }
pmic 1:93d997d6b232 110 }
pmic 6:e1fa1a2d7483 111