TreppenRobo / Mbed OS Prototyp

Workshop 2

Dependencies:   PM2_Libary

main.cpp@36:c6961428c1e1, 2022-03-30 (annotated)

Committer:
raomen
Date:
Wed Mar 30 11:22:42 2022 +0200
Revision:
36:c6961428c1e1
Parent:
34:4c04c6c04c55
Child:
37:6ac4db3cc57b
updated pin declaration

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----------------------------------------------------------
pmic 17:c19b471f05cb 15
lupomic 34:4c04c6c04c55 16 int ToNextFunction;
lupomic 34:4c04c6c04c55 17 float trigerValueRearSensor = 0.0f;
lupomic 34:4c04c6c04c55 18 float trigerValueFrontSensor = 0.0f;
pmic 17:c19b471f05cb 19
pmic 6:e1fa1a2d7483 20
lupomic 34:4c04c6c04c55 21 //-----------------------------------------actors Definition----------------------------------------------------
raomen 36:c6961428c1e1 22 // Infrarot sensors pin declaration
raomen 36:c6961428c1e1 23 AnalogIn FrontSensor(PC_6); //IR TOF sensor at the front facing down
raomen 36:c6961428c1e1 24 AnalogIn RearSensor(PC_8); //IR TOF sensor at the back facing down
raomen 36:c6961428c1e1 25 AnalogIn LeftEyeSensor(PC_2); // Infrared sensor on top in "head" part left eye
raomen 36:c6961428c1e1 26 AnalogIn RightEyeSensor(PC_3); // Infrared sensor on top in "head" part right eye
pmic 17:c19b471f05cb 27
raomen 36:c6961428c1e1 28 //Encoder pin declaration
raomen 36:c6961428c1e1 29 EncoderCounter encoder_M_right(PA_6, PC_7); //encoder pin decalaration for wheels right side
raomen 36:c6961428c1e1 30 EncoderCounter encoder_M_left(PB_6, PB_7); //encoder pin decalaration for wheels left side
raomen 36:c6961428c1e1 31 EncoderCounter encoder_M_arm(PA_0, PA_1); //encoder pin decalaration for arm
pmic 17:c19b471f05cb 32
raomen 36:c6961428c1e1 33 //motor pin declaration
raomen 36:c6961428c1e1 34 FastPWM pwm_M_right(PA_9);
raomen 36:c6961428c1e1 35 FastPWM pwm_M_left(PB_13);
raomen 36:c6961428c1e1 36 FastPWM pwm_M_arm(PA_10);
pmic 17:c19b471f05cb 37
pmic 30:1e8295770bc1 38 // create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
pmic 24:86f1a63e35a0 39 float max_voltage = 12.0f; // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
raomen 36:c6961428c1e1 40 float counts_per_turn_wheels = 2000.0f * 100.0f; // define counts per turn at gearbox end (counts/turn * gearratio) for wheels
raomen 36:c6961428c1e1 41 float counts_per_turn_arm = 40000.0f * 100.0f; // define counts per turn at gearbox end (counts/turn * gearratio) for arm
pmic 25:ea1d6e27c895 42 float kn = 180.0f / 12.0f; // define motor constant in rpm per V
pmic 30:1e8295770bc1 43 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 44 float kp = 0.1f; // define custom kp, this is the default speed controller gain for gear box 78.125:1
pmic 6:e1fa1a2d7483 45
lupomic 34:4c04c6c04c55 46 //motors for tracks
raomen 36:c6961428c1e1 47 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
raomen 36:c6961428c1e1 48 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 49
lupomic 34:4c04c6c04c55 50 //Arm Motor
raomen 36:c6961428c1e1 51 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 52
lupomic 34:4c04c6c04c55 53 //-----------------------------------------Functions----------------------------------------------------------
lupomic 34:4c04c6c04c55 54 //only moves the arm in to the starting position
lupomic 34:4c04c6c04c55 55 int StartPosition(void){
lupomic 34:4c04c6c04c55 56 if(true){
lupomic 34:4c04c6c04c55 57 return 1;
lupomic 34:4c04c6c04c55 58 }
lupomic 34:4c04c6c04c55 59 return NULL;
lupomic 34:4c04c6c04c55 60 }
lupomic 34:4c04c6c04c55 61
lupomic 34:4c04c6c04c55 62 //Drives forward into the next step
lupomic 34:4c04c6c04c55 63 int Drive(int8_t dist){
lupomic 34:4c04c6c04c55 64 int8_t i = 0; //prov condition variable
lupomic 34:4c04c6c04c55 65
pmic 17:c19b471f05cb 66
lupomic 34:4c04c6c04c55 67 int8_t distance = dist; //distance which will be driven in [mm]
lupomic 34:4c04c6c04c55 68 float factor = 1.0f; //factor for calculating the value in to the float which will be given to the setDesiredRotation function
lupomic 34:4c04c6c04c55 69 float distanceValue = float(distance)*factor;
lupomic 34:4c04c6c04c55 70 do{
lupomic 34:4c04c6c04c55 71 positionController_M_right.setDesiredRotation(distanceValue);
lupomic 34:4c04c6c04c55 72 positionController_M_left.setDesiredRotation(distanceValue);
lupomic 34:4c04c6c04c55 73 i++;
lupomic 34:4c04c6c04c55 74 }while(i < 1);
lupomic 34:4c04c6c04c55 75 return 0;
lupomic 34:4c04c6c04c55 76
lupomic 34:4c04c6c04c55 77 }
pmic 17:c19b471f05cb 78
lupomic 34:4c04c6c04c55 79 //only turns the arm until the robot is on the next step
lupomic 34:4c04c6c04c55 80 //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 81 int LiftUp(int8_t deg){
lupomic 34:4c04c6c04c55 82 int8_t rotation = deg;
lupomic 34:4c04c6c04c55 83 int8_t i = 0; //prov condition variable
lupomic 34:4c04c6c04c55 84 do{
lupomic 34:4c04c6c04c55 85 positionController_M_Arm.setDesiredRotation(deg);
lupomic 34:4c04c6c04c55 86
lupomic 34:4c04c6c04c55 87 i++;
lupomic 34:4c04c6c04c55 88 }while(i < 1);
lupomic 34:4c04c6c04c55 89 return 0;
lupomic 34:4c04c6c04c55 90 }
pmic 1:93d997d6b232 91
lupomic 34:4c04c6c04c55 92
pmic 17:c19b471f05cb 93
lupomic 34:4c04c6c04c55 94
pmic 1:93d997d6b232 95 int main()
pmic 23:26b3a25fc637 96 {
lupomic 34:4c04c6c04c55 97 while (true) {
lupomic 34:4c04c6c04c55 98 ToNextFunction = StartPosition();
lupomic 34:4c04c6c04c55 99 if(NULL != ToNextFunction){
lupomic 34:4c04c6c04c55 100 ToNextFunction = Drive(3);
lupomic 34:4c04c6c04c55 101 if(NULL != ToNextFunction){
lupomic 34:4c04c6c04c55 102 ToNextFunction = LiftUp(5);
lupomic 34:4c04c6c04c55 103 if(NULL != ToNextFunction){
lupomic 34:4c04c6c04c55 104 ToNextFunction = Drive(-1);
pmic 6:e1fa1a2d7483 105
lupomic 34:4c04c6c04c55 106 }
pmic 8:9bb806a7f585 107 }
pmic 1:93d997d6b232 108 }
lupomic 34:4c04c6c04c55 109
lupomic 34:4c04c6c04c55 110 thread_sleep_for(10);
pmic 1:93d997d6b232 111 }
pmic 1:93d997d6b232 112 }
pmic 6:e1fa1a2d7483 113