Almir Jashari / Mbed OS Projekt_Hfecter

Projekt code für Hfecter

Dependencies:   PM2_Libary PM2_Example_PES_board

main.cpp@14:cfe1311a63f0, 2021-05-09 (annotated)

Committer:
jashaalm
Date:
Sun May 09 15:02:06 2021 +0000
Revision:
14:cfe1311a63f0
Parent:
13:35f55464048e
Child:
15:1ebd2ee7b1c8
first working version; ;

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