Almir Jashari / Mbed OS Projekt_Hfecter

Projekt code für Hfecter

Dependencies:   PM2_Libary PM2_Example_PES_board

main.cpp@15:1ebd2ee7b1c8, 2021-05-13 (annotated)

Committer:
jashaalm
Date:
Thu May 13 16:28:04 2021 +0000
Revision:
15:1ebd2ee7b1c8
Parent:
14:cfe1311a63f0
Child:
16:5d4a58eecf55
small adjustments made;

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"
jashaalm 14:cfe1311a63f0 8 #include "PositionController.h"
pmic 9:f10b974d01e0 9 #include "FastPWM.h"
jashaalm 14:cfe1311a63f0 10 #include "RangeFinder.h"
jashaalm 15:1ebd2ee7b1c8 11 /*Defines initialisieren*/
jashaalm 12:b87a1e165fb8 12 #define AUFLOESUNG 0.25412f
jashaalm 14:cfe1311a63f0 13 #define FUELSENSOR 1
jashaalm 14:cfe1311a63f0 14 #define BATTERIE 2
jashaalm 14:cfe1311a63f0 15 #define SENSOR 3
jashaalm 14:cfe1311a63f0 16 #define MOTOR 4
jashaalm 14:cfe1311a63f0 17 #define SPERREN 5
jashaalm 14:cfe1311a63f0 18 #define F_FUELSENSOR 1
jashaalm 14:cfe1311a63f0 19 #define F_BATTERIE 2
jashaalm 14:cfe1311a63f0 20
pmic 6:e1fa1a2d7483 21 using namespace std::chrono;
pmic 6:e1fa1a2d7483 22
pmic 8:9bb806a7f585 23 InterruptIn user_button(USER_BUTTON);
jashaalm 14:cfe1311a63f0 24 InterruptIn sensor(PA_1);
jashaalm 15:1ebd2ee7b1c8 25 /*Initialisierung outputs*/
jashaalm 15:1ebd2ee7b1c8 26 DigitalOut led(LED1);
jashaalm 15:1ebd2ee7b1c8 27 AnalogOut BatteryLED (PA_5);
jashaalm 15:1ebd2ee7b1c8 28 AnalogOut WeightLED (PA_4);
jashaalm 15:1ebd2ee7b1c8 29 /*Initialisierung inputs*/
jashaalm 15:1ebd2ee7b1c8 30 AnalogIn adc_vbat(ADC_VBAT);
jashaalm 15:1ebd2ee7b1c8 31 AnalogIn analogIn_2(PA_0);
jashaalm 15:1ebd2ee7b1c8 32 DigitalIn digitalIn(D5);
jashaalm 14:cfe1311a63f0 33 //AnalogIn analogIn_1(PA_1);
jashaalm 14:cfe1311a63f0 34
pmic 6:e1fa1a2d7483 35
pmic 6:e1fa1a2d7483 36 bool executeMainTask = false;
pmic 6:e1fa1a2d7483 37 Timer user_button_timer, loop_timer;
jashaalm 14:cfe1311a63f0 38
pmic 6:e1fa1a2d7483 39
pmic 6:e1fa1a2d7483 40 /* declaration of custom button functions */
pmic 6:e1fa1a2d7483 41 void button_fall();
pmic 6:e1fa1a2d7483 42 void button_rise();
jashaalm 14:cfe1311a63f0 43 void sensor_fall();
jashaalm 14:cfe1311a63f0 44 void led_blinken(int fehler);
pmic 6:e1fa1a2d7483 45 /* create analog input object */
jashaalm 15:1ebd2ee7b1c8 46
jashaalm 14:cfe1311a63f0 47
jashaalm 15:1ebd2ee7b1c8 48 /*Variabeln initialisieren*/
jashaalm 13:35f55464048e 49 float movement = 0.0f;
jashaalm 12:b87a1e165fb8 50 float batt = 0.0f;
jashaalm 14:cfe1311a63f0 51 float weight = 0.0f;
jashaalm 14:cfe1311a63f0 52 float rotation = 0.0f;
jashaalm 15:1ebd2ee7b1c8 53 int situation = FUELSENSOR;
jashaalm 15:1ebd2ee7b1c8 54 int Fehler = 0;
jashaalm 15:1ebd2ee7b1c8 55 int sensor_trig = 0;
jashaalm 15:1ebd2ee7b1c8 56 int timer_1 = 0;
jashaalm 15:1ebd2ee7b1c8 57 /*Initialisierung motor ¦ entnommen aus PM2_example_PES_board*/
pmic 6:e1fa1a2d7483 58 /* create enable dc motor digital out object */
pmic 6:e1fa1a2d7483 59 DigitalOut enable_motors(PB_15);
pmic 10:c5d85e35758c 60 /* create pwm objects */
jashaalm 13:35f55464048e 61 //FastPWM pwmOut_M1(PB_13);
pmic 10:c5d85e35758c 62 FastPWM pwmOut_M2(PA_9);
jashaalm 13:35f55464048e 63 //FastPWM pwmOut_M3(PA_10);
pmic 10:c5d85e35758c 64 double Ts_pwm_s = 0.00005; // this needs to be a double value (no f at the end)
pmic 6:e1fa1a2d7483 65 /* create encoder read objects */
jashaalm 14:cfe1311a63f0 66
pmic 10:c5d85e35758c 67 EncoderCounter encoderCounter_M2(PB_6, PB_7);
pmic 10:c5d85e35758c 68 /* create speed controller objects, only M1 and M2, M3 is used open-loop */
pmic 10:c5d85e35758c 69 float counts_per_turn = 20.0f*78.125f; // counts/turn * gearratio
pmic 10:c5d85e35758c 70 float kn = 180.0f/12.0f; // (RPM/V)
pmic 10:c5d85e35758c 71 float max_voltage = 12.0f; // adjust this to 6.0f if only one batterypack is used
jashaalm 14:cfe1311a63f0 72 //SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
jashaalm 14:cfe1311a63f0 73 PositionController positioncontroller_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
pmic 6:e1fa1a2d7483 74
jashaalm 13:35f55464048e 75 /* analog output */
jashaalm 15:1ebd2ee7b1c8 76
pmic 1:93d997d6b232 77
jashaalm 15:1ebd2ee7b1c8 78 /*Hauptprogramm*/
pmic 1:93d997d6b232 79 int main()
pmic 9:f10b974d01e0 80 {
jashaalm 14:cfe1311a63f0 81
pmic 6:e1fa1a2d7483 82 user_button.fall(&button_fall);
pmic 6:e1fa1a2d7483 83 user_button.rise(&button_rise);
jashaalm 14:cfe1311a63f0 84 sensor.fall(&sensor_fall);
jashaalm 14:cfe1311a63f0 85
pmic 6:e1fa1a2d7483 86 loop_timer.start();
pmic 6:e1fa1a2d7483 87
pmic 10:c5d85e35758c 88 /* enable hardwaredriver dc motors */
pmic 10:c5d85e35758c 89 enable_motors = 1;
jashaalm 14:cfe1311a63f0 90
pmic 9:f10b974d01e0 91
jashaalm 14:cfe1311a63f0 92 float movementold = 0;
pmic 1:93d997d6b232 93 while (true) {
jashaalm 14:cfe1311a63f0 94
jashaalm 12:b87a1e165fb8 95 batt = adc_vbat.read()* (3.3f/AUFLOESUNG);
pmic 6:e1fa1a2d7483 96 loop_timer.reset();
jashaalm 15:1ebd2ee7b1c8 97 weight = analogIn_2.read() * 5;
pmic 6:e1fa1a2d7483 98 /* ------------- start hacking ------------- -------------*/
jashaalm 15:1ebd2ee7b1c8 99
jashaalm 14:cfe1311a63f0 100 movement = 0;
jashaalm 14:cfe1311a63f0 101 rotation = positioncontroller_M2.getRotation();
jashaalm 14:cfe1311a63f0 102 switch (situation) {
jashaalm 14:cfe1311a63f0 103 case FUELSENSOR:
pmic 6:e1fa1a2d7483 104
jashaalm 15:1ebd2ee7b1c8 105 if(weight < 4) {//Ist Der Füllstand Unter 25%?
jashaalm 15:1ebd2ee7b1c8 106 WeightLED = 1;//Anzeige LED
jashaalm 15:1ebd2ee7b1c8 107 situation = BATTERIE;//Weiter zu Batteriestand
jashaalm 15:1ebd2ee7b1c8 108 if (weight < 3.5) {//Ist der Füllstand unter 10%
jashaalm 15:1ebd2ee7b1c8 109 Fehler = F_FUELSENSOR;//Weitergabe Feler
jashaalm 15:1ebd2ee7b1c8 110 situation = SPERREN;//Gerät sperren
jashaalm 14:cfe1311a63f0 111
jashaalm 14:cfe1311a63f0 112 }
jashaalm 14:cfe1311a63f0 113 } else {
jashaalm 15:1ebd2ee7b1c8 114 WeightLED = 0;//LED AUS
jashaalm 15:1ebd2ee7b1c8 115 situation = BATTERIE;//Weiter zu Batteriestand
jashaalm 14:cfe1311a63f0 116 }
pmic 6:e1fa1a2d7483 117
jashaalm 14:cfe1311a63f0 118 break;
jashaalm 15:1ebd2ee7b1c8 119 case BATTERIE: //Der Batteriestand
jashaalm 15:1ebd2ee7b1c8 120 if(batt <3.2) { //Ist der Batteriesand Unter 25%
jashaalm 15:1ebd2ee7b1c8 121 BatteryLED = 1;//Anzeige LED
jashaalm 15:1ebd2ee7b1c8 122 situation = SENSOR;//Weiter zu Sensor
jashaalm 15:1ebd2ee7b1c8 123 if(batt < 3) {//Ist der Batteriestand unter 10%
jashaalm 15:1ebd2ee7b1c8 124 fehler = F_BATTERIE;//Weitergabe Fehler
jashaalm 15:1ebd2ee7b1c8 125 situation = FEHLER;//Gerät sperren
jashaalm 14:cfe1311a63f0 126 }
jashaalm 14:cfe1311a63f0 127 } else {
jashaalm 15:1ebd2ee7b1c8 128 BatteryLED = 0;//LED AUS
jashaalm 15:1ebd2ee7b1c8 129 situation = SENSOR;//Weiter zu Sensor
jashaalm 14:cfe1311a63f0 130 }
jashaalm 14:cfe1311a63f0 131 break;
jashaalm 15:1ebd2ee7b1c8 132 case SENSOR: //Sensor
jashaalm 15:1ebd2ee7b1c8 133 if (sensor_trig == 1) {//Wurde der Bewegungsensor Betätigt
jashaalm 15:1ebd2ee7b1c8 134 situation = MOTOR;//Weiter zu Motor
jashaalm 15:1ebd2ee7b1c8 135 sensor_trig = 0;
jashaalm 14:cfe1311a63f0 136 } else {
jashaalm 15:1ebd2ee7b1c8 137 situation = FUELSENSOR;//Weiter zu Füllsensor
jashaalm 14:cfe1311a63f0 138 }
jashaalm 14:cfe1311a63f0 139 break;
jashaalm 15:1ebd2ee7b1c8 140 case MOTOR: //Motor
jashaalm 15:1ebd2ee7b1c8 141 if(timer_1 > 200) { //Ist genug zeit vergangen das jemand seine Hand wegnemmen konte(hier 2 sek)
jashaalm 15:1ebd2ee7b1c8 142 enable_motors = 1; //motor ein
jashaalm 14:cfe1311a63f0 143 positioncontroller_M2.setDesiredRotation(1.0f);
jashaalm 15:1ebd2ee7b1c8 144 if (timer_1 ==500) {//Hat der motor lange genug gedreht?
jashaalm 14:cfe1311a63f0 145 positioncontroller_M2.setDesiredRotation(0.0f);
jashaalm 15:1ebd2ee7b1c8 146 enable_motors = 0;//Motor aus
jashaalm 14:cfe1311a63f0 147 timer_1 = 0;
jashaalm 15:1ebd2ee7b1c8 148 situation = FUELSENSOR;//weiter zu Füllsensor
jashaalm 14:cfe1311a63f0 149 } else {
jashaalm 15:1ebd2ee7b1c8 150 timer_1 ++; //warten
jashaalm 14:cfe1311a63f0 151 }
pmic 9:f10b974d01e0 152
jashaalm 14:cfe1311a63f0 153 } else {
jashaalm 15:1ebd2ee7b1c8 154 timer_1 ++; //warten
jashaalm 14:cfe1311a63f0 155 }
jashaalm 14:cfe1311a63f0 156 break;
jashaalm 15:1ebd2ee7b1c8 157 /*Gerät Sperren*/
jashaalm 14:cfe1311a63f0 158 case SPERREN:
jashaalm 15:1ebd2ee7b1c8 159 if (Fehler == F_FUELSENSOR) { //Liegt der Fehler beim Füllsensor?
jashaalm 15:1ebd2ee7b1c8 160 led_blinken(F_FUELSENSOR);//Anzeige LED Blinken
jashaalm 15:1ebd2ee7b1c8 161 if(weight > 4.2) { //Liegt der Füllstand über 30%
jashaalm 15:1ebd2ee7b1c8 162 WeightLED = 0; //LED aus
jashaalm 15:1ebd2ee7b1c8 163 situation = BATTERIE; //Weiter Batteriestand
jashaalm 14:cfe1311a63f0 164 }
jashaalm 14:cfe1311a63f0 165 } else {
jashaalm 15:1ebd2ee7b1c8 166 led_blinken(F_BATTERIE);//Anzeige LED Blinken
jashaalm 14:cfe1311a63f0 167 if(batt >3.2) {
jashaalm 15:1ebd2ee7b1c8 168 BatteryLED = 0; //LED aus
jashaalm 15:1ebd2ee7b1c8 169 situation = SENSOR;//Weiter zu Sensor
jashaalm 14:cfe1311a63f0 170 }
jashaalm 14:cfe1311a63f0 171 }
jashaalm 14:cfe1311a63f0 172 break;
pmic 6:e1fa1a2d7483 173
jashaalm 14:cfe1311a63f0 174
pmic 6:e1fa1a2d7483 175
jashaalm 14:cfe1311a63f0 176 }
pmic 6:e1fa1a2d7483 177
jashaalm 14:cfe1311a63f0 178 led = !led;
pmic 6:e1fa1a2d7483 179
pmic 10:c5d85e35758c 180 /* do only output via serial what's really necessary (this makes your code slow)*/
jashaalm 14:cfe1311a63f0 181 printf("%3.3f\n, %3.3f\n, %3.3f\n, %3d\n,%3d\n ",
jashaalm 13:35f55464048e 182 movement,
jashaalm 14:cfe1311a63f0 183 weight,
jashaalm 12:b87a1e165fb8 184 batt,
jashaalm 15:1ebd2ee7b1c8 185 timer_1);
pmic 8:9bb806a7f585 186
pmic 6:e1fa1a2d7483 187 /* ------------- stop hacking ------------- -------------*/
pmic 6:e1fa1a2d7483 188
jashaalm 14:cfe1311a63f0 189 // int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
jashaalm 14:cfe1311a63f0 190 // int dT_loop_ms = Ts_ms - T_loop_ms;
jashaalm 14:cfe1311a63f0 191 // thread_sleep_for(dT_loop_ms);
jashaalm 13:35f55464048e 192 thread_sleep_for(10);
jashaalm 14:cfe1311a63f0 193
pmic 1:93d997d6b232 194 }
pmic 1:93d997d6b232 195 }
pmic 6:e1fa1a2d7483 196
pmic 6:e1fa1a2d7483 197 void button_fall()
pmic 6:e1fa1a2d7483 198 {
pmic 6:e1fa1a2d7483 199 user_button_timer.reset();
pmic 6:e1fa1a2d7483 200 user_button_timer.start();
pmic 6:e1fa1a2d7483 201 }
pmic 6:e1fa1a2d7483 202
jashaalm 14:cfe1311a63f0 203 void sensor_fall()
jashaalm 14:cfe1311a63f0 204 {
jashaalm 15:1ebd2ee7b1c8 205 if (situation != MOTOR && situation != SPERREN) {
jashaalm 15:1ebd2ee7b1c8 206 sensor_trig = 1;
jashaalm 14:cfe1311a63f0 207 }
jashaalm 14:cfe1311a63f0 208
jashaalm 14:cfe1311a63f0 209
jashaalm 14:cfe1311a63f0 210 }
jashaalm 14:cfe1311a63f0 211 void led_blinken(int fehler)
jashaalm 14:cfe1311a63f0 212 {
jashaalm 14:cfe1311a63f0 213 static int timer = 0;
jashaalm 14:cfe1311a63f0 214
jashaalm 14:cfe1311a63f0 215 if (fehler == F_FUELSENSOR) {
jashaalm 14:cfe1311a63f0 216 if (timer == 50) {
jashaalm 14:cfe1311a63f0 217 timer = 0;
jashaalm 14:cfe1311a63f0 218
jashaalm 14:cfe1311a63f0 219 WeightLED = !WeightLED;
jashaalm 14:cfe1311a63f0 220 }
jashaalm 14:cfe1311a63f0 221 } else {
jashaalm 14:cfe1311a63f0 222 if (timer == 50) {
jashaalm 14:cfe1311a63f0 223
jashaalm 14:cfe1311a63f0 224 BatteryLED = !BatteryLED;
jashaalm 14:cfe1311a63f0 225 timer = 0;
jashaalm 14:cfe1311a63f0 226 }
jashaalm 14:cfe1311a63f0 227 }
jashaalm 14:cfe1311a63f0 228 timer ++;
jashaalm 14:cfe1311a63f0 229 }
pmic 6:e1fa1a2d7483 230 void button_rise()
pmic 6:e1fa1a2d7483 231 {
pmic 6:e1fa1a2d7483 232 int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 233 user_button_timer.stop();
pmic 8:9bb806a7f585 234 if (t_button_ms > 200) {
pmic 6:e1fa1a2d7483 235 executeMainTask = !executeMainTask;
pmic 8:9bb806a7f585 236 }
pmic 6:e1fa1a2d7483 237 }