Diff: main.cpp

Revision:

46:31e06f30e91c

Parent:

45:a8a2670980cd

Child:

47:1fe4d9135c03

--- a/main.cpp	Wed May 18 06:52:25 2022 +0000
+++ b/main.cpp	Thu May 19 09:19:03 2022 +0200
@@ -15,6 +15,7 @@
 InterruptIn user_button(PC_13);     // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
 void user_button_pressed_fcn();     // custom functions which gets executed when user button gets pressed and released, definition below
 void user_button_released_fcn();
+float dist_conversion(float dist);
 
 int main()
 {
@@ -28,6 +29,31 @@
     // 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);
+
+    enable_motors = 1;
+
+    EncoderCounter  encoder_M1(PA_6, PC_7);
+    EncoderCounter  encoder_M2(PB_6, PB_7);
+
+    const float max_voltage = 12.0;
+    const float counts_per_turn = 20.0 * 78.125;
+    const float kn = 180.0/12.0;
+
+
+    SpeedController s1(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1);
+    s1.setMaxAccelerationRPM(99999.0);
+    s1.setSpeedCntrlGain(0.025);
+
+    // AnalogIn ir_analog_in(PC_2);
+    // float ir_distance_mV = 0.0f;
+    // float ir_distance_cm;
+
     // attach button fall and rise functions to user button object
     user_button.fall(&user_button_pressed_fcn);
     user_button.rise(&user_button_released_fcn);
@@ -35,15 +61,20 @@
     // start timer
     main_task_timer.start();
 
-    while (true) { // this loop will run forever
+    while (true) { // this loop will run foreverd
 
         main_task_timer.reset();
+
+        // ir_distance_mV = ir_analog_in.read() *3.3f *1.0e3f;
+        // ir_distance_cm = dist_conversion(ir_distance_mV);
         
        
         if (do_execute_main_task) {
 
+            s1.setDesiredSpeedRPS(2.0);
+
         } else {
-            
+            s1.setDesiredSpeedRPS(0.0);
         }
 
         // user_led is switching its state every second
@@ -51,6 +82,10 @@
             user_led = !user_led;
         }
         main_task_cntr++;
+
+        printf("%f\n", s1.getSpeedRPS());
+
+        // printf("IR sensor (mV): %f\nCM:%f\n", ir_distance_mV, ir_distance_cm);
         
         // do only output via serial what's really necessary (this makes your code slow)
         /*
@@ -82,4 +117,11 @@
     if (user_button_elapsed_time_ms > 200) {
         do_execute_main_task = !do_execute_main_task;
     }
-}
\ No newline at end of file
+}
+
+// float dist_conversion(float dist){
+//     float a = 4.655;
+//     float c = 1.092e+04;
+    
+//     return c/(dist + 1) * a;
+// }
\ No newline at end of file