Almir Jashari / Mbed OS Projekt_Hfecter

Projekt code für Hfecter

Dependencies:   PM2_Libary PM2_Example_PES_board

main.cpp@13:35f55464048e, 2021-05-06 (annotated)

Committer:
jashaalm
Date:
Thu May 06 11:52:53 2021 +0000
Revision:
13:35f55464048e
Parent:
12:b87a1e165fb8
Child:
14:cfe1311a63f0
Commit before big change;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 1:93d997d6b232 1 #include "mbed.h"
pmic 1:93d997d6b232 2 #include "platform/mbed_thread.h"
pmic 6:e1fa1a2d7483 3
pmic 6:e1fa1a2d7483 4 /* PM2_Libary */
pmic 1:93d997d6b232 5 #include "EncoderCounter.h"
pmic 1:93d997d6b232 6 #include "Servo.h"
pmic 3:d22942631cd7 7 #include "SpeedController.h"
pmic 9:f10b974d01e0 8 #include "FastPWM.h"
jashaalm 12:b87a1e165fb8 9 #define AUFLOESUNG 0.25412f
pmic 6:e1fa1a2d7483 10 using namespace std::chrono;
pmic 6:e1fa1a2d7483 11
pmic 8:9bb806a7f585 12 InterruptIn user_button(USER_BUTTON);
pmic 8:9bb806a7f585 13 DigitalOut led(LED1);
pmic 6:e1fa1a2d7483 14
pmic 6:e1fa1a2d7483 15 bool executeMainTask = false;
pmic 6:e1fa1a2d7483 16 Timer user_button_timer, loop_timer;
pmic 7:c0f5bb355f41 17 int Ts_ms = 50;
pmic 6:e1fa1a2d7483 18
pmic 6:e1fa1a2d7483 19 /* declaration of custom button functions */
pmic 6:e1fa1a2d7483 20 void button_fall();
pmic 6:e1fa1a2d7483 21 void button_rise();
pmic 6:e1fa1a2d7483 22
pmic 6:e1fa1a2d7483 23 /* create analog input object */
jashaalm 12:b87a1e165fb8 24 AnalogIn adc_vbat(ADC_VBAT);
jashaalm 13:35f55464048e 25 DigitalIn digitalIn(D5);
jashaalm 13:35f55464048e 26 AnalogIn analogIn(PA_1);
jashaalm 13:35f55464048e 27
jashaalm 13:35f55464048e 28 float movement = 0.0f;
jashaalm 12:b87a1e165fb8 29 float batt = 0.0f;
pmic 6:e1fa1a2d7483 30
pmic 6:e1fa1a2d7483 31 /* create enable dc motor digital out object */
pmic 6:e1fa1a2d7483 32 DigitalOut enable_motors(PB_15);
pmic 10:c5d85e35758c 33 /* create pwm objects */
jashaalm 13:35f55464048e 34 //FastPWM pwmOut_M1(PB_13);
pmic 10:c5d85e35758c 35 FastPWM pwmOut_M2(PA_9);
jashaalm 13:35f55464048e 36 //FastPWM pwmOut_M3(PA_10);
pmic 10:c5d85e35758c 37 double Ts_pwm_s = 0.00005; // this needs to be a double value (no f at the end)
pmic 6:e1fa1a2d7483 38 /* create encoder read objects */
jashaalm 13:35f55464048e 39 //EncoderCounter encoderCounter_M1(PA_6, PC_7);
pmic 10:c5d85e35758c 40 EncoderCounter encoderCounter_M2(PB_6, PB_7);
jashaalm 13:35f55464048e 41 //EncoderCounter encoderCounter_M3(PA_0, PA_1);
pmic 10:c5d85e35758c 42 /* create speed controller objects, only M1 and M2, M3 is used open-loop */
pmic 10:c5d85e35758c 43 float counts_per_turn = 20.0f*78.125f; // counts/turn * gearratio
pmic 10:c5d85e35758c 44 float kn = 180.0f/12.0f; // (RPM/V)
pmic 10:c5d85e35758c 45 float max_voltage = 12.0f; // adjust this to 6.0f if only one batterypack is used
jashaalm 13:35f55464048e 46 //SpeedController speedController_M1(counts_per_turn, kn, max_voltage, pwmOut_M1, encoderCounter_M1);
pmic 10:c5d85e35758c 47 SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
pmic 6:e1fa1a2d7483 48
jashaalm 13:35f55464048e 49 /* analog output */
jashaalm 13:35f55464048e 50 AnalogOut BatteryLED (PA_5);
jashaalm 12:b87a1e165fb8 51
pmic 1:93d997d6b232 52
pmic 1:93d997d6b232 53 int main()
pmic 9:f10b974d01e0 54 {
pmic 6:e1fa1a2d7483 55 user_button.fall(&button_fall);
pmic 6:e1fa1a2d7483 56 user_button.rise(&button_rise);
pmic 6:e1fa1a2d7483 57 loop_timer.start();
pmic 6:e1fa1a2d7483 58
pmic 10:c5d85e35758c 59 /* enable hardwaredriver dc motors */
pmic 10:c5d85e35758c 60 enable_motors = 1;
pmic 10:c5d85e35758c 61 /* initialize pwm for motor M3*/
jashaalm 13:35f55464048e 62 //pwmOut_M3.period(Ts_pwm_s);
pmic 6:e1fa1a2d7483 63 /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */
jashaalm 13:35f55464048e 64 //pwmOut_M3.write(0.5);
pmic 9:f10b974d01e0 65
jashaalm 13:35f55464048e 66 float movementold = 0;
pmic 1:93d997d6b232 67 while (true) {
jashaalm 12:b87a1e165fb8 68 batt = adc_vbat.read()* (3.3f/AUFLOESUNG);
pmic 6:e1fa1a2d7483 69 loop_timer.reset();
pmic 6:e1fa1a2d7483 70
pmic 6:e1fa1a2d7483 71 /* ------------- start hacking ------------- -------------*/
pmic 6:e1fa1a2d7483 72
jashaalm 13:35f55464048e 73
jashaalm 13:35f55464048e 74 movement = roundf(analogIn.read()*1.2);
jashaalm 13:35f55464048e 75 // if ( movement > 1) movement = 1;
jashaalm 13:35f55464048e 76 // else movement = 0;
pmic 6:e1fa1a2d7483 77
pmic 6:e1fa1a2d7483 78 /* read analog input */
jashaalm 13:35f55464048e 79 BatteryLED = 1;
jashaalm 13:35f55464048e 80 //ceil()
jashaalm 13:35f55464048e 81 if (movementold > movement)//WIP
jashaalm 13:35f55464048e 82 {
pmic 10:c5d85e35758c 83 /* command a speed to dc motors M1 and M2*/
jashaalm 13:35f55464048e 84 //speedController_M1.setDesiredSpeedRPS( 2.0f);
jashaalm 13:35f55464048e 85 speedController_M2.setDesiredSpeedRPS(2.0f);
jashaalm 13:35f55464048e 86 enable_motors = 1;
jashaalm 13:35f55464048e 87
jashaalm 13:35f55464048e 88 thread_sleep_for(3000);// int Wert in ms
pmic 10:c5d85e35758c 89 /* write output voltage to motor M3 */
jashaalm 13:35f55464048e 90 //pwmOut_M3.write(1.0);
jashaalm 13:35f55464048e 91 }
jashaalm 12:b87a1e165fb8 92
pmic 6:e1fa1a2d7483 93 /* visual feedback that the main task is executed */
jashaalm 13:35f55464048e 94
pmic 9:f10b974d01e0 95
jashaalm 13:35f55464048e 96 else {
jashaalm 13:35f55464048e 97
jashaalm 13:35f55464048e 98 //movement = 0.0f;
pmic 6:e1fa1a2d7483 99
jashaalm 13:35f55464048e 100 // speedController_M1.setDesiredSpeedRPS(0.0f);
jashaalm 13:35f55464048e 101 // speedController_M2.setDesiredSpeedRPS(0.0f);
jashaalm 13:35f55464048e 102 // BatteryLED = 0;
jashaalm 13:35f55464048e 103 enable_motors = 0;
jashaalm 13:35f55464048e 104 // pwmOut_M3.write(-1);
pmic 6:e1fa1a2d7483 105
jashaalm 12:b87a1e165fb8 106
pmic 6:e1fa1a2d7483 107
pmic 6:e1fa1a2d7483 108 led = 0;
pmic 1:93d997d6b232 109 }
jashaalm 13:35f55464048e 110 led = !led;
pmic 6:e1fa1a2d7483 111
pmic 10:c5d85e35758c 112 /* do only output via serial what's really necessary (this makes your code slow)*/
jashaalm 13:35f55464048e 113 printf("%3.3f\n, %3.3f\n, %3.3f;\r\n",
jashaalm 13:35f55464048e 114 movement,
jashaalm 12:b87a1e165fb8 115 batt,
jashaalm 13:35f55464048e 116 //encoderCounter_M3.read(),
jashaalm 13:35f55464048e 117 //speedController_M1.getSpeedRPS(),
pmic 10:c5d85e35758c 118 speedController_M2.getSpeedRPS());
pmic 8:9bb806a7f585 119
pmic 6:e1fa1a2d7483 120 /* ------------- stop hacking ------------- -------------*/
pmic 6:e1fa1a2d7483 121
jashaalm 13:35f55464048e 122 //int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
jashaalm 13:35f55464048e 123 //int dT_loop_ms = Ts_ms - T_loop_ms;
jashaalm 13:35f55464048e 124 //thread_sleep_for(dT_loop_ms);
jashaalm 13:35f55464048e 125 thread_sleep_for(10);
jashaalm 13:35f55464048e 126 movementold = movement;
pmic 1:93d997d6b232 127 }
pmic 1:93d997d6b232 128 }
pmic 6:e1fa1a2d7483 129
pmic 6:e1fa1a2d7483 130 void button_fall()
pmic 6:e1fa1a2d7483 131 {
pmic 6:e1fa1a2d7483 132 user_button_timer.reset();
pmic 6:e1fa1a2d7483 133 user_button_timer.start();
pmic 6:e1fa1a2d7483 134 }
pmic 6:e1fa1a2d7483 135
pmic 6:e1fa1a2d7483 136 void button_rise()
pmic 6:e1fa1a2d7483 137 {
pmic 6:e1fa1a2d7483 138 int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 139 user_button_timer.stop();
pmic 8:9bb806a7f585 140 if (t_button_ms > 200) {
pmic 6:e1fa1a2d7483 141 executeMainTask = !executeMainTask;
pmic 8:9bb806a7f585 142 }
pmic 6:e1fa1a2d7483 143 }