Diff: main.cpp

Revision:

14:cfe1311a63f0

Parent:

13:35f55464048e

Child:

15:1ebd2ee7b1c8

--- a/main.cpp	Thu May 06 11:52:53 2021 +0000
+++ b/main.cpp	Sun May 09 15:02:06 2021 +0000
@@ -5,29 +5,47 @@
 #include "EncoderCounter.h"
 #include "Servo.h"
 #include "SpeedController.h"
+#include "PositionController.h"
 #include "FastPWM.h"
+#include "RangeFinder.h"
+
 #define AUFLOESUNG 0.25412f
+#define FUELSENSOR 1
+#define BATTERIE 2
+#define SENSOR 3
+#define MOTOR 4
+#define SPERREN 5
+#define F_FUELSENSOR 1
+#define F_BATTERIE 2
+
 using namespace std::chrono;
 
 InterruptIn user_button(USER_BUTTON);
+InterruptIn sensor(PA_1);
+
+//AnalogIn analogIn_1(PA_1);
+
 DigitalOut  led(LED1);
 
 bool  executeMainTask = false;
 Timer user_button_timer, loop_timer;
-int   Ts_ms = 50;
+
 
 /* declaration of custom button functions */
 void button_fall();
 void button_rise();
-
+void sensor_fall();
+void led_blinken(int fehler);
 /* create analog input object */
 AnalogIn adc_vbat(ADC_VBAT);
 DigitalIn digitalIn(D5);
-AnalogIn analogIn(PA_1);
+
+AnalogIn analogIn_2(PA_0);
 
 float    movement = 0.0f;
 float    batt = 0.0f;
-
+float    weight = 0.0f;
+float    rotation = 0.0f;
 /* create enable dc motor digital out object */
 DigitalOut enable_motors(PB_15);
 /* create pwm objects */
@@ -36,94 +54,139 @@
 //FastPWM pwmOut_M3(PA_10);
 double  Ts_pwm_s = 0.00005; // this needs to be a double value (no f at the end)
 /* create encoder read objects */
-//EncoderCounter  encoderCounter_M1(PA_6, PC_7);
+
 EncoderCounter  encoderCounter_M2(PB_6, PB_7);
-//EncoderCounter  encoderCounter_M3(PA_0, PA_1);
 /* create speed controller objects, only M1 and M2, M3 is used open-loop */
 float counts_per_turn = 20.0f*78.125f; // counts/turn * gearratio
 float kn = 180.0f/12.0f;               // (RPM/V)
 float max_voltage = 12.0f;             // adjust this to 6.0f if only one batterypack is used
-//SpeedController speedController_M1(counts_per_turn, kn, max_voltage, pwmOut_M1, encoderCounter_M1);
-SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
+//SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
+PositionController positioncontroller_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
 
 /* analog output */
- AnalogOut BatteryLED (PA_5); 
-
+AnalogOut BatteryLED (PA_5);
+AnalogOut WeightLED (PA_4);
+int situation = FUELSENSOR;
+int Fehler = 0;
+int sensor_trig = 0;
+int timer_1 = 0;
 
 int main()
 {
+
     user_button.fall(&button_fall);
     user_button.rise(&button_rise);
+    sensor.fall(&sensor_fall);
+
     loop_timer.start();
 
     /* enable hardwaredriver dc motors */
     enable_motors = 1;
-    /* initialize pwm for motor M3*/
-    //pwmOut_M3.period(Ts_pwm_s);
-    /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */
-    //pwmOut_M3.write(0.5);
+
 
-float movementold = 0;
+    float movementold = 0;
     while (true) {
+
         batt = adc_vbat.read()* (3.3f/AUFLOESUNG);
         loop_timer.reset();
 
         /* ------------- start hacking ------------- -------------*/
+        weight = analogIn_2.read() * 5;
+        movement = 0;
+//roundf(analogIn_1.read()*1.2);
+        rotation = positioncontroller_M2.getRotation();
+        switch (situation) {
+            case FUELSENSOR:
 
-    
-movement = roundf(analogIn.read()*1.2);
-// if ( movement > 1) movement = 1;
-// else movement = 0;
+                if(weight < 4) {
+                    WeightLED = 1;
+                    situation = BATTERIE;
+                    if (weight < 3.5) {
+                        Fehler = F_FUELSENSOR;
+                        situation = SPERREN;
+
+                    }
+                } else {
+                    WeightLED = 0;
+                    situation = BATTERIE;
+                }
 
-            /* read analog input */
-          BatteryLED = 1; 
-            //ceil()
-            if (movementold > movement)//WIP
-            {
-            /* command a speed to dc motors M1 and M2*/
-            //speedController_M1.setDesiredSpeedRPS( 2.0f);
-            speedController_M2.setDesiredSpeedRPS(2.0f);
-             enable_motors = 1;
-             
-              thread_sleep_for(3000);// int Wert in ms
-            /* write output voltage to motor M3 */
-            //pwmOut_M3.write(1.0);
-            }
-        
-            /* visual feedback that the main task is executed */
-           
+                break;
+            case BATTERIE:
+                if(batt <3.2) {
+                    BatteryLED = 1;
+                    situation = SENSOR;
+                    if(batt < 3) {
+                        //fehler = F_BATTERIE;
+                        //situation = FEHLER;
+                    }
+                } else {
+                    BatteryLED = 0;
+                    situation = SENSOR;
+                }
+                break;
+            case SENSOR:
+                if (sensor_trig == 1) {
+                    situation = MOTOR;
+                     sensor_trig = 0;
+                } else {
+                    situation = FUELSENSOR;
+                }
+                break;
+            case MOTOR:
+                if(timer_1 > 200) {
+                    enable_motors = 1;
+                    positioncontroller_M2.setDesiredRotation(1.0f);
+                    if (timer_1 ==500) {
+                        positioncontroller_M2.setDesiredRotation(0.0f);
+                        enable_motors = 0;
+                        timer_1 = 0;
+                        situation = FUELSENSOR;
+                    } else {
+                        timer_1 ++;
+                    }
 
-         else {
-
-            //movement = 0.0f;
+                } else {
+                    timer_1 ++;
+                }
+                break;
+            case SPERREN:
+                if (Fehler == F_FUELSENSOR) {
+                    led_blinken(F_FUELSENSOR);
+                    if(weight > 4.2) {
+                        WeightLED = 0;
+                        situation = BATTERIE;
+                    }
+                } else {
+                    led_blinken(F_BATTERIE);
+                    if(batt >3.2) {
+                        BatteryLED = 0;
+                        situation = SENSOR;
+                    }
+                }
+                break;
 
-           // speedController_M1.setDesiredSpeedRPS(0.0f);
-         //  speedController_M2.setDesiredSpeedRPS(0.0f);
-  //       BatteryLED = 0;
-            enable_motors = 0;
-         //   pwmOut_M3.write(-1);
+
 
-           
+        }
 
-            led = 0;
-        }
-         led = !led;
+        led = !led;
 
         /* do only output via serial what's really necessary (this makes your code slow)*/
-        printf("%3.3f\n, %3.3f\n,  %3.3f;\r\n",
+        printf("%3.3f\n, %3.3f\n, %3.3f\n, %3d\n,%3d\n ",
                movement,
+               weight,
                batt,
-               //encoderCounter_M3.read(),
-               //speedController_M1.getSpeedRPS(),
-               speedController_M2.getSpeedRPS());
+               timer_1,
+               motor);
 
         /* ------------- stop hacking ------------- -------------*/
 
-        //int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
-        //int dT_loop_ms = Ts_ms - T_loop_ms;
-        //thread_sleep_for(dT_loop_ms);
+        // int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
+        // int dT_loop_ms = Ts_ms - T_loop_ms;
+        //  thread_sleep_for(dT_loop_ms);
         thread_sleep_for(10);
-    movementold = movement;
+
     }
 }
 
@@ -133,6 +196,33 @@
     user_button_timer.start();
 }
 
+void sensor_fall()
+{
+    if (situation == SENSOR) {
+        motor = 1;
+    }
+
+
+}
+void led_blinken(int fehler)
+{
+    static int timer = 0;
+
+    if (fehler == F_FUELSENSOR) {
+        if (timer == 50) {
+            timer = 0;
+
+            WeightLED = !WeightLED;
+        }
+    } else {
+        if (timer == 50) {
+
+            BatteryLED = !BatteryLED;
+            timer = 0;
+        }
+    }
+    timer ++;
+}
 void button_rise()
 {
     int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();