Diff: main.cpp

Revision:

46:e5fb4dd7b531

Parent:

45:42adc921bc66

Child:

47:31726ce58a6d

--- a/main.cpp	Wed May 18 06:52:18 2022 +0000
+++ b/main.cpp	Wed May 18 09:29:01 2022 -0500
@@ -1,12 +1,12 @@
 #include <mbed.h>
-
 #include "PM2_Libary.h"
 #include "Eigen/Dense.h"
 
 # define M_PI 3.14159265358979323846  // number pi
 
+
 //Workshop 1
-
+float ir_distance_mV2cm (float ir_distance_mV);
 // 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
 
@@ -21,13 +21,33 @@
     // while loop gets executed every main_task_period_ms milliseconds
     const int main_task_period_ms = 10;   // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
     Timer main_task_timer;                // create Timer object which we use to run the main task every main task period time in ms
-
+    //printf("here1");
     // a coutner
     uint32_t main_task_cntr = 0;
 
     // led on nucleo board
     DigitalOut user_led(LED1);      // create DigitalOut object to command user led
 
+    DigitalOut enable_motors(PB_15);
+    FastPWM pwm_M1(PB_13);
+    FastPWM pwm_M2(PA_9);
+    
+    const float pwm_period_s = .00005f;
+    pwm_M1.period(pwm_period_s);
+    pwm_M2.period(pwm_period_s);
+    //pwm_M1.write(0.5f);
+
+    enable_motors = 1;
+
+    AnalogIn ir_analog_in(PC_2);
+    float ir_distance_mV = 0.0f;
+    float ir_distance_cm = 0.0f;
+    float distAxisToSensor = 0.12f;
+
+    //I2C i2c(PB_9, PB_8);
+
+    //SensorBar light_sensor(i2c, distAxisToSensor);
+
     // attach button fall and rise functions to user button object
     user_button.fall(&user_button_pressed_fcn);
     user_button.rise(&user_button_released_fcn);
@@ -38,19 +58,30 @@
     while (true) { // this loop will run forever
 
         main_task_timer.reset();
+        //ir_distance_mV = ir_analog_in.read()* 3.3f * 1.0e3f;
+
+        //ir_distance_cm = ir_distance_mV2cm(ir_distance_mV);
         
-       
-        if (do_execute_main_task) {
 
+        //pwm_M1.write(.75f);
+        if (do_execute_main_task) {
+            pwm_M1.write(.75f);
         } else {
-            
-        }
+            pwm_M1.write(.5f);
+        } 
+        
 
         // user_led is switching its state every second
         if ( (main_task_cntr%(1000 / main_task_period_ms) == 0) && (main_task_cntr!=0) ) {
             user_led = !user_led;
         }
         main_task_cntr++;
+        //printf("%f \r\n", light_sensor.getAngleRad());
+        //printf("IR sensor (mV): %f \r\n", ir_distance_mV);
+        //printf("IR sensor (cm): %f \r\n", ir_distance_cm);
+
+        
+        
         
         // do only output via serial what's really necessary (this makes your code slow)
         /*
@@ -82,4 +113,14 @@
     if (user_button_elapsed_time_ms > 200) {
         do_execute_main_task = !do_execute_main_task;
     }
+}
+
+float ir_distance_mV2cm (float ir_distance_mV)
+{
+    
+     // Coefficients (with 95% confidence bounds):
+       static float a =      0.8255;  //(-4.031, 5.682)
+       static float c =   1.463e+04;  //(1.224e+04, 1.702e+04)
+       float f1 = c/(ir_distance_mV + 1) + a;
+       return f1;
 }
\ No newline at end of file