Diff: main.cpp

Revision:

40:924bdbc33391

Parent:

39:f336caef17d9

Child:

42:d2d2db5974c9

--- a/main.cpp	Tue May 10 14:11:06 2022 +0200
+++ b/main.cpp	Wed May 11 09:42:50 2022 +0200
@@ -1,9 +1,15 @@
 #include <stdio.h>
 #include <mbed.h>
+#include <vector>
+
 #include "IRSensor.h"
 #include "EncoderCounterROME2.h"
 #include "Controller.h"
 
+#include "Eigen/Dense.h"
+
+# define M_PI 3.14159265358979323846  // number pi
+
 // logical variable main task
 bool do_execute_main_task = false;  // this variable will be toggled via the user button (blue button) to or not to execute the main task
 
@@ -37,6 +43,7 @@
 DigitalOut led3(PD_2);
 DigitalOut led4(PD_7);
 DigitalOut led5(PD_5);
+std::vector<DigitalOut> leds = {led0, led1, led2, led3, led4, led5};
 
 // create IR sensor objects  
 AnalogIn dist(PA_0);
@@ -79,24 +86,56 @@
     
     enableMotorDriver = 0;
     state = ROBOT_OFF;
+
+    Eigen::Matrix<float, 6, 1> irSensor_distance; // transform wheel to robot
     
     while (true) { // this loop will run forever
         
         main_task_timer.reset();
 
+        // read the distance sensors
+        irSensor_distance << irSensor0.read(),
+                             irSensor1.read(),
+                             irSensor2.read(),
+                             irSensor3.read(),
+                             irSensor4.read(),
+                             irSensor5.read();
+
         // set the leds based on distance measurements
-        led0 = irSensor0 < DISTANCE_THRESHOLD;
-        led1 = irSensor1 < DISTANCE_THRESHOLD;
-        led2 = irSensor2 < DISTANCE_THRESHOLD;
-        led3 = irSensor3 < DISTANCE_THRESHOLD;
-        led4 = irSensor4 < DISTANCE_THRESHOLD;
-        led5 = irSensor5 < DISTANCE_THRESHOLD;
+        ///*
+        // iterator based foor loop
+        int cnt = 0;
+        for(auto it = leds.begin(); it != leds.end(); it++){
+            *it =  irSensor_distance(cnt) < DISTANCE_THRESHOLD;
+            cnt++;
+        }
+        //*/
+        /*
+        // index based for loop
+        for (int i = 0; i < leds.size(); i++) {
+            leds[i] =  irSensor_distance(i) < DISTANCE_THRESHOLD;
+        }
+        */
+        /*
+        // range based for loop
+        int cnt = 0;
+        for(DigitalOut led : leds){
+            led =  irSensor_distance(cnt) < DISTANCE_THRESHOLD;
+            cnt++;
+        }
+        /*
+        led0 = irSensor_distance(0) < DISTANCE_THRESHOLD;
+        led1 = irSensor_distance(1) < DISTANCE_THRESHOLD;
+        led2 = irSensor_distance(2) < DISTANCE_THRESHOLD;
+        led3 = irSensor_distance(3) < DISTANCE_THRESHOLD;
+        led4 = irSensor_distance(4) < DISTANCE_THRESHOLD;
+        led5 = irSensor_distance(5) < DISTANCE_THRESHOLD;
+        */
 
         switch (state) {
             
             // enables Motors, sets translational speed and switches to MOVE_FORWARD
-            case ROBOT_OFF:     
-                printf("   ROBOT_OFF\r\n");      
+            case ROBOT_OFF:           
                 if (do_execute_main_task) {
                     enableMotorDriver = 1;
                     controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
@@ -111,27 +150,25 @@
                     controller.setTranslationalVelocity(0);
                     controller.setRotationalVelocity(0);
                     state = SLOWING_DOWN;
-                    printf("state = SLOWING_DOWN\r\n");
                     break;
                 }
-             
-                if (irSensor2 < DISTANCE_THRESHOLD && irSensor2 < DISTANCE_THRESHOLD) {
+
+                // ???
+                if (irSensor_distance(2) < DISTANCE_THRESHOLD && irSensor_distance(2) < DISTANCE_THRESHOLD) {
                     controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY/2);
                     controller.setRotationalVelocity(-ROTATIONAL_VELOCITY);
                     state = TURN_RIGHT;
-                    printf("state = TURN_RIGHT\r\n");
                     break;
-                } else if (irSensor4 < DISTANCE_THRESHOLD && irSensor4 < DISTANCE_THRESHOLD) {
+                // ???
+                } else if (irSensor_distance(4) < DISTANCE_THRESHOLD && irSensor_distance(4) < DISTANCE_THRESHOLD) {
                     controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY/2);
                     controller.setRotationalVelocity(ROTATIONAL_VELOCITY);
                     state = TURN_LEFT;
-                    printf("state = TURN_LEFT\r\n");
                     break;
-                } else if (irSensor3 < DISTANCE_THRESHOLD/2) {
+                } else if (irSensor_distance(3) < DISTANCE_THRESHOLD/2) {
                     controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY);
                     controller.setRotationalVelocity(-ROTATIONAL_VELOCITY);
                     state = TURN_RIGHT;
-                    printf("state = TURN_RIGHT\r\n");
                     break;
                 }
                 
@@ -145,7 +182,7 @@
                     break;
                 }
 
-                if ( (irSensor2 > DISTANCE_THRESHOLD) && (irSensor3 > DISTANCE_THRESHOLD) && (irSensor4 > DISTANCE_THRESHOLD) ) {
+                if ( (irSensor_distance(2) > DISTANCE_THRESHOLD) && (irSensor_distance(3) > DISTANCE_THRESHOLD) && (irSensor_distance(4) > DISTANCE_THRESHOLD) ) {
                     controller.setRotationalVelocity(0);
                     controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
                     state = MOVE_FORWARD;
@@ -161,7 +198,7 @@
                     break;
                 }   
 
-                if ( (irSensor2 > DISTANCE_THRESHOLD) && (irSensor3 > DISTANCE_THRESHOLD) && (irSensor4 > DISTANCE_THRESHOLD) ) {
+                if ( (irSensor_distance(2) > DISTANCE_THRESHOLD) && (irSensor_distance(3) > DISTANCE_THRESHOLD) && (irSensor_distance(4) > DISTANCE_THRESHOLD) ) {
                     controller.setRotationalVelocity(0);
                     controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
                     state = MOVE_FORWARD;
@@ -192,10 +229,11 @@
         else
             printf("button NOT pressed\r\n");
         */
-        printf("actual velocity: %.3f [m/s] / %.3f [rad/s]\r\n", controller.getActualTranslationalVelocity(), controller.getActualRotationalVelocity());
+        // printf("actual velocity: %.3f [m/s] / %.3f [rad/s]\r\n", controller.getActualTranslationalVelocity(), controller.getActualRotationalVelocity());
+        printf("%f, %f, %f, %f, %f, %f\r\n", irSensor_distance(0), irSensor_distance(1), irSensor_distance(2), irSensor_distance(3), irSensor_distance(4), irSensor_distance(5));
         
         // read timer and make the main thread sleep for the remaining time span (non blocking)
-        int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
+        auto main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
         thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
     }
 }